AP_Vehicle: added getters for waypoint info, refactored osd publish_nav_info()

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -4,6 +4,8 @@
 #include <AP_Common/AP_FWVersion.h>
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Frsky_Telem/AP_Frsky_Parameters.h>
+#include <AP_Mission/AP_Mission.h>
+#include <AP_OSD/AP_OSD.h>
 
 #define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros)
 
@@ -191,6 +193,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
 #if GENERATOR_ENABLED
     SCHED_TASK_CLASS(AP_Generator, &vehicle.generator,      update,                   10,  50),
 #endif
+#if OSD_ENABLED
+    SCHED_TASK(publish_osd_info, 1, 10),
+#endif
 };
 
 void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
@@ -320,6 +325,34 @@ void AP_Vehicle::reboot(bool hold_in_bootloader)
     hal.scheduler->reboot(hold_in_bootloader);
 }
 
+#if OSD_ENABLED
+void AP_Vehicle::publish_osd_info()
+{
+    AP_Mission *mission = AP::mission();
+    if (mission == nullptr) {
+        return;
+    }
+    AP_OSD *osd = AP::osd();
+    if (osd == nullptr) {
+        return;
+    }
+    AP_OSD::NavInfo nav_info;
+    if(!get_wp_distance_m(nav_info.wp_distance)) {
+        return;
+    }
+    float wp_bearing_deg;
+    if (!get_wp_bearing_deg(wp_bearing_deg)) {
+        return;
+    }
+    nav_info.wp_bearing = (int32_t)wp_bearing_deg * 100; // OSD expects cd
+    if (!get_wp_crosstrack_error_m(nav_info.wp_xtrack_error)) {
+        return;
+    }
+    nav_info.wp_number = mission->get_current_nav_index();
+    osd->set_nav_info(nav_info);
+}
+#endif
+
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()

libraries/AP_Vehicle/AP_Vehicle.h

@@ -214,6 +214,24 @@ public:
     // and flashing LEDs as appropriate
     void reboot(bool hold_in_bootloader);
 
+    /*
+      get the distance to next wp in meters
+      return false if failed or n/a
+     */
+    virtual bool get_wp_distance_m(float &distance) const { return false; }
+
+    /*
+      get the current wp bearing in degrees
+      return false if failed or n/a
+     */
+    virtual bool get_wp_bearing_deg(float &bearing) const { return false; }
+
+    /*
+      get the current wp crosstrack error in meters
+      return false if failed or n/a
+     */
+    virtual bool get_wp_crosstrack_error_m(float &xtrack_error) const { return false; }
+
 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     AP_Frsky_Parameters frsky_parameters;
 #endif
@@ -294,6 +312,10 @@ protected:
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Scheduler::Task scheduler_tasks[];
 
+#if OSD_ENABLED
+    void publish_osd_info();
+#endif
+
 private:
 
     // delay() callback that processing MAVLink packets