1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /*! \file driver_FXAS21002.c
10 \brief Provides init() and read() functions for the FXAS21002 gyroscope
11 */
12
13 #include "board.h" // generated by Kinetis Expert. Long term - merge sensor_board.h into this file
14 #include "sensor_fusion.h" // Sensor fusion structures and types
15 #include "sensor_drv.h"
16 #include "sensor_io_i2c.h" // Low level IS-SDK prototype driver
17 #include "drivers.h" // Device specific drivers supplied by NXP (can be replaced with user drivers)
18 #include "fxas21002.h"
19
20 // Includes support for pre-production FXAS21000 registers and constants which are not supported via IS-SDK
21 #define FXAS21000_STATUS 0x00
22 #define FXAS21000_F_STATUS 0x08
23 #define FXAS21000_F_SETUP 0x09
24 #define FXAS21000_WHO_AM_I 0x0C
25 #define FXAS21000_CTRL_REG0 0x0D
26 #define FXAS21000_CTRL_REG1 0x13
27 #define FXAS21000_CTRL_REG2 0x14
28 #define FXAS21000_WHO_AM_I_VALUE 0xD1 // engineering and production
29 #define FXAS21000_COUNTSPERDEGPERSEC 20 // 1600dps range
30 #define FXAS21002_COUNTSPERDEGPERSEC 16 // for 2000dps=32000 counts
31
32 #define FXAS21002_GYRO_FIFO_SIZE 32 ///< FXAX21000, FXAS21002 have 32 element FIFO
33
34 #if F_USING_GYRO
35
36 // Command definition to read the WHO_AM_I value.
37 const registerreadlist_t FXAS21002_WHO_AM_I_READ[] =
38 {
39 { .readFrom = FXAS21002_WHO_AM_I, .numBytes = 1 }, __END_READ_DATA__
40 };
41
42 // Command definition to read the number of entries in the gyro status register.
43 const registerreadlist_t FXAS21002_F_STATUS_READ[] =
44 {
45 { .readFrom = FXAS21002_STATUS, .numBytes = 1 }, __END_READ_DATA__
46 };
47
48 // Command definition to read the number of entries in the accel FIFO.
49 registerreadlist_t FXAS21002_DATA_READ[] =
50 {
51 { .readFrom = FXAS21002_OUT_X_MSB, .numBytes = 1 }, __END_READ_DATA__
52 };
53
54 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
55 const registerwritelist_t FXAS21000_Initialization[] =
56 {
57 // write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS21000 in Standby
58 // [7]: ZR_cond=0
59 // [6]: RST=0
60 // [5]: ST=0 self test disabled
61 // [4-2]: DR[2-0]=000 for 200Hz ODR
62 // [1]: Active=0 for Standby mode
63 // [0]: Ready=0 but irrelevant since Active bit over-rides
64 { FXAS21000_CTRL_REG1, 0x00, 0x00 }, // write 0100 0000 = 0x40 to F_SETUP to enable FIFO in continuous mode
65
66 // [7-6]: F_MODE[1-0]=01 for FIFO continuous mode
67 // [5-0]: F_WMRK[5-0]=000000 for no FIFO watermark
68 { FXAS21000_F_SETUP, 0x40, 0x00 },
69
70 // write 0000 0000 = 0x00 to CTRL_REG0 to configure range
71 // [7-6]: unused=00
72 // [5]: SPIW=0 4 wire SPI (irrelevant)
73 // [4-3]: SEL[1-0]=00 for HPF cutoff but disabled in HPF_EN
74 // [2]: HPF_EN=0 disable HPF
75 // [1-0]: FS[1-0]=00 for 1600dps
76 { FXAS21000_CTRL_REG0, 0x00, 0x00 },
77
78 // write 000X XX10 to CTRL_REG1 to configure ODR and enter Active mode
79 // [7]: Reserved=0
80 // [6]: RST=0 for no reset
81 // [5]: ST=0 self test disabled
82 // [4-2]: DR[2-0]=111 also for 12.5Hz ODR giving 0x1E
83 // [4-2]: DR[2-0]=110 for 12.5Hz ODR giving 0x1A
84 // [4-2]: DR[2-0]=101 for 25Hz ODR giving 0x16
85 // [4-2]: DR[2-0]=100 for 50Hz ODR giving 0x12
86 // [4-2]: DR[2-0]=011 for 100Hz ODR giving 0x0E
87 // [4-2]: DR[2-0]=010 for 200Hz ODR giving 0x0A
88 // [4-2]: DR[2-0]=001 for 400Hz ODR giving 0x06
89 // [4-2]: DR[2-0]=000 for 800Hz ODR giving 0x02
90 // [1]: Active=1 for Active mode
91 // [0]: Ready=0 but irrelevant since Active bit over-rides
92 #if (GYRO_ODR_HZ <= 1) // select 1.5625Hz ODR
93 { FXAS21000_CTRL_REG1, 0x1E, 0x00 },
94 #elif (GYRO_ODR_HZ <= 3) // select 3.125Hz ODR
95 { FXAS21000_CTRL_REG1, 0x1A, 0x00 },
96 #elif (GYRO_ODR_HZ <= 6) // select 6.25Hz ODR
97 { FXAS21000_CTRL_REG1, 0x16, 0x00 },
98 #elif (GYRO_ODR_HZ <= 12) // select 12.5Hz ODR
99 { FXAS21000_CTRL_REG1, 0x12, 0x00 },
100 #elif (GYRO_ODR_HZ <= 25) // select 25.0Hz ODR
101 { FXAS21000_CTRL_REG1, 0x0E, 0x00 },
102 #elif (GYRO_ODR_HZ <= 50) // select 50.0Hz ODR
103 { FXAS21000_CTRL_REG1, 0x0A, 0x00 },
104 #elif (GYRO_ODR_HZ <= 100) // select 100.0Hz ODR
105 { FXAS21000_CTRL_REG1, 0x06, 0x00 },
106 #else // select 200Hz ODR
107 { FXAS21000_CTRL_REG1, 0x02, 0x00 },
108 #endif
109 __END_WRITE_DATA__
110 };
111
112 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
113 const registerwritelist_t FXAS21002_Initialization[] =
114 {
115 // write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS21000 in Standby
116 // [7]: ZR_cond=0
117 // [6]: RST=0
118 // [5]: ST=0 self test disabled
119 // [4-2]: DR[2-0]=000 for 200Hz ODR
120 // [1]: Active=0 for Standby mode
121 // [0]: Ready=0 but irrelevant since Active bit over-rides
122 { FXAS21002_CTRL_REG1, 0x00, 0x00 },
123
124 // [7-6]: F_MODE[1-0]=01 for FIFO continuous mode
125 // [5-0]: F_WMRK[5-0]=000000 for no FIFO watermark
126 { FXAS21002_F_SETUP, 0x40, 0x00 },
127
128 // write 0000 0000 = 0x00 to CTRL_REG0 to configure range and LPF
129 // [7-6]: BW[1-0]=00 for least aggressive LPF (0.32 * ODR cutoff for all ODR ie 64Hz cutoff at 200Hz ODR)
130 // [5]: SPIW=0 4 wire SPI (irrelevant)
131 // [4-3]: SEL[1-0]=00 for HPF cutoff but disabled in HPF_EN
132 // [2]: HPF_EN=0 to disable HPF
133 // [1-0]: FS[1-0]=00 for 2000dps
134 { FXAS21002_CTRL_REG0, 0x00, 0x00 },
135
136 // write 0000 1000 = 0x08 to CTRL_REG3 to set FIFO address wraparound on read
137 // [7-4]: Reserved=0000
138 // [3]: WRAPTOONE=1 to permit burst FIFO read with address wrapround to OUT_X_MSB
139 // [2]: EXTCTRLEN=0 for default INT2 configuration as output
140 // [1]: Reserved=0
141 // [0]: FS_DOUBLE=0 for normal 2000dps range
142 { FXAS21002_CTRL_REG3, 0x08, 0x00 },
143
144 // write 000X XX10 to CTRL_REG1 to configure ODR and enter Active mode
145 // [7]: Reserved=0
146 // [6]: RST=0 for no reset
147 // [5]: ST=0 self test disabled
148 // [4-2]: DR[2-0]=111 also for 12.5Hz ODR giving 0x1E
149 // [4-2]: DR[2-0]=110 for 12.5Hz ODR giving 0x1A
150 // [4-2]: DR[2-0]=101 for 25Hz ODR giving 0x16
151 // [4-2]: DR[2-0]=100 for 50Hz ODR giving 0x12
152 // [4-2]: DR[2-0]=011 for 100Hz ODR giving 0x0E
153 // [4-2]: DR[2-0]=010 for 200Hz ODR giving 0x0A
154 // [4-2]: DR[2-0]=001 for 400Hz ODR giving 0x06
155 // [4-2]: DR[2-0]=000 for 800Hz ODR giving 0x02
156 // [1]: Active=1 for Active mode
157 // [0]: Ready=0 but irrelevant since Active bit over-rides
158 // These values are different than the FXAS21000 values
159 #if (GYRO_ODR_HZ <= 12) // select 12.5Hz ODR
160 { FXAS21002_CTRL_REG1, 0x1A, 0x00 },
161 #elif (GYRO_ODR_HZ <= 25) // select 25Hz ODR
162 { FXAS21002_CTRL_REG1, 0x16, 0x00 },
163 #elif (GYRO_ODR_HZ <= 50) // select 50Hz ODR
164 { FXAS21002_CTRL_REG1, 0x12, 0x00 },
165 #elif (GYRO_ODR_HZ <= 100) // select 100Hz ODR
166 { FXAS21002_CTRL_REG1, 0x0E, 0x00 },
167 #elif (GYRO_ODR_HZ <= 200) // select 200Hz ODR
168 { FXAS21002_CTRL_REG1, 0x0A, 0x00 },
169 #elif (GYRO_ODR_HZ <= 400) // select 400Hz ODR
170 { FXAS21002_CTRL_REG1, 0x06, 0x00 },
171 #else // select 800Hz ODR
172 { FXAS21002_CTRL_REG1, 0x02, 0x00 },
173 #endif
174 __END_WRITE_DATA__
175 };
176
177 // All sensor drivers and initialization functions have the same prototype
178 // sensor = pointer to linked list element used by the sensor fusion subsystem to specify required sensors
179
180 // sfg = pointer to top level (generally global) data structure for sensor fusion
FXAS21002_Init(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)181 int8_t FXAS21002_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
182 {
183
184 int32_t status;
185 uint8_t reg;
186
187 status = Register_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_WHO_AM_I, 1, ®);
188 if (status==SENSOR_ERROR_NONE) {
189 sfg->Gyro.iWhoAmI = reg;
190 switch (reg) {
191 case FXAS21002_WHO_AM_I_WHOAMI_PROD_VALUE:
192 case FXAS21002_WHO_AM_I_WHOAMI_PRE_VALUE:
193 case FXAS21002_WHO_AM_I_WHOAMI_OLD_VALUE: break;
194 default:
195 // whoAmI will retain default value of zero
196 return SENSOR_ERROR_INIT; // return with error
197 }
198 } else {
199 return status; // return with error
200 }
201
202 // configure FXAS21000 or FXAS21002 depending on WHOAMI value read
203 switch (sfg->Gyro.iWhoAmI) {
204 case (FXAS21000_WHO_AM_I_VALUE):
205 // Configure and start the FXAS21000 sensor. This does multiple register writes
206 // (see FXAS21009_Initialization definition above)
207 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21000_Initialization );
208 sfg->Gyro.iCountsPerDegPerSec = FXAS21000_COUNTSPERDEGPERSEC;
209 sfg->Gyro.fDegPerSecPerCount = 1.0F / FXAS21000_COUNTSPERDEGPERSEC;
210 break;
211 case (FXAS21002_WHO_AM_I_WHOAMI_PRE_VALUE):
212 case (FXAS21002_WHO_AM_I_WHOAMI_PROD_VALUE):
213 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_Initialization );
214 sfg->Gyro.iCountsPerDegPerSec = FXAS21002_COUNTSPERDEGPERSEC;
215 sfg->Gyro.fDegPerSecPerCount = 1.0F / FXAS21002_COUNTSPERDEGPERSEC;
216 break;
217 }
218 sfg->Gyro.iFIFOCount=0;
219 sensor->isInitialized = F_USING_GYRO;
220 sfg->Gyro.isEnabled = true;
221 return (status);
222 }
223
224 // read FXAS21002 gyro over I2C
FXAS21002_Read(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)225 int8_t FXAS21002_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
226 {
227 uint8_t I2C_Buffer[6 * FXAS21002_GYRO_FIFO_SIZE]; // I2C read buffer
228 int8_t j; // scratch
229 uint8_t sensor_fifo_count = 1;
230 int32_t status;
231 int16_t sample[3];
232
233 if(sensor->isInitialized != F_USING_GYRO)
234 {
235 return SENSOR_ERROR_INIT;
236 }
237
238 // read the F_STATUS register (mapped to STATUS) and extract number of measurements available (lower 6 bits)
239 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_F_STATUS_READ, I2C_Buffer );
240 if (status==SENSOR_ERROR_NONE) {
241 #ifdef SIMULATOR_MODE
242 sensor_fifo_count = 1;
243 #else
244 sensor_fifo_count = I2C_Buffer[0] & FXAS21002_F_STATUS_F_CNT_MASK ;
245 #endif
246 // return if there are no measurements in the FIFO.
247 // this will only occur when the FAST_LOOP_HZ equals or exceeds GYRO_ODR_HZ
248 if (sensor_fifo_count == 0) return(SENSOR_ERROR_READ);
249 } else {
250 return(status);
251 }
252
253 // at this point there must be at least one measurement in the FIFO available to read.
254 // handle the FXAS21000 and FXAS21002 differently because only FXAS21002 supports WRAPTOONE feature.
255 if (sfg->Gyro.iWhoAmI == FXAS21002_WHO_AM_I_WHOAMI_OLD_VALUE)
256 {
257 // read six sequential gyro output bytes
258 FXAS21002_DATA_READ[0].readFrom = FXAS21002_OUT_X_MSB;
259 FXAS21002_DATA_READ[0].numBytes = 6;
260
261 // for FXAS21000, perform sequential 6 byte reads
262 for (j = 0; j < sensor_fifo_count; j++)
263 {
264 // read one set of measurements totalling 6 bytes
265 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_DATA_READ, I2C_Buffer );
266
267 if (status==SENSOR_ERROR_NONE) {
268 // place the measurements read into the gyroscope buffer structure
269 sample[CHX] = (I2C_Buffer[0] << 8) | I2C_Buffer[1];
270 sample[CHY] = (I2C_Buffer[2] << 8) | I2C_Buffer[3];
271 sample[CHZ] = (I2C_Buffer[4] << 8) | I2C_Buffer[5];
272 conditionSample(sample); // truncate negative values to -32767
273 addToFifo((union FifoSensor*) &(sfg->Gyro), GYRO_FIFO_SIZE, sample);
274 }
275 }
276 } // end of FXAS21000 FIFO read
277 else
278 {
279 // for FXAS21002, clear the FIFO in one using WRAPTOONE feature
280 FXAS21002_DATA_READ[0].readFrom = FXAS21002_OUT_X_MSB;
281 FXAS21002_DATA_READ[0].numBytes = 6 * sensor_fifo_count;
282 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_DATA_READ, I2C_Buffer );
283
284 if (status==SENSOR_ERROR_NONE) {
285 for (j = 0; j < sensor_fifo_count; j++) {
286 // place the measurements read into the gyroscope buffer structure
287 sample[CHX] = (I2C_Buffer[6*j + 0] << 8) | I2C_Buffer[6*j + 1];
288 sample[CHY] = (I2C_Buffer[6*j + 2] << 8) | I2C_Buffer[6*j + 3];
289 sample[CHZ] = (I2C_Buffer[6*j + 4] << 8) | I2C_Buffer[6*j + 5];
290 conditionSample(sample); // truncate negative values to -32767
291 addToFifo((union FifoSensor*) &(sfg->Gyro), GYRO_FIFO_SIZE, sample);
292 }
293 }
294 } // end of optimized FXAS21002 FIFO read
295
296 return status;
297 }
298
299
300 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
301 const registerwritelist_t FXAS21002_IDLE[] =
302 {
303 // Reset to Standby
304 { FXAS21000_CTRL_REG1, 0x00, 0x00 },
305 __END_WRITE_DATA__
306 };
307
308 // FXAS21002_Idle places the gyro into READY mode (wakeup time = 1/ODR+5ms)
FXAS21002_Idle(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)309 int8_t FXAS21002_Idle(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
310 {
311 int32_t status;
312 if(sensor->isInitialized == F_USING_GYRO) {
313 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXAS21002_IDLE );
314 sensor->isInitialized = 0;
315 sfg->Gyro.isEnabled = false;
316 } else {
317 return SENSOR_ERROR_INIT;
318 }
319 return status;
320 }
321 #endif
322
