From 4cdbcab7230eda15a8612d05d4b387f16636c687 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 6 Jul 2018 11:34:29 +1000
Subject: [PATCH] Rover: move handling of command-int MAV_CMD_DO_SET_HOME up

---
 APMrover2/GCS_Mavlink.cpp | 49 ++++++---------------------------------
 APMrover2/GCS_Mavlink.h   |  3 +++
 2 files changed, 10 insertions(+), 42 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 2d42c75984..6136d5c7b4 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -569,6 +569,13 @@ MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlin
     return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
 }
 
+bool GCS_MAVLINK_Rover::set_home_to_current_location(bool lock) {
+    return rover.set_home_to_current_location(lock);
+}
+bool GCS_MAVLINK_Rover::set_home(const Location& loc, bool lock) {
+    return rover.set_home(loc, lock);
+}
+
 MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet)
 {
     switch (packet.command) {
@@ -581,48 +588,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in
         }
         return MAV_RESULT_ACCEPTED;
 
-    case MAV_CMD_DO_SET_HOME: {
-        // assume failure
-        if (is_equal(packet.param1, 1.0f)) {
-            // if param1 is 1, use current location
-            if (rover.set_home_to_current_location(true)) {
-                return MAV_RESULT_ACCEPTED;
-            }
-            return MAV_RESULT_FAILED;
-        }
-        // ensure param1 is zero
-        if (!is_zero(packet.param1)) {
-            return MAV_RESULT_FAILED;
-        }
-        // check frame type is supported
-        if (packet.frame != MAV_FRAME_GLOBAL &&
-            packet.frame != MAV_FRAME_GLOBAL_INT &&
-            packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
-            packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
-            return MAV_RESULT_FAILED;
-        }
-        // sanity check location
-        if (!check_latlng(packet.x, packet.y)) {
-            return MAV_RESULT_FAILED;
-        }
-        Location new_home_loc {};
-        new_home_loc.lat = packet.x;
-        new_home_loc.lng = packet.y;
-        new_home_loc.alt = packet.z * 100;
-        // handle relative altitude
-        if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
-            if (!rover.ahrs.home_is_set()) {
-                // cannot use relative altitude if home is not set
-                return MAV_RESULT_FAILED;
-            }
-            new_home_loc.alt += rover.ahrs.get_home().alt;
-        }
-        if (!rover.set_home(new_home_loc, true)) {
-            return MAV_RESULT_FAILED;
-        }
-        return MAV_RESULT_ACCEPTED;
-    }
-
     case MAV_CMD_DO_SET_REVERSE:
         // param1 : Direction (0=Forward, 1=Reverse)
         rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index f94426a0aa..047bbd3155 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -33,6 +33,9 @@ protected:
 
     void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
 
+    bool set_home_to_current_location(bool lock) override;
+    bool set_home(const Location& loc, bool lock) override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;