Rover: publish nav data to OSD

APMrover2/Rover.cpp

@@ -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);

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,