forked from sabas1080/FXAS21002C_Arduino_Library
/
FXAS21002C.cpp
204 lines (169 loc) · 6.65 KB
/
FXAS21002C.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
/*
* derived from: https://github.com/sabas1080/FXAS21002C_Arduino_Library
*
* modified by Francesco Ferraro 01/2016
* francesco.ferrarogm@gmail.com
*
*/
#include <Wire.h>
#include <math.h>
#include <FXAS21002C.h>
// Public Methods //////////////////////////////////////////////////////////////
FXAS21002C::FXAS21002C(byte addr)
{
address = addr;
gyroODR = GODR_200HZ; // In hybrid mode, accel/mag data sample rates are half of this value
gyroFSR = GFS_250DPS;
}
void FXAS21002C::writeReg(byte reg, byte value)
{
Wire1.beginTransmission(address);
Wire1.write(reg);
Wire1.write(value);
Wire1.endTransmission();
}
// Reads a register
byte FXAS21002C::readReg(byte reg)
{
byte value;
Wire1.beginTransmission(address);
Wire1.write(reg);
Wire1.endTransmission();
Wire1.requestFrom(address, (uint8_t)1);
value = Wire1.read();
Wire1.endTransmission();
return value;
}
void FXAS21002C::readRegs(byte reg, uint8_t count, byte dest[])
{
uint8_t i = 0;
Wire1.beginTransmission(address); // Initialize the Tx buffer
Wire1.write(reg); // Put slave register address in Tx buffer
Wire1.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire1.requestFrom(address, count); // Read bytes from slave register address
while (Wire1.available()) {
dest[i++] = Wire1.read(); // Put read results in the Rx buffer
}
}
// Read the temperature data
void FXAS21002C::readTempData()
{
tempData = readReg(FXAS21002C_H_TEMP);
}
// Put the FXAS21002C into standby mode.
// It must be in standby for modifying most registers
void FXAS21002C::standby()
{
byte c = readReg(FXAS21002C_H_CTRL_REG1);
writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03));// Clear bits 0 and 1; standby mode
}
// Sets the FXAS21000 to active mode.
// Needs to be in this mode to output data
void FXAS21002C::ready()
{
byte c = readReg(FXAS21002C_H_CTRL_REG1);
writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03)); // Clear bits 0 and 1; standby mode
writeReg(FXAS21002C_H_CTRL_REG1, c | 0x01); // Set bit 0 to 1, ready mode; no data acquisition yet
}
// Put the FXAS21002C into active mode.
// Needs to be in this mode to output data.
void FXAS21002C::active()
{
byte c = readReg(FXAS21002C_H_CTRL_REG1);
writeReg(FXAS21002C_H_CTRL_REG1, c & ~(0x03)); // Clear bits 0 and 1; standby mode
writeReg(FXAS21002C_H_CTRL_REG1, c | 0x02); // Set bit 1 to 1, active mode; data acquisition enabled
}
void FXAS21002C::init()
{
standby(); // Must be in standby to change registers
// Set up the full scale range to 250, 500, 1000, or 2000 deg/s.
writeReg(FXAS21002C_H_CTRL_REG0, gyroFSR);
// Setup the 3 data rate bits, 4:2
if (gyroODR < 8)
writeReg(FXAS21002C_H_CTRL_REG1, gyroODR << 2);
// Disable FIFO, route FIFO and rate threshold interrupts to INT2, enable data ready interrupt, route to INT1
// Active HIGH, push-pull output driver on interrupts
writeReg(FXAS21002C_H_CTRL_REG2, 0x0E);
// Set up rate threshold detection; at max rate threshold = FSR; rate threshold = THS*FSR/128
writeReg(FXAS21002C_H_RT_CFG, 0x07); // enable rate threshold detection on all axes
writeReg(FXAS21002C_H_RT_THS, 0x00 | 0x0D); // unsigned 7-bit THS, set to one-tenth FSR; set clearing debounce counter
writeReg(FXAS21002C_H_RT_COUNT, 0x04); // set to 4 (can set up to 255)
// Configure interrupts 1 and 2
//writeReg(CTRL_REG3, readReg(CTRL_REG3) & ~(0x02)); // clear bits 0, 1
//writeReg(CTRL_REG3, readReg(CTRL_REG3) | (0x02)); // select ACTIVE HIGH, push-pull interrupts
//writeReg(CTRL_REG4, readReg(CTRL_REG4) & ~(0x1D)); // clear bits 0, 3, and 4
//writeReg(CTRL_REG4, readReg(CTRL_REG4) | (0x1D)); // DRDY, Freefall/Motion, P/L and tap ints enabled
//writeReg(CTRL_REG5, 0x01); // DRDY on INT1, P/L and taps on INT2
active(); // Set to active to start reading
}
// Read the gyroscope data
void FXAS21002C::readGyroData()
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]); // Read the six raw data registers into data array
gyroData.x = ((int16_t)( rawData[0] << 8 | rawData[1])) >> 2;
gyroData.y = ((int16_t)( rawData[2] << 8 | rawData[3])) >> 2;
gyroData.z = ((int16_t)( rawData[4] << 8 | rawData[5])) >> 2;
}
// Get accelerometer resolution
float FXAS21002C::getGres(void)
{
switch (gyroFSR)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (11), 500 DPS (10), 1000 DPS (01), and 2000 DPS (00).
case GFS_2000DPS:
gRes = 1600.0/8192.0;
break;
case GFS_1000DPS:
gRes = 800.0/8192.0;
break;
case GFS_500DPS:
gRes = 400.0/8192.0;
break;
case GFS_250DPS:
gRes = 200.0/8192.0;
}
}
void FXAS21002C::calibrate(float * gBias)
{
int32_t gyro_bias[3] = {0, 0, 0};
uint16_t ii, fcount;
int16_t temp[3];
// Clear all interrupts by reading the data output and STATUS registers
//readGyroData(temp);
readReg(FXAS21002C_H_STATUS);
standby(); // Must be in standby to change registers
writeReg(FXAS21002C_H_CTRL_REG1, 0x08); // select 50 Hz ODR
fcount = 50; // sample for 1 second
writeReg(FXAS21002C_H_CTRL_REG0, 0x03); // select 200 deg/s full scale
uint16_t gyrosensitivity = 41; // 40.96 LSB/deg/s
active(); // Set to active to start collecting data
uint8_t rawData[6]; // x/y/z FIFO accel data stored here
for(ii = 0; ii < fcount; ii++) // construct count sums for each axis
{
readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]); // Read the FIFO data registers into data array
temp[0] = ((int16_t)( rawData[0] << 8 | rawData[1])) >> 2;
temp[1] = ((int16_t)( rawData[2] << 8 | rawData[3])) >> 2;
temp[2] = ((int16_t)( rawData[4] << 8 | rawData[5])) >> 2;
gyro_bias[0] += (int32_t) temp[0];
gyro_bias[1] += (int32_t) temp[1];
gyro_bias[2] += (int32_t) temp[2];
delay(25); // wait for next data sample at 50 Hz rate
}
gyro_bias[0] /= (int32_t) fcount; // get average values
gyro_bias[1] /= (int32_t) fcount;
gyro_bias[2] /= (int32_t) fcount;
gBias[0] = (float)gyro_bias[0]/(float) gyrosensitivity; // get average values
gBias[1] = (float)gyro_bias[1]/(float) gyrosensitivity; // get average values
gBias[2] = (float)gyro_bias[2]/(float) gyrosensitivity; // get average values
ready(); // Set to ready
}
void FXAS21002C::reset()
{
writeReg(FXAS21002C_H_CTRL_REG1, 0x20); // set reset bit to 1 to assert software reset to zero at end of boot process
delay(100);
while(!(readReg(FXAS21002C_H_INT_SRC_FLAG) & 0x08)) { // wait for boot end flag to be set
}
}
// Private Methods //////////////////////////////////////////////////////////////