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 sensor_fusion.c
10 \brief The sensor_fusion.c file implements the top level programming interface
11 */
12
13 /* Including needed modules to compile this module/procedure */
14 #include <stdio.h>
15 #include "sensor_fusion.h"
16 #include "magnetic.h"
17 #include "drivers.h"
18 #include "sensor_drv.h"
19 #include "status.h"
20 #include "control.h"
21 #include "fusion.h"
22 //#include "debug.h"
23
24 /// Poor man's inheritance for status subsystem setStatus command
25 /// This function is normally involved via the "sfg." global pointer.
setStatus(SensorFusionGlobals * sfg,fusion_status_t status)26 void setStatus(SensorFusionGlobals *sfg, fusion_status_t status)
27 {
28 sfg->pStatusSubsystem->set(sfg->pStatusSubsystem, status);
29 }
30
31 /// Poor man's inheritance for status subsystem queueStatus command.
32 /// This function is normally involved via the "sfg." global pointer.
queueStatus(SensorFusionGlobals * sfg,fusion_status_t status)33 void queueStatus(SensorFusionGlobals *sfg, fusion_status_t status)
34 {
35 sfg->pStatusSubsystem->queue(sfg->pStatusSubsystem, status);
36 }
37
38 /// Poor man's inheritance for status subsystem updateStatus command.
39 /// This function is normally involved via the "sfg." global pointer.
updateStatus(SensorFusionGlobals * sfg)40 void updateStatus(SensorFusionGlobals *sfg)
41 {
42 sfg->pStatusSubsystem->update(sfg->pStatusSubsystem);
43 }
44
testStatus(SensorFusionGlobals * sfg)45 void testStatus(SensorFusionGlobals *sfg)
46 {
47 sfg->pStatusSubsystem->test(sfg->pStatusSubsystem);
48 }
49
50 /// utility function to insert default values in the top level structure
initSensorFusionGlobals(SensorFusionGlobals * sfg,StatusSubsystem * pStatusSubsystem,ControlSubsystem * pControlSubsystem)51 void initSensorFusionGlobals(SensorFusionGlobals *sfg,
52 StatusSubsystem *pStatusSubsystem,
53 ControlSubsystem *pControlSubsystem)
54 {
55 sfg->iFlags = // all of the following defines are either 0x0000 or a 1-bit value (2, 4, 8 ...) and are defined in build.h
56 F_USING_ACCEL |
57 F_USING_MAG |
58 F_USING_GYRO |
59 F_USING_PRESSURE |
60 F_USING_TEMPERATURE |
61 F_ALL_SENSORS | // refers to all applicable sensor types for the given physical unit
62 F_1DOF_P_BASIC | // 1DOF pressure (altitude) and temperature: (pressure)
63 F_3DOF_G_BASIC | // 3DOF accel tilt: (accel)
64 F_3DOF_B_BASIC | // 3DOF mag eCompass (vehicle): (mag)
65 F_3DOF_Y_BASIC | // 3DOF gyro integration: (gyro)
66 F_6DOF_GB_BASIC | // 6DOF accel and mag eCompass)
67 F_6DOF_GY_KALMAN | // 6DOF accel and gyro (Kalman): (accel + gyro)
68 F_9DOF_GBY_KALMAN ; // 9DOF accel, mag and gyro (Kalman): (accel + mag + gyro)
69
70 sfg->pControlSubsystem = pControlSubsystem;
71 sfg->pStatusSubsystem = pStatusSubsystem;
72 sfg->loopcounter = 0; // counter incrementing each iteration of sensor fusion (typically 25Hz)
73 sfg->systick_I2C = 0; // systick counter to benchmark I2C reads
74 sfg->systick_Spare = 0; // systick counter for counts spare waiting for timing interrupt
75 sfg->iPerturbation = 0; // no perturbation to be applied
76 sfg->installSensor = installSensor; // function for installing a new sensor into the structures
77 sfg->initializeFusionEngine = initializeFusionEngine; // function for installing a new sensor into the structures
78 sfg->readSensors = readSensors; // function for installing a new sensor into the structures
79 sfg->runFusion = runFusion; // function for installing a new sensor into the structures
80 sfg->applyPerturbation = ApplyPerturbation; // function used for step function testing
81 sfg->conditionSensorReadings = conditionSensorReadings; // function does averaging, HAL adjustments, etc.
82 sfg->clearFIFOs = clearFIFOs; // function to clear FIFO flags sfg->applyPerturbation = ApplyPerturbation; // function used for step function testing
83 sfg->setStatus = setStatus; // function to immediately set status change
84 sfg->queueStatus = queueStatus; // function to queue status change
85 sfg->updateStatus = updateStatus; // function to promote queued status change
86 sfg->testStatus = testStatus; // function for unit testing the status subsystem
87 sfg->pSensors = NULL; // pointer to linked list of physical sensors
88 // put error value into whoAmI as initial value
89 #if F_USING_ACCEL
90 sfg->Accel.iWhoAmI = 0;
91 #endif
92 #if F_USING_MAG
93 sfg->Mag.iWhoAmI = 0;
94 #endif
95 #if F_USING_GYRO
96 sfg->Gyro.iWhoAmI = 0;
97 #endif
98 #if F_USING_PRESSURE
99 sfg->Pressure.iWhoAmI = 0;
100 #endif
101 }
102 /// installSensor is used to instantiate a physical sensor driver into the
103 /// sensor fusion system.
104 /// This function is normally involved via the "sfg." global pointer.
installSensor(SensorFusionGlobals * sfg,struct PhysicalSensor * pSensor,uint16_t addr,uint16_t schedule,void * bus_driver,registerDeviceInfo_t * busInfo,initializeSensor_t * initialize,readSensor_t * read)105 int8_t installSensor(
106 SensorFusionGlobals *sfg, ///< top level fusion structure
107 struct PhysicalSensor *pSensor, ///< pointer to structure describing physical sensor
108 uint16_t addr, ///< I2C address for sensor (if applicable)
109 uint16_t schedule, ///< Parameter to control sensor sampling rate
110 void *bus_driver, ///< ISSDK sensor bus driver (usually KSDK I2C bus)
111 registerDeviceInfo_t *busInfo, ///< information required for bus power management
112 initializeSensor_t *initialize, ///< pointer to sensor initialization function
113 readSensor_t *read) ///< pointer to sensor read function
114 {
115 if (sfg && pSensor && bus_driver && initialize && read)
116 {
117 pSensor->bus_driver = bus_driver;
118 pSensor->deviceInfo.deviceInstance = busInfo->deviceInstance;
119 pSensor->deviceInfo.functionParam = busInfo->functionParam;
120 pSensor->deviceInfo.idleFunction = busInfo->idleFunction;
121
122 pSensor->initialize = initialize; // The initialization function is responsible for putting the sensor
123 // into the proper mode for sensor fusion. It is normally KSDK-based.
124 pSensor->read = read; // The read function is responsible for taking sensor readings and
125 // loading them into the sensor fusion input structures. Also KDSK-based.
126 pSensor->addr = addr; // I2C address if applicable
127 pSensor->schedule = schedule;
128 pSensor->slaveParams.pReadPreprocessFN = NULL; // SPI-specific parameters get overwritten later if used
129 pSensor->slaveParams.pWritePreprocessFN = NULL;
130 pSensor->slaveParams.pTargetSlavePinID = NULL;
131 pSensor->slaveParams.spiCmdLen = 0;
132 pSensor->slaveParams.ssActiveValue = 0;
133 // Now add the new sensor at the head of the linked list
134 pSensor->next = sfg->pSensors;
135 sfg->pSensors = pSensor;
136 return (0);
137 }
138 else
139 {
140 return (1);
141 }
142 }
143 // The initializeSensors function traverses the linked list of physical sensor
144 // types and calls the initialization function for each one.
initializeSensors(SensorFusionGlobals * sfg)145 int8_t initializeSensors(SensorFusionGlobals *sfg)
146 {
147 struct PhysicalSensor *pSensor;
148 int8_t s;
149 int8_t status = 0;
150 for (pSensor = sfg->pSensors; pSensor != NULL; pSensor = pSensor->next)
151 {
152 s = pSensor->initialize(pSensor, sfg);
153 if (status == 0) status = s; // will return 1st error flag, but try all sensors
154 }
155 return (status);
156 }
157
158 // process<Sensor>Data routines do post processing for HAL and averaging. They
159 // are called from the readSensors() function below.
160 #if F_USING_ACCEL
processAccelData(SensorFusionGlobals * sfg)161 void processAccelData(SensorFusionGlobals *sfg)
162 {
163 int32 iSum[3]; // channel sums
164 int16 i, j; // counters
165 if (sfg->Accel.iFIFOExceeded > 0) {
166 sfg->setStatus(sfg, SOFT_FAULT);
167 }
168
169 ApplyAccelHAL(&(sfg->Accel)); // This function is board-dependent
170
171 // calculate the average HAL-corrected measurement
172 for (j = CHX; j <= CHZ; j++) iSum[j] = 0;
173 for (i = 0; i < sfg->Accel.iFIFOCount; i++)
174 for (j = CHX; j <= CHZ; j++) iSum[j] += sfg->Accel.iGsFIFO[i][j];
175 if (sfg->Accel.iFIFOCount > 0)
176 {
177 for (j = CHX; j <= CHZ; j++)
178 {
179 sfg->Accel.iGs[j] = (int16)(iSum[j] / (int32) sfg->Accel.iFIFOCount);
180 sfg->Accel.fGs[j] = (float)sfg->Accel.iGs[j] * sfg->Accel.fgPerCount;
181 }
182 }
183
184 // apply precision accelerometer calibration (offset V, inverse gain invW and rotation correction R^T)
185 // to map fGs onto fGc (g), iGc (counts)
186 fInvertAccelCal(&(sfg->Accel), &(sfg->AccelCal));
187
188 // update the precision accelerometer data buffer
189 fUpdateAccelBuffer(&(sfg->AccelCal),
190 &(sfg->AccelBuffer),
191 &(sfg->Accel),
192 &(sfg->pControlSubsystem->AccelCalPacketOn));
193 return;
194 }
195 #endif
196 #if F_USING_MAG
processMagData(SensorFusionGlobals * sfg)197 void processMagData(SensorFusionGlobals *sfg)
198 {
199 int32 iSum[3]; // channel sums
200 int16 i, j; // counters
201
202 // printf("ProcessingMagData()\n");
203 if (sfg->Mag.iFIFOExceeded > 0) {
204 sfg->setStatus(sfg, SOFT_FAULT);
205 }
206
207 ApplyMagHAL(&(sfg->Mag)); // This function is board-dependent
208
209 // calculate the average HAL-corrected measurement
210 for (j = CHX; j <= CHZ; j++) iSum[j] = 0;
211 for (i = 0; i < sfg->Mag.iFIFOCount; i++)
212 for (j = CHX; j <= CHZ; j++) iSum[j] += sfg->Mag.iBsFIFO[i][j];
213 if (sfg->Mag.iFIFOCount > 0)
214 {
215 for (j = CHX; j <= CHZ; j++)
216 {
217 sfg->Mag.iBs[j] = (int16)(iSum[j] / (int32) sfg->Mag.iFIFOCount);
218 sfg->Mag.fBs[j] = (float)sfg->Mag.iBs[j] * sfg->Mag.fuTPerCount;
219 }
220 }
221
222 // remove hard and soft iron terms from fBs (uT) to get calibrated data fBc (uT), iBc (counts) and
223 // update magnetic buffer avoiding a write while a magnetic calibration is in progress.
224 // run one iteration of the time sliced magnetic calibration
225 fInvertMagCal(&(sfg->Mag), &(sfg->MagCal));
226 if (!sfg->MagCal.iMagBufferReadOnly)
227 iUpdateMagBuffer(&(sfg->MagBuffer), &(sfg->Mag), sfg->loopcounter);
228 fRunMagCalibration(&(sfg->MagCal), &(sfg->MagBuffer), &(sfg->Mag),
229 sfg->loopcounter);
230
231 return;
232 }
233 #endif
234 #if F_USING_GYRO
processGyroData(SensorFusionGlobals * sfg)235 void processGyroData(SensorFusionGlobals *sfg)
236 {
237 int32 iSum[3]; // channel sums
238 int16 i, j; // counters
239 if (sfg->Gyro.iFIFOExceeded > 0) {
240 sfg->setStatus(sfg, SOFT_FAULT);
241 }
242
243 ApplyGyroHAL(&(sfg->Gyro)); // This function is board-dependent
244
245 // calculate the average HAL-corrected measurement. This is used for offset
246 // initialization, display purposes and in the 3-axis gyro-only algorithm.
247 // The Kalman filters both do the full incremental rotation integration
248 // right in the filters themselves.
249 for (j = CHX; j <= CHZ; j++) iSum[j] = 0;
250 for (i = 0; i < sfg->Gyro.iFIFOCount; i++)
251 for (j = CHX; j <= CHZ; j++)
252 iSum[j] += sfg->Gyro.iYsFIFO[i][j];
253 if (sfg->Gyro.iFIFOCount > 0)
254 {
255 for (j = CHX; j <= CHZ; j++)
256 {
257 sfg->Gyro.iYs[j] = (int16)(iSum[j] / (int32) sfg->Gyro.iFIFOCount);
258 sfg->Gyro.fYs[j] = (float)sfg->Gyro.iYs[j] * sfg->Gyro.fDegPerSecPerCount;
259 }
260 }
261 return;
262 }
263 #endif
264 /// readSensors traverses the linked list of physical sensors, calling the
265 /// individual read functions one by one.
266 /// This function is normally involved via the "sfg." global pointer.
readSensors(SensorFusionGlobals * sfg,uint16_t read_loop_counter)267 int8_t readSensors(
268 SensorFusionGlobals *sfg, ///< pointer to global sensor fusion data structure
269 uint16_t read_loop_counter ///< current loop counter (used for multirate processing)
270 )
271 {
272 struct PhysicalSensor *pSensor;
273 int8_t s;
274 int8_t status = 0;
275 float remainder;
276
277 pSensor = sfg->pSensors;
278
279 for (pSensor = sfg->pSensors; pSensor != NULL; pSensor = pSensor->next)
280 { if (pSensor->isInitialized) {
281 remainder = fmod(read_loop_counter, pSensor->schedule);
282 if (remainder==0) {
283 s = pSensor->read(pSensor, sfg);
284 if (status == 0) status = s; // will return 1st error flag, but try all sensors
285 }
286 }
287 }
288 if (status==SENSOR_ERROR_INIT) sfg->setStatus(sfg, HARD_FAULT); // Never returns
289 return (status);
290 }
291 /// conditionSensorReadings() transforms raw software FIFO readings into forms that
292 /// can be consumed by the sensor fusion engine. This include sample averaging
293 /// and (in the case of the gyro) integrations, applying hardware abstraction layers,
294 /// and calibration functions.
295 /// This function is normally involved via the "sfg." global pointer.
conditionSensorReadings(SensorFusionGlobals * sfg)296 void conditionSensorReadings(SensorFusionGlobals *sfg) {
297 #if F_USING_ACCEL
298 if (sfg->Accel.isEnabled) processAccelData(sfg);
299 #endif
300
301 #if F_USING_MAG
302 if (sfg->Mag.isEnabled) processMagData(sfg);
303 #endif
304
305 #if F_USING_GYRO
306 if (sfg->Gyro.isEnabled) processGyroData(sfg);
307 #endif
308 return;
309 }
310
zeroArray(StatusSubsystem * pStatus,void * data,uint16_t size,uint16_t numElements,uint8_t check)311 void zeroArray(StatusSubsystem *pStatus, void* data, uint16_t size, uint16_t numElements, uint8_t check) {
312 uint16_t i;
313 uint8_t *d8;
314 uint16_t *d16;
315 uint32_t *d32;
316 switch(size) {
317 case 8:
318 d8 = data;
319 for (i=0; i<numElements; i++) d8[i]=0;
320 break;
321 case 16:
322 d16 = data;
323 for (i=0; i<numElements; i++) d16[i]=0;
324 break;
325 case 32:
326 d32 = data;
327 for (i=0; i<numElements; i++) d32[i]=0;
328 break;
329 default:
330 pStatus->set(pStatus, HARD_FAULT);
331 }
332 if (check) {
333 switch(size) {
334 case 8:
335 d8 = data;
336 for (i=0; i<numElements; i++)
337 if (d8[i]!=0) pStatus->set(pStatus, HARD_FAULT);
338 break;
339 case 16:
340 d16 = data;
341 for (i=0; i<numElements; i++)
342 if (d16[i]!=0) pStatus->set(pStatus, HARD_FAULT);
343 break;
344 case 32:
345 d32 = data;
346 for (i=0; i<numElements; i++)
347 if (d32[i]!=0) pStatus->set(pStatus, HARD_FAULT);
348 break;
349 }
350 return;
351 }
352 }
353 /// Function to clear FIFO at the end of each fusion computation
clearFIFOs(SensorFusionGlobals * sfg)354 void clearFIFOs(SensorFusionGlobals *sfg) {
355 // We only clear FIFOs if the sensors are enabled. This allows us
356 // to continue to use these values when we've shut higher power consumption
357 // sensors down during periods of no activity.
358 #if F_USING_ACCEL
359 sfg->Accel.iFIFOCount=0;
360 sfg->Accel.iFIFOExceeded = false;
361 #endif
362 #if F_USING_MAG
363 sfg->Mag.iFIFOCount=0;
364 sfg->Mag.iFIFOExceeded = false;
365 #endif
366 #if F_USING_GYRO
367 sfg->Gyro.iFIFOCount=0;
368 sfg->Gyro.iFIFOExceeded = false;
369 #endif
370 }
371
372 /// runFusion the top level call that actually runs the sensor fusion.
373 /// This is a utility function which manages the various defines in build.h.
374 /// You should feel free to drop down a level and implement only those portions
375 /// of fFuseSensors() that your application needs.
376 /// This function is normally involved via the "sfg." global pointer.
runFusion(SensorFusionGlobals * sfg)377 void runFusion(SensorFusionGlobals *sfg)
378 {
379 struct SV_1DOF_P_BASIC *pSV_1DOF_P_BASIC;
380 struct SV_3DOF_G_BASIC *pSV_3DOF_G_BASIC;
381 struct SV_3DOF_B_BASIC *pSV_3DOF_B_BASIC;
382 struct SV_3DOF_Y_BASIC *pSV_3DOF_Y_BASIC;
383 struct SV_6DOF_GB_BASIC *pSV_6DOF_GB_BASIC;
384 struct SV_6DOF_GY_KALMAN *pSV_6DOF_GY_KALMAN;
385 struct SV_9DOF_GBY_KALMAN *pSV_9DOF_GBY_KALMAN;
386 struct AccelSensor *pAccel;
387 struct MagSensor *pMag;
388 struct GyroSensor *pGyro;
389 struct PressureSensor *pPressure;
390 struct MagCalibration *pMagCal;
391 #if F_1DOF_P_BASIC
392 pSV_1DOF_P_BASIC = &(sfg->SV_1DOF_P_BASIC);
393 #else
394 pSV_1DOF_P_BASIC = NULL;
395 #endif
396 #if F_3DOF_G_BASIC
397 pSV_3DOF_G_BASIC = &(sfg->SV_3DOF_G_BASIC) ;
398 #else
399 pSV_3DOF_G_BASIC = NULL;
400 #endif
401 #if F_3DOF_B_BASIC
402 pSV_3DOF_B_BASIC = &(sfg->SV_3DOF_B_BASIC);
403 #else
404 pSV_3DOF_B_BASIC = NULL;
405 #endif
406 #if F_3DOF_Y_BASIC
407 pSV_3DOF_Y_BASIC = &(sfg->SV_3DOF_Y_BASIC);
408 #else
409 pSV_3DOF_Y_BASIC = NULL;
410 #endif
411 #if F_6DOF_GB_BASIC
412 pSV_6DOF_GB_BASIC = &(sfg->SV_6DOF_GB_BASIC);
413 #else
414 pSV_6DOF_GB_BASIC = NULL;
415 #endif
416 #if F_6DOF_GY_KALMAN
417 pSV_6DOF_GY_KALMAN = &(sfg->SV_6DOF_GY_KALMAN);
418 #else
419 pSV_6DOF_GY_KALMAN = NULL;
420 #endif
421 #if F_9DOF_GBY_KALMAN
422 pSV_9DOF_GBY_KALMAN = &(sfg->SV_9DOF_GBY_KALMAN);
423 #else
424 pSV_9DOF_GBY_KALMAN = NULL;
425 #endif
426 #if F_USING_ACCEL
427 pAccel = &(sfg->Accel);
428 #else
429 pAccel = NULL;
430 #endif
431 #if F_USING_MAG
432 pMag = &(sfg->Mag);
433 pMagCal = &(sfg->MagCal);
434 #else
435 pMag = NULL;
436 pMagCal = NULL;
437 #endif
438 #if F_USING_GYRO
439 pGyro = &(sfg->Gyro);
440 #else
441 pGyro = NULL;
442 #endif
443 #if F_USING_PRESSURE
444 pPressure = &(sfg->Pressure);
445 #else
446 pPressure = NULL;
447 #endif
448
449 // conditionSensorReadings(sfg); must be called prior to this function
450 // fuse the sensor data
451 fFuseSensors(pSV_1DOF_P_BASIC, pSV_3DOF_G_BASIC,
452 pSV_3DOF_B_BASIC, pSV_3DOF_Y_BASIC,
453 pSV_6DOF_GB_BASIC, pSV_6DOF_GY_KALMAN,
454 pSV_9DOF_GBY_KALMAN, pAccel, pMag, pGyro,
455 pPressure, pMagCal);
456 clearFIFOs(sfg);
457 }
458
459 /// This function is responsible for initializing the system prior to starting
460 /// the main fusion loop.
461 /// This function is normally involved via the "sfg." global pointer.
initializeFusionEngine(SensorFusionGlobals * sfg)462 void initializeFusionEngine(SensorFusionGlobals *sfg)
463 {
464 int16_t status = 0;
465 struct ControlSubsystem *pComm;
466 pComm = sfg->pControlSubsystem;
467
468 // configure the 24 bit downwards ARM systick timer and wait 50ms=CORE_SYSTICK_HZ / 20 clock ticks
469 // to avoid a race condition between Kinetis and the sensors after power on.
470 ARM_systick_enable();
471 // wait 50ms to avoid a race condition with sensors at power on
472 ARM_systick_delay_ms(CORE_SYSTICK_HZ, 50);
473
474 sfg->setStatus(sfg, INITIALIZING);
475 status = initializeSensors(sfg);
476 if (status!=SENSOR_ERROR_NONE) { // fault condition found
477 sfg->setStatus(sfg, HARD_FAULT); // Never returns
478 }
479
480 // recall: typedef enum quaternion {Q3, Q3M, Q3G, Q6MA, Q6AG, Q9} quaternion_type;
481 // Set the default quaternion to the most sophisticated supported by this build
482 pComm->DefaultQuaternionPacketType = Q3;
483 if (sfg->iFlags & F_3DOF_B_BASIC) pComm->DefaultQuaternionPacketType = Q3M;
484 if (sfg->iFlags & F_3DOF_Y_BASIC) pComm->DefaultQuaternionPacketType = Q3G;
485 if (sfg->iFlags & F_6DOF_GB_BASIC) pComm->DefaultQuaternionPacketType = Q6MA;
486 if (sfg->iFlags & F_6DOF_GY_KALMAN) pComm->DefaultQuaternionPacketType = Q6AG;
487 if (sfg->iFlags & F_9DOF_GBY_KALMAN) pComm->DefaultQuaternionPacketType = Q9;
488 pComm->QuaternionPacketType = pComm->DefaultQuaternionPacketType ;
489
490 // initialize the sensor fusion algorithms
491 fInitializeFusion(sfg);
492
493 // reset the loop counter to zero for first iteration
494 sfg->loopcounter = 0;
495
496 // initialize the magnetic calibration and magnetometer data buffer
497 #if F_USING_MAG
498 fInitializeMagCalibration(&sfg->MagCal, &sfg->MagBuffer);
499 #endif
500
501 // initialize the precision accelerometer calibration and accelerometer data buffer
502 #if F_USING_ACCEL
503 fInitializeAccelCalibration(&sfg->AccelCal, &sfg->AccelBuffer, &sfg->pControlSubsystem->AccelCalPacketOn );
504 #endif
505 sfg->setStatus(sfg, NORMAL);
506
507 clearFIFOs(sfg);
508 }
509
conditionSample(int16_t sample[3])510 void conditionSample(int16_t sample[3])
511 {
512 // This function should be called for every 16 bit sample read from sensor hardware.
513 // It is responsible for making sure that we never pass on the value of -32768.
514 // That value cannot be properly negated using 16-bit twos complement math.
515 // The ability to be later negated is required for general compatibility
516 // with possible HAL (Hardware abstraction logic) which is run later in
517 // the processing pipeline.
518 if (sample[CHX] == -32768) sample[CHX]++;
519 if (sample[CHY] == -32768) sample[CHY]++;
520 if (sample[CHZ] == -32768) sample[CHZ]++;
521 }
addToFifo(union FifoSensor * sensor,uint16_t maxFifoSize,int16_t sample[3])522 void addToFifo(union FifoSensor *sensor, uint16_t maxFifoSize, int16_t sample[3])
523 {
524 // Note that FifoSensor is a union of GyroSensor, MagSensor and AccelSensor.
525 // All contain FIFO structures in the same location. We use the Accel
526 // structure to index here.
527
528 // example usage: if (status==SENSOR_ERROR_NONE) addToFifo((FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
529 uint8_t fifoCount = sensor->Accel.iFIFOCount;
530 if (fifoCount < maxFifoSize) {
531 // we have room for the new sample
532 sensor->Accel.iGsFIFO[fifoCount][CHX] = sample[CHX];
533 sensor->Accel.iGsFIFO[fifoCount][CHY] = sample[CHY];
534 sensor->Accel.iGsFIFO[fifoCount][CHZ] = sample[CHZ];
535 sensor->Accel.iFIFOCount += 1;
536 sensor->Accel.iFIFOExceeded = 0;
537 } else {
538 //there is no room for a new sample
539 sensor->Accel.iFIFOExceeded += 1;
540 }
541 }
542
543
