ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
fusion_exec.c
Go to the documentation of this file.
1 // Copyright (c) 2014, 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 "MKL25Z4.h"
39 #include "math.h"
40 #include "PE_Types.h"
41 #include "fusion.h"
42 #include "fusion_config.h"
43 #include "fusion_exec.h"
44 
45 #define START_TIMER(x) x = SYST_CVR & 0x00FFFFFF
46 #define STOP_TIMER(x) x -= SYST_CVR & 0x00FFFFFF; if (x < 0) x += SYST_RVR
47 
48 // function initializes the sensor fusion and magnetic calibration and sets loopcounter to zero
50 {
51 
52  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
53  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
54 
55  // magnetic DOF: reset magnetic calibration and magnetometer data buffer (not needed for 3DOF_G)
56 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
57  fInitMagCalibration(pThisMagCal, pThisMagBuffer);
58 #endif
59 
60  // force a reset of all the algorithms next time they execute
61 #if defined COMPUTE_1DOF_P_BASIC
62  pState->thisSV_1DOF_P_BASIC.resetflag = true;
63 #endif
64 #if defined COMPUTE_3DOF_G_BASIC
65  pState->thisSV_3DOF_G_BASIC.resetflag = true;
66  pState->algorithmToUse = Q3;
67 #endif
68 
69 #if defined COMPUTE_3DOF_B_BASIC
70  pState->thisSV_3DOF_B_BASIC.resetflag = true;
71  pState->algorithmToUse =Q3M;
72 #endif
73 
74 #if defined COMPUTE_3DOF_Y_BASIC
75  pState->thisSV_3DOF_Y_BASIC.resetflag = true;
76  pState->algorithmToUse = Q3G;
77 #endif
78 
79 #if defined COMPUTE_6DOF_GB_BASIC
80  pState->thisSV_6DOF_GB_BASIC.resetflag = true;
81  pState->algorithmToUse = Q6MA;
82 #endif
83 
84 #if defined COMPUTE_6DOF_GY_KALMAN
85  pState->thisSV_6DOF_GY_KALMAN.resetflag = true;
86  pState->algorithmToUse = Q6AG;
87 #endif
88 
89 #if defined COMPUTE_9DOF_GBY_KALMAN
90  pState->thisSV_9DOF_GBY_KALMAN.resetflag = true;
91  pState->algorithmToUse = Q9;
92 #endif
93 
94  pState->loopcounter = 0;
95 }
96 
97 
98 // function runs the sensor fusion algorithms
100 {
101  int8 initiatemagcal = 0; // flag to initiate a new magnetic calibration
102 
103 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
104  {
105  struct AccelSensor *pThisAccel = &(pState->thisAccel);
106  struct MagSensor *pThisMag = &(pState->thisMag);
107  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
108  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
109 
110  // remove hard and soft iron terms from Bp (uT) to get calibrated data Bc (uT)
111  fInvertMagCal(pThisMag, pThisMagCal);
112 
113  // update magnetic buffer checking for no calibration in progress
114  if ((pState->loopcounter > 1) && !pThisMagCal->iCalInProgress)
115  {
116  // update the magnetometer measurement buffer integer magnetometer data (typically at 25Hz)
117  iUpdateMagnetometerBuffer(pThisMagBuffer, pThisAccel, pThisMag, pState->loopcounter);
118  }
119 
120  ++pState->loopcounter;
121  }
122 #endif
123 
124  // 1DOF Pressure low pass filter algorithm
125 #if defined COMPUTE_1DOF_P_BASIC
126  {
127  struct PressureSensor *pThisPressure = &(pState->thisPressure);
128  START_TIMER(pState->thisSV_1DOF_P_BASIC.systick);
129  fRun_1DOF_P_BASIC(&(pState->thisSV_1DOF_P_BASIC), pThisPressure, pState->loopcounter);
130  STOP_TIMER(pState->thisSV_1DOF_P_BASIC.systick);
131  }
132 
133 #endif
134 
135  // 3DOF Accel Basic: call the tilt algorithm, low pass filters and Euler angle calculation
136 #if defined COMPUTE_3DOF_G_BASIC
137  {
138  struct AccelSensor *pThisAccel = &(pState->thisAccel);
139  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3))
140  {
142  fRun_3DOF_G_BASIC(&(pState->thisSV_3DOF_G_BASIC), pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
144  }
145  }
146 #endif
147 
148  // 3DOF Magnetometer Basic: call the 2D vehicle compass algorithm
149 #if defined COMPUTE_3DOF_B_BASIC
150  {
151  struct MagSensor *pThisMag = &(pState->thisMag);
152 
153  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3M))
154  {
156  fRun_3DOF_B_BASIC(&(pState->thisSV_3DOF_B_BASIC), pThisMag, pState->loopcounter, THISCOORDSYSTEM);
158  }
159  }
160 #endif
161 
162  // 3DOF Gyro Basic: call the gyro integration algorithm
163 #if defined COMPUTE_3DOF_Y_BASIC
164  {
165  struct GyroSensor *pThisGyro = &(pState->thisGyro);
166  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q3G))
167  {
171  }
172  }
173 #endif
174 
175  // 6DOF Accel / Mag: Basic: call the eCompass orientation algorithm, low pass filters and Euler angle calculation
176 #if defined COMPUTE_6DOF_GB_BASIC
177  {
178  struct AccelSensor *pThisAccel = &(pState->thisAccel);
179  struct MagSensor *pThisMag = &(pState->thisMag);
180 
181  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q6MA))
182  {
184  fRun_6DOF_GB_BASIC(&(pState->thisSV_6DOF_GB_BASIC), pThisMag, pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
186  }
187  }
188 #endif
189 
190  // 6DOF Accel / Gyro: call the Kalman orientation algorithm
191 #if defined COMPUTE_6DOF_GY_KALMAN
192  {
193  struct AccelSensor *pThisAccel = &(pState->thisAccel);
194  struct GyroSensor *pThisGyro = &(pState->thisGyro);
195  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q6AG))
196  {
200  }
201  }
202 #endif
203 
204  // 9DOF Accel / Mag / Gyro: apply the Kalman filter
205 #if defined COMPUTE_9DOF_GBY_KALMAN
206  {
207  struct AccelSensor *pThisAccel = &(pState->thisAccel);
208  struct MagSensor *pThisMag = &(pState->thisMag);
209  struct GyroSensor *pThisGyro = &(pState->thisGyro);
210  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
211  if (PARALLELNOTSEQUENTIAL || (pState->algorithmToUse == Q9))
212  {
214  fRun_9DOF_GBY_KALMAN(&(pState->thisSV_9DOF_GBY_KALMAN), pThisAccel, pThisMag, pThisGyro, pThisMagCal, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
216  }
217  }
218 #endif // COMPUTE_9DOF_GBY_KALMAN
219 
220  // decide whether to initiate a magnetic calibration
221 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
222  {
223  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
224  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
225 
226  // check no magnetic calibration is in progress
227  if (!pThisMagCal->iCalInProgress)
228  {
229  // do the first 4 element calibration immediately there are a minimum of MINMEASUREMENTS4CAL
230  initiatemagcal = (!pThisMagCal->iMagCalHasRun && (pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL));
231 
232  // otherwise initiate a calibration at intervals depending on the number of measurements available
233  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS4CAL) &&
234  (pThisMagBuffer->iMagBufferCount < MINMEASUREMENTS7CAL) &&
235  !(pState->loopcounter % INTERVAL4CAL));
236  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS7CAL) &&
237  (pThisMagBuffer->iMagBufferCount < MINMEASUREMENTS10CAL) &&
238  !(pState->loopcounter % INTERVAL7CAL));
239  initiatemagcal |= ((pThisMagBuffer->iMagBufferCount >= MINMEASUREMENTS10CAL) &&
240  !(pState->loopcounter % INTERVAL10CAL));
241 
242  // initiate the magnetic calibration if any of the conditions are met
243  if (initiatemagcal)
244  {
245  // set the flags denoting that a calibration is in progress
246  pThisMagCal->iCalInProgress = 1;
247  pThisMagCal->iMagCalHasRun = 1;
248 
249  } // end of test whether to call calibration functions
250  } // end of test that no calibration is already in progress
251  }
252 #endif
253 
254  return initiatemagcal;
255 }
256 
258 {
259  // prevent compilation errors when magnetic calibration is not required
260 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
261  struct MagSensor *pThisMag = &(pState->thisMag);
262  struct MagCalibration *pThisMagCal = &(pState->thisMagCal);
263  struct MagneticBuffer *pThisMagBuffer = &(pState->thisMagBuffer);
264 
265  // and run the magnetic calibration
266  fRunMagCalibration(pThisMagCal, pThisMagBuffer, pThisMag);
267 #endif
268 }
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:270
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:49
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:99
struct MagSensor thisMag
Definition: fusion_exec.h:59
struct MagCalibration thisMagCal
Definition: fusion_exec.h:60
struct SV_9DOF_GBY_KALMAN thisSV_9DOF_GBY_KALMAN
Definition: fusion_exec.h:102
void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:1110
struct AccelSensor thisAccel
Definition: fusion_exec.h:55
#define INTERVAL7CAL
#define INTERVAL4CAL
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:403
#define MINMEASUREMENTS4CAL
#define GYRO_OVERSAMPLE_RATIO
Definition: fusion_config.h:24
signed char int8
Definition: basic_types.h:12
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:42
struct SV_6DOF_GY_KALMAN thisSV_6DOF_GY_KALMAN
Definition: fusion_exec.h:97
int8 iCalInProgress
Definition: magnetic.h:59
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:593
#define PARALLELNOTSEQUENTIAL
Definition: fusion_config.h:17
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:461
int8 iMagCalHasRun
Definition: magnetic.h:60
struct SV_3DOF_B_BASIC thisSV_3DOF_B_BASIC
Definition: fusion_exec.h:82
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:79
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
#define THISCOORDSYSTEM
Definition: fusion_config.h:37
int32 loopcounter
Definition: fusion_exec.h:42
struct SV_6DOF_GB_BASIC thisSV_6DOF_GB_BASIC
Definition: fusion_exec.h:92
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:247
#define START_TIMER(x)
Definition: fusion_exec.c:45
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:340
#define INTERVAL10CAL
struct SV_3DOF_G_BASIC thisSV_3DOF_G_BASIC
Definition: fusion_exec.h:77
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:257
struct SV_3DOF_Y_BASIC thisSV_3DOF_Y_BASIC
Definition: fusion_exec.h:87
struct GyroSensor thisGyro
Definition: fusion_exec.h:66
int16 iMagBufferCount
Definition: magnetic.h:38
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
Definition: fusion.c:323
#define MINMEASUREMENTS10CAL
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:532
#define STOP_TIMER(x)
Definition: fusion_exec.c:46
#define MINMEASUREMENTS7CAL
struct MagneticBuffer thisMagBuffer
Definition: fusion_exec.h:61