Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Henry Wurzburg 2023-03-27 10:23:25 -05:00 committed by Andrew Tridgell
parent d8416eb38f
commit 08b3609fa9
3 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_OSD/AP_OSD.cpp
View File

@ -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),

2
libraries/AP_OSD/AP_OSD.h
View File

@ -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 {

39
libraries/AP_OSD/AP_OSD_Screen.cpp
View File

@ -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);
}