From fb66b8b96105a1b72917cdcd86e83dbb497f1ef9 Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Sat, 21 Sep 2019 14:49:31 -0500
Subject: [PATCH] Rover: publish nav data to OSD

---
 APMrover2/Rover.cpp | 23 ++++++++++++++++++++++-
 APMrover2/Rover.h   |  4 ++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp
index 56a4315ca9..be2e872ca9 100644
--- a/APMrover2/Rover.cpp
+++ b/APMrover2/Rover.cpp
@@ -22,7 +22,7 @@
 
    Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin, Grant Morphett
 
-   Thanks to:  Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier 
+   Thanks to:  Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
 
    APMrover alpha version tester: Franco Borasio, Daniel Chapelat...
 
@@ -105,6 +105,9 @@ 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
 };
 
 constexpr int8_t Rover::_failsafe_priorities[7];
@@ -324,6 +327,24 @@ void Rover::update_mission(void)
     }
 }
 
+#if OSD_ENABLED == ENABLED
+void Rover::publish_osd_info()
+{
+    AP_OSD::NavInfo nav_info {};
+    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();
+    }
+    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);
+}
+#endif
+
 Rover rover;
 
 AP_HAL_MAIN_CALLBACKS(&rover);
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index cdda630c2f..baf4e59f57 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -449,6 +449,10 @@ private:
     bool should_log(uint32_t mask);
     bool is_boat() const;
 
+#if OSD_ENABLED == ENABLED
+    void publish_osd_info();
+#endif
+
     enum Failsafe_Action {
         Failsafe_Action_None          = 0,
         Failsafe_Action_RTL           = 1,