From d73a7c051d0a1fe43d47aac72df2ccde6465d4d2 Mon Sep 17 00:00:00 2001 From: Alexander Malishev Date: Wed, 4 Jul 2018 01:33:53 +0400 Subject: [PATCH] AP_OSD: show direction of the groundspeed vector --- libraries/AP_OSD/AP_OSD_Screen.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 3be55e59cc..82a7e87b8e 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -263,8 +263,15 @@ void AP_OSD_Screen::draw_message(uint8_t x, uint8_t y) void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y) { - float v = AP::ahrs().groundspeed() * 3.6; - backend->write(x, y, false, "%3.0f%c", v, SYM_KMH); + Vector2f v = AP::ahrs().groundspeed_vector(); + float v_length = v.length(); + char arrow = SYM_ARROW_START; + if (v_length > 1.0f) { + int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.x, v.y)); + int32_t interval = 36000 / SYM_ARROW_COUNT; + arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; + } + backend->write(x, y, false, "%c%3.0f%c", arrow, v_length * 3.6, SYM_KMH); } //Thanks to betaflight/inav for simple and clean artificial horizon visual design