diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index db440ffa9c..cf49f4522f 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -213,17 +213,17 @@ void GCS_MAVLINK_Sub::get_sensor_status_flags(uint32_t &control_sensors_present,
     return sub.get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health);
 }
 
-void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
+void GCS_MAVLINK_Sub::send_nav_controller_output() const
 {
-    const Vector3f &targets = attitude_control.get_att_target_euler_cd();
+    const Vector3f &targets = sub.attitude_control.get_att_target_euler_cd();
     mavlink_msg_nav_controller_output_send(
         chan,
         targets.x * 1.0e-2f,
         targets.y * 1.0e-2f,
         targets.z * 1.0e-2f,
-        wp_nav.get_wp_bearing_to_destination() * 1.0e-2f,
-        MIN(wp_nav.get_wp_distance_to_destination() * 1.0e-2f, UINT16_MAX),
-        pos_control.get_alt_error() * 1.0e-2f,
+        sub.wp_nav.get_wp_bearing_to_destination() * 1.0e-2f,
+        MIN(sub.wp_nav.get_wp_distance_to_destination() * 1.0e-2f, UINT16_MAX),
+        sub.pos_control.get_alt_error() * 1.0e-2f,
         0,
         0);
 }
@@ -381,11 +381,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
         send_info();
         break;
 
-    case MSG_NAV_CONTROLLER_OUTPUT:
-        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
-        sub.send_nav_controller_output(chan);
-        break;
-
     case MSG_RPM:
 #if RPM_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(RPM);
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index ef6ca90e8c..0f27935d52 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -35,6 +35,8 @@ protected:
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
 
+    void send_nav_controller_output() const override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 8a5f0d4182..6044803b1e 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -478,7 +478,6 @@ private:
     void get_sensor_status_flags(uint32_t &control_sensors_present,
                                  uint32_t &control_sensors_enabled,
                                  uint32_t &control_sensors_health);
-    void send_nav_controller_output(mavlink_channel_t chan);
 #if RPM_ENABLED == ENABLED
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();