From b405286262d73b6dffad007822d1c1d26bcc96d9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 11 Dec 2018 23:04:50 +0900
Subject: [PATCH] Rover: nav-controller-output message sourced from flight mode

future change will allow overriding these values so each mode can provide the best value
---
 APMrover2/GCS_Mavlink.cpp | 12 +++++-------
 APMrover2/mode.cpp        | 33 ++++++++++++++++++++++++++++++---
 APMrover2/mode.h          |  5 +++++
 3 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 0d56af0323..34f2e0e980 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -107,12 +107,12 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
         chan,
         0,  // roll
         degrees(g2.attitude_control.get_desired_pitch()),
-        nav_controller->nav_bearing_cd() * 0.01f,
-        nav_controller->target_bearing_cd() * 0.01f,
+        control_mode->nav_bearing(),
+        control_mode->wp_bearing(),
         MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
         0,
         control_mode->speed_error(),
-        nav_controller->crosstrack_error());
+        control_mode->crosstrack_error());
 }
 
 void Rover::send_servo_out(mavlink_channel_t chan)
@@ -329,10 +329,8 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         break;
 
     case MSG_NAV_CONTROLLER_OUTPUT:
-        if (rover.control_mode->is_autopilot_mode()) {
-            CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
-            rover.send_nav_controller_output(chan);
-        }
+        CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
+        rover.send_nav_controller_output(chan);
         break;
 
     case MSG_SERVO_OUT:
diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index edf9957130..e3d0d788d6 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -163,6 +163,33 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_
     speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
 }
 
+// return heading (in degrees) to target destination (aka waypoint)
+float Mode::wp_bearing() const
+{
+    if (!is_autopilot_mode()) {
+        return 0.0f;
+    }
+    return rover.nav_controller->target_bearing_cd() * 0.01f;
+}
+
+// return short-term target heading in degrees (i.e. target heading back to line between waypoints)
+float Mode::nav_bearing() const
+{
+    if (!is_autopilot_mode()) {
+        return 0.0f;
+    }
+    return rover.nav_controller->nav_bearing_cd() * 0.01f;
+}
+
+// return cross track error (i.e. vehicle's distance from the line between waypoints)
+float Mode::crosstrack_error() const
+{
+    if (!is_autopilot_mode()) {
+        return 0.0f;
+    }
+    return rover.nav_controller->crosstrack_error();
+}
+
 // set desired location
 void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
@@ -406,9 +433,9 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
 
     // calculate distance from vehicle to line + wp_overshoot
     const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd);
-    const float crosstrack_error = rover.nav_controller->crosstrack_error();
-    const float dist_from_line = fabsf(crosstrack_error);
-    const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_error);
+    const float xtrack_error = rover.nav_controller->crosstrack_error();
+    const float dist_from_line = fabsf(xtrack_error);
+    const bool heading_away = is_positive(line_yaw_diff) == is_positive(xtrack_error);
     const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
 
     // calculate radius of circle that touches vehicle's current position and heading and target position and heading
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index b002ca1e80..c5db805526 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -79,6 +79,11 @@ public:
     virtual bool requires_position() const { return true; }
     virtual bool requires_velocity() const { return true; }
 
+    // return heading (in degrees) and cross track error (in meteres) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    float wp_bearing() const;
+    float nav_bearing() const;
+    float crosstrack_error() const;
+
     //
     // navigation methods
     //