From 09d4e369707f70e8687427adcdc4c317d8cb876c Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 17 Mar 2018 19:19:14 +1100
Subject: [PATCH] GCS_MAVLink: handle preflight calibrations

---
 libraries/GCS_MAVLink/GCS.h          |  4 ++++
 libraries/GCS_MAVLink/GCS_Common.cpp | 19 +++++++++++++++++++
 2 files changed, 23 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index fbcfda4311..fb61003928 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -297,6 +297,8 @@ protected:
     virtual uint32_t telem_delay() const = 0;
 
     MAV_RESULT handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet);
+    virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet);
+
     MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_long_message(mavlink_command_long_t &packet);
     MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
@@ -316,6 +318,8 @@ protected:
     void handle_data_packet(mavlink_message_t *msg);
 private:
 
+    MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet);
+
     float       adjust_rate_for_stream_trigger(enum streams stream_num);
 
     MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 83f4fb4a1e..938f25ba35 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -2219,6 +2219,21 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlin
     return MAV_RESULT_ACCEPTED;
 }
 
+MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
+{
+    return MAV_RESULT_UNSUPPORTED;
+}
+
+MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_command_long_t &packet)
+{
+    if (hal.util->get_soft_armed()) {
+        // *preflight*, remember?
+        return MAV_RESULT_FAILED;
+    }
+    // now call subclass methods:
+    return _handle_command_preflight_calibration(packet);
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_long_t &packet)
 {
     Compass *compass = get_compass();
@@ -2306,6 +2321,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack
         break;
     }
 
+    case MAV_CMD_PREFLIGHT_CALIBRATION:
+        result = handle_command_preflight_calibration(packet);
+        break;
+
     case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: {
         result = handle_command_preflight_set_sensor_offsets(packet);
         break;