Reset fusion filter after substantial magcal changes

imuread.h

@@ -38,7 +38,7 @@
   #define PORT "/dev/cu.usbmodemfd132"
 #endif
 
-#define TIMEOUT_MSEC 33
+#define TIMEOUT_MSEC 14
 
 #define MAGBUFFSIZE 650 // Freescale's lib needs at least 392
 
@@ -73,7 +73,7 @@ void visualize_init(void);
 void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out);
 void display_callback(void);
 void resize_callback(int width, int height);
-void MagCal_Run(void);
+int MagCal_Run(void);
 void quality_reset(void);
 void quality_update(const Point_t *point);
 float quality_surface_gap_error(void);
@@ -128,39 +128,24 @@ void fmatrixAeqRenormRotA(float A[][3]);
 #define G_PER_COUNT 0.0001220703125F  // = 1/8192
 typedef struct
 {
-	//int32_t iSumGpFast[3];    // sum of fast measurements
 	float GpFast[3];       // fast (typically 200Hz) readings (g)
 	float Gp[3];           // slow (typically 25Hz) averaged readings (g)
-	//float fgPerCount;       // initialized to FGPERCOUNT
-	//int16_t iGpFast[3];       // fast (typically 200Hz) readings
-	//int16_t iGp[3];           // slow (typically 25Hz) averaged readings (counts)
 } AccelSensor_t;
 
 // magnetometer sensor structure definition
 #define UT_PER_COUNT 0.1F
 typedef struct
 {
-	//int32_t iSumBpFast[3];    // sum of fast measurements
-	//float fBpFast[3];       // fast (typically 200Hz) raw readings (uT)
-	//float fBp[3];           // slow (typically 25Hz) averaged raw readings (uT)
 	float BcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
 	float Bc[3];           // slow (typically 25Hz) averaged calibrated readings (uT)
-	//float fuTPerCount;      // initialized to FUTPERCOUNT
-	//float fCountsPeruT;     // initialized to FCOUNTSPERUT
-	//int16_t iBpFast[3];       // fast (typically 200Hz) raw readings (counts)
-	//int16_t iBp[3];           // slow (typically 25Hz) averaged raw readings (counts)
-	//int16_t iBc[3];           // slow (typically 25Hz) averaged calibrated readings (counts)
 } MagSensor_t;
 
 // gyro sensor structure definition
 #define DEG_PER_SEC_PER_COUNT 0.0625F  // = 1/16
 typedef struct
 {
-	//int32_t iSumYpFast[3];                    // sum of fast measurements
 	float Yp[3];                           // raw gyro sensor output (deg/s)
-	//float fDegPerSecPerCount;               // initialized to FDEGPERSECPERCOUNT
 	int16_t YpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
-	//int16_t iYp[3];                           // averaged gyro sensor output (counts)
 } GyroSensor_t;
 
 

magcal.c

@@ -44,8 +44,8 @@
 #define MINMEASUREMENTS4CAL 40      // minimum number of measurements for 4 element calibration
 #define MINMEASUREMENTS7CAL 100     // minimum number of measurements for 7 element calibration
 #define MINMEASUREMENTS10CAL 150    // minimum number of measurements for 10 element calibration
-#define MINBFITUT 15.0F             // minimum geomagnetic field B (uT) for valid calibration
-#define MAXBFITUT 80.0F             // maximum geomagnetic field B (uT) for valid calibration
+#define MINBFITUT 22.0F             // minimum geomagnetic field B (uT) for valid calibration
+#define MAXBFITUT 67.0F             // maximum geomagnetic field B (uT) for valid calibration
 #define FITERRORAGINGSECS 7200.0F   // 2 hours: time for fit error to increase (age) by e=2.718
 
 static void fUpdateCalibration4INV(MagCalibration_t *MagCal);
@@ -55,7 +55,7 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal);
 
 
 // run the magnetic calibration
-void MagCal_Run(void)
+int MagCal_Run(void)
 {
 	int i, j;			// loop counters
 	int isolver;		// magnetic solver used
@@ -63,7 +63,7 @@ void MagCal_Run(void)
 	static int waitcount=0;
 
 	// only do the calibration occasionally
-	if (++waitcount < 20) return;
+	if (++waitcount < 20) return 0;
 	waitcount = 0;
 
 	// count number of data points
@@ -71,7 +71,7 @@ void MagCal_Run(void)
 		if (magcal.valid[i]) count++;
 	}
 
-	if (count < MINMEASUREMENTS4CAL) return;
+	if (count < MINMEASUREMENTS4CAL) return 0;
 
 	if (magcal.ValidMagCal) {
 		// age the existing fit error to avoid one good calibration locking out future updates
@@ -118,8 +118,10 @@ void MagCal_Run(void)
 					magcal.invW[i][j] = magcal.trinvW[i][j];
 				}
 			}
+			return 1; // indicates new calibration applied
 		}
 	}
+	return 0;
 }
 
 

rawdata.c

@@ -60,11 +60,34 @@ static void add_magcal_data(const int16_t *data)
 
 void raw_data(const int16_t *data)
 {
-	float x, y, z, ratio;
+	static int force_orientation_counter=0;
+	float x, y, z, ratio, magdiff;
 	Point_t point;
 
 	add_magcal_data(data);
-	MagCal_Run();
+	x = magcal.V[0];
+	y = magcal.V[1];
+	z = magcal.V[2];
+	if (MagCal_Run()) {
+		x -= magcal.V[0];
+		y -= magcal.V[1];
+		z -= magcal.V[2];
+		magdiff = sqrtf(x * x + y * y + z * z);
+		//printf("magdiff = %.2f\n", magdiff);
+		if (magdiff > 0.8f) {
+			fInit_9DOF_GBY_KALMAN(&fusionstate);
+			rawcount = OVERSAMPLE_RATIO;
+			force_orientation_counter = 240;
+		}
+	}
+
+	if (force_orientation_counter > 0) {
+		if (--force_orientation_counter == 0) {
+			//printf("delayed forcible orientation reset\n");
+			fInit_9DOF_GBY_KALMAN(&fusionstate);
+			rawcount = OVERSAMPLE_RATIO;
+		}
+	}
 
 	if (rawcount >= OVERSAMPLE_RATIO) {
 		memset(&accel, 0, sizeof(accel));

visualize.c

@@ -105,7 +105,7 @@ void display_callback(void)
 			}
 		}
 	}
-#if 1
+#if 0
 	printf(" quality: %5.1f  %5.1f  %5.1f  %5.1f\n",
 		quality_surface_gap_error(),
 		quality_magnitude_variance_error(),