| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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
48 void Fusion_Init(fusion_state_t *pState)
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
97 int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
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
268 void MagCal_Run(fusion_state_t *pState)
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 }
MagneticBuffer
Definition: magnetic.h:50
fRunMagCalibration
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:278
Fusion_Init
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:48
fRun_1DOF_P_BASIC
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
Definition: fusion.c:361
fRun_6DOF_GB_BASIC
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: fusion.c:551
Fusion_Run
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:97
Q6AG
Definition: fusion_types.h:51
fusion.h
STOP_TIMER
#define STOP_TIMER(x)
Definition: fusion_exec.c:45
Q3M
Definition: fusion_types.h:51
fRun_3DOF_B_BASIC
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
Definition: fusion.c:437
INTERVAL7CAL
#define INTERVAL7CAL
Definition: magnetic_config.h:41
X
Definition: matrix.h:38
Q6MA
Definition: fusion_types.h:51
Q3
Definition: fusion_types.h:51
MagSensor::iBs
int16 iBs[3]
Definition: sensor_data_types.h:56
INTERVAL4CAL
#define INTERVAL4CAL
Definition: magnetic_config.h:40
MINMEASUREMENTS4CAL
#define MINMEASUREMENTS4CAL
Definition: magnetic_config.h:33
fusion_exec.h
fInitMagCalibration
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:38
AccelSensor
Definition: sensor_data_types.h:33
ApplyMagHAL
void ApplyMagHAL(struct MagSensor *pthisMag)
Definition: fusion_exec.c:282
MagSensor
Definition: sensor_data_types.h:47
Q9
Definition: fusion_types.h:51
MagCalibration::iCalInProgress
int8 iCalInProgress
Definition: magnetic.h:75
Q3G
Definition: fusion_types.h:51
fRun_3DOF_Y_BASIC
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
Definition: fusion.c:489
iUpdateMagnetometerBuffer
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:75
PARALLELNOTSEQUENTIAL
#define PARALLELNOTSEQUENTIAL
Definition: fusion_config.h:17
int16
signed short int int16
Definition: isf_types.h:73
MagCalibration::iMagCalHasRun
int8 iMagCalHasRun
Definition: magnetic.h:76
fusion_state_t::algorithmToUse
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
isf_systick_defs.h
Register definitions for accessing the standard ARM SYSTICK-related registers.
Z
Definition: matrix.h:40
fusion_config.h
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
fRun_9DOF_GBY_KALMAN
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
fRun_3DOF_G_BASIC
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
Definition: fusion.c:380
fusion_state_t::loopcounter
int32 loopcounter
Definition: fusion_exec.h:42
Y
Definition: matrix.h:39
fusion_state_t
Definition: fusion_exec.h:36
MagCalibration
Definition: magnetic.h:59
fInvertMagCal
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:242
START_TIMER
#define START_TIMER(x)
Definition: fusion_exec.c:44
INTERVAL10CAL
#define INTERVAL10CAL
Definition: magnetic_config.h:42
MagCal_Run
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:268
fRun_6DOF_GY_KALMAN
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
Definition: fusion.c:606
MagneticBuffer::iMagBufferCount
int16 iMagBufferCount
Definition: magnetic.h:55
MINMEASUREMENTS10CAL
#define MINMEASUREMENTS10CAL
Definition: magnetic_config.h:35
fusion_algorithmConfig_t
Definition: fusion_types.h:264
GyroSensor
Definition: sensor_data_types.h:64
MINMEASUREMENTS7CAL
#define MINMEASUREMENTS7CAL
Definition: magnetic_config.h:34
PressureSensor
Definition: sensor_data_types.h:20
int8
signed char int8
Definition: isf_types.h:72
- © Freescale Semiconductor, Inc. 2016. All Rights Reserved.