From 762b31b1475ab267d9435e102ba39514944f0d95 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 17 Mar 2018 19:19:30 +1100
Subject: [PATCH] Rover: create a preflight calibration function

Some functionality has moved up.
---
 APMrover2/GCS_Mavlink.cpp | 92 +++++++++++++++++----------------------
 APMrover2/GCS_Mavlink.h   |  2 +
 2 files changed, 42 insertions(+), 52 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 96a31f1841..1d892b9634 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -639,6 +639,46 @@ void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &c
     // nothing to do
 }
 
+MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
+{
+    if (is_equal(packet.param4, 1.0f)) {
+        rover.trim_radio();
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    if (is_equal(packet.param5, 1.0f)) {
+        // start with gyro calibration
+        rover.ins.init_gyro();
+        if (!rover.ins.gyro_calibrated_ok_all()) {
+            return MAV_RESULT_FAILED;
+        }
+        // reset ahrs gyro bias
+        rover.ahrs.reset_gyro_drift();
+        rover.ins.acal_init();
+        rover.ins.get_acal()->start(this);
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    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)) {
+            return MAV_RESULT_FAILED;
+        }
+        // reset ahrs's trim to suggested values from calibration routine
+        rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    if (is_equal(packet.param5,4.0f)) {
+        // simple accel calibration
+        return rover.ins.simple_accel_cal(rover.ahrs);
+    }
+    return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
+}
+
 void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
 {
     switch (msg->msgid) {
@@ -794,58 +834,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
-            case MAV_CMD_PREFLIGHT_CALIBRATION:
-                if (hal.util->get_soft_armed()) {
-                    result = MAV_RESULT_FAILED;
-                    break;
-                }
-                if (is_equal(packet.param1, 1.0f)) {
-                    rover.ins.init_gyro();
-                    if (rover.ins.gyro_calibrated_ok_all()) {
-                        rover.ahrs.reset_gyro_drift();
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                } else if (is_equal(packet.param3, 1.0f)) {
-                    rover.init_barometer(false);
-                    result = MAV_RESULT_ACCEPTED;
-                } else if (is_equal(packet.param4, 1.0f)) {
-                    rover.trim_radio();
-                    result = MAV_RESULT_ACCEPTED;
-                } else if (is_equal(packet.param5, 1.0f)) {
-                    result = MAV_RESULT_ACCEPTED;
-                    // start with gyro calibration
-                    rover.ins.init_gyro();
-                    // reset ahrs gyro bias
-                    if (rover.ins.gyro_calibrated_ok_all()) {
-                        rover.ahrs.reset_gyro_drift();
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                    rover.ins.acal_init();
-                    rover.ins.get_acal()->start(this);
-
-                } 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)) {
-                        // reset ahrs's trim to suggested values from calibration routine
-                        rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                } else if (is_equal(packet.param5,4.0f)) {
-                    // simple accel calibration
-                    result = rover.ins.simple_accel_cal(rover.ahrs);
-                } else {
-                    send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
-                }
-                break;
-
         case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
             if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) {
                 // when packet.param1 == 3 we reboot to hold in bootloader
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index db35102f6d..158dd1a038 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -30,6 +30,8 @@ protected:
 
     bool set_mode(uint8_t mode) override;
 
+    MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;