AP_IMU: improved the gyro calibration code

this should give a much more accurate result

libraries/AP_IMU/AP_IMU_INS.cpp

@@ -56,18 +56,15 @@ AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(b
 void
 AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
 {
-	int flashcount = 0;
-	float adc_in;
-	float prev[3] = {0,0,0};
-	float total_change;
-	float max_offset;
-    float ins_gyro[6];
+    Vector3f last_average, best_avg;
+    float ins_gyro[3];
+    float best_diff;
 
 	// cold start
-	delay_cb(500);
+	delay_cb(100);
 	Serial.printf_P(PSTR("Init Gyro"));
 
-	for(int c = 0; c < 25; c++){
+	for(int c = 0; c < 25; c++) {
     // Mostly we are just flashing the LED's here
     // to tell the user to keep the IMU still
         FLASH_LEDS(true);
@@ -80,49 +77,62 @@ AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(
 		delay_cb(20);
 	}
 
-	for (int j = 0; j <= 2; j++)
-	    _sensor_cal[j] = 500;		// Just a large value to load prev[j] the first time
+    // the strategy is to average 100 points, then do it again and
+    // see if the 2nd average is within a small margin of the first
 
-	do {
+    last_average.zero();
 
-    _ins->update();
-    _ins->get_gyros(ins_gyro);
+    // we try to get a good calibration estimate for up to 10 seconds
+    // if the gyros are stable, we should get it in 2 seconds
+	for (int j = 0; j <= 10; j++) {
+        Vector3f gyro_sum, gyro_avg, gyro_diff;
+        float diff_norm;
+        uint8_t i;
 
-		for (int j = 0; j <= 2; j++){
-			prev[j]     = _sensor_cal[j];
-			adc_in      = ins_gyro[j];
-			_sensor_cal[j]	= adc_in;
-		}
+        Serial.printf_P(PSTR("*"));
 
-		for(int i = 0; i < 50; i++){
-
-      _ins->update();
-      _ins->get_gyros(ins_gyro);
-
-			for (int j = 0; j < 3; j++){
-				adc_in = ins_gyro[j];
-				// filter
-				_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
-			}
-
-			delay_cb(20);
-			if(flashcount == 5) {
-				Serial.printf_P(PSTR("*"));
+        gyro_sum.zero();
+        for (i=0; i<200; i++) {
+            _ins->update();
+            _ins->get_gyros(ins_gyro);
+            gyro_sum += Vector3f(ins_gyro[0], ins_gyro[1], ins_gyro[2]);
+            if (i % 40 == 20) {
                 FLASH_LEDS(true);
-			}
-
-			if(flashcount >= 10) {
-				flashcount = 0;
+            } else if (i % 40 == 20) {
                 FLASH_LEDS(false);
-			}
-			flashcount++;
-		}
+            }
+            delay_cb(5);
+        }
+        gyro_avg = gyro_sum / i;
 
-		total_change    = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]);
-		max_offset      = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
-		max_offset      = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
-		delay_cb(500);
-	} while (  total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
+        gyro_diff = last_average - gyro_avg;
+        diff_norm = gyro_diff.length();
+
+        if (j == 0) {
+            best_diff = diff_norm;
+            best_avg = gyro_avg;
+        } else if (gyro_diff.length() < ToRad(0.04)) {
+            // we want the average to be within 0.1 bit, which is 0.04 degrees/s
+            last_average = (gyro_avg * 0.5) + (last_average * 0.5);
+            _sensor_cal[0] = last_average.x;
+            _sensor_cal[1] = last_average.y;
+            _sensor_cal[2] = last_average.z;
+            // all done
+            return;
+        } else if (diff_norm < best_diff) {
+            best_diff = diff_norm;
+            best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
+        }
+        last_average = gyro_avg;
+    }
+
+    // we've kept the user waiting long enough - use the best pair we
+    // found so far
+    Serial.printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
+
+    _sensor_cal[0] = best_avg.x;
+    _sensor_cal[1] = best_avg.y;
+    _sensor_cal[2] = best_avg.z;
 }
 
 void

libraries/AP_IMU/AP_IMU_INS.h

@@ -75,10 +75,8 @@ private:
 
     float _calibrated(uint8_t channel, float ins_value);
 
-	// Gyro and Accelerometer calibration criterial
+	// Gyro and Accelerometer calibration criteria
 	//
-	static const float		_gyro_total_cal_change = 4.0;		// Experimentally derived - allows for some minor motion
-	static const float		_gyro_max_cal_offset = 320.0;
 	static const float		_accel_total_cal_change = 4.0;
 	static const float		_accel_max_cal_offset = 250.0;