Rover: replace SKIP_GYRO_CAL with INS_GYR_CAL

Also calibrate gyros during accel trim

APMrover2/GCS_Mavlink.cpp

@@ -145,7 +145,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
     if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
-    if (!ins.get_gyro_health_all() || (!g.skip_gyro_cal && !ins.gyro_calibrated_ok_all())) {
+    if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
     }
     if (!ins.get_accel_health_all()) {
@@ -974,10 +974,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 } else if (is_equal(packet.param5,1.0f)) {
                     float trim_roll, trim_pitch;
                     AP_InertialSensor_UserInteract_MAVLink interact(this);
-                    if (rover.g.skip_gyro_cal) {
+                    // start with gyro calibration
-                        // start with gyro calibration, otherwise if the user
-                        // has SKIP_GYRO_CAL=1 they don't get to do it
                     rover.ins.init_gyro();
+                    // reset ahrs gyro bias
+                    if (rover.ins.gyro_calibrated_ok_all()) {
+                        rover.ahrs.reset_gyro_drift();
                     }
                     if(rover.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
                         // reset ahrs's trim to suggested values from calibration routine
@@ -987,6 +988,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                         result = MAV_RESULT_FAILED;
                     }
                 } else if (is_equal(packet.param5,2.0f)) {
+                    // start with gyro calibration
+                    rover.ins.init_gyro();
                     // accel trim
                     float trim_roll, trim_pitch;
                     if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) {

APMrover2/Parameters.cpp

@@ -77,13 +77,6 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
     // @Bitmask: 0:Steering
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
-    // @Param: SKIP_GYRO_CAL
-    // @DisplayName: Skip gyro calibration
-    // @Description: When enabled this tells the APM to skip the normal gyroscope calibration at startup, and instead use the saved gyro calibration from the last flight. You should only enable this if you are careful to check that your aircraft has good attitude control before flying, as some boards may have significantly different gyro calibration between boots, especially if the temperature changes a lot. If gyro calibration is skipped then APM relies on using the gyro drift detection code to get the right gyro calibration in the few minutes after it boots. This option is mostly useful where the requirement to hold the vehicle still while it is booting is a significant problem.
-    // @Values: 0:Disabled,1:Enabled
-    // @User: Advanced
-    GSCALAR(skip_gyro_cal,           "SKIP_GYRO_CAL",   0),
-
     // @Param: MAG_ENABLED
     // @DisplayName: Magnetometer (compass) enabled
     // @Description: This should be set to 1 if a compass is installed

APMrover2/Parameters.h

@@ -67,7 +67,7 @@ public:
         k_param_serial0_baud_old,
         k_param_serial1_baud_old,
         k_param_telem_delay,
-        k_param_skip_gyro_cal,
+        k_param_skip_gyro_cal, // unused
         k_param_gcs2,       // stream rates for uartD
         k_param_serial2_baud_old,
         k_param_serial2_protocol,   // deprecated, can be deleted
@@ -214,7 +214,6 @@ public:
 	AP_Int16    sysid_this_mav;
 	AP_Int16    sysid_my_gcs;
     AP_Int8     telem_delay;
-    AP_Int8     skip_gyro_cal;
 #if CLI_ENABLED == ENABLED
     AP_Int8     cli_enabled;
 #endif

APMrover2/system.cpp

@@ -405,14 +405,7 @@ void Rover::startup_INS_ground(void)
 	ahrs.set_fly_forward(true);
     ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
 
-    AP_InertialSensor::Start_style style;
+	ins.init(ins_sample_rate);
-    if (g.skip_gyro_cal) {
-        style = AP_InertialSensor::WARM_START;
-    } else {
-        style = AP_InertialSensor::COLD_START;
-    }
-
-	ins.init(style, ins_sample_rate);
 
     ahrs.reset();
 }

APMrover2/test.cpp

@@ -308,8 +308,7 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
 	//cliSerial->printf_P(PSTR("Calibrating."));
 	ahrs.init();
     ahrs.set_fly_forward(true);
-	ins.init(AP_InertialSensor::COLD_START, 
+	ins.init(ins_sample_rate);
-             ins_sample_rate);
     ahrs.reset();
 
 	print_hit_enter();
@@ -372,8 +371,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
     ahrs.set_compass(&compass);
 
     // we need the AHRS initialised for this test
-	ins.init(AP_InertialSensor::COLD_START, 
+	ins.init(ins_sample_rate);
-             ins_sample_rate);
     ahrs.reset();
 
 	int counter = 0;