diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 614c13be30..48c600372d 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -1397,12 +1397,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_SET_MODE:
-    {
-        handle_set_mode(msg, FUNCTOR_BIND(&plane, &Plane::mavlink_set_mode, bool, uint8_t));
-        break;
-    }
-
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
     {
         // mark the firmware version in the tlog
@@ -1909,3 +1903,35 @@ AP_Rally *GCS_MAVLINK_Plane::get_rally() const
 {
     return &plane.rally;
 }
+
+/*
+  set_mode() wrapper for MAVLink SET_MODE
+ */
+bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode)
+{
+    switch (mode) {
+    case MANUAL:
+    case CIRCLE:
+    case STABILIZE:
+    case TRAINING:
+    case ACRO:
+    case FLY_BY_WIRE_A:
+    case AUTOTUNE:
+    case FLY_BY_WIRE_B:
+    case CRUISE:
+    case AVOID_ADSB:
+    case GUIDED:
+    case AUTO:
+    case RTL:
+    case LOITER:
+    case QSTABILIZE:
+    case QHOVER:
+    case QLOITER:
+    case QLAND:
+    case QRTL:
+        plane.set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
+        return true;
+    }
+    return false;
+}
+
diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
index 8e3498b38d..bb01aeb331 100644
--- a/ArduPlane/GCS_Mavlink.h
+++ b/ArduPlane/GCS_Mavlink.h
@@ -30,6 +30,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/ArduPlane/Plane.h b/ArduPlane/Plane.h
index c0aed6afd0..c745de7167 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -963,7 +963,6 @@ private:
     void startup_ground(void);
     enum FlightMode get_previous_mode();
     void set_mode(enum FlightMode mode, mode_reason_t reason);
-    bool mavlink_set_mode(uint8_t mode);
     void exit_mode(enum FlightMode mode);
     void check_long_failsafe();
     void check_short_failsafe();
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 542e29a464..e775886ec8 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -462,37 +462,6 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
     steerController.reset_I();    
 }
 
-/*
-  set_mode() wrapper for MAVLink SET_MODE
- */
-bool Plane::mavlink_set_mode(uint8_t mode)
-{
-    switch (mode) {
-    case MANUAL:
-    case CIRCLE:
-    case STABILIZE:
-    case TRAINING:
-    case ACRO:
-    case FLY_BY_WIRE_A:
-    case AUTOTUNE:
-    case FLY_BY_WIRE_B:
-    case CRUISE:
-    case AVOID_ADSB:
-    case GUIDED:
-    case AUTO:
-    case RTL:
-    case LOITER:
-    case QSTABILIZE:
-    case QHOVER:
-    case QLOITER:
-    case QLAND:
-    case QRTL:
-        set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
-        return true;
-    }
-    return false;
-}
-
 // exit_mode - perform any cleanup required when leaving a flight mode
 void Plane::exit_mode(enum FlightMode mode)
 {