ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
fusion_exec.c
Go to the documentation of this file.
1 // Copyright (c) 2015, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file defines sensor fusion apps in terms of very high level functions.
27 // It also includes functions for applying hardware abstraction mapping (HAL functions).
28 // These include:
29 // RdSensData_Init()
30 // Fusion_Init()
31 // RdSensData_Run()
32 // Fusion_Run()
33 // MagCal_Run()
34 // ApplyAccelHAL()
35 // ApplyMagHAL()
36 // ApplyGyroHAL()
37 
38 #include "isf_systick_defs.h"
39 #include "math.h"
40 #include "fusion.h"
41 #include "fusion_config.h"
42 #include "fusion_exec.h"
43 
44 #define START_TIMER(x) x = SYST_CVR & 0x00FFFFFF
45 #define STOP_TIMER(x) x -= SYST_CVR & 0x00FFFFFF; if (x < 0) x += SYST_RVR
46 void ApplyMagHAL(struct MagSensor *pthisMag);
47 // function initializes the sensor fusion and magnetic calibration and sets loopcounter to zero
49 {
50  // magnetic DOF: reset magnetic calibration and magnetometer data buffer (not needed for 3DOF_G)
51 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
52  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
53  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
54 
55  fInitMagCalibration(pThisMagCal, pThisMagBuffer);
56 #endif
57 
58  // force a reset of all the algorithms next time they execute
59 #if defined COMPUTE_1DOF_P_BASIC
60  pState->thisSV_1DOF_P_BASIC.resetflag = true;
61 #endif
62 #if defined COMPUTE_3DOF_G_BASIC
63  pState->thisSV_3DOF_G_BASIC.resetflag = true;
64  pState->algorithmToUse = Q3;
65 #endif
66 
67 #if defined COMPUTE_3DOF_B_BASIC
68  pState->thisSV_3DOF_B_BASIC.resetflag = true;
69  pState->algorithmToUse =Q3M;
70 #endif
71 
72 #if defined COMPUTE_3DOF_Y_BASIC
73  pState->thisSV_3DOF_Y_BASIC.resetflag = true;
74  pState->algorithmToUse = Q3G;
75 #endif
76 
77 #if defined COMPUTE_6DOF_GB_BASIC
78  pState->thisSV_6DOF_GB_BASIC.resetflag = true;
79  pState->algorithmToUse = Q6MA;
80 #endif
81 
82 #if defined COMPUTE_6DOF_GY_KALMAN
83  pState->thisSV_6DOF_GY_KALMAN.resetflag = true;
84  pState->algorithmToUse = Q6AG;
85 #endif
86 
87 #if defined COMPUTE_9DOF_GBY_KALMAN
88  pState->thisSV_9DOF_GBY_KALMAN.resetflag = true;
89  pState->algorithmToUse = Q9;
90 #endif
91 
92  pState->loopcounter = 0;
93 }
94 
95 
96 // function runs the sensor fusion algorithms
98 {
99  int8 initiatemagcal = 0; // flag to initiate a new magnetic calibration
100 
101 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
102  {
103 # ifdef USE_ACCELEROMETER
104  struct AccelSensor *pThisAccel = &(pState->thisAccel);
105 # endif
106  struct MagSensor *pThisMag = &(pState->thisMag);
107  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
108  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
109 
110  //ApplyMagHAL(pThisMag);
111  // remove hard and soft iron terms from Bp (uT) to get calibrated data Bc (uT)
112  fInvertMagCal(pThisMag, pThisMagCal);
113 
114 # if defined USE_ACCELEROMETER && defined USE_MAGNETOMETER
115 #if 0
116  // update magnetic buffer checking for no calibration in progress
117  if ((pState->loopcounter > 1) && !pThisMagCal->iCalInProgress)
118  {
119  // update the magnetometer measurement buffer integer magnetometer data (typically at 25Hz)
120  iUpdateMagnetometerBuffer(pThisMagBuffer, pThisMag, pState->loopcounter);
121  }
122 #endif
123 # endif
124 
125  ++pState->loopcounter;
126  }
127 #endif
128 
129  // 1DOF Pressure low pass filter algorithm
130 #if defined COMPUTE_1DOF_P_BASIC
131  {
132  struct PressureSensor *pThisPressure = &(pState->thisPressure);
133  START_TIMER(pState->thisSV_1DOF_P_BASIC.systick);
134  fRun_1DOF_P_BASIC(&(pState->thisSV_1DOF_P_BASIC), pThisPressure, pState->loopcounter);
135  STOP_TIMER(pState->thisSV_1DOF_P_BASIC.systick);
136  }
137 
138 #endif
139 
140  // 3DOF Accel Basic: call the tilt algorithm, low pass filters and Euler angle calculation
141 #if defined COMPUTE_3DOF_G_BASIC
142  {
143  struct AccelSensor *pThisAccel = &(pState->thisAccel);
144  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3))
145  {
146  START_TIMER(pState->thisSV_3DOF_G_BASIC.systick);
147  //fRun_3DOF_G_BASIC(&(pState->thisSV_3DOF_G_BASIC), pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
148  fRun_3DOF_G_BASIC(&(pState->thisSV_3DOF_G_BASIC), pThisAccel);
149  STOP_TIMER(pState->thisSV_3DOF_G_BASIC.systick);
150  }
151  }
152 #endif
153 
154  // 3DOF Magnetometer Basic: call the 2D vehicle compass algorithm
155 #if defined COMPUTE_3DOF_B_BASIC
156  {
157  struct MagSensor *pThisMag = &(pState->thisMag);
158 
159  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3M))
160  {
161  START_TIMER(pState->thisSV_3DOF_B_BASIC.systick);
162  //fRun_3DOF_B_BASIC(&(pState->thisSV_3DOF_B_BASIC), pThisMag, pState->loopcounter, THISCOORDSYSTEM);
163  fRun_3DOF_B_BASIC(&(pState->thisSV_3DOF_B_BASIC), pThisMag);
164  STOP_TIMER(pState->thisSV_3DOF_B_BASIC.systick);
165  }
166  }
167 #endif
168 
169  // 3DOF Gyro Basic: call the gyro integration algorithm
170 #if defined COMPUTE_3DOF_Y_BASIC
171  {
172  struct GyroSensor *pThisGyro = &(pState->thisGyro);
173  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3G))
174  {
175  START_TIMER(pState->thisSV_3DOF_Y_BASIC.systick);
176  //fRun_3DOF_Y_BASIC(&(pState->thisSV_3DOF_Y_BASIC), pThisGyro, pState->loopcounter, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
177  fRun_3DOF_Y_BASIC(&(pState->thisSV_3DOF_Y_BASIC), pThisGyro);
178  STOP_TIMER(pState->thisSV_3DOF_Y_BASIC.systick);
179  }
180  }
181 #endif
182 
183  // 6DOF Accel / Mag: Basic: call the eCompass orientation algorithm, low pass filters and Euler angle calculation
184 #if defined COMPUTE_6DOF_GB_BASIC
185  {
186  struct AccelSensor *pThisAccel = &(pState->thisAccel);
187  struct MagSensor *pThisMag = &(pState->thisMag);
188 
189  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q6MA))
190  {
191  START_TIMER(pState->thisSV_6DOF_GB_BASIC.systick);
192  //fRun_6DOF_GB_BASIC(&(pState->thisSV_6DOF_GB_BASIC), pThisMag, pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
193  fRun_6DOF_GB_BASIC(&(pState->thisSV_6DOF_GB_BASIC), pThisMag, pThisAccel);
194  STOP_TIMER(pState->thisSV_6DOF_GB_BASIC.systick);
195  }
196  }
197 #endif
198 
199  // 6DOF Accel / Gyro: call the Kalman orientation algorithm
200 #if defined COMPUTE_6DOF_GY_KALMAN
201  {
202  struct AccelSensor *pThisAccel = &(pState->thisAccel);
203  struct GyroSensor *pThisGyro = &(pState->thisGyro);
204  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q6AG))
205  {
206  START_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
207  //fRun_6DOF_GY_KALMAN(&(pState->thisSV_6DOF_GY_KALMAN), pThisAccel, pThisGyro, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
208  fRun_6DOF_GY_KALMAN(&(pState->thisSV_6DOF_GY_KALMAN), pThisAccel, pThisGyro);
209  STOP_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
210  }
211  }
212 #endif
213 
214  // 9DOF Accel / Mag / Gyro: apply the Kalman filter
215 #if defined COMPUTE_9DOF_GBY_KALMAN
216  {
217  struct AccelSensor *pThisAccel = &(pState->thisAccel);
218  struct MagSensor *pThisMag = &(pState->thisMag);
219  struct GyroSensor *pThisGyro = &(pState->thisGyro);
220  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
221  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q9))
222  {
223  START_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
224  //fRun_9DOF_GBY_KALMAN(&(pState->thisSV_9DOF_GBY_KALMAN), pThisAccel, pThisMag, pThisGyro, pThisMagCal, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
225  fRun_9DOF_GBY_KALMAN(&(pState->thisSV_9DOF_GBY_KALMAN), pThisAccel, pThisMag, pThisGyro, pThisMagCal);
226  STOP_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
227  }
228  }
229 #endif // COMPUTE_9DOF_GBY_KALMAN
230 
231  // decide whether to initiate a magnetic calibration
232 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
233  {
234  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
235  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
236 
237  // check no magnetic calibration is in progress
238  if (!pThisMagCal->iCalInProgress)
239  {
240  // do the first 4 element calibration immediately there are a minimum of MINMEASUREMENTS4CAL
241  initiatemagcal = (!pThisMagCal->iMagCalHasRun && (pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL));
242 
243  // otherwise initiate a calibration at intervals depending on the number of measurements available
244  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL) &&
245  (pThisMagBuffer->iMagBufferCount < MINMEASUREMENTS7CAL) &&
246  !(pState->loopcounter % INTERVAL4CAL));
247  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL) &&
248  (pThisMagBuffer->iMagBufferCount < MINMEASUREMENTS10CAL) &&
249  !(pState->loopcounter % INTERVAL7CAL));
250  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL) &&
251  !(pState->loopcounter % INTERVAL10CAL));
252 
253  // initiate the magnetic calibration if any of the conditions are met
254  if (initiatemagcal)
255  {
256  // set the flags denoting that a calibration is in progress
257  pThisMagCal->iCalInProgress = 1;
258  pThisMagCal->iMagCalHasRun = 1;
259 
260  } // end of test whether to call calibration functions
261  } // end of test that no calibration is already in progress
262  }
263 #endif
264 
265  return initiatemagcal;
266 }
267 
269 {
270  // prevent compilation errors when magnetic calibration is not required
271 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
272  struct MagSensor *pThisMag = &(pState->thisMag);
273  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
274  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
275 
276  // and run the magnetic calibration
277  fRunMagCalibration(pThisMagCal, pThisMagBuffer, pThisMag);
278 #endif
279 }
280 
281 // function applies the hardware abstraction layer to the magnetometer readings
282 void ApplyMagHAL(struct MagSensor *pthisMag)
283 {
284 #if THISCOORDSYSTEM == NED
285  int16 itmp16;
286  itmp16 = pthisMag->iBs[X];
287  pthisMag->iBs[X] = -pthisMag->iBs[Y];
288  pthisMag->iBs[Y] = -itmp16;
289  pthisMag->iBs[Z] = -pthisMag->iBs[Z];
290 #endif // NED
291 #if THISCOORDSYSTEM == ANDROID
292  pthisMag->iBs[X] = -pthisMag->iBs[X];
293  pthisMag->iBs[Y] = -pthisMag->iBs[Y];
294 #endif // Android
295 #if THISCOORDSYSTEM == WIN8
296  pthisMag->iBs[X] = -pthisMag->iBs[X];
297  pthisMag->iBs[Y] = -pthisMag->iBs[Y];
298 #endif
299 
300  // finally correct for the left handed magnetic coordinate system in MAG3110
301 #if defined USE_MAG3110
302  pthisMag->iBs[Z] = -pthisMag->iBs[Z];
303 #endif
304 
305  return;
306 }
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:278
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:48
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
Definition: fusion.c:361
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: fusion.c:551
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:97
#define STOP_TIMER(x)
Definition: fusion_exec.c:45
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
Definition: fusion.c:437
#define INTERVAL7CAL
Definition: matrix.h:38
int16 iBs[3]
#define INTERVAL4CAL
#define MINMEASUREMENTS4CAL
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:38
void ApplyMagHAL(struct MagSensor *pthisMag)
Definition: fusion_exec.c:282
int8 iCalInProgress
Definition: magnetic.h:75
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
Definition: fusion.c:489
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:75
#define PARALLELNOTSEQUENTIAL
Definition: fusion_config.h:17
signed short int int16
Definition: isf_types.h:73
int8 iMagCalHasRun
Definition: magnetic.h:76
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
Register definitions for accessing the standard ARM SYSTICK-related registers.
Definition: matrix.h:40
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
Definition: fusion.c:940
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
Definition: fusion.c:380
int32 loopcounter
Definition: fusion_exec.h:42
Definition: matrix.h:39
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:242
#define START_TIMER(x)
Definition: fusion_exec.c:44
#define INTERVAL10CAL
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:268
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
Definition: fusion.c:606
int16 iMagBufferCount
Definition: magnetic.h:55
#define MINMEASUREMENTS10CAL
#define MINMEASUREMENTS7CAL
signed char int8
Definition: isf_types.h:72