Sub: move common calibration functions up

ArduSub/GCS_Mavlink.cpp

@@ -863,13 +863,6 @@ void GCS_MAVLINK_Sub::handle_change_alt_request(AP_Mission::Mission_Command &cmd
 
 MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
 {
-    if (is_equal(packet.param1,1.0f)) {
-        if (!sub.calibrate_gyros()) {
-            return MAV_RESULT_FAILED;
-        }
-        return MAV_RESULT_ACCEPTED;
-    }
-
     if (is_equal(packet.param3,1.0f)) {
         if (!sub.sensor_health.depth || sub.motors.armed()) {
             return MAV_RESULT_FAILED;
@@ -878,40 +871,6 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_
         return MAV_RESULT_ACCEPTED;
     }
 
-    if (is_equal(packet.param4,1.0f)) {
-        return MAV_RESULT_UNSUPPORTED;
-    }
-
-    if (is_equal(packet.param5,1.0f)) {
-        // 3d accel calibration
-        if (!sub.calibrate_gyros()) {
-            return MAV_RESULT_FAILED;
-        }
-        sub.ins.acal_init();
-        sub.ins.get_acal()->start(this);
-        return MAV_RESULT_ACCEPTED;
-    }
-
-    if (is_equal(packet.param5,2.0f)) {
-        // calibrate gyros
-        if (!sub.calibrate_gyros()) {
-            return MAV_RESULT_FAILED;
-        }
-        // accel trim
-        float trim_roll, trim_pitch;
-        if (!sub.ins.calibrate_trim(trim_roll, trim_pitch)) {
-            return MAV_RESULT_FAILED;
-        }
-        // reset ahrs's trim to suggested values from calibration routine
-        sub.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-        return MAV_RESULT_ACCEPTED;
-    }
-
-    if (is_equal(packet.param5,4.0f)) {
-        // simple accel calibration
-        return sub.ins.simple_accel_cal(sub.ahrs);
-    }
-
     if (is_equal(packet.param6,1.0f)) {
         // compassmot calibration
         //result = sub.mavlink_compassmot(chan);

ArduSub/Sub.h

@@ -633,7 +633,6 @@ private:
     bool terrain_use();
     void init_ardupilot();
     void startup_INS_ground();
-    bool calibrate_gyros();
     bool position_ok();
     bool ekf_position_ok();
     bool optflow_position_ok();

ArduSub/system.cpp

@@ -230,20 +230,6 @@ void Sub::startup_INS_ground()
 }
 
 // calibrate gyros - returns true if successfully calibrated
-bool Sub::calibrate_gyros()
-{
-    // gyro offset calibration
-    sub.ins.init_gyro();
-
-    // reset ahrs gyro bias
-    if (sub.ins.gyro_calibrated_ok_all()) {
-        sub.ahrs.reset_gyro_drift();
-        return true;
-    }
-
-    return false;
-}
-
 // position_ok - returns true if the horizontal absolute position is ok and home position is set
 bool Sub::position_ok()
 {