From 030a1997f5efa72e7e4895b70142eb607ae856b4 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 17 Mar 2018 22:40:57 +1100
Subject: [PATCH] Tracker: factor out preflight calibration function

---
 AntennaTracker/GCS_Mavlink.cpp | 102 +++++++++++++++++----------------
 AntennaTracker/GCS_Mavlink.h   |   2 +
 2 files changed, 55 insertions(+), 49 deletions(-)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 0798f69341..e18d185f10 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -446,6 +446,59 @@ uint8_t GCS_MAVLINK_Tracker::sysid_my_gcs() const
     return tracker.g.sysid_my_gcs;
 }
 
+MAV_RESULT GCS_MAVLINK_Tracker::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
+{
+    if (is_equal(packet.param1,1.0f)) {
+        tracker.ins.init_gyro();
+        if (!tracker.ins.gyro_calibrated_ok_all()) {
+            return MAV_RESULT_FAILED;
+        }
+        tracker.ahrs.reset_gyro_drift();
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    if (is_equal(packet.param3,1.0f)) {
+        tracker.init_barometer(false);
+        // zero the altitude difference on next baro update
+        tracker.nav_status.need_altitude_calibration = true;
+        return MAV_RESULT_ACCEPTED;
+    }
+    if (is_equal(packet.param4,1.0f)) {
+        // Can't trim radio
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    if (is_equal(packet.param5,1.0f)) {
+        // start with gyro calibration
+        tracker.ins.init_gyro();
+        // reset ahrs gyro bias
+        if (!tracker.ins.gyro_calibrated_ok_all()) {
+            return MAV_RESULT_FAILED;
+        }
+        tracker.ahrs.reset_gyro_drift();
+        // start accel cal
+        tracker.ins.acal_init();
+        tracker.ins.get_acal()->start(this);
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    if (is_equal(packet.param5,2.0f)) {
+        // start with gyro calibration
+        tracker.ins.init_gyro();
+        // accel trim
+        float trim_roll, trim_pitch;
+        if (!tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
+            return MAV_RESULT_FAILED;
+        }
+        // reset ahrs's trim to suggested values from calibration routine
+        tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
+}
+
+
 void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
 {
     switch (msg->msgid) {
@@ -474,55 +527,6 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
         send_text(MAV_SEVERITY_INFO,"Command received: ");
         
         switch(packet.command) {
-            
-            case MAV_CMD_PREFLIGHT_CALIBRATION:
-            {
-                if (is_equal(packet.param1,1.0f)) {
-                    tracker.ins.init_gyro();
-                    if (tracker.ins.gyro_calibrated_ok_all()) {
-                        tracker.ahrs.reset_gyro_drift();
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                }
-                if (is_equal(packet.param3,1.0f)) {
-                    tracker.init_barometer(false);
-                    // zero the altitude difference on next baro update
-                    tracker.nav_status.need_altitude_calibration = true;
-                    result = MAV_RESULT_ACCEPTED;
-                }
-                if (is_equal(packet.param4,1.0f)) {
-                    // Can't trim radio
-                    result = MAV_RESULT_UNSUPPORTED;
-                } else if (is_equal(packet.param5,1.0f)) {
-                    result = MAV_RESULT_ACCEPTED;
-                    // start with gyro calibration
-                    tracker.ins.init_gyro();
-                    // reset ahrs gyro bias
-                    if (tracker.ins.gyro_calibrated_ok_all()) {
-                        tracker.ahrs.reset_gyro_drift();
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                    // start accel cal
-                    tracker.ins.acal_init();
-                    tracker.ins.get_acal()->start(this);
-                } else if (is_equal(packet.param5,2.0f)) {
-                    // start with gyro calibration
-                    tracker.ins.init_gyro();
-                    // accel trim
-                    float trim_roll, trim_pitch;
-                    if (tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
-                        // reset ahrs's trim to suggested values from calibration routine
-                        tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                        result = MAV_RESULT_ACCEPTED;
-                    } else {
-                        result = MAV_RESULT_FAILED;
-                    }
-                }
-                break;
-            }
 
             case MAV_CMD_COMPONENT_ARM_DISARM:
                 if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h
index a37daada31..2efddddb03 100644
--- a/AntennaTracker/GCS_Mavlink.h
+++ b/AntennaTracker/GCS_Mavlink.h
@@ -28,6 +28,8 @@ protected:
 
     bool set_mode(uint8_t mode) override;
 
+    MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
+
 private:
 
     void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;