|
|
|
@ -2139,9 +2139,11 @@ void AP_OSD_Screen::draw_rngf(uint8_t x, uint8_t y)
@@ -2139,9 +2139,11 @@ void AP_OSD_Screen::draw_rngf(uint8_t x, uint8_t y)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (rangefinder->status_orient(ROTATION_PITCH_270) <= RangeFinder::Status::NoData) { |
|
|
|
|
backend->write(x, y, false, "%cNO DATA", SYMBOL(SYM_RNGFD)); |
|
|
|
|
backend->write(x, y, false, "%c----%c", SYMBOL(SYM_RNGFD), u_icon(DISTANCE)); |
|
|
|
|
} else { |
|
|
|
|
backend->write(x, y, false, "%c%2.2f%c", SYMBOL(SYM_RNGFD), u_scale(DISTANCE, (rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f)), u_icon(DISTANCE)); |
|
|
|
|
const float distance = rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f; |
|
|
|
|
const char *format = distance < 9.995 ? "%c %1.2f%c" : "%c%2.2f%c"; |
|
|
|
|
backend->write(x, y, false, format, SYMBOL(SYM_RNGFD), u_scale(DISTANCE, distance), u_icon(DISTANCE)); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|