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

@@ -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,
+        control_mode->nav_bearing(),
-        nav_controller->target_bearing_cd() * 0.01f,
+        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);
-            CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
+        rover.send_nav_controller_output(chan);
-            rover.send_nav_controller_output(chan);
-        }
         break;
 
     case MSG_SERVO_OUT:

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 xtrack_error = rover.nav_controller->crosstrack_error();
-    const float dist_from_line = fabsf(crosstrack_error);
+    const float dist_from_line = fabsf(xtrack_error);
-    const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_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

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
     //