From d1cada0e25ca617c933596ad8c0d3cbae0c9eb3b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 5 May 2018 10:09:34 +1000
Subject: [PATCH] Rover: move try_send_message of nav_controller_output up

---
 APMrover2/GCS_Mavlink.cpp | 15 ++++++++-------
 APMrover2/GCS_Mavlink.h   |  2 ++
 APMrover2/Rover.h         |  1 -
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index aaf797291e..057972f1da 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -86,12 +86,18 @@ void GCS_MAVLINK_Rover::get_sensor_status_flags(uint32_t &present,
     health = rover.control_sensors_health;
 }
 
-void Rover::send_nav_controller_output(mavlink_channel_t chan)
+void GCS_MAVLINK_Rover::send_nav_controller_output() const
 {
+    if (!rover.control_mode->is_autopilot_mode()) {
+        return;
+    }
+
+    const Mode *control_mode = rover.control_mode;
+
     mavlink_msg_nav_controller_output_send(
         chan,
         0,  // roll
-        degrees(g2.attitude_control.get_desired_pitch()),
+        degrees(rover.g2.attitude_control.get_desired_pitch()),
         control_mode->nav_bearing(),
         control_mode->wp_bearing(),
         MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
@@ -318,11 +324,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
 
     switch (id) {
 
-    case MSG_NAV_CONTROLLER_OUTPUT:
-        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
-        rover.send_nav_controller_output(chan);
-        break;
-
     case MSG_SERVO_OUT:
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
         rover.send_servo_out(chan);
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index 5156a7b91e..4fd0344062 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -35,6 +35,8 @@ protected:
     bool set_home_to_current_location(bool lock) override;
     bool set_home(const Location& loc, bool lock) override;
 
+    void send_nav_controller_output() const override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index edff66bce8..acf018ee69 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -437,7 +437,6 @@ private:
     void fence_check();
 
     // GCS_Mavlink.cpp
-    void send_nav_controller_output(mavlink_channel_t chan);
     void send_servo_out(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);
     void send_rpm(mavlink_channel_t chan);