AP_OSD: added packed decimal support to MSP DisplayPort

This allows to take advantage of ArduPilot's compact decimal fonts

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
 }
 

libraries/AP_OSD/AP_OSD_Backend.h

@@ -76,6 +76,9 @@ class AP_OSD_Backend
         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);
 

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)

libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h

@@ -15,9 +15,6 @@ class AP_OSD_MSP_DisplayPort : public AP_OSD_Backend
     //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 @@ class AP_OSD_MSP_DisplayPort : public AP_OSD_Backend
     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);