Rover: follow provides distance and bearing to target

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;

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();
+}