From 3d923d05726434e2b7e01823cef5adffff60208c Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 25 Jul 2018 14:32:42 -0700
Subject: [PATCH] Plane: Send a quadplane version of NAV_CONTROLLER_OUTPUT

---
 ArduPlane/GCS_Mavlink.cpp | 36 ++++++++++++++++++++++++++----------
 ArduPlane/quadplane.cpp   |  6 ++++++
 ArduPlane/quadplane.h     |  3 +++
 3 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 43344d95a5..dfdec3548a 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -180,16 +180,32 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
 
 void Plane::send_nav_controller_output(mavlink_channel_t chan)
 {
-    mavlink_msg_nav_controller_output_send(
-        chan,
-        nav_roll_cd * 0.01f,
-        nav_pitch_cd * 0.01f,
-        nav_controller->nav_bearing_cd() * 0.01f,
-        nav_controller->target_bearing_cd() * 0.01f,
-        MIN(auto_state.wp_distance, UINT16_MAX),
-        altitude_error_cm * 0.01f,
-        airspeed_error * 100,
-        nav_controller->crosstrack_error());
+    if (quadplane.in_vtol_mode()) {
+        const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
+        bool wp_nav_valid = quadplane.using_wp_nav();
+
+        mavlink_msg_nav_controller_output_send(
+            chan,
+            targets.x * 1.0e-2f,
+            targets.y * 1.0e-2f,
+            targets.z * 1.0e-2f,
+            wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
+            wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0,
+            (plane.control_mode != QSTABILIZE) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
+            airspeed_error * 100,
+            wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
+    } else {
+        mavlink_msg_nav_controller_output_send(
+            chan,
+            nav_roll_cd * 0.01f,
+            nav_pitch_cd * 0.01f,
+            nav_controller->nav_bearing_cd() * 0.01f,
+            nav_controller->target_bearing_cd() * 0.01f,
+            MIN(auto_state.wp_distance, UINT16_MAX),
+            altitude_error_cm * 0.01f,
+            airspeed_error * 100,
+            nav_controller->crosstrack_error());
+    }
 }
 
 void GCS_MAVLINK_Plane::send_position_target_global_int()
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 4dbff14a39..f9d317e8a6 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2498,6 +2498,12 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
     return true;
 }
 
+// return true if the wp_nav controller is being updated
+bool QuadPlane::using_wp_nav(void) const
+{
+    return plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL;
+}
+
 /*
   return mav_type for heartbeat
  */
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 90535400b3..5f5c0354ae 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -109,6 +109,9 @@ public:
     
     // user initiated takeoff for guided mode
     bool do_user_takeoff(float takeoff_altitude);
+
+    // return true if the wp_nav controller is being updated
+    bool using_wp_nav(void) const;
     
     struct PACKED log_QControl_Tuning {
         LOG_PACKET_HEADER;