From 60b37f395e184f75ee8137b0c5038a0e3517ae1d Mon Sep 17 00:00:00 2001 From: vierfuffzig Date: Thu, 1 Oct 2020 21:40:03 +0200 Subject: [PATCH] AP_OSD_Screen.cpp: add decimal precision to vspd --- libraries/AP_OSD/AP_OSD_Screen.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 42ee68f556..b8eacb25b7 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1394,6 +1394,7 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y) { Vector3f v; float vspd; + float vs_scaled; AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); if (ahrs.get_velocity_NED(v)) { @@ -1413,8 +1414,12 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y) } else { sym = SYM_DOWN_DOWN; } - vspd = fabsf(vspd); - backend->write(x, y, false, "%c%2d%c", sym, (int)u_scale(VSPEED, vspd), u_icon(VSPEED)); + vs_scaled = u_scale(VSPEED, fabsf(vspd)); + if (vs_scaled < 5.0f) { + backend->write(x, y, false, "%c%2.1f%c", sym, (float)vs_scaled, u_icon(VSPEED)); + } else { + backend->write(x, y, false, "%c%3d%c", sym, (int)vs_scaled, u_icon(VSPEED)); + } } #ifdef HAVE_AP_BLHELI_SUPPORT