diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 8220333162..7575649c85 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -70,6 +70,13 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = { // @User: Standard AP_GROUPINFO("_SW_METHOD", 7, AP_OSD, sw_method, AP_OSD::TOGGLE), + // @Param: _OPTIONS + // @DisplayName: OSD Options + // @Description: This sets options that change the display + // @Bitmask: 0:UseDecimalPack + // @User: Standard + AP_GROUPINFO("_OPTIONS", 8, AP_OSD, options, 0), + AP_GROUPEND }; diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 4bdd218621..91c7f874dc 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -162,6 +162,12 @@ public: AP_Int8 rc_channel; AP_Int8 sw_method; + enum { + OPTION_DECIMAL_PACK = 1U<<0, + }; + + AP_Int32 options; + AP_OSD_Screen screen[AP_OSD_NUM_SCREENS]; private: diff --git a/libraries/AP_OSD/AP_OSD_Backend.cpp b/libraries/AP_OSD/AP_OSD_Backend.cpp index 564d6d45a0..675aa05bce 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.cpp +++ b/libraries/AP_OSD/AP_OSD_Backend.cpp @@ -16,15 +16,27 @@ #include #include +#include extern const AP_HAL::HAL& hal; +#define SYM_DIG_OFS 0x90 + void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) { char buff[32]; va_list ap; va_start(ap, fmt); int res = hal.util->vsnprintf(buff, sizeof(buff), fmt, ap); + if (res > 0 && _osd.options.get() & AP_OSD::OPTION_DECIMAL_PACK) { + // automatically use packed decimal characters + char *p = strchr(&buff[1],'.'); + if (p && isdigit(p[1]) && isdigit(p[-1])) { + p[-1] += SYM_DIG_OFS; + memmove(p, p+1, strlen(p+1)+1); + res--; + } + } if (res < int(sizeof(buff))) { write(x, y, buff, blink? AP_OSD_Backend::BLINK : 0); }