diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 055dcb6fe..64821a73d 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -228,4 +228,71 @@ bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t return false; } } + +static void vtxCommonSerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst) +{ + const bool customDeviceStatusAvailable = vtxDevice && vtxDevice->vTable->serializeCustomDeviceStatus; + + if (customDeviceStatusAvailable) { + vtxDevice->vTable->serializeCustomDeviceStatus(vtxDevice, dst); + } else { + sbufWriteU8(dst, 0); + } +} + +static void vtxCommonSerializePowerLevels(const vtxDevice_t *vtxDevice, sbuf_t *dst) +{ + uint16_t levels[VTX_TABLE_MAX_POWER_LEVELS]; + uint16_t powers[VTX_TABLE_MAX_POWER_LEVELS]; + + const uint8_t powerLevelCount = vtxCommonGetVTXPowerLevels(vtxDevice, levels, powers); + + sbufWriteU8(dst, powerLevelCount); + + for (int i = 0; i < powerLevelCount; i++) { + sbufWriteU16(dst, levels[i]); + sbufWriteU16(dst, powers[i]); + } +} + +void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst) +{ + if (vtxDevice) { + const vtxDevType_e vtxType = vtxCommonGetDeviceType(vtxDevice); + const bool deviceReady = vtxCommonDeviceIsReady(vtxDevice); + + uint8_t band = 0; + uint8_t channel = 0; + const bool bandAndChannelAvailable = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel); + + uint8_t powerIndex = 0; + const bool powerIndexAvailable = vtxCommonGetPowerIndex(vtxDevice, &powerIndex); + + uint16_t frequency = 0; + const bool frequencyAvailable = vtxCommonGetFrequency(vtxDevice, &frequency); + + unsigned vtxStatus = 0; // pitmode and/or locked + const bool vtxStatusAvailable = vtxCommonGetStatus(vtxDevice, &vtxStatus); + + sbufWriteU8(dst, vtxType); + sbufWriteU8(dst, deviceReady); + + sbufWriteU8(dst, bandAndChannelAvailable); + sbufWriteU8(dst, band); + sbufWriteU8(dst, channel); + + sbufWriteU8(dst, powerIndexAvailable); + sbufWriteU8(dst, powerIndex); + + sbufWriteU8(dst, frequencyAvailable); + sbufWriteU16(dst, frequency); + + sbufWriteU8(dst, vtxStatusAvailable); + sbufWriteU32(dst, vtxStatus); + + vtxCommonSerializePowerLevels(vtxDevice, dst); + vtxCommonSerializeCustomDeviceStatus(vtxDevice, dst); + } +} + #endif diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 9b58142f9..1bd0aa765 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -26,6 +26,7 @@ #include "platform.h" #include "common/time.h" +#include "common/streambuf.h" #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting @@ -112,6 +113,8 @@ typedef struct vtxVTable_s { bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); bool (*getStatus)(const vtxDevice_t *vtxDevice, unsigned *status); uint8_t (*getPowerLevels)(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers); + + void (*serializeCustomDeviceStatus)(const vtxDevice_t *vtxDevice, sbuf_t *dst); } vtxVTable_t; // 3.1.0 @@ -127,15 +130,19 @@ vtxDevice_t *vtxCommonDevice(void); void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice); bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice); + void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t level); void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t freq); + bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex); bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status); uint8_t vtxCommonGetVTXPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers); +// end of VTable functions + const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band); char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band); char vtxCommonGetBandLetter(const vtxDevice_t *vtxDevice, int band); @@ -144,3 +151,4 @@ uint16_t vtxCommonLookupFrequency(const vtxDevice_t *vtxDevice, int band, int ch void vtxCommonLookupBandChan(const vtxDevice_t *vtxDevice, uint16_t freq, uint8_t *pBand, uint8_t *pChannel); const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index); bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t *pPowerValue); +void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst); diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index 2ed643984..9789d4c7a 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -245,7 +245,9 @@ static vtxVTable_t rtc6705VTable = { .getFrequency = vtxRTC6705GetFreq, .getStatus = vtxRTC6705GetStatus, .getPowerLevels = vtxRTC6705GetPowerLevels, + .serializeCustomDeviceStatus = NULL, }; + #endif // VTX_COMMON #endif // VTX_RTC6705 diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index ee6702668..9e8288caa 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1047,6 +1047,20 @@ static uint8_t vtxSAGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *level return saSupportedNumPowerLevels; } +#define VTX_CUSTOM_DEVICE_STATUS_SIZE 5 + +static void vtxSASerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst) +{ + UNUSED(vtxDevice); + sbufWriteU8(dst, VTX_CUSTOM_DEVICE_STATUS_SIZE); + sbufWriteU8(dst, saDevice.version); + sbufWriteU8(dst, saDevice.mode); + sbufWriteU16(dst, saDevice.orfreq); // pit frequency + sbufWriteU8(dst, saDevice.willBootIntoPitMode); +} + +#undef VTX_CUSTOM_DEVICE_STATUS_SIZE + static const vtxVTable_t saVTable = { .process = vtxSAProcess, .getDeviceType = vtxSAGetDeviceType, @@ -1060,6 +1074,7 @@ static const vtxVTable_t saVTable = { .getFrequency = vtxSAGetFreq, .getStatus = vtxSAGetStatus, .getPowerLevels = vtxSAGetPowerLevels, + .serializeCustomDeviceStatus = vtxSASerializeCustomDeviceStatus, }; #endif // VTX_COMMON diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 9c0e5c789..562eb3031 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -643,6 +643,7 @@ static const vtxVTable_t trampVTable = { .getFrequency = vtxTrampGetFreq, .getStatus = vtxTrampGetStatus, .getPowerLevels = vtxTrampGetPowerLevels, + .serializeCustomDeviceStatus = NULL, }; #endif diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 06e364090..594776b7d 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1201,6 +1201,15 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) } break; +#ifdef USE_VTX_COMMON + case MSP2_GET_VTX_DEVICE_STATUS: + { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + vtxCommonSerializeDeviceStatus(vtxDevice, dst); + } + break; +#endif + case MSP_RC: for (int i = 0; i < rxRuntimeState.channelCount; i++) { sbufWriteU16(dst, rcData[i]); diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h index fd11c5873..9c1d938f3 100644 --- a/src/main/msp/msp_protocol_v2_betaflight.h +++ b/src/main/msp/msp_protocol_v2_betaflight.h @@ -22,4 +22,4 @@ #define MSP2_MOTOR_OUTPUT_REORDERING 0x3001 #define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002 #define MSP2_SEND_DSHOT_COMMAND 0x3003 - +#define MSP2_GET_VTX_DEVICE_STATUS 0x3004