Merge pull request #2146 from raphaelcoeffic/vtx-msp

Vtx MSP requests (rebased / squashed)

src/main/drivers/vtx_common.c

@@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs)
 
 vtxDevType_e vtxCommonGetDeviceType(void)
 {
-    if (!vtxDevice)
+    if (!vtxDevice || !vtxDevice->vTable->getDeviceType)
         return VTXDEV_UNKNOWN;
 
-    return vtxDevice->devtype;
+    return vtxDevice->vTable->getDeviceType();
 }
 
 // band and chan are 1 origin
@@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan)
     if (!vtxDevice)
         return;
 
+    if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan))
+        return;
+    
     if (vtxDevice->vTable->setBandChan)
         vtxDevice->vTable->setBandChan(band, chan);
 }
@@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index)
     if (!vtxDevice)
         return;
 
+    if (index > vtxDevice->numPower)
+        return;
+    
     if (vtxDevice->vTable->setPowerByIndex)
         vtxDevice->vTable->setPowerByIndex(index);
 }

src/main/drivers/vtx_common.h

@@ -18,9 +18,12 @@
 /* Created by jflyper */
 
 typedef enum {
-    VTXDEV_UNKNOWN    = 0,
+    VTXDEV_UNSUPPORTED = 0, // reserved for MSP
+    // 1 reserved
+    // 2 reserved
     VTXDEV_SMARTAUDIO = 3,
     VTXDEV_TRAMP      = 4,
+    VTXDEV_UNKNOWN    = 0xFF,
 } vtxDevType_e;
 
 struct vtxVTable_s;
@@ -28,8 +31,6 @@ struct vtxVTable_s;
 typedef struct vtxDevice_s {
     const struct vtxVTable_s *vTable;
 
-    vtxDevType_e devtype;      // 3.1 only; eventually goes away
-
     uint8_t numBand;
     uint8_t numChan;
     uint8_t numPower;

src/main/fc/fc_msp.c

@@ -47,6 +47,7 @@
 #include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
 #include "drivers/serial_escserial.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -1187,6 +1188,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         }
         break;
 
+#if defined(VTX_COMMON)
+    case MSP_VTX_CONFIG:
+        {
+            uint8_t deviceType = vtxCommonGetDeviceType();
+            if (deviceType != VTXDEV_UNKNOWN) {
+
+                uint8_t band=0, channel=0;
+                vtxCommonGetBandChan(&band,&channel);
+                
+                uint8_t powerIdx=0; // debug
+                vtxCommonGetPowerIndex(&powerIdx);
+                
+                uint8_t pitmode=0;
+                vtxCommonGetPitmode(&pitmode);
+                
+                sbufWriteU8(dst, deviceType);
+                sbufWriteU8(dst, band);
+                sbufWriteU8(dst, channel);
+                sbufWriteU8(dst, powerIdx);
+                sbufWriteU8(dst, pitmode);
+                // future extensions here...
+            }
+            else {
+                sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected
+            }
+        }
+#endif
+        break;
+
     default:
         return false;
     }
@@ -1634,15 +1664,44 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
-#ifdef USE_RTC6705
+#if defined(USE_RTC6705) || defined(VTX_COMMON)
     case MSP_SET_VTX_CONFIG:
-        ;
+        {
-        uint16_t tmp = sbufReadU16(src);
+            uint16_t tmp = sbufReadU16(src);
-        if  (tmp < 40)
+#if defined(USE_RTC6705)
-            masterConfig.vtx_channel = tmp;
+            if  (tmp < 40)
-        if (current_vtx_channel != masterConfig.vtx_channel) {
+                masterConfig.vtx_channel = tmp;
-            current_vtx_channel = masterConfig.vtx_channel;
+            if (current_vtx_channel != masterConfig.vtx_channel) {
-            rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+                current_vtx_channel = masterConfig.vtx_channel;
+                rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+            }
+#else
+            if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) {
+
+                uint8_t band    = (tmp / 8) + 1;
+                uint8_t channel = (tmp % 8) + 1;
+
+                uint8_t current_band=0, current_channel=0;
+                vtxCommonGetBandChan(&current_band,&current_channel);
+                if ((current_band != band) || (current_channel != channel))
+                    vtxCommonSetBandChan(band,channel);
+
+                if (sbufBytesRemaining(src) < 2)
+                    break;
+            
+                uint8_t power = sbufReadU8(src);
+                uint8_t current_power = 0;
+                vtxCommonGetPowerIndex(&current_power);
+                if (current_power != power)
+                    vtxCommonSetPowerByIndex(power);
+            
+                uint8_t pitmode = sbufReadU8(src);
+                uint8_t current_pitmode = 0;
+                vtxCommonGetPitmode(&current_pitmode);
+                if (current_pitmode != pitmode)
+                    vtxCommonSetPitmode(pitmode);
+            }
+#endif
         }
         break;
 #endif

src/main/io/vtx_tramp.c

@@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = {
 };
 
 static const char * const trampPowerNames[] = {
-    "25 ", "100", "200", "400", "600"
+    "---", "25 ", "100", "200", "400", "600"
 };
 #endif
 
 #if defined(VTX_COMMON)
-static vtxVTable_t trampVTable;    // Forward
 static vtxDevice_t vtxTramp = {
-    .vTable = &trampVTable,
+    .vTable = NULL,
     .numBand = 5,
     .numChan = 8,
     .numPower = sizeof(trampPowerTable),
@@ -309,26 +308,6 @@ void trampQueryS(void)
     trampQuery('s');
 }
 
-bool trampInit()
-{
-    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
-
-    if (portConfig) {
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
-    }
-
-    if (!trampSerialPort) {
-        return false;
-    }
-
-#if defined(VTX_COMMON)
-    vtxTramp.vTable = &trampVTable;
-    vtxCommonRegisterDevice(&vtxTramp);
-#endif
-
-    return true;
-}
-
 void vtxTrampProcess(uint32_t currentTimeUs)
 {
     static uint32_t lastQueryTimeUs = 0;
@@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL }
 
 static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 };
 
-static uint8_t trampCmsPower = 0;
+static uint8_t trampCmsPower = 1;
 
-static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL };
+static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL };
 
 static void trampCmsUpdateFreqRef(void)
 {
@@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self)
     UNUSED(self);
 
     trampSetBandChan(trampCmsBand, trampCmsChan);
-    trampSetRFPower(trampPowerTable[trampCmsPower]);
+    trampSetRFPower(trampPowerTable[trampCmsPower-1]);
 
     // If it fails, the user should retry later
     trampCommitChanges();
@@ -559,7 +538,7 @@ static void trampCmsInitSettings()
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                trampCmsPower = i;
+                trampCmsPower = i + 1;
                 break;
             }
         }
@@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan)
 void vtxTrampSetPowerByIndex(uint8_t index)
 {
     if (index) {
-        trampSetRFPower(trampPowerTable[index]);
+        trampSetRFPower(trampPowerTable[index - 1]);
         trampCommitChanges();
     }
 }
@@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
     if (trampCurConfigPower > 0) {
         for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
             if (trampCurConfigPower <= trampPowerTable[i]) {
-                *pIndex = i;
+                *pIndex = i + 1;
                 break;
             }
         }
@@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = {
 
 #endif
 
+bool trampInit()
+{
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP);
+
+    if (portConfig) {
+        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS);
+    }
+
+    if (!trampSerialPort) {
+        return false;
+    }
+
+#if defined(VTX_COMMON)
+    vtxTramp.vTable = &trampVTable;
+    vtxCommonRegisterDevice(&vtxTramp);
+#endif
+    
+    return true;
+}
+
 #endif // VTX_TRAMP