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,