diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index cd3471aa8b..87f5924eae 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -718,7 +718,6 @@ private:
 
     // GCS_Mavlink.cpp
     void gcs_send_heartbeat(void);
-    void send_nav_controller_output(mavlink_channel_t chan);
     void send_rpm(mavlink_channel_t chan);
 
     // heli.cpp
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 9dbcfa7ab2..dd62e34680 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -129,9 +129,10 @@ void GCS_MAVLINK_Copter::get_sensor_status_flags(uint32_t &present,
     health = copter.control_sensors_health;
 }
 
-void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
+void GCS_MAVLINK_Copter::send_nav_controller_output() const
 {
-    const Vector3f &targets = attitude_control->get_att_target_euler_cd();
+    const Vector3f &targets = copter.attitude_control->get_att_target_euler_cd();
+    const Copter::Mode *flightmode = copter.flightmode;
     mavlink_msg_nav_controller_output_send(
         chan,
         targets.x * 1.0e-2f,
@@ -139,7 +140,7 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
         targets.z * 1.0e-2f,
         flightmode->wp_bearing() * 1.0e-2f,
         MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
-        pos_control->get_alt_error() * 1.0e-2f,
+        copter.pos_control->get_alt_error() * 1.0e-2f,
         0,
         flightmode->crosstrack_error() * 1.0e-2f);
 }
@@ -254,11 +255,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
 
     switch(id) {
 
-    case MSG_NAV_CONTROLLER_OUTPUT:
-        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
-        copter.send_nav_controller_output(chan);
-        break;
-
     case MSG_RPM:
 #if RPM_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(RPM);
diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h
index ffb991461b..9b52071536 100644
--- a/ArduCopter/GCS_Mavlink.h
+++ b/ArduCopter/GCS_Mavlink.h
@@ -39,6 +39,7 @@ 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: