AP_OSD: add waypoint and xtrack_error items

libraries/AP_OSD/AP_OSD.cpp

@@ -278,3 +278,10 @@ void AP_OSD::next_screen()
     } while (t != current_screen && !screen[t].enabled);
     current_screen = t;
 }
+
+// set navigation information for display
+void AP_OSD::set_nav_info(NavInfo &navinfo)
+{
+    // do this without a lock for now
+    nav_info = navinfo;
+}

libraries/AP_OSD/AP_OSD.h

@@ -102,6 +102,8 @@ private:
     AP_OSD_Setting pitch_angle{false, 0, 0};
     AP_OSD_Setting temp{false, 0, 0};
     AP_OSD_Setting hdop{false, 0, 0};
+    AP_OSD_Setting waypoint{false, 0, 0};
+    AP_OSD_Setting xtrack_error{false, 0, 0};
 
     bool check_option(uint32_t option);
 
@@ -152,10 +154,13 @@ private:
     void draw_pitch_angle(uint8_t x, uint8_t y);
     void draw_temp(uint8_t x, uint8_t y);
     void draw_hdop(uint8_t x, uint8_t y);
+    void draw_waypoint(uint8_t x, uint8_t y);
+    void draw_xtrack_error(uint8_t x, uint8_t y);
 };
 
 class AP_OSD {
 public:
+    friend class AP_OSD_Screen;
     //constructor
     AP_OSD();
 
@@ -213,6 +218,15 @@ public:
 
     AP_OSD_Screen screen[AP_OSD_NUM_SCREENS];
 
+    struct NavInfo {
+        float wp_distance;
+        int32_t wp_bearing;
+        float wp_xtrack_error;
+        uint16_t wp_number;
+    };
+
+    void set_nav_info(NavInfo &nav_info);
+    
 private:
     void osd_thread();
     void update_osd();
@@ -226,4 +240,5 @@ private:
     uint16_t previous_channel_value;
     bool switch_debouncer;
     uint32_t last_switch_ms;
+    struct NavInfo nav_info;
 };

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -161,6 +161,14 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Path: AP_OSD_Setting.cpp
     AP_SUBGROUPINFO(hdop, "HDOP", 29, AP_OSD_Screen, AP_OSD_Setting),
 
+    // @Group: WAYPOINT
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(waypoint, "WAYPOINT", 30, AP_OSD_Screen, AP_OSD_Setting),
+
+    // @Group: XTRACK
+    // @Path: AP_OSD_Setting.cpp
+    AP_SUBGROUPINFO(xtrack_error, "XTRACK", 31, AP_OSD_Screen, AP_OSD_Setting),
+
     AP_GROUPEND
 };
 
@@ -795,6 +803,25 @@ void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y)
     backend->write(x, y, false, "%c%c%3.2f", SYM_HDOP_L, SYM_HDOP_R, hdp);
 }
 
+void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y)
+{
+    AP_AHRS &ahrs = AP::ahrs();
+    int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor);
+    int32_t interval = 36000 / SYM_ARROW_COUNT;
+    if (osd->nav_info.wp_distance < 2.0f) {
+        //avoid fast rotating arrow at small distances
+        angle = 0;
+    }
+    char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
+    backend->write(x,y, false, "%c%2u%c",SYM_WPNO, osd->nav_info.wp_number, arrow);
+    draw_distance(x+4, y, osd->nav_info.wp_distance);
+}
+
+void AP_OSD_Screen::draw_xtrack_error(uint8_t x, uint8_t y)
+{
+    backend->write(x, y, false, "%c%4d", SYM_XERR, (int)osd->nav_info.wp_xtrack_error);
+}
+
 #define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
 
 void AP_OSD_Screen::draw(void)
@@ -810,6 +837,8 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(horizon);
     DRAW_SETTING(compass);
     DRAW_SETTING(altitude);
+    DRAW_SETTING(waypoint);
+    DRAW_SETTING(xtrack_error);
     DRAW_SETTING(bat_volt);
     DRAW_SETTING(rssi);
     DRAW_SETTING(current);
@@ -837,3 +866,4 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(gps_latitude);
     DRAW_SETTING(gps_longitude);
 }
+