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_FXOS8700.c
10 \brief Provides init() and read() functions for the FXOS8700 6-axis accel plus mag
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_io_i2c.h" // Required for registerreadlist_t / registerwritelist_t declarations
16 #include "fxos8700.h" // describes the FXOS8700 register definition and its bit mask
17 #include "fxos8700_drv.h" // Low level IS-SDK prototype driver
18 #include "drivers.h" // Device specific drivers supplied by NXP (can be replaced with user drivers)
19 #include "status.h"
20
21 #define FXOS8700_ACCEL_FIFO_SIZE 32 ///< FXOS8700 (accel), MMA8652, FXLS8952 all have 32 element FIFO
22 #define FXOS8700_MAG_FIFO_SIZE 1 ///< FXOS8700 (mag), MAG3110 have no FIFO so equivalent to 1 element FIFO
23
24 // Command definition to read the WHO_AM_I value.
25 const registerreadlist_t FXOS8700_WHO_AM_I_READ[] =
26 {
27 { .readFrom = FXOS8700_WHO_AM_I, .numBytes = 1 }, __END_READ_DATA__
28 };
29
30 // Command definition to read the number of entries in the accel FIFO.
31 const registerreadlist_t FXOS8700_F_STATUS_READ[] =
32 {
33 { .readFrom = FXOS8700_STATUS, .numBytes = 1 }, __END_READ_DATA__
34 };
35
36 // Command definition to read the number of entries in the accel FIFO.
37 registerreadlist_t FXOS8700_DATA_READ[] =
38 {
39 { .readFrom = FXOS8700_OUT_X_MSB, .numBytes = 6 }, __END_READ_DATA__
40 };
41
42 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
43 const registerwritelist_t FXOS8700_Initialization[] =
44 {
45 // Command definition to write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS8700 into standby
46 // [7-1] = 0000 000
47 // [0]: active=0
48 { FXOS8700_CTRL_REG1, 0x00, 0x00 }, // write 0100 0000 = 0x40 to F_SETUP to enable FIFO in continuous (circular) mode
49
50 // [7-6]: f_mode[1-0]=01 for FIFO continuous mode
51 // [5-0]: f_wmrk[5-0]=000000 for no FIFO watermark
52 { FXOS8700_F_SETUP, 0x40, 0x00 },
53
54 // write 0001 1111 = 0x1F to M_CTRL_REG1
55 // [7]: m_acal=0: auto calibration disabled
56 // [6]: m_rst=0: one-shot magnetic reset disabled
57 // [5]: m_ost=0: one-shot magnetic measurement disabled
58 // [4-2]: m_os=111=7: maximum oversampling to reduce magnetometer noise
59 // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active
60 { FXOS8700_M_CTRL_REG1, 0x1F, 0x00 }, // write 0000 0000 = 0x00 to M_CTRL_REG2
61
62 // [7]: reserved
63 // [6]: reserved
64 // [5]: hyb_autoinc_mode=0 to ensure address wraparound to 0x00 to clear accelerometer FIFO in one read
65 // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used
66 // [3]: m_maxmin_dis_ths=0
67 // [2]: m_maxmin_rst=0
68 // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle
69 { FXOS8700_M_CTRL_REG2, 0x00, 0x00 },
70
71 // write 0000 0001= 0x01 to XYZ_DATA_CFG register
72 // [7]: reserved
73 // [6]: reserved
74 // [5]: reserved
75 // [4]: hpf_out=0
76 // [3]: reserved
77 // [2]: reserved
78 // [1-0]: fs=01 for 4g mode: 2048 counts / g = 8192 counts / g after 2 bit left shift
79 { FXOS8700_XYZ_DATA_CFG, 0x01, 0x00 }, // write 0000 0010 = 0x02 to CTRL_REG2 to set MODS bits
80
81 // [7]: st=0: self test disabled
82 // [6]: rst=0: reset disabled
83 // [5]: unused
84 // [4-3]: smods=00
85 // [2]: slpe=0: auto sleep disabled
86 // [1-0]: mods=10 for high resolution (maximum over sampling)
87 { FXOS8700_CTRL_REG2, 0x02, 0x00 },
88
89 // write 00XX X101 = 0x0D to accelerometer control register 1
90 // since this is a hybrid sensor with identical accelerometer and magnetometer ODR the value for ACCEL_ODR_HZ is used
91 // [7-6]: aslp_rate=00
92 // [5-3]: dr=111 for 0.78Hz data rate giving 0x3D
93 // [5-3]: dr=110 for 3.125Hz data rate giving 0x35
94 // [5-3]: dr=101 for 6.25Hz data rate giving 0x2D
95 // [5-3]: dr=100 for 25Hz data rate giving 0x25
96 // [5-3]: dr=011 for 50Hz data rate giving 0x1D
97 // [5-3]: dr=010 for 100Hz data rate giving 0x15
98 // [5-3]: dr=001 for 200Hz data rate giving 0x0D
99 // [5-3]: dr=000 for 400Hz data rate giving 0x05
100 // [2]: lnoise=1 for low noise mode (since we're in 4g mode)
101 // [1]: f_read=0 for normal 16 bit reads
102 // [0]: active=1 to take the part out of standby and enable sampling
103 #if (ACCEL_ODR_HZ <= 1) // select 0.78Hz ODR
104 { FXOS8700_CTRL_REG1, 0x3D, 0x00 },
105 #elif (ACCEL_ODR_HZ <= 3) // select 3.125Hz ODR
106 { FXOS8700_CTRL_REG1, 0x35, 0x00 },
107 #elif (ACCEL_ODR_HZ <= 6) // select 6.25Hz ODR
108 { FXOS8700_CTRL_REG1, 0x2D, 0x00 },
109 #elif (ACCEL_ODR_HZ <= 30) // select 25Hz ODR
110 { FXOS8700_CTRL_REG1, 0x25, 0x00 },
111 #elif (ACCEL_ODR_HZ <= 50) // select 50Hz ODR
112 { FXOS8700_CTRL_REG1, 0x1D, 0x00 },
113 #elif (ACCEL_ODR_HZ <= 100) // select 100Hz ODR
114 { FXOS8700_CTRL_REG1, 0x15, 0x00 },
115 #elif (ACCEL_ODR_HZ <= 200) // select 200Hz ODR
116 { FXOS8700_CTRL_REG1, 0x0D, 0x00 },
117 #else // select 400Hz ODR
118 { FXOS8700_CTRL_REG1, 0x05, 0x00 },
119 #endif
120 __END_WRITE_DATA__
121 };
122
123 #define FXOS8700_COUNTSPERG 8192.0
124 #define FXOS8700_COUNTSPERUT 10
125
126 // All sensor drivers and initialization functions have the same prototype
127 // sensor = pointer to linked list element used by the sensor fusion subsystem to specify required sensors
128
129 // sfg = pointer to top level (generally global) data structure for sensor fusion
FXOS8700_Init(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)130 int8_t FXOS8700_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
131 {
132 int32_t status;
133 uint8_t reg;
134
135 status = Register_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_WHO_AM_I, 1, ®);
136
137 if (status==SENSOR_ERROR_NONE) {
138 #if F_USING_ACCEL
139 sfg->Accel.iWhoAmI = reg;
140 sfg->Accel.iCountsPerg = FXOS8700_COUNTSPERG;
141 sfg->Accel.fgPerCount = 1.0F / FXOS8700_COUNTSPERG;
142 #endif
143 #if F_USING_MAG
144 sfg->Mag.iWhoAmI = reg;
145 sfg->Mag.iCountsPeruT = FXOS8700_COUNTSPERUT;
146 sfg->Mag.fCountsPeruT = (float) FXOS8700_COUNTSPERUT;
147 sfg->Mag.fuTPerCount = 1.0F / FXOS8700_COUNTSPERUT;
148 #endif
149 if (reg != FXOS8700_WHO_AM_I_PROD_VALUE) {
150 return SENSOR_ERROR_INIT; // The whoAmI did not match
151 }
152 } else {
153 // whoAmI will rettain default value of zero
154 // return with error
155 return status;
156 }
157
158 // Configure and start the fxos8700 sensor. This does multiple register writes
159 // (see FXOS8700_Initialization definition above)
160 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_Initialization );
161 sensor->isInitialized = F_USING_ACCEL | F_USING_MAG;
162 #if F_USING_ACCEL
163 sfg->Accel.isEnabled = true;
164 #endif
165 #if F_USING_MAG
166 sfg->Mag.isEnabled = true;
167 #endif
168
169 return (status);
170 }
171
172 #if F_USING_ACCEL
FXOS8700_ReadAccData(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)173 int8_t FXOS8700_ReadAccData(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
174 {
175 uint8_t I2C_Buffer[6 * FXOS8700_ACCEL_FIFO_SIZE]; // I2C read buffer
176 int32_t status; // I2C transaction status
177 int8_t j; // scratch
178 uint8_t sensor_fifo_count;
179 int16_t sample[3];
180
181 if(!(sensor->isInitialized & F_USING_ACCEL))
182 {
183 return SENSOR_ERROR_INIT;
184 }
185
186 // read the F_STATUS register (mapped to STATUS) and extract number of measurements available (lower 6 bits)
187 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_F_STATUS_READ, I2C_Buffer );
188 if (status != SENSOR_ERROR_NONE) return(status);
189 else {
190 #ifdef SIMULATOR_MODE
191 sensor_fifo_count = 1;
192 #else
193 sensor_fifo_count = I2C_Buffer[0] & 0x3F;
194 #endif
195 // return if there are no measurements in the sensor FIFO.
196 // this will only occur when the FAST_LOOP_HZ equals or exceeds ACCEL_ODR_HZ
197 if (sensor_fifo_count == 0) return(SENSOR_ERROR_READ);
198 }
199
200 FXOS8700_DATA_READ[0].readFrom = FXOS8700_OUT_X_MSB;
201 FXOS8700_DATA_READ[0].numBytes = 6 * sensor_fifo_count;
202 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_DATA_READ, I2C_Buffer );
203
204 if (status==SENSOR_ERROR_NONE) {
205 for (j = 0; j < sensor_fifo_count; j++) {
206 sample[CHX] = (I2C_Buffer[6 * j] << 8) | (I2C_Buffer[6 * j + 1]); // decode X
207 sample[CHY] = (I2C_Buffer[6 * j + 2] << 8) | (I2C_Buffer[6 * j + 3]); // decode Y
208 sample[CHZ] = (I2C_Buffer[6 * j + 4] << 8) | (I2C_Buffer[6 * j + 5]); // decode Z
209 conditionSample(sample); // truncate negative values to -32767
210 // place the 6 bytes read into the 16 bit accelerometer structure
211 addToFifo((union FifoSensor*) &(sfg->Accel), ACCEL_FIFO_SIZE, sample);
212 }
213 }
214
215 return (status);
216 }
217 #endif
218 #if F_USING_MAG
219 // read FXOS8700 magnetometer over I2C
FXOS8700_ReadMagData(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)220 int8_t FXOS8700_ReadMagData(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
221 {
222 uint8_t I2C_Buffer[6]; // I2C read buffer
223 int32_t status; // I2C transaction status
224 int16_t sample[3];
225
226 if(!(sensor->isInitialized & F_USING_MAG))
227 {
228 return SENSOR_ERROR_INIT;
229 }
230
231 // read the six sequential magnetometer output bytes
232 FXOS8700_DATA_READ[0].readFrom = FXOS8700_M_OUT_X_MSB;
233 FXOS8700_DATA_READ[0].numBytes = 6;
234 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_DATA_READ, I2C_Buffer );
235 if (status==SENSOR_ERROR_NONE) {
236 // place the 6 bytes read into the magnetometer structure
237 sample[CHX] = (I2C_Buffer[0] << 8) | I2C_Buffer[1];
238 sample[CHY] = (I2C_Buffer[2] << 8) | I2C_Buffer[3];
239 sample[CHZ] = (I2C_Buffer[4] << 8) | I2C_Buffer[5];
240 conditionSample(sample); // truncate negative values to -32767
241 addToFifo((union FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
242 }
243 return status;
244 }
245 #endif
246 // This is the composite read function that handles both accel and mag portions of the FXOS8700
247
248 // It returns the first failing status flag
FXOS8700_Read(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)249 int8_t FXOS8700_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
250 {
251 int8_t sts1 = 0;
252 int8_t sts2 = 0;
253 #if F_USING_ACCEL
254 sts1 = FXOS8700_ReadAccData(sensor, sfg);
255 #endif
256
257 #if F_USING_MAG
258 sts2 = FXOS8700_ReadMagData(sensor, sfg);
259 #endif
260
261 if (sts1)
262 return (sts1);
263 else
264 return (sts2);
265 }
266
267 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
268 const registerwritelist_t FXOS8700_FULL_IDLE[] =
269 {
270 // Set ACTIVE = other bits unchanged
271 { FXOS8700_CTRL_REG1, 0x00, 0x01 },
272 __END_WRITE_DATA__
273 };
274
275 // FXOS8700_Idle places the entire sensor into STANDBY mode (wakeup time = 1/ODR+1ms)
276 // This driver is all-on or all-off. It does not support mag or accel only.
277 // If you want that functionality, you can write your own using the initialization
278 // function in this file as a starting template. We've chosen not to cover all
279 // permutations in the interest of simplicity.
FXOS8700_Idle(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)280 int8_t FXOS8700_Idle(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
281 {
282 int32_t status;
283 if(sensor->isInitialized == (F_USING_ACCEL|F_USING_MAG)) {
284 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, FXOS8700_FULL_IDLE );
285 sensor->isInitialized = 0;
286 #if F_USING_ACCEL
287 sfg->Accel.isEnabled = false;
288 #endif
289 #if F_USING_MAG
290 sfg->Mag.isEnabled = false;
291 #endif
292 } else {
293 return SENSOR_ERROR_INIT;
294 }
295 return status;
296 }
297
