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

---
 AntennaTracker/GCS_Mavlink.cpp | 23 +++++++++++++++++------
 AntennaTracker/GCS_Mavlink.h   |  2 ++
 AntennaTracker/Tracker.h       |  1 -
 AntennaTracker/system.cpp      | 17 -----------------
 4 files changed, 19 insertions(+), 24 deletions(-)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 2d44ce6569..d153fa2184 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -740,12 +740,6 @@ mission_failed:
         break;
     }
 
-    case MAVLINK_MSG_ID_SET_MODE:
-    {
-        handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
-        break;
-    }
-
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
         handle_serial_control(msg, tracker.gps);
         break;
@@ -831,6 +825,23 @@ Compass *GCS_MAVLINK_Tracker::get_compass() const
     return &tracker.compass;
 }
 
+/*
+  set_mode() wrapper for MAVLink SET_MODE
+ */
+bool GCS_MAVLINK_Tracker::set_mode(uint8_t mode)
+{
+    switch (mode) {
+    case AUTO:
+    case MANUAL:
+    case SCAN:
+    case SERVO_TEST:
+    case STOP:
+        tracker.set_mode((enum ControlMode)mode);
+        return true;
+    }
+    return false;
+}
+
 /* dummy methods to avoid having to link against AP_Camera */
 void AP_Camera::control_msg(mavlink_message_t const*) {}
 void AP_Camera::configure(float, float, float, float, float, float, float) {}
diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h
index df75dfcf1a..ee3a867585 100644
--- a/AntennaTracker/GCS_Mavlink.h
+++ b/AntennaTracker/GCS_Mavlink.h
@@ -25,6 +25,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/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index c47537cccb..2d761eef79 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -241,7 +241,6 @@ private:
     void disarm_servos();
     void prepare_servos();
     void set_mode(enum ControlMode mode);
-    bool mavlink_set_mode(uint8_t mode);
     void check_usb_mux(void);
     void update_vehicle_pos_estimate();
     void update_tracker_position();
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index 8ad4982662..6f33462227 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -218,23 +218,6 @@ void Tracker::set_mode(enum ControlMode mode)
 	DataFlash.Log_Write_Mode(control_mode);
 }
 
-/*
-  set_mode() wrapper for MAVLink SET_MODE
- */
-bool Tracker::mavlink_set_mode(uint8_t mode)
-{
-    switch (mode) {
-    case AUTO:
-    case MANUAL:
-    case SCAN:
-    case SERVO_TEST:
-    case STOP:
-        set_mode((enum ControlMode)mode);
-        return true;
-    }
-    return false;
-}
-
 void Tracker::check_usb_mux(void)
 {
     bool usb_check = hal.gpio->usb_connected();