diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 099f04f9c3..38e48e50e1 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -215,7 +215,7 @@ private: #endif //helper functions - void draw_speed_vector(uint8_t x, uint8_t y, Vector2f v, int32_t yaw); + 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); #ifdef HAVE_AP_BLHELI_SUPPORT diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index b8eacb25b7..f7d73d40b7 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -36,6 +36,10 @@ #include #include #include +#if APM_BUILD_TYPE(APM_BUILD_Rover) +#include +#endif + #include #include @@ -288,7 +292,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = { // @Param: WIND_EN // @DisplayName: WIND_EN - // @Description: Displays wind speed and relative direction + // @Description: Displays wind speed and relative direction, on Rover this is the apparent wind speed and direction from the windvane, if fitted // @Values: 0:Disabled,1:Enabled // @Param: WIND_X @@ -1205,19 +1209,15 @@ void AP_OSD_Screen::draw_message(uint8_t x, uint8_t y) } } -void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw) +// 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) { - float v_length = v.length(); - char arrow = SYM_ARROW_START; - if (v_length > 1.0f) { - int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw); - int32_t interval = 36000 / SYM_ARROW_COUNT; - arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; - } - if (u_scale(SPEED, v_length) < 10.0) { - backend->write(x, y, false, "%c%3.1f%c", arrow, u_scale(SPEED, v_length), u_icon(SPEED)); + static const int32_t interval = 36000 / SYM_ARROW_COUNT; + char arrow = SYM_ARROW_START + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYM_ARROW_COUNT; + if (u_scale(SPEED, magnitude) < 10.0) { + backend->write(x, y, false, "%c%3.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED)); } else { - backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED)); + backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, magnitude), u_icon(SPEED)); } } @@ -1227,7 +1227,14 @@ 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", SYM_GSPD); - draw_speed_vector(x + 1, y, v, ahrs.yaw_sensor); + + float angle = 0; + const float length = v.length(); + if (length > 1.0f) { + angle = wrap_2PI(atan2f(v.y, v.x) - ahrs.yaw); + } + + draw_speed(x + 1, y, angle, length); } //Thanks to betaflight/inav for simple and clean artificial horizon visual design @@ -1367,14 +1374,28 @@ void AP_OSD_Screen::draw_compass(uint8_t x, uint8_t y) void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y) { +#if !APM_BUILD_TYPE(APM_BUILD_Rover) AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); Vector3f v = ahrs.wind_estimate(); - if (check_option(AP_OSD::OPTION_INVERTED_WIND)) { - v = -v; + float angle = 0; + const float length = v.length(); + if (length > 1.0f) { + if (check_option(AP_OSD::OPTION_INVERTED_WIND)) { + angle = M_PI; + } + angle = wrap_2PI(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()); } +#endif + backend->write(x, y, false, "%c", SYM_WSPD); - draw_speed_vector(x + 1, y, Vector2f(v.x, v.y), ahrs.yaw_sensor); } void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)