diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7836b44a26b..87fd22ac9e7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2715,6 +2715,30 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) osdStartFullRedraw(); break; + case MSP2_INAV_OSD_UPDATE_POSITION: { + if (dataSize == 3) { + uint8_t item; + sbufReadU8Safe(&item, src); + if (item >= OSD_ITEM_COUNT) { + return MSP_RESULT_ERROR; + } + + uint16_t pos = sbufReadU16(src); + + osdEraseCustomItem(item); + osdLayoutsConfigMutable()->item_pos[getCurrentLayout()][item] = pos | (1 << 13); + osdDrawCustomItem(item); + + return MSP_RESULT_ACK; + + } else{ + return MSP_RESULT_ERROR; + } + + } + + + case MSP_OSD_CHAR_WRITE: if (dataSize >= 55) { osdCharacter_t chr; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8acf668892c..ff15b4ca601 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6579,4 +6579,56 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) return elemAttr; } +void osdDrawCustomItem(uint8_t item){ + osdDrawSingleElement(item); +} + +void osdEraseCustomItem(uint8_t item){ + uint8_t customElementIndex = 0; + + uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; + uint8_t elemPosX = OSD_X(pos); + uint8_t elemPosY = OSD_Y(pos); + + switch(item){ + case 147: + customElementIndex = 0; + break; + case 148: + customElementIndex = 1; + break; + case 149: + customElementIndex = 2; + break; + case 154: + customElementIndex = 3; + break; + case 155: + customElementIndex = 4; + break; + case 156: + customElementIndex = 5; + break; + case 157: + customElementIndex = 6; + break; + case 158: + customElementIndex = 7; + break; + default: + return; + } + + uint8_t len = customElementLength(customElementIndex); + + for(uint8_t i = 0; i < len; i++){ + displayWriteChar(osdDisplayPort, elemPosX+i, elemPosY, SYM_BLANK); + } + +} + #endif // OSD + +unsigned getCurrentLayout(void){ + return(currentLayout); +} diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 9e48142d71f..7cb880ee713 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -546,6 +546,10 @@ PG_DECLARE(osdConfig_t, osdConfig); typedef struct displayPort_s displayPort_t; typedef struct displayCanvas_s displayCanvas_t; +void osdDrawCustomItem(uint8_t item); +void osdEraseCustomItem(uint8_t item); +unsigned getCurrentLayout(void); + void osdInit(displayPort_t *osdDisplayPort); bool osdDisplayIsPAL(void); void osdUpdate(timeUs_t currentTimeUs); diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index 15dc8714c9f..2ad5a82885e 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -33,6 +33,8 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements, PG_OSD_CUSTOM_ELEMENTS_CONFIG, 1); +static uint8_t prevLength[MAX_CUSTOM_ELEMENTS]; + void pgResetFn_osdCustomElements(osdCustomElement_t *instance) { for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) { @@ -228,8 +230,6 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){ return; } - static uint8_t prevLength[MAX_CUSTOM_ELEMENTS]; - uint8_t buffSeek = 0; const osdCustomElement_t* customElement = osdCustomElements(customElementIndex); if(isCustomelementVisible(customElement)) @@ -246,3 +246,7 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){ } prevLength[customElementIndex] = buffSeek; } + +uint8_t customElementLength(uint8_t customElementIndex){ + return prevLength[customElementIndex] ? prevLength[customElementIndex] : 1; +} diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index 8f5ee158811..5a28c91e19d 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -80,4 +80,5 @@ typedef struct { PG_DECLARE_ARRAY(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements); -void customElementDrawElement(char *buff, uint8_t customElementIndex); \ No newline at end of file +void customElementDrawElement(char *buff, uint8_t customElementIndex); +uint8_t customElementLength(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 dcbdeb5e71f..e5da129e98a 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -46,6 +46,8 @@ #define MSP2_INAV_OSD_PREFERENCES 0x2016 #define MSP2_INAV_OSD_SET_PREFERENCES 0x2017 +#define MSP2_INAV_OSD_UPDATE_POSITION 0x2118 + #define MSP2_INAV_SELECT_BATTERY_PROFILE 0x2018 #define MSP2_INAV_DEBUG 0x2019