Copter: calibrate gyros depending on INS GYR_CAL parameter

ArduCopter/Copter.h

@@ -912,7 +912,8 @@ private:
     void save_trim();
     void auto_trim();
     void init_ardupilot();
-    void startup_ground(bool force_gyro_cal);
+    void startup_INS_ground();
+    bool calibrate_gyros();
     bool position_ok();
     bool ekf_position_ok();
     bool optflow_position_ok();

ArduCopter/GCS_Mavlink.cpp

@@ -1310,11 +1310,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
             }
             if (is_equal(packet.param1,1.0f)) {
-                // gyro offset calibration
-                copter.ins.init_gyro();
-                // reset ahrs gyro bias
-                if (copter.ins.gyro_calibrated_ok_all()) {
-                    copter.ahrs.reset_gyro_drift();
+                if (copter.calibrate_gyros()) {
                     result = MAV_RESULT_ACCEPTED;
                 } else {
                     result = MAV_RESULT_FAILED;

ArduCopter/motors.cpp

@@ -124,7 +124,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
     // Flag used to track if we have armed the motors the first time.
     // This is used to decide if we should run the ground_start routine
     // which calibrates the IMU
-    static bool did_ground_start = false;
+    static bool did_gyro_cal = false;
     static bool in_arm_motors = false;
 
     // exit immediately if already in this function
@@ -173,17 +173,19 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
     }
     calc_distance_and_bearing();
 
-    if(did_ground_start == false) {
-        startup_ground(true);
-        // final check that gyros calibrated successfully
-        if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) {
-            gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyro calibration failed"));
-            AP_Notify::flags.armed = false;
-            failsafe_enable();
-            in_arm_motors = false;
-            return false;
+    // gyro calibration on first arming
+    if ((did_gyro_cal == false) && (ins.gyro_calibration_timing() == AP_InertialSensor::GYRO_CAL_STARTUP_AND_FIRST_BOOT)) {
+        if (!calibrate_gyros()) {
+            // final check that gyros calibrated successfully
+            if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS))) {
+                gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyro calibration failed"));
+                AP_Notify::flags.armed = false;
+                failsafe_enable();
+                in_arm_motors = false;
+                return false;
+            }
         }
-        did_ground_start = true;
+        did_gyro_cal = true;
     }
 
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/system.cpp

@@ -262,7 +262,11 @@ void Copter::init_ardupilot()
     reset_control_switch();
     init_aux_switches();
 
-    startup_ground(true);
+    startup_INS_ground();
+
+    // set landed flags
+    set_land_complete(true);
+    set_land_complete_maybe(true);
 
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we are
@@ -288,30 +292,32 @@ void Copter::init_ardupilot()
 //******************************************************************************
 //This function does all the calibrations, etc. that we need during a ground start
 //******************************************************************************
-void Copter::startup_ground(bool force_gyro_cal)
+void Copter::startup_INS_ground()
 {
-    gcs_send_text_P(MAV_SEVERITY_WARNING,PSTR("GROUND START"));
-
     // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
     ahrs.init();
     ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
 
-    // Warm up and read Gyro offsets
-    // -----------------------------
-    ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
-             ins_sample_rate);
- #if CLI_ENABLED == ENABLED
-    report_ins();
- #endif
+    // Warm up and calibrate gyro offsets
+    ins.init(ins_sample_rate);
+
+    // reset ahrs including gyro bias
+    ahrs.reset();
+}
+
+// calibrate gyros - returns true if succesfully calibrated
+bool Copter::calibrate_gyros()
+{
+    // gyro offset calibration
+    copter.ins.init_gyro();
 
     // reset ahrs gyro bias
-    if (force_gyro_cal) {
-        ahrs.reset_gyro_drift();
+    if (copter.ins.gyro_calibrated_ok_all()) {
+        copter.ahrs.reset_gyro_drift();
+        return true;
     }
 
-    // set landed flag
-    set_land_complete(true);
-    set_land_complete_maybe(true);
+    return false;
 }
 
 // position_ok - returns true if the horizontal absolute position is ok and home position is set

ArduCopter/test.cpp

@@ -84,8 +84,7 @@ int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
     report_compass();
 
     // we need the AHRS initialised for this test
-    ins.init(AP_InertialSensor::COLD_START, 
-             ins_sample_rate);
+    ins.init(ins_sample_rate);
     ahrs.reset();
     int16_t counter = 0;
     float heading = 0;
@@ -153,8 +152,7 @@ int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv)
     delay(1000);
 
     ahrs.init();
-    ins.init(AP_InertialSensor::COLD_START, 
-             ins_sample_rate);
+    ins.init(ins_sample_rate);
     cliSerial->printf_P(PSTR("...done\n"));
 
     delay(50);