From 3495596e6e593191f178c0a4ac90d483ad5daad7 Mon Sep 17 00:00:00 2001 From: error414 Date: Wed, 14 Feb 2024 06:03:41 +0100 Subject: [PATCH] OSD custom elements --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/cli.c | 140 +++++++++++++++++++++++++ src/main/fc/fc_msp.c | 40 ++++++++ src/main/io/osd.c | 17 ++++ src/main/io/osd.h | 3 + src/main/io/osd/custom_elements.c | 141 ++++++++++++++++++++++++++ src/main/io/osd/custom_elements.h | 61 +++++++++++ src/main/msp/msp_protocol_v2_inav.h | 4 + 9 files changed, 410 insertions(+), 1 deletion(-) create mode 100644 src/main/io/osd/custom_elements.c create mode 100644 src/main/io/osd/custom_elements.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 13ac4a92911..7f076b5e29e 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index c6588b8289b..822e85f137a 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8b973f619b9..7b9445c5baa 100644 --- a/src/main/fc/cli.c +++ b/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", "<#>

\r\n" "\treset\r\n", cliPid), + + CLI_COMMAND_DEF("osd_custom_elements", "configurable OSD custom elements", + "<#> \r\n" + , osdCustom), #endif CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), CLI_COMMAND_DEF("smix", "servo mixer", diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6ede14dee84..25e9d4c135d 100644 --- a/src/main/fc/fc_msp.c +++ b/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) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 339a999dc90..7e67faef01f 100644 --- a/src/main/io/osd.c +++ b/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(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7fd68f89898..8e1a38891f5 100644 --- a/src/main/io/osd.h +++ b/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; diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c new file mode 100644 index 00000000000..fbd05e2be61 --- /dev/null +++ b/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 . + */ + + +#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; +} + diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h new file mode 100644 index 00000000000..a55b010f01a --- /dev/null +++ b/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 . + */ + +#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); \ No newline at end of file diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index c30d37b6f78..356cd54ca5a 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 +