From 6b5ff939f21217285f64060f9be2ba7cd2c2db01 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 12 Dec 2018 10:07:47 +0900
Subject: [PATCH] Rover: follow provides distance and bearing to target

---
 APMrover2/mode.h          | 19 +++++++++++++++----
 APMrover2/mode_follow.cpp | 12 ++++++++++++
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index c5db805526..795adf3a67 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -79,10 +79,10 @@ 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;
+    // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    virtual float wp_bearing() const;
+    virtual float nav_bearing() const;
+    virtual float crosstrack_error() const;
 
     //
     // navigation methods
@@ -493,6 +493,17 @@ public:
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
+    // attributes of the mode
+    bool is_autopilot_mode() const override { return true; }
+
+    // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    float wp_bearing() const override;
+    float nav_bearing() const override { return wp_bearing(); }
+    float crosstrack_error() const override { return 0.0f; }
+
+    // return distance (in meters) to destination
+    float get_distance_to_destination() const override;
+
 protected:
 
     bool _enter() override;
diff --git a/APMrover2/mode_follow.cpp b/APMrover2/mode_follow.cpp
index 31d79f3ad1..7d95ce88d7 100644
--- a/APMrover2/mode_follow.cpp
+++ b/APMrover2/mode_follow.cpp
@@ -71,3 +71,15 @@ void ModeFollow::update()
     calc_steering_to_heading(_desired_yaw_cd);
     calc_throttle(calc_reduced_speed_for_turn_or_distance(desired_speed), false, true);
 }
+
+// return desired heading (in degrees) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+float ModeFollow::wp_bearing() const
+{
+    return g2.follow.get_bearing_to_target();
+}
+
+// return distance (in meters) to destination
+float ModeFollow::get_distance_to_destination() const
+{
+    return g2.follow.get_distance_to_target();
+}