From f3a0d2b02b652062f155ee68ebeb78aeceeee819 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 11 Aug 2017 15:52:27 +1000
Subject: [PATCH] Rover: use GCS_MAVLINK subclasses to handle set_mode

---
 APMrover2/GCS_Mavlink.cpp | 15 +++++++++------
 APMrover2/GCS_Mavlink.h   |  2 ++
 APMrover2/system.cpp      | 12 ------------
 3 files changed, 11 insertions(+), 18 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index fd5f5c8a3d..c3c3d26732 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -941,12 +941,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
-    case MAVLINK_MSG_ID_SET_MODE:
-        {
-            handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
-            break;
-        }
-
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
         {
             // mark the firmware version in the tlog
@@ -1426,3 +1420,12 @@ AP_Mission *GCS_MAVLINK_Rover::get_mission()
 {
     return &rover.mission;
 }
+
+bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
+{
+    Mode *new_mode = rover.control_mode_from_num((enum mode)mode);
+    if (new_mode == nullptr) {
+        return false;
+    }
+    return rover.set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
+}
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index bcf8193afc..a3c523fe95 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -26,6 +26,8 @@ protected:
 
     uint8_t sysid_my_gcs() const override;
 
+    bool set_mode(uint8_t mode) override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index 3bcc733f32..91a102ca44 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -244,18 +244,6 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
     return true;
 }
 
-/*
-  set_mode() wrapper for MAVLink SET_MODE
- */
-bool Rover::mavlink_set_mode(uint8_t mode)
-{
-    Mode *new_mode = control_mode_from_num((enum mode)mode);
-    if (new_mode == nullptr) {
-        return false;
-    }
-    return set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
-}
-
 void Rover::startup_INS_ground(void)
 {
     gcs().send_text(MAV_SEVERITY_INFO, "Warming up ADC");