AP_OSD:add option to convert home,wind,waypoint and gndspd arrows for BF font set

libraries/AP_OSD/AP_OSD.cpp

@@ -85,7 +85,7 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = {
     // @Param: _OPTIONS
     // @DisplayName: OSD Options
     // @Description: This sets options that change the display
-    // @Bitmask: 0:UseDecimalPack, 1:InvertedWindPointer, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair
+    // @Bitmask: 0:UseDecimalPack, 1:InvertedWindArrow, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair, 5:TranslateArrows
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 8, AP_OSD, options, OPTION_DECIMAL_PACK),
 

libraries/AP_OSD/AP_OSD.h

@@ -271,6 +271,7 @@ private:
     //helper functions
     void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude);
     void draw_distance(uint8_t x, uint8_t y, float distance);
+    char get_arrow_font_index (int32_t angle_cd);
 #if HAL_WITH_ESC_TELEM
     void draw_esc_temp(uint8_t x, uint8_t y);
     void draw_esc_rpm(uint8_t x, uint8_t y);
@@ -537,6 +538,7 @@ public:
         OPTION_INVERTED_AH_ROLL = 1U<<2,
         OPTION_IMPERIAL_MILES = 1U<<3,
         OPTION_DISABLE_CROSSHAIR = 1U<<4,
+        OPTION_BF_ARROWS = 1U<<5,
     };
 
     enum {

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -1291,6 +1291,17 @@ float AP_OSD_AbstractScreen::u_scale(enum unit_type unit, float value)
     return value * scale[units][unit] + (offsets[units]?offsets[units][unit]:0);
 }
 
+char AP_OSD_Screen::get_arrow_font_index(int32_t angle_cd)
+{
+    uint32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
+    angle_cd = wrap_360_cd(angle_cd);
+    // if using BF font table must translate arrows
+    if (check_option(AP_OSD::OPTION_BF_ARROWS)) {
+        angle_cd = angle_cd > 18000? 54000 - angle_cd : 18000- angle_cd;
+    } 
+    return SYMBOL(SYM_ARROW_START) + ((angle_cd + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
+}
+
 void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y)
 {
     float alt;
@@ -1510,8 +1521,8 @@ void AP_OSD_Screen::draw_message(uint8_t x, uint8_t y)
 // draw a arrow at the given angle, and print the given magnitude
 void AP_OSD_Screen::draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude)
 {
-    static const int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
+    int32_t angle_cd = angle_rad * DEGX100;
-    char arrow = SYMBOL(SYM_ARROW_START) + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
+    char arrow = get_arrow_font_index(angle_cd);
     if (u_scale(SPEED, magnitude) < 9.95) {
         backend->write(x, y, false, "%c %1.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED));
     } else {
@@ -1525,13 +1536,11 @@ void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y)
     WITH_SEMAPHORE(ahrs.get_semaphore());
     Vector2f v = ahrs.groundspeed_vector();
     backend->write(x, y, false, "%c", SYMBOL(SYM_GSPD));
-
     float angle = 0;
     const float length = v.length();
     if (length > 1.0f) {
-        angle = wrap_2PI(atan2f(v.y, v.x) - ahrs.yaw);
+        angle = atan2f(v.y, v.x) - ahrs.yaw;
     }
-
     draw_speed(x + 1, y, angle, length);
 }
 
@@ -1617,13 +1626,12 @@ void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
     if (ahrs.get_location(loc) && ahrs.home_is_set()) {
         const Location &home_loc = ahrs.get_home();
         float distance = home_loc.get_distance(loc);
-        int32_t angle = wrap_360_cd(loc.get_bearing_to(home_loc) - ahrs.yaw_sensor);
+        int32_t angle_cd = loc.get_bearing_to(home_loc) - ahrs.yaw_sensor;
-        int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
         if (distance < 2.0f) {
             //avoid fast rotating arrow at small distances
-            angle = 0;
+            angle_cd = 0;
         }
-        char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
+        char arrow = get_arrow_font_index(angle_cd);
         backend->write(x, y, false, "%c%c", SYMBOL(SYM_HOME), arrow);
         draw_distance(x+2, y, distance);
     } else {
@@ -1755,14 +1763,14 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y)
         if (check_option(AP_OSD::OPTION_INVERTED_WIND)) {
             angle = M_PI;
         }
-        angle = wrap_2PI(angle + atan2f(v.y, v.x) - ahrs.yaw);
+        angle = angle + atan2f(v.y, v.x) - ahrs.yaw;
-    }
+    } 
     draw_speed(x + 1, y, angle, length);
 
 #else
     const AP_WindVane* windvane = AP_WindVane::get_singleton();
     if (windvane != nullptr) {
-        draw_speed(x + 1, y, wrap_2PI(windvane->get_apparent_wind_direction_rad() + M_PI), windvane->get_apparent_wind_speed());
+        draw_speed(x + 1, y, windvane->get_apparent_wind_direction_rad() + M_PI, windvane->get_apparent_wind_speed());
     }
 #endif
 
@@ -1924,13 +1932,12 @@ void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y)
 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 angle_cd = osd->nav_info.wp_bearing - ahrs.yaw_sensor;
-    int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
     if (osd->nav_info.wp_distance < 2.0f) {
         //avoid fast rotating arrow at small distances
-        angle = 0;
+        angle_cd = 0;
     }
-    char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
+    char arrow = get_arrow_font_index(angle_cd);
     backend->write(x,y, false, "%c%2u%c",SYMBOL(SYM_WPNO), osd->nav_info.wp_number, arrow);
     draw_distance(x+4, y, osd->nav_info.wp_distance);
 }