Merge pull request #10413 from limonspb/smartaudio_version

Added vtx device status to MSP

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

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);

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

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
 

src/main/io/vtx_tramp.c

@@ -643,6 +643,7 @@ static const vtxVTable_t trampVTable = {
     .getFrequency = vtxTrampGetFreq,
     .getStatus = vtxTrampGetStatus,
     .getPowerLevels = vtxTrampGetPowerLevels,
+    .serializeCustomDeviceStatus = NULL,
 };
 #endif
 

src/main/msp/msp.c

@@ -1202,6 +1202,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]);

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