OSD custom elements

src/main/CMakeLists.txt

@@ -371,6 +371,8 @@ main_sources(COMMON_SRC
     io/rcdevice_cam.c
     io/rcdevice_cam.h
 
+    io/osd/custom_elements.c
+
     msp/msp_serial.c
     msp/msp_serial.h
 

src/main/config/parameter_group_ids.h

@@ -125,7 +125,8 @@
 #define PG_OSD_JOYSTICK_CONFIG 1035
 #define PG_FW_AUTOLAND_CONFIG 1036
 #define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
-#define PG_INAV_END PG_FW_AUTOLAND_APPROACH_CONFIG
+#define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
+#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/cli.c

@@ -93,6 +93,7 @@ bool cliMode = false;
 #include "io/gps_ublox.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
+#include "io/osd/custom_elements.h"
 #include "io/serial.h"
 
 #include "fc/fc_msp_box.h"
@@ -2333,6 +2334,137 @@ static void cliPid(char *cmdline) {
     }
 }
 
+static void printOsdCustomElements(uint8_t dumpMask, const osdCustomElement_t *osdCustomElements, const osdCustomElement_t *defaultosdCustomElements)
+{
+    const char *format = "osd_custom_elements %d %d %d %d %d %d %d %d %d \"%s\"";
+
+    if(CUSTOM_ELEMENTS_PARTS != 3)
+    {
+        cliPrintHashLine("Incompatible count of elements for custom OSD elements");
+    }
+
+    for (uint8_t i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+        bool equalsDefault = false;
+
+        const osdCustomElement_t osdCustomElement = osdCustomElements[i];
+        if(defaultosdCustomElements){
+            const osdCustomElement_t defaultValue = defaultosdCustomElements[i];
+            equalsDefault =
+                    osdCustomElement.part[0].type == defaultValue.part[0].type &&
+                    osdCustomElement.part[0].value == defaultValue.part[0].value &&
+                    osdCustomElement.part[1].type == defaultValue.part[1].type &&
+                    osdCustomElement.part[1].value == defaultValue.part[1].value &&
+                    osdCustomElement.part[2].type == defaultValue.part[2].type &&
+                    osdCustomElement.part[2].value == defaultValue.part[2].value &&
+                    osdCustomElement.visibility.type == defaultValue.visibility.type &&
+                    osdCustomElement.visibility.value == defaultValue.visibility.value &&
+                    strcmp(osdCustomElement.osdCustomElementText, defaultValue.osdCustomElementText) == 0;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                osdCustomElement.part[0].type,
+                osdCustomElement.part[0].value,
+                osdCustomElement.part[1].type,
+                osdCustomElement.part[1].value,
+                osdCustomElement.part[2].type,
+                osdCustomElement.part[2].value,
+                osdCustomElement.visibility.type,
+                osdCustomElement.visibility.value,
+                osdCustomElement.osdCustomElementText
+            );
+        }
+
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            osdCustomElement.part[0].type,
+            osdCustomElement.part[0].value,
+            osdCustomElement.part[1].type,
+            osdCustomElement.part[1].value,
+            osdCustomElement.part[2].type,
+            osdCustomElement.part[2].value,
+            osdCustomElement.visibility.type,
+            osdCustomElement.visibility.value,
+            osdCustomElement.osdCustomElementText
+        );
+    }
+}
+
+static void osdCustom(char *cmdline){
+    char * saveptrMain;
+    char * saveptrParams;
+    int args[10], check = 0;
+    char text[OSD_CUSTOM_ELEMENT_TEXT_SIZE];
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printOsdCustomElements(DUMP_MASTER, osdCustomElements(0), NULL);
+    } else {
+        //split by ", first are params second is text
+        char *ptrMain = strtok_r(cmdline, "\"", &saveptrMain);
+        enum {
+            INDEX = 0,
+            PART0_TYPE,
+            PART0_VALUE,
+            PART1_TYPE,
+            PART1_VALUE,
+            PART2_TYPE,
+            PART2_VALUE,
+            VISIBILITY_TYPE,
+            VISIBILITY_VALUE,
+            ARGS_COUNT
+        };
+        char *ptrParams = strtok_r(ptrMain, " ", &saveptrParams);
+        while (ptrParams != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptrParams);
+            ptrParams = strtok_r(NULL, " ", &saveptrParams);
+        }
+
+        if (check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        //text
+        char *ptrText = strtok_r(NULL, "\"", &saveptrMain);
+        size_t copySize = 0;
+        if(ptrText != NULL){
+            copySize = MIN(strlen(ptrText), (size_t)(sizeof(text) - 1));
+            if(copySize > 0){
+                memcpy(text, ptrText, copySize);
+            }
+        }
+        text[copySize] = '\0';
+
+        int32_t i = args[INDEX];
+        if (
+                i >= 0 && i < MAX_CUSTOM_ELEMENTS &&
+                args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 7 &&
+                args[PART0_VALUE] >= 0 && args[PART0_VALUE] <= UINT8_MAX &&
+                args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 7 &&
+                args[PART1_VALUE] >= 0 && args[PART1_VALUE] <= UINT8_MAX &&
+                args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 7 &&
+                args[PART2_VALUE] >= 0 && args[PART2_VALUE] <= UINT8_MAX &&
+                args[VISIBILITY_TYPE] >= 0 && args[VISIBILITY_TYPE] <= 2 &&
+                args[VISIBILITY_VALUE] >= 0 && args[VISIBILITY_VALUE] <= UINT8_MAX
+                ) {
+            osdCustomElementsMutable(i)->part[0].type = args[PART0_TYPE];
+            osdCustomElementsMutable(i)->part[0].value = args[PART0_VALUE];
+            osdCustomElementsMutable(i)->part[1].type = args[PART1_TYPE];
+            osdCustomElementsMutable(i)->part[1].value = args[PART1_VALUE];
+            osdCustomElementsMutable(i)->part[2].type = args[PART2_TYPE];
+            osdCustomElementsMutable(i)->part[2].value = args[PART2_VALUE];
+            osdCustomElementsMutable(i)->visibility.type = args[VISIBILITY_TYPE];
+            osdCustomElementsMutable(i)->visibility.value = args[VISIBILITY_VALUE];
+            memcpy(osdCustomElementsMutable(i)->osdCustomElementText, text, OSD_CUSTOM_ELEMENT_TEXT_SIZE);
+
+            osdCustom("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
+
 #endif
 
 #ifdef USE_SDCARD
@@ -3972,6 +4104,10 @@ static void printConfig(const char *cmdline, bool doDiff)
         cliPrintHashLine("Programming: PID controllers");
         printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+        cliPrintHashLine("OSD: custom elements");
+        printOsdCustomElements(dumpMask, osdCustomElements_CopyArray, osdCustomElements(0));
+#endif
 
         cliPrintHashLine("master");
         dumpAllValues(MASTER_VALUE, dumpMask);
@@ -4201,6 +4337,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("pid", "configurable PID controllers",
         "<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
         "\treset\r\n", cliPid),
+
+    CLI_COMMAND_DEF("osd_custom_elements", "configurable OSD custom elements",
+                    "<#> <part0 type> <part0 value> <part1 type> <part1 value> <part2 type> <part2 value> <visibility type> <visibility value> <text>\r\n"
+                    , osdCustom),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
     CLI_COMMAND_DEF("smix", "servo mixer",

src/main/fc/fc_msp.c

@@ -96,6 +96,8 @@
 #include "io/vtx_string.h"
 #include "io/gps_private.h"  //for MSP_SIMULATOR
 
+#include "io/osd/custom_elements.h"
+
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
@@ -1637,12 +1639,30 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    case MSP2_INAV_CUSTOM_OSD_ELEMENTS:
+        sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
+        sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
 
+        for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+            const osdCustomElement_t *customElement = osdCustomElements(i);
+            for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
+                sbufWriteU8(dst, customElement->part[ii].type);
+                sbufWriteU16(dst, customElement->part[ii].value);
+            }
+            sbufWriteU8(dst, customElement->visibility.type);
+            sbufWriteU16(dst, customElement->visibility.value);
+            for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
+                sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+            }
+        }
+        break;
     default:
         return false;
     }
     return true;
 }
+#endif
 
 #ifdef USE_SAFE_HOME
 static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
@@ -3232,6 +3252,25 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;    
 
 #endif
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    case MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (MAX_CUSTOM_ELEMENTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
+            for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) {
+                osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src);
+                osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src);
+            }
+            osdCustomElementsMutable(tmp_u8)->visibility.type = sbufReadU8(src);
+            osdCustomElementsMutable(tmp_u8)->visibility.value = sbufReadU16(src);
+            for (int i = 0; i < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; i++) {
+                osdCustomElementsMutable(tmp_u8)->osdCustomElementText[i] = sbufReadU8(src);
+            }
+            osdCustomElementsMutable(tmp_u8)->osdCustomElementText[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1] = '\0';
+        } else{
+            return MSP_RESULT_ERROR;
+        }
+
+        break;
 
 
     default:
@@ -3239,6 +3278,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     }
     return MSP_RESULT_ACK;
 }
+#endif
 
 static const setting_t *mspReadSetting(sbuf_t *src)
 {

src/main/io/osd.c

@@ -72,6 +72,8 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
+#include "io/osd/custom_elements.h"
+
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
@@ -1695,6 +1697,21 @@ static bool osdDrawSingleElement(uint8_t item)
     char buff[32] = {0};
 
     switch (item) {
+    case OSD_CUSTOM_ELEMENT_1:
+    {
+        customElementDrawElement(buff, 0);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_2:
+    {
+        customElementDrawElement(buff, 1);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_3:
+    {
+        customElementDrawElement(buff, 2);
+        break;
+    }
     case OSD_RSSI_VALUE:
         {
             uint16_t osdRssi = osdConvertRSSI();

src/main/io/osd.h

@@ -281,6 +281,9 @@ typedef enum {
     OSD_MULTI_FUNCTION,
     OSD_ODOMETER,
     OSD_PILOT_LOGO,
+    OSD_CUSTOM_ELEMENT_1,
+    OSD_CUSTOM_ELEMENT_2,
+    OSD_CUSTOM_ELEMENT_3,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/io/osd/custom_elements.c

@@ -0,0 +1,141 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "common/string_light.h"
+#include "common/maths.h"
+
+#include "programming/logic_condition.h"
+#include "programming/global_variables.h"
+
+#include "io/osd.h"
+#include "io/osd/custom_elements.h"
+
+#include "drivers/osd_symbols.h"
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements, PG_OSD_CUSTOM_ELEMENTS_CONFIG, 1);
+
+void pgResetFn_osdCustomElements(osdCustomElement_t *instance)
+{
+    for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
+        RESET_CONFIG(osdCustomElement_t, &instance[i],
+            .part[0]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .part[1]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .part[2]              = {.type = CUSTOM_ELEMENT_TYPE_NONE, .value = 0},
+            .visibility           = {.type = CUSTOM_ELEMENT_VISIBILITY_ALWAYS, .value = 0},
+            .osdCustomElementText   = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+        );
+    }
+}
+
+bool isCustomelementVisible(const osdCustomElement_t* customElement){
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_ALWAYS){
+        return true;
+    }
+
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_GV && gvGet(customElement->visibility.value)){
+        return true;
+    }
+
+    if(customElement->visibility.type == CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON && logicConditionGetValue(customElement->visibility.value)){
+        return true;
+    }
+
+    return false;
+}
+
+uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t customElementItemIndex){
+    const osdCustomElement_t* customElement = osdCustomElements(customElementIndex);
+    const int customPartType = osdCustomElements(customElementIndex)->part[customElementItemIndex].type;
+    const int customPartValue = osdCustomElements(customElementIndex)->part[customElementItemIndex].value;
+
+    switch (customPartType) {
+        case CUSTOM_ELEMENT_TYPE_GV:
+        {
+            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue) * (int32_t) 100, 1, 0, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT:
+        {
+            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue), 1, 2, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_SMALL:
+        {
+            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 1000 ) * (int32_t) 100), 1, 0, 0, 3, false);
+            return 3;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT:
+        {
+            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 100)  * (int32_t) 10), 1, 1, 0, 2, false);
+            return 2;
+        }
+        case CUSTOM_ELEMENT_TYPE_ICON_GV:
+        {
+            *buff = (uint8_t)gvGet(customPartValue);
+            return 1;
+        }
+        case CUSTOM_ELEMENT_TYPE_ICON_STATIC:
+        {
+            *buff = (uint8_t)customPartValue;
+            return 1;
+        }
+        case CUSTOM_ELEMENT_TYPE_TEXT:
+        {
+            for (int i = 0; i < OSD_CUSTOM_ELEMENT_TEXT_SIZE; i++) {
+                if (customElement->osdCustomElementText[i] == 0){
+                    return i;
+                }
+                *buff = sl_toupper((unsigned char)customElement->osdCustomElementText[i]);
+                buff++;
+            }
+            return OSD_CUSTOM_ELEMENT_TEXT_SIZE;
+        }
+    }
+
+    return 0;
+}
+
+void customElementDrawElement(char *buff, uint8_t customElementIndex){
+
+    if(customElementIndex >= MAX_CUSTOM_ELEMENTS){
+        return;
+    }
+
+    static uint8_t prevLength[MAX_CUSTOM_ELEMENTS];
+
+    uint8_t buffSeek = 0;
+    const osdCustomElement_t* customElement = osdCustomElements(customElementIndex);
+    if(isCustomelementVisible(customElement))
+    {
+        for (uint8_t i = 0; i < CUSTOM_ELEMENTS_PARTS; ++i) {
+            uint8_t currentSeek = customElementDrawPart(buff, customElementIndex, i);
+            buff += currentSeek;
+            buffSeek += currentSeek;
+        }
+    }
+
+    for (uint8_t i = buffSeek; i < prevLength[customElementIndex]; i++) {
+        *buff++ = SYM_BLANK;
+    }
+    prevLength[customElementIndex] = buffSeek;
+}
+

src/main/io/osd/custom_elements.h

@@ -0,0 +1,61 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+
+#define OSD_CUSTOM_ELEMENT_TEXT_SIZE 16
+#define CUSTOM_ELEMENTS_PARTS 3
+#define MAX_CUSTOM_ELEMENTS 3
+
+typedef enum {
+    CUSTOM_ELEMENT_TYPE_NONE            = 0,
+    CUSTOM_ELEMENT_TYPE_TEXT            = 1,
+    CUSTOM_ELEMENT_TYPE_ICON_STATIC     = 2,
+    CUSTOM_ELEMENT_TYPE_ICON_GV         = 3,
+    CUSTOM_ELEMENT_TYPE_GV              = 4,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT        = 5,
+    CUSTOM_ELEMENT_TYPE_GV_SMALL        = 6,
+    CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT  = 7,
+} osdCustomElementType_e;
+
+typedef enum {
+    CUSTOM_ELEMENT_VISIBILITY_ALWAYS    = 0,
+    CUSTOM_ELEMENT_VISIBILITY_GV        = 1,
+    CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON = 2,
+} osdCustomElementTypeVisibility_e;
+
+typedef struct {
+    osdCustomElementType_e  type;
+    uint16_t                value;
+} osdCustomElementItem_t;
+
+typedef struct {
+    osdCustomElementTypeVisibility_e    type;
+    uint16_t                            value;
+} osdCustomElementVisibility_t;
+
+typedef struct  {
+    osdCustomElementItem_t          part[CUSTOM_ELEMENTS_PARTS];
+    osdCustomElementVisibility_t    visibility;
+    char                            osdCustomElementText[OSD_CUSTOM_ELEMENT_TEXT_SIZE];
+} osdCustomElement_t;
+
+PG_DECLARE_ARRAY(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements);
+
+void customElementDrawElement(char *buff, uint8_t customElementIndex);

src/main/msp/msp_protocol_v2_inav.h

@@ -102,3 +102,7 @@
 #define MSP2_INAV_EZ_TUNE_SET                   0x2071
 
 #define MSP2_INAV_SELECT_MIXER_PROFILE          0x2080
+
+#define MSP2_INAV_CUSTOM_OSD_ELEMENTS           0x2100
+#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2101
+