44 #define START_TIMER(x) x = SYST_CVR & 0x00FFFFFF
45 #define STOP_TIMER(x) x -= SYST_CVR & 0x00FFFFFF; if (x < 0) x += SYST_RVR
51 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
59 #if defined COMPUTE_1DOF_P_BASIC
60 pState->thisSV_1DOF_P_BASIC.resetflag =
true;
62 #if defined COMPUTE_3DOF_G_BASIC
63 pState->thisSV_3DOF_G_BASIC.resetflag =
true;
67 #if defined COMPUTE_3DOF_B_BASIC
68 pState->thisSV_3DOF_B_BASIC.resetflag =
true;
72 #if defined COMPUTE_3DOF_Y_BASIC
73 pState->thisSV_3DOF_Y_BASIC.resetflag =
true;
77 #if defined COMPUTE_6DOF_GB_BASIC
78 pState->thisSV_6DOF_GB_BASIC.resetflag =
true;
82 #if defined COMPUTE_6DOF_GY_KALMAN
83 pState->thisSV_6DOF_GY_KALMAN.resetflag =
true;
87 #if defined COMPUTE_9DOF_GBY_KALMAN
88 pState->thisSV_9DOF_GBY_KALMAN.resetflag =
true;
99 int8 initiatemagcal = 0;
101 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
103 # ifdef USE_ACCELEROMETER
104 struct AccelSensor *pThisAccel = &(pState->thisAccel);
106 struct MagSensor *pThisMag = &(pState->thisMag);
114 # if defined USE_ACCELEROMETER && defined USE_MAGNETOMETER
130 #if defined COMPUTE_1DOF_P_BASIC
135 STOP_TIMER(pState->thisSV_1DOF_P_BASIC.systick);
141 #if defined COMPUTE_3DOF_G_BASIC
143 struct AccelSensor *pThisAccel = &(pState->thisAccel);
149 STOP_TIMER(pState->thisSV_3DOF_G_BASIC.systick);
155 #if defined COMPUTE_3DOF_B_BASIC
157 struct MagSensor *pThisMag = &(pState->thisMag);
164 STOP_TIMER(pState->thisSV_3DOF_B_BASIC.systick);
170 #if defined COMPUTE_3DOF_Y_BASIC
172 struct GyroSensor *pThisGyro = &(pState->thisGyro);
178 STOP_TIMER(pState->thisSV_3DOF_Y_BASIC.systick);
184 #if defined COMPUTE_6DOF_GB_BASIC
186 struct AccelSensor *pThisAccel = &(pState->thisAccel);
187 struct MagSensor *pThisMag = &(pState->thisMag);
194 STOP_TIMER(pState->thisSV_6DOF_GB_BASIC.systick);
200 #if defined COMPUTE_6DOF_GY_KALMAN
202 struct AccelSensor *pThisAccel = &(pState->thisAccel);
203 struct GyroSensor *pThisGyro = &(pState->thisGyro);
206 START_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
209 STOP_TIMER(pState->thisSV_6DOF_GY_KALMAN.systick);
215 #if defined COMPUTE_9DOF_GBY_KALMAN
217 struct AccelSensor *pThisAccel = &(pState->thisAccel);
218 struct MagSensor *pThisMag = &(pState->thisMag);
219 struct GyroSensor *pThisGyro = &(pState->thisGyro);
223 START_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
225 fRun_9DOF_GBY_KALMAN(&(pState->thisSV_9DOF_GBY_KALMAN), pThisAccel, pThisMag, pThisGyro, pThisMagCal);
226 STOP_TIMER(pState->thisSV_9DOF_GBY_KALMAN.systick);
229 #endif // COMPUTE_9DOF_GBY_KALMAN
232 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
265 return initiatemagcal;
271 #if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
272 struct MagSensor *pThisMag = &(pState->thisMag);
284 #if THISCOORDSYSTEM == NED
286 itmp16 = pthisMag->
iBs[
X];
287 pthisMag->
iBs[
X] = -pthisMag->
iBs[
Y];
288 pthisMag->
iBs[
Y] = -itmp16;
289 pthisMag->
iBs[
Z] = -pthisMag->
iBs[
Z];
291 #if THISCOORDSYSTEM == ANDROID
292 pthisMag->
iBs[
X] = -pthisMag->
iBs[
X];
293 pthisMag->
iBs[
Y] = -pthisMag->
iBs[
Y];
295 #if THISCOORDSYSTEM == WIN8
296 pthisMag->
iBs[
X] = -pthisMag->
iBs[
X];
297 pthisMag->
iBs[
Y] = -pthisMag->
iBs[
Y];
301 #if defined USE_MAG3110
302 pthisMag->
iBs[
Z] = -pthisMag->
iBs[
Z];
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
void Fusion_Init(fusion_state_t *pState)
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
int8 Fusion_Run(fusion_state_t *pState, fusion_algorithmConfig_t *pAlgorithmConfig)
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
#define MINMEASUREMENTS4CAL
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
void ApplyMagHAL(struct MagSensor *pthisMag)
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
#define PARALLELNOTSEQUENTIAL
quaternion_type algorithmToUse
Register definitions for accessing the standard ARM SYSTICK-related registers.
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)
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
void MagCal_Run(fusion_state_t *pState)
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
#define MINMEASUREMENTS10CAL
#define MINMEASUREMENTS7CAL