Rover: use GCS_MAVLINK subclasses to handle set_mode

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);
+}

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;

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");