From f5c540a127635725a2e00883e8d3e962ea0b8b07 Mon Sep 17 00:00:00 2001
From: Alexander Malishev <malishev@gmail.com>
Date: Thu, 26 Jul 2018 03:47:06 +0400
Subject: [PATCH] Copter: publish navigation info to OSD

---
 ArduCopter/ArduCopter.cpp | 15 +++++++++++++++
 ArduCopter/Copter.h       |  4 ++++
 2 files changed, 19 insertions(+)

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
index ca2b8de545..92e5a40fd8 100644
--- a/ArduCopter/ArduCopter.cpp
+++ b/ArduCopter/ArduCopter.cpp
@@ -190,6 +190,9 @@ 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::read_aux_all()
@@ -580,4 +583,16 @@ void Copter::update_altitude()
     }
 }
 
+#if OSD_ENABLED == ENABLED
+void Copter::publish_osd_info()
+{
+    AP_OSD::NavInfo nav_info;
+    nav_info.wp_distance = flightmode->wp_distance();
+    nav_info.wp_bearing = flightmode->wp_bearing();
+    nav_info.wp_xtrack_error = flightmode->crosstrack_error();
+    nav_info.wp_number = mission.get_current_nav_index();
+    osd.set_nav_info(nav_info);
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&copter);
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 03fd54c1d2..4bcd40cccd 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -919,6 +919,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
+
 #include "mode.h"
 
     Mode *flightmode;