From 8ea896e138014ca2acbfe3ab190b0a9782c7ea5c Mon Sep 17 00:00:00 2001
From: yaapu <alex.apostoli@gmail.com>
Date: Sat, 5 Dec 2020 19:45:11 +0100
Subject: [PATCH] Rover: added virtual getters for waypoint info

---
 Rover/Rover.cpp | 47 ++++++++++++++++++++++++++++++-----------------
 Rover/Rover.h   |  7 ++++---
 2 files changed, 34 insertions(+), 20 deletions(-)

diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp
index 22c22b091b3..e6091757b72 100644
--- a/Rover/Rover.cpp
+++ b/Rover/Rover.cpp
@@ -107,9 +107,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(afs_fs_check,           10,    200),
 #endif
     SCHED_TASK(read_airspeed,          10,    100),
-#if OSD_ENABLED == ENABLED
-    SCHED_TASK(publish_osd_info,        1,     10),
-#endif
 };
 
 
@@ -384,23 +381,39 @@ void Rover::update_mission(void)
     }
 }
 
-#if OSD_ENABLED == ENABLED
-void Rover::publish_osd_info()
+// vehicle specific waypoint info helpers
+bool Rover::get_wp_distance_m(float &distance) const
 {
-    AP_OSD::NavInfo nav_info {0};
-    if (control_mode == &mode_loiter) {
-        nav_info.wp_xtrack_error = control_mode->get_distance_to_destination();
-    } else {
-        nav_info.wp_xtrack_error = control_mode->crosstrack_error();
+    // see GCS_MAVLINK_Rover::send_nav_controller_output()
+    if (!rover.control_mode->is_autopilot_mode()) {
+        return false;
     }
-    nav_info.wp_distance = control_mode->get_distance_to_destination();
-    nav_info.wp_bearing = control_mode->wp_bearing() * 100.0f;
-    if (control_mode == &mode_auto) {
-         nav_info.wp_number = mode_auto.mission.get_current_nav_index();
-    }
-    osd.set_nav_info(nav_info);
+    distance = control_mode->get_distance_to_destination();
+    return true;
 }
-#endif
+
+// vehicle specific waypoint info helpers
+bool Rover::get_wp_bearing_deg(float &bearing) const
+{
+    // see GCS_MAVLINK_Rover::send_nav_controller_output()
+    if (!rover.control_mode->is_autopilot_mode()) {
+        return false;
+    }
+    bearing = control_mode->wp_bearing();
+    return true;
+}
+
+// vehicle specific waypoint info helpers
+bool Rover::get_wp_crosstrack_error_m(float &xtrack_error) const
+{
+    // see GCS_MAVLINK_Rover::send_nav_controller_output()
+    if (!rover.control_mode->is_autopilot_mode()) {
+        return false;
+    }
+    xtrack_error = control_mode->crosstrack_error();
+    return true;
+}
+
 
 Rover rover;
 AP_Vehicle& vehicle = rover;
diff --git a/Rover/Rover.h b/Rover/Rover.h
index 97daa488b66..d2ccdd4dc34 100644
--- a/Rover/Rover.h
+++ b/Rover/Rover.h
@@ -390,9 +390,10 @@ private:
     bool should_log(uint32_t mask);
     bool is_boat() const;
 
-#if OSD_ENABLED || OSD_PARAM_ENABLED
-    void publish_osd_info();
-#endif
+    // vehicle specific waypoint info helpers
+    bool get_wp_distance_m(float &distance) const override;
+    bool get_wp_bearing_deg(float &bearing) const override;
+    bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
 
     enum Failsafe_Action {
         Failsafe_Action_None          = 0,