From 08b3609fa91ea30687c10d740d7594a4af3a199f Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 27 Mar 2023 10:23:25 -0500 Subject: [PATCH] AP_OSD:add option to convert home,wind,waypoint and gndspd arrows for BF font set --- libraries/AP_OSD/AP_OSD.cpp | 2 +- libraries/AP_OSD/AP_OSD.h | 2 ++ libraries/AP_OSD/AP_OSD_Screen.cpp | 39 ++++++++++++++++++------------ 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 3088cd96c6..e883c8c1de 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/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), diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index eaadd0e0d1..9dcc8b1fc8 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/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 { diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index ca931576a9..86ca4647db 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/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); - char arrow = SYMBOL(SYM_ARROW_START) + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = angle_rad * DEGX100; + 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 interval = 36000 / SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = loc.get_bearing_to(home_loc) - ahrs.yaw_sensor; 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 interval = 36000 / SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = osd->nav_info.wp_bearing - ahrs.yaw_sensor; 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); }