| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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
49 void Fusion_Init(fusion_state_t *pState)
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
99 int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
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 {
141 START_TIMER(pState->thisSV_3DOF_G_BASIC.systick);
142 fRun_3DOF_G_BASIC(&(pState->thisSV_3DOF_G_BASIC), pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
143 STOP_TIMER(pState->thisSV_3DOF_G_BASIC.systick);
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 {
155 START_TIMER(pState->thisSV_3DOF_B_BASIC.systick);
156 fRun_3DOF_B_BASIC(&(pState->thisSV_3DOF_B_BASIC), pThisMag, pState->loopcounter, THISCOORDSYSTEM);
157 STOP_TIMER(pState->thisSV_3DOF_B_BASIC.systick);
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 {
168 START_TIMER(pState->thisSV_3DOF_Y_BASIC.systick);
169 fRun_3DOF_Y_BASIC(&(pState->thisSV_3DOF_Y_BASIC), pThisGyro, pState->loopcounter, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
170 STOP_TIMER(pState->thisSV_3DOF_Y_BASIC.systick);
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 {
183 START_TIMER(pState->thisSV_6DOF_GB_BASIC.systick);
184 fRun_6DOF_GB_BASIC(&(pState->thisSV_6DOF_GB_BASIC), pThisMag, pThisAccel, pState->loopcounter, THISCOORDSYSTEM);
185 STOP_TIMER(pState->thisSV_6DOF_GB_BASIC.systick);
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 {
197 START_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
198 fRun_6DOF_GY_KALMAN(&(pState->thisSV_6DOF_GY_KALMAN), pThisAccel, pThisGyro, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
199 STOP_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
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 {
213 START_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
214 fRun_9DOF_GBY_KALMAN(&(pState->thisSV_9DOF_GBY_KALMAN), pThisAccel, pThisMag, pThisGyro, pThisMagCal, THISCOORDSYSTEM, GYRO_OVERSAMPLE_RATIO);
215 STOP_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
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
257 void MagCal_Run(fusion_state_t *pState)
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 }
MagneticBuffer
Definition: magnetic.h:33
fRunMagCalibration
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
Definition: magnetic.c:270
Fusion_Init
void Fusion_Init(fusion_state_t *pState)
Definition: fusion_exec.c:49
SV_6DOF_GB_BASIC::systick
int32 systick
Definition: fusion_types.h:141
SV_6DOF_GY_KALMAN::systick
int32 systick
Definition: fusion_types.h:169
Fusion_Run
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
Definition: fusion_exec.c:99
Q6AG
Definition: fusion_types.h:20
fusion.h
fusion_state_t::thisMag
struct MagSensor thisMag
Definition: fusion_exec.h:59
fusion_state_t::thisMagCal
struct MagCalibration thisMagCal
Definition: fusion_exec.h:60
fusion_state_t::thisSV_9DOF_GBY_KALMAN
struct SV_9DOF_GBY_KALMAN thisSV_9DOF_GBY_KALMAN
Definition: fusion_exec.h:102
Q3M
Definition: fusion_types.h:20
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, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:1110
fusion_state_t::thisAccel
struct AccelSensor thisAccel
Definition: fusion_exec.h:55
INTERVAL7CAL
#define INTERVAL7CAL
Definition: magnetic_config.h:36
Q6MA
Definition: fusion_types.h:20
Q3
Definition: fusion_types.h:20
INTERVAL4CAL
#define INTERVAL4CAL
Definition: magnetic_config.h:35
fRun_3DOF_B_BASIC
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:403
MINMEASUREMENTS4CAL
#define MINMEASUREMENTS4CAL
Definition: magnetic_config.h:28
GYRO_OVERSAMPLE_RATIO
#define GYRO_OVERSAMPLE_RATIO
Definition: fusion_config.h:24
fusion_exec.h
int8
signed char int8
Definition: basic_types.h:12
fInitMagCalibration
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
Definition: magnetic.c:42
AccelSensor
Definition: sensor_data_types.h:28
fusion_state_t::thisSV_6DOF_GY_KALMAN
struct SV_6DOF_GY_KALMAN thisSV_6DOF_GY_KALMAN
Definition: fusion_exec.h:97
SV_6DOF_GY_KALMAN::resetflag
int8 resetflag
Definition: fusion_types.h:198
MagSensor
Definition: sensor_data_types.h:39
Q9
Definition: fusion_types.h:20
MagCalibration::iCalInProgress
int8 iCalInProgress
Definition: magnetic.h:59
Q3G
Definition: fusion_types.h:20
fRun_6DOF_GY_KALMAN
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
PARALLELNOTSEQUENTIAL
#define PARALLELNOTSEQUENTIAL
Definition: fusion_config.h:17
SV_9DOF_GBY_KALMAN::systick
int32 systick
Definition: fusion_types.h:218
fRun_3DOF_Y_BASIC
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
Definition: fusion.c:461
SV_3DOF_Y_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:121
SV_3DOF_Y_BASIC::systick
int32 systick
Definition: fusion_types.h:116
MagCalibration::iMagCalHasRun
int8 iMagCalHasRun
Definition: magnetic.h:60
fusion_state_t::thisSV_3DOF_B_BASIC
struct SV_3DOF_B_BASIC thisSV_3DOF_B_BASIC
Definition: fusion_exec.h:82
iUpdateMagnetometerBuffer
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, int32 loopcounter)
Definition: magnetic.c:79
SV_3DOF_B_BASIC::systick
int32 systick
Definition: fusion_types.h:90
fusion_state_t::algorithmToUse
quaternion_type algorithmToUse
Definition: fusion_exec.h:39
fusion_config.h
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
SV_3DOF_G_BASIC::systick
int32 systick
Definition: fusion_types.h:64
THISCOORDSYSTEM
#define THISCOORDSYSTEM
Definition: fusion_config.h:37
fusion_state_t::loopcounter
int32 loopcounter
Definition: fusion_exec.h:42
SV_6DOF_GB_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:149
fusion_state_t::thisSV_6DOF_GB_BASIC
struct SV_6DOF_GB_BASIC thisSV_6DOF_GB_BASIC
Definition: fusion_exec.h:92
fusion_state_t
Definition: fusion_exec.h:36
MagCalibration
Definition: magnetic.h:42
fInvertMagCal
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:247
START_TIMER
#define START_TIMER(x)
Definition: fusion_exec.c:45
SV_9DOF_GBY_KALMAN::resetflag
int8 resetflag
Definition: fusion_types.h:255
fRun_3DOF_G_BASIC
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
Definition: fusion.c:340
INTERVAL10CAL
#define INTERVAL10CAL
Definition: magnetic_config.h:37
fusion_state_t::thisSV_3DOF_G_BASIC
struct SV_3DOF_G_BASIC thisSV_3DOF_G_BASIC
Definition: fusion_exec.h:77
SV_3DOF_G_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:70
MagCal_Run
void MagCal_Run(fusion_state_t *pState)
Definition: fusion_exec.c:257
SV_3DOF_B_BASIC::resetflag
int8 resetflag
Definition: fusion_types.h:96
fusion_state_t::thisSV_3DOF_Y_BASIC
struct SV_3DOF_Y_BASIC thisSV_3DOF_Y_BASIC
Definition: fusion_exec.h:87
fusion_state_t::thisGyro
struct GyroSensor thisGyro
Definition: fusion_exec.h:66
MagneticBuffer::iMagBufferCount
int16 iMagBufferCount
Definition: magnetic.h:38
fRun_1DOF_P_BASIC
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
Definition: fusion.c:323
MINMEASUREMENTS10CAL
#define MINMEASUREMENTS10CAL
Definition: magnetic_config.h:30
fusion_algorithmConfig_t
Definition: fusion_types.h:258
GyroSensor
Definition: sensor_data_types.h:54
fRun_6DOF_GB_BASIC
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
STOP_TIMER
#define STOP_TIMER(x)
Definition: fusion_exec.c:46
MINMEASUREMENTS7CAL
#define MINMEASUREMENTS7CAL
Definition: magnetic_config.h:29
PressureSensor
Definition: sensor_data_types.h:15
fusion_state_t::thisMagBuffer
struct MagneticBuffer thisMagBuffer
Definition: fusion_exec.h:61
- © Freescale Semiconductor, Inc. 2015. All Rights Reserved.