Consolidate fusion algorithm to single file

fusion.c

@@ -80,6 +80,87 @@ static float fatan_deg(float x);
 static float fatan2_deg(float y, float x);
 static float fatan_15deg(float x);
 
+// 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure
+typedef struct
+{
+	// start: elements common to all motion state vectors
+	// Euler angles
+	float PhiPl;			// roll (deg)
+	float ThePl;			// pitch (deg)
+	float PsiPl;			// yaw (deg)
+	float RhoPl;			// compass (deg)
+	float ChiPl;			// tilt from vertical (deg)
+	// orientation matrix, quaternion and rotation vector
+	float RPl[3][3];		// a posteriori orientation matrix
+	Quaternion_t qPl;		// a posteriori orientation quaternion
+	float RVecPl[3];		// rotation vector
+	// angular velocity
+	float Omega[3];			// angular velocity (deg/s)
+	// systick timer for benchmarking
+	int32_t systick;		// systick timer;
+	// end: elements common to all motion state vectors
+
+	// elements transmitted over bluetooth in kalman packet
+	float bPl[3];			// gyro offset (deg/s)
+	float ThErrPl[3];		// orientation error (deg)
+	float bErrPl[3];		// gyro offset error (deg/s)
+	// end elements transmitted in kalman packet
+
+	float dErrGlPl[3];		// magnetic disturbance error (uT, global frame)
+	float dErrSePl[3];		// magnetic disturbance error (uT, sensor frame)
+	float aErrSePl[3];		// linear acceleration error (g, sensor frame)
+	float aSeMi[3];			// linear acceleration (g, sensor frame)
+	float DeltaPl;			// inclination angle (deg)
+	float aSePl[3];			// linear acceleration (g, sensor frame)
+	float aGlPl[3];			// linear acceleration (g, global frame)
+	float gErrSeMi[3];		// difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
+	float mErrSeMi[3];		// difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
+	float gSeGyMi[3];		// gravity vector (g, sensor frame) measurement from gyro
+	float mSeGyMi[3];		// geomagnetic vector (uT, sensor frame) measurement from gyro
+	float mGl[3];			// geomagnetic vector (uT, global frame)
+	float QvAA;			// accelerometer terms of Qv
+	float QvMM;			// magnetometer terms of Qv
+	float PPlus12x12[12][12];	// covariance matrix P+
+	float K12x6[12][6];		// kalman filter gain matrix K
+	float Qw12x12[12][12];		// covariance matrix Qw
+	float C6x12[6][12];		// measurement matrix C
+	float RMi[3][3];		// a priori orientation matrix
+	Quaternion_t Deltaq;		// delta quaternion
+	Quaternion_t qMi;		// a priori orientation quaternion
+	float casq;			// FCA * FCA;
+	float cdsq;			// FCD * FCD;
+	float Fastdeltat;		// sensor sampling interval (s) = 1 / SENSORFS
+	float deltat;			// kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
+	float deltatsq;			// fdeltat * fdeltat
+	float QwbplusQvG;		// FQWB + FQVG
+	int16_t FirstOrientationLock;	// denotes that 9DOF orientation has locked to 6DOF
+	int8_t resetflag;		// flag to request re-initialization on next pass
+} SV_9DOF_GBY_KALMAN_t;
+
+
+SV_9DOF_GBY_KALMAN_t fusionstate;
+
+void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV);
+void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
+	const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
+	const MagCalibration_t *MagCal);
+
+
+void fusion_init(void)
+{
+	fInit_9DOF_GBY_KALMAN(&fusionstate);
+}
+
+void fusion_update(const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
+	const MagCalibration_t *MagCal)
+{
+	fRun_9DOF_GBY_KALMAN(&fusionstate, Accel, Mag, Gyro, MagCal);
+}
+
+void fusion_read(Quaternion_t *q)
+{
+	memcpy(q, &(fusionstate.qPl), sizeof(Quaternion_t));
+}
 
 // function initializes the 9DOF Kalman filter
 void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV)
@@ -227,8 +308,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	for (j = 0; j < OVERSAMPLE_RATIO; j++) {
 		// compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
 		for (i = X; i <= Z; i++) {
-			rvec[i] = (((float)Gyro->YpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->bPl[i])
+			rvec[i] = (Gyro->YpFast[j][i] - SV->bPl[i]) * SV->Fastdeltat;
-				* SV->Fastdeltat;
 		}
 
 		// compute the incremental quaternion fDeltaq from the rotation vector

imuread.h

@@ -128,16 +128,16 @@ void fmatrixAeqRenormRotA(float A[][3]);
 #define G_PER_COUNT 0.0001220703125F  // = 1/8192
 typedef struct
 {
-	float GpFast[3];       // fast (typically 200Hz) readings (g)
 	float Gp[3];           // slow (typically 25Hz) averaged readings (g)
+	float GpFast[3];       // fast (typically 200Hz) readings (g)
 } AccelSensor_t;
 
 // magnetometer sensor structure definition
 #define UT_PER_COUNT 0.1F
 typedef struct
 {
-	float BcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
 	float Bc[3];           // slow (typically 25Hz) averaged calibrated readings (uT)
+	float BcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
 } MagSensor_t;
 
 // gyro sensor structure definition
@@ -145,74 +145,15 @@ typedef struct
 typedef struct
 {
 	float Yp[3];                           // raw gyro sensor output (deg/s)
-	int16_t YpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
+	float YpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
 } GyroSensor_t;
 
 
 
-// 9DOF Kalman filter accelerometer, magnetometer and gyroscope state vector structure
+void fusion_init(void);
-typedef struct
+void fusion_update(const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
-{
+    const MagCalibration_t *MagCal);
-	// start: elements common to all motion state vectors
+void fusion_read(Quaternion_t *q);
-	// Euler angles
-	float PhiPl;			// roll (deg)
-	float ThePl;			// pitch (deg)
-	float PsiPl;			// yaw (deg)
-	float RhoPl;			// compass (deg)
-	float ChiPl;			// tilt from vertical (deg)
-	// orientation matrix, quaternion and rotation vector
-	float RPl[3][3];		// a posteriori orientation matrix
-	Quaternion_t qPl;		// a posteriori orientation quaternion
-	float RVecPl[3];		// rotation vector
-	// angular velocity
-	float Omega[3];			// angular velocity (deg/s)
-	// systick timer for benchmarking
-	int32_t systick;		// systick timer;
-	// end: elements common to all motion state vectors
-
-	// elements transmitted over bluetooth in kalman packet
-	float bPl[3];			// gyro offset (deg/s)
-	float ThErrPl[3];		// orientation error (deg)
-	float bErrPl[3];		// gyro offset error (deg/s)
-	// end elements transmitted in kalman packet
-
-	float dErrGlPl[3];		// magnetic disturbance error (uT, global frame)
-	float dErrSePl[3];		// magnetic disturbance error (uT, sensor frame)
-	float aErrSePl[3];		// linear acceleration error (g, sensor frame)
-	float aSeMi[3];			// linear acceleration (g, sensor frame)
-	float DeltaPl;			// inclination angle (deg)
-	float aSePl[3];			// linear acceleration (g, sensor frame)
-	float aGlPl[3];			// linear acceleration (g, global frame)
-	float gErrSeMi[3];		// difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
-	float mErrSeMi[3];		// difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
-	float gSeGyMi[3];		// gravity vector (g, sensor frame) measurement from gyro
-	float mSeGyMi[3];		// geomagnetic vector (uT, sensor frame) measurement from gyro
-	float mGl[3];			// geomagnetic vector (uT, global frame)
-	float QvAA;			// accelerometer terms of Qv
-	float QvMM;			// magnetometer terms of Qv
-	float PPlus12x12[12][12];	// covariance matrix P+
-	float K12x6[12][6];		// kalman filter gain matrix K
-	float Qw12x12[12][12];		// covariance matrix Qw
-	float C6x12[6][12];		// measurement matrix C
-	float RMi[3][3];		// a priori orientation matrix
-	Quaternion_t Deltaq;		// delta quaternion
-	Quaternion_t qMi;		// a priori orientation quaternion
-	float casq;			// FCA * FCA;
-	float cdsq;			// FCD * FCD;
-	float Fastdeltat;		// sensor sampling interval (s) = 1 / SENSORFS
-	float deltat;			// kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
-	float deltatsq;			// fdeltat * fdeltat
-	float QwbplusQvG;		// FQWB + FQVG
-	int16_t FirstOrientationLock;	// denotes that 9DOF orientation has locked to 6DOF
-	int8_t resetflag;		// flag to request re-initialization on next pass
-} SV_9DOF_GBY_KALMAN_t;
-
-void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV);
-void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
-	const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
-	const MagCalibration_t *MagCal);
-
-
 
 #ifdef __cplusplus
 } // extern "C"

rawdata.c

@@ -5,12 +5,11 @@ static int rawcount=OVERSAMPLE_RATIO;
 static AccelSensor_t accel;
 static MagSensor_t   mag;
 static GyroSensor_t  gyro;
-SV_9DOF_GBY_KALMAN_t fusionstate;
 
 void raw_data_reset(void)
 {
 	rawcount = OVERSAMPLE_RATIO;
-	fInit_9DOF_GBY_KALMAN(&fusionstate);
+	fusion_init();
 	memset(&magcal, 0, sizeof(magcal));
 	magcal.V[2] = 80.0f;  // initial guess
 	magcal.invW[0][0] = 1.0f;
@@ -75,7 +74,7 @@ void raw_data(const int16_t *data)
 		magdiff = sqrtf(x * x + y * y + z * z);
 		//printf("magdiff = %.2f\n", magdiff);
 		if (magdiff > 0.8f) {
-			fInit_9DOF_GBY_KALMAN(&fusionstate);
+			fusion_init();
 			rawcount = OVERSAMPLE_RATIO;
 			force_orientation_counter = 240;
 		}
@@ -84,7 +83,7 @@ void raw_data(const int16_t *data)
 	if (force_orientation_counter > 0) {
 		if (--force_orientation_counter == 0) {
 			//printf("delayed forcible orientation reset\n");
-			fInit_9DOF_GBY_KALMAN(&fusionstate);
+			fusion_init();
 			rawcount = OVERSAMPLE_RATIO;
 		}
 	}
@@ -111,9 +110,9 @@ void raw_data(const int16_t *data)
 	gyro.Yp[0] += x;
 	gyro.Yp[1] += y;
 	gyro.Yp[2] += z;
-	gyro.YpFast[rawcount][0] = data[3];
+	gyro.YpFast[rawcount][0] = x;
-	gyro.YpFast[rawcount][1] = data[4];
+	gyro.YpFast[rawcount][1] = y;
-	gyro.YpFast[rawcount][2] = data[5];
+	gyro.YpFast[rawcount][2] = z;
 
 	apply_calibration(data[6], data[7], data[8], &point);
 	mag.BcFast[0] = point.x;
@@ -135,9 +134,8 @@ void raw_data(const int16_t *data)
 		mag.Bc[0] *= ratio;
 		mag.Bc[1] *= ratio;
 		mag.Bc[2] *= ratio;
-		fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro, &magcal);
+		fusion_update(&accel, &mag, &gyro, &magcal);
-
+		fusion_read(&current_orientation);
-		memcpy(&current_orientation, &(fusionstate.qPl), sizeof(Quaternion_t));
 	}
 }