Add vtx table for msp-vtx devices

src/main/CMakeLists.txt

@@ -563,6 +563,8 @@ main_sources(COMMON_SRC
     io/vtx_control.h
     io/vtx_msp.c
     io/vtx_msp.h
+    io/vtx_table.c
+    io/vtx_table.h
 
     navigation/navigation.c
     navigation/navigation.h

src/main/config/parameter_group_ids.h

@@ -129,7 +129,8 @@
 #define PG_GIMBAL_CONFIG 1039
 #define PG_GIMBAL_SERIAL_CONFIG 1040
 #define PG_HEADTRACKER_CONFIG 1041
-#define PG_INAV_END PG_HEADTRACKER_CONFIG
+#define PG_VTX_TABLE_CONFIG 1042
+#define PG_INAV_END PG_VTX_TABLE_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/fc_msp.c

@@ -97,6 +97,7 @@
 #include "io/serial_4way.h"
 #include "io/vtx.h"
 #include "io/vtx_string.h"
+#include "io/vtx_table.h"
 #include "io/gps_private.h"  //for MSP_SIMULATOR
 #include "io/headtracker_msp.h"
 
@@ -1502,6 +1503,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
         break;
 
+
     case MSP_NAME:
         {
             const char *name = systemConfig()->craftName;
@@ -2736,6 +2738,81 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
+#ifdef USE_VTX_TABLE
+    case MSP_SET_VTXTABLE_BAND:
+        {
+            char bandName[VTX_TABLE_BAND_NAME_LENGTH + 1];
+            memset(bandName, 0, VTX_TABLE_BAND_NAME_LENGTH + 1);
+            uint16_t frequencies[VTX_TABLE_MAX_CHANNELS];
+            const uint8_t band = sbufReadU8(src);
+            const uint8_t bandNameLength = sbufReadU8(src);
+            for (int i = 0; i < bandNameLength; i++) {
+                const char nameChar = sbufReadU8(src);
+                if (i < VTX_TABLE_BAND_NAME_LENGTH) {
+                    bandName[i] = toupper(nameChar);
+                }
+            }
+            const char bandLetter = toupper(sbufReadU8(src));
+            const bool isFactoryBand = (bool)sbufReadU8(src);
+            const uint8_t channelCount = sbufReadU8(src);
+            for (int i = 0; i < channelCount; i++) {
+                const uint16_t frequency = sbufReadU16(src);
+                if (i < vtxTableConfig()->channels) {
+                    frequencies[i] = frequency;
+                }
+            }
+
+            if (band > 0 && band <= vtxTableConfig()->bands) {
+                vtxTableStrncpyWithPad(vtxTableConfigMutable()->bandNames[band - 1], bandName, VTX_TABLE_BAND_NAME_LENGTH);
+                vtxTableConfigMutable()->bandLetters[band - 1] = bandLetter;
+                vtxTableConfigMutable()->isFactoryBand[band - 1] = isFactoryBand;
+                for (int i = 0; i < vtxTableConfig()->channels; i++) {
+                    vtxTableConfigMutable()->frequency[band - 1][i] = frequencies[i];
+                }
+                // If this is the currently selected band then reset the frequency
+                if (band == vtxSettingsConfig()->band) {
+                    uint16_t newChannel = 0;
+                    if (vtxSettingsConfig()->channel > 0 && vtxSettingsConfig()->channel <= vtxTableConfig()->channels) {
+                        newChannel = vtxSettingsConfig()->channel;
+                    }
+                    vtxSettingsConfigMutable()->channel = newChannel;
+                    vtxCommonSetBandAndChannel(vtxCommonDevice(), band, newChannel);
+                }
+                //vtxTableNeedsInit = true;  // reinintialize vtxtable after eeprom write
+            } else {
+                return MSP_RESULT_ERROR;
+            }
+            //setMspVtxDeviceStatusReady(srcDesc);
+        }
+        break;
+
+    case MSP_SET_VTXTABLE_POWERLEVEL:
+        {
+            char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
+            memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
+            const uint8_t powerLevel = sbufReadU8(src);
+            const uint16_t powerValue = sbufReadU16(src);
+            const uint8_t powerLevelLabelLength = sbufReadU8(src);
+            for (int i = 0; i < powerLevelLabelLength; i++) {
+                const char labelChar = sbufReadU8(src);
+                if (i < VTX_TABLE_POWER_LABEL_LENGTH) {
+                    powerLevelLabel[i] = toupper(labelChar);
+                }
+            }
+
+            if (powerLevel > 0 && powerLevel <= vtxTableConfig()->powerLevels) {
+                vtxTableConfigMutable()->powerValues[powerLevel - 1] = powerValue;
+                vtxTableStrncpyWithPad(vtxTableConfigMutable()->powerLabels[powerLevel - 1], powerLevelLabel, VTX_TABLE_POWER_LABEL_LENGTH);
+                //vtxTableNeedsInit = true;  // reinintialize vtxtable after eeprom write
+                vtxCommonSetPowerByIndex(vtxCommonDevice(), powerLevel - 1);
+            } else {
+                return MSP_RESULT_ERROR;
+            }
+            //setMspVtxDeviceStatusReady(srcDesc);
+        }
+        break;
+#endif
+
 #ifdef USE_FLASHFS
     case MSP_DATAFLASH_ERASE:
         flashfsEraseCompletely();
@@ -4074,8 +4151,61 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ERROR;
         }
         break;
-#endif
+#endif 
 
+#ifdef USE_VTX_TABLE
+    case MSP_VTXTABLE_BAND:
+        {
+            const uint8_t band = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
+            if (band > 0 && band <= VTX_TABLE_MAX_BANDS) {
+                sbufWriteU8(dst, band);  // band number (same as request)
+                sbufWriteU8(dst,
+                            VTX_TABLE_BAND_NAME_LENGTH);  // band name length
+                for (int i = 0; i < VTX_TABLE_BAND_NAME_LENGTH;
+                     i++) {  // band name bytes
+                    sbufWriteU8(dst, vtxTableConfig()->bandNames[band - 1][i]);
+                }
+                sbufWriteU8(
+                    dst,
+                    vtxTableConfig()->bandLetters[band - 1]);  // band letter
+                sbufWriteU8(
+                    dst,
+                    vtxTableConfig()
+                        ->isFactoryBand[band - 1]);  // CUSTOM = 0; FACTORY = 1
+                sbufWriteU8(
+                    dst,
+                    vtxTableConfig()
+                        ->channels);  // number of channel frequencies to follow
+                for (int i = 0; i < vtxTableConfig()->channels;
+                     i++) {  // the frequency for each channel
+                    sbufWriteU16(dst, vtxTableConfig()->frequency[band - 1][i]);
+                }
+                *ret = MSP_RESULT_ACK;
+            } else {
+                *ret = MSP_RESULT_ERROR;
+            }
+            //setMspVtxDeviceStatusReady(srcDesc);
+        }
+        break;
+    case MSP_VTXTABLE_POWERLEVEL:
+        {
+            const uint8_t powerLevel = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
+            if (powerLevel > 0 && powerLevel <= VTX_TABLE_MAX_POWER_LEVELS) {
+                sbufWriteU8(dst, powerLevel);  // powerLevel number (same as request)
+                sbufWriteU16(dst, vtxTableConfig()->powerValues[powerLevel - 1]);
+                sbufWriteU8(dst, VTX_TABLE_POWER_LABEL_LENGTH); // powerLevel label length
+                for (int i = 0; i < VTX_TABLE_POWER_LABEL_LENGTH; i++) { // powerlevel label bytes
+                    sbufWriteU8(dst, vtxTableConfig()->powerLabels[powerLevel - 1][i]);
+                }
+                *ret = MSP_RESULT_ACK;
+            } else {
+                *ret = MSP_RESULT_ERROR;
+            }
+            //setMspVtxDeviceStatusReady(srcDesc);
+        }
+        break;
+#endif
+
     default:
         // Not handled
         return false;

src/main/io/vtx_msp.c

@@ -78,15 +78,15 @@ static bool prevLowPowerDisarmedState = false;
 static const vtxVTable_t mspVTable; // forward
 static vtxDevice_t vtxMsp = {
     .vTable = &mspVTable,
-    .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
-    .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
-    .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
+    .capability.bandCount = VTX_TABLE_MAX_BANDS,
+    .capability.channelCount = VTX_TABLE_MAX_CHANNELS,
+    .capability.powerCount = VTX_TABLE_MAX_POWER_LEVELS,
     .capability.bandNames = (char **)vtx58BandNames,
     .capability.channelNames = (char **)vtx58ChannelNames,
     .capability.powerNames = (char**)saPowerNames
-
 };
 
+
 STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
 static uint8_t mspVtxPortIdentifier = 255;
 
@@ -328,7 +328,7 @@ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
     LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
     UNUSED(vtxDevice);
 
-    if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
+    if (index > 0 && (index < VTX_TABLE_MAX_POWER_LEVELS))
     {
         if (index != mspConfPowerIndex)
         {
@@ -386,7 +386,7 @@ static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 
     uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
     // Special case, power not set
-    if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
+    if (power > VTX_TABLE_MAX_POWER_LEVELS) {
         *pIndex = 0;
         //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
         return true;

src/main/io/vtx_msp.h

@@ -27,25 +27,14 @@
 #include "msp/msp_protocol.h"
 #include "msp/msp_serial.h"
 
+#include "io/vtx_table.h"
+
 typedef enum {
     // Offline - device hasn't responded yet
     MSP_VTX_STATUS_OFFLINE = 0,
     MSP_VTX_STATUS_READY,
 } mspVtxStatus_e;
 
-typedef struct mspPowerTable_s {
-    int mW;         // rfpower
-    int16_t dbi;    // valueV1
-} mspPowerTable_t;
-
-#define VTX_MSP_TABLE_MAX_BANDS             5 // default freq table has 5 bands
-#define VTX_MSP_TABLE_MAX_CHANNELS          8 // and eight channels
-#define VTX_MSP_TABLE_MAX_POWER_LEVELS      5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT
-#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH   1
-#define VTX_MSP_TABLE_BAND_NAME_LENGTH      8
-#define VTX_MSP_TABLE_POWER_LABEL_LENGTH    3
-
-
 bool vtxMspInit(void);
 void setMspVtxDeviceStatusReady(const int descriptor);
 void prepareMspFrame(uint8_t *mspFrame);