From 497dbd14148b7ad316fda2c06c5ffbb32a70699e Mon Sep 17 00:00:00 2001
From: yaapu <alex.apostoli@gmail.com>
Date: Sat, 5 Dec 2020 19:44:07 +0100
Subject: [PATCH] ArduCopter: added virtual getters for waypopint info

---
 ArduCopter/Copter.cpp | 33 +++++++++++++++++++++------------
 ArduCopter/Copter.h   |  7 ++++---
 2 files changed, 25 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index e5edb8bae9..9fd0b43c88 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -202,9 +202,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if STATS_ENABLED == ENABLED
     SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100),
 #endif
-#if OSD_ENABLED == ENABLED
-    SCHED_TASK(publish_osd_info, 1, 10),
-#endif
 };
 
 void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -617,17 +614,29 @@ void Copter::update_altitude()
     }
 }
 
-#if OSD_ENABLED == ENABLED
-void Copter::publish_osd_info()
+// vehicle specific waypoint info helpers
+bool Copter::get_wp_distance_m(float &distance) const
 {
-    AP_OSD::NavInfo nav_info;
-    nav_info.wp_distance = flightmode->wp_distance() * 1.0e-2f;
-    nav_info.wp_bearing = flightmode->wp_bearing();
-    nav_info.wp_xtrack_error = flightmode->crosstrack_error() * 1.0e-2f;
-    nav_info.wp_number = mode_auto.mission.get_current_nav_index();
-    osd.set_nav_info(nav_info);
+    // see GCS_MAVLINK_Copter::send_nav_controller_output()
+    distance = flightmode->wp_distance() * 0.01;
+    return true;
+}
+
+// vehicle specific waypoint info helpers
+bool Copter::get_wp_bearing_deg(float &bearing) const
+{
+    // see GCS_MAVLINK_Copter::send_nav_controller_output()
+    bearing = flightmode->wp_bearing() * 0.01;
+    return true;
+}
+
+// vehicle specific waypoint info helpers
+bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
+{
+    // see GCS_MAVLINK_Copter::send_nav_controller_output()
+    xtrack_error = flightmode->crosstrack_error() * 0.01;
+    return true;
 }
-#endif
 
 /*
   constructor for main Copter class
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 358ddf918e..dca8651757 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -887,9 +887,10 @@ private:
     void userhook_auxSwitch2(uint8_t ch_flag);
     void userhook_auxSwitch3(uint8_t ch_flag);
 
-#if OSD_ENABLED == 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;
 
     Mode *flightmode;
 #if MODE_ACRO_ENABLED == ENABLED