diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 62c3320487..61e321f50f 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -830,11 +830,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-    case MAVLINK_MSG_ID_SET_MODE: {     // MAV ID: 11
-        handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t));
-        break;
-    }
-
     case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {       // MAV ID: 21
         // mark the firmware version in the tlog
         send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
@@ -1610,5 +1605,10 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long
     return MAV_RESULT_FAILED;
 }
 
+bool GCS_MAVLINK_Sub::set_mode(uint8_t mode)
+{
+    return sub.set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND);
+}
+
 // dummy method to avoid linking AFS
 bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) { return false; }
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 2586dc03b7..1a72b74365 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -24,6 +24,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/ArduSub/Sub.h b/ArduSub/Sub.h
index 53a61f442d..2d6ed09c14 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -599,9 +599,6 @@ private:
     void fence_check();
     void fence_send_mavlink_status(mavlink_channel_t chan);
     bool set_mode(control_mode_t mode, mode_reason_t reason);
-    bool gcs_set_mode(uint8_t mode) {
-        return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND);
-    }
     void update_flight_mode();
     void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
     bool mode_requires_GPS(control_mode_t mode);