diff --git a/libraries/AP_OSD/AP_OSD_Backend.cpp b/libraries/AP_OSD/AP_OSD_Backend.cpp index f53f818006..705a468c9d 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.cpp +++ b/libraries/AP_OSD/AP_OSD_Backend.cpp @@ -24,32 +24,69 @@ constexpr uint8_t AP_OSD_Backend::symbols[AP_OSD_NUM_SYMBOLS]; #define SYM_DIG_OFS_1 0x90 #define SYM_DIG_OFS_2 0xA0 + +uint8_t AP_OSD_Backend::convert_to_decimal_packed_characters(char* buff, uint8_t size) { + if (size == 0) { + return 0; + } +#if OSD_ENABLED + // use packed decimal characters based on fiam idea implemented in inav osd + // search the decimal separator with a bound and always terminate the string + char* p = (char*)memchr(&buff[1],'.',size-1); + if (p && isdigit(p[1]) && isdigit(p[-1])) { + // remove the decimal separator and replace the digit before and after + p[-1] += SYM_DIG_OFS_1; + p[1] += SYM_DIG_OFS_2; + // shift anything after p[1] 1 character to the left + const char* move_start = p+1; + const uint8_t move_size = size-(move_start-buff); + memmove(p, move_start, move_size); + p[move_size] = 0x00; // terminate + return size-1; + } +#else + // we guarantee string is terminated + buff[size-1] = 0x00; +#endif + return size; +} + +uint8_t AP_OSD_Backend::format_string_for_osd(char* buff, uint8_t size, bool decimal_packed, const char *fmt, va_list ap) +{ + if (size == 0) { + return 0; + } +#if OSD_ENABLED + // note: vsnprintf() always terminates the string + int res = hal.util->vsnprintf(buff, size, fmt, ap); + res = MIN(res, size); + if (res > 0 && decimal_packed) { + // note: convert_to_decimal_packed_characters() always terminates the string + res = convert_to_decimal_packed_characters(buff, res); + } + return res; +#else + // we guarantee string is terminated + buff[0] = 0x00; + // and notify the caller we actually failed + return 0; +#endif +} + void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) { #if OSD_ENABLED if (blink && (blink_phase < 2)) { return; } - char buff[32+1]; // +1 for snprintf null-termination va_list ap; va_start(ap, fmt); - int res = hal.util->vsnprintf(buff, sizeof(buff), fmt, ap); - res = MIN(res, int(sizeof(buff))); - if (res > 0 && check_option(AP_OSD::OPTION_DECIMAL_PACK)) { - // automatically use packed decimal characters - // based on fiam idea implemented in inav osd - char *p = strchr(&buff[1],'.'); - if (p && isdigit(p[1]) && isdigit(p[-1])) { - p[-1] += SYM_DIG_OFS_1; - p[1] += SYM_DIG_OFS_2; - memmove(p, p+1, strlen(p+1)+1); - res--; - } - } - if (res < int(sizeof(buff))-1) { - write(x, y, buff); - } + char buff[32+1]; // +1 for null-termination + // note: format_string_for_osd() always terminates the string + IGNORE_RETURN(format_string_for_osd(buff, sizeof(buff), check_option(AP_OSD::OPTION_DECIMAL_PACK), fmt, ap)); va_end(ap); + // buff is null terminated, this call should be safe without further checks + write(x, y, buff); #endif } diff --git a/libraries/AP_OSD/AP_OSD_Backend.h b/libraries/AP_OSD/AP_OSD_Backend.h index feacb55c07..bd91c643b8 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.h +++ b/libraries/AP_OSD/AP_OSD_Backend.h @@ -76,6 +76,9 @@ protected: return (_osd.options & option) != 0; } + uint8_t convert_to_decimal_packed_characters(char* buff, uint8_t size); + virtual uint8_t format_string_for_osd(char* dst, uint8_t size, bool decimal_packed, const char *fmt, va_list ap); + // load a font from sdcard or ROMFS FileData *load_font_data(uint8_t font_num); diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp index 1ea73116d9..f2d14bb1a1 100644 --- a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp @@ -67,20 +67,11 @@ void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, const char* text) _displayport->msp_displayport_write_string(x, y, 0, text); } -void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) +uint8_t AP_OSD_MSP_DisplayPort::format_string_for_osd(char* buff, uint8_t size, bool decimal_packed, const char *fmt, va_list ap) { - if (blink && !_blink_on) { - return; - } - char buf[32+1]; // +1 for snprintf null-termination - va_list ap; - va_start(ap, fmt); - int res = hal.util->vsnprintf(buf, sizeof(buf), fmt, ap); - res = MIN(res, int(sizeof(buf))); - if (res < int(sizeof(buf))-1) { - _displayport->msp_displayport_write_string(x, y, blink, buf); - } - va_end(ap); + const AP_MSP *msp = AP::msp(); + const bool pack = decimal_packed && msp && !msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS); + return AP_OSD_Backend::format_string_for_osd(buff, size, pack, fmt, ap); } void AP_OSD_MSP_DisplayPort::flush(void) diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h index 5c5a3856cf..98c70d69a1 100644 --- a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h @@ -15,9 +15,6 @@ public: //draw given text to framebuffer void write(uint8_t x, uint8_t y, const char* text) override; - //draw formatted text to framebuffer - void write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) override FMT_PRINTF(5, 6); - //flush framebuffer to screen void flush() override; @@ -28,6 +25,9 @@ public: void init_symbol_set(uint8_t *lookup_table, const uint8_t size) override; +protected: + uint8_t format_string_for_osd(char* dst, uint8_t size, bool decimal_packed, const char *fmt, va_list ap) override; + private: void setup_defaults(void);