Merge pull request #6680 from mikeller/add_vtx_control_disable_mode

Added 'VTX CONTROL DISABLE' mode.

src/main/fc/rc_modes.h

@@ -75,6 +75,7 @@ typedef enum {
     BOXUSER4,
     BOXPIDAUDIO,
     BOXACROTRAINER,
+    BOXVTXCONTROLDISABLE,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/interface/msp_box.c

@@ -94,6 +94,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXPARALYZE, "PARALYZE", 45 },
     { BOXGPSRESCUE, "GPS RESCUE", 46 },
     { BOXACROTRAINER, "ACRO TRAINER", 47 },
+    { BOXVTXCONTROLDISABLE, "DISABLE VTX CONTROL", 48},
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -260,6 +261,7 @@ void initActiveBoxIds(void)
 
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
     BME(BOXVTXPITMODE);
+    BME(BOXVTXCONTROLDISABLE);
 #endif
 
     BME(BOXPARALYZE);

src/main/io/vtx.c

@@ -104,7 +104,7 @@ STATIC_UNIT_TESTED vtxSettingsConfig_t vtxGetSettings(void)
     };
 
 #if defined(VTX_SETTINGS_FREQCMD)
-    if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) {
+    if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && settings.pitModeFreq) {
         settings.band = 0;
         settings.freq = settings.pitModeFreq;
         settings.power = VTX_SETTINGS_DEFAULT_POWER;
@@ -168,13 +168,11 @@ static bool vtxProcessPower(vtxDevice_t *vtxDevice)
 
 static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
 {
-    uint8_t pitOnOff;
-
-    bool        currPmSwitchState;
     static bool prevPmSwitchState = false;
 
+    uint8_t pitOnOff;
     if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
-        currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
+        bool currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
 
         if (currPmSwitchState != prevPmSwitchState) {
             prevPmSwitchState = currPmSwitchState;
@@ -185,20 +183,21 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
                     return false;
                 }
 #endif
-                if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
+                if (!pitOnOff) {
-                    if (!pitOnOff) {
+                    vtxCommonSetPitMode(vtxDevice, true);
-                        vtxCommonSetPitMode(vtxDevice, true);
+
-                        return true;
+                    return true;
-                    }
                 }
             } else {
                 if (pitOnOff) {
                     vtxCommonSetPitMode(vtxDevice, false);
+
                     return true;
                 }
             }
         }
     }
+
     return false;
 }
 

src/main/io/vtx_smartaudio.c

@@ -268,6 +268,12 @@ static void saProcessResponse(uint8_t *buf, int len)
 {
     uint8_t resp = buf[0];
 
+    if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
+        sa_outstanding = SA_CMD_NONE;
+
+        return;
+    }
+
     if (resp == sa_outstanding) {
         sa_outstanding = SA_CMD_NONE;
     } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) {
@@ -355,7 +361,7 @@ static void saProcessResponse(uint8_t *buf, int len)
 // Datalink
 //
 
-static void saReceiveFramer(uint8_t c)
+static void saReceiveFrame(uint8_t c)
 {
 
     static enum saFramerState_e {
@@ -435,21 +441,26 @@ static void saReceiveFramer(uint8_t c)
 
 static void saSendFrame(uint8_t *buf, int len)
 {
-    switch (smartAudioSerialPort->identifier) {
+    if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
-        case SERIAL_PORT_SOFTSERIAL1:
+        switch (smartAudioSerialPort->identifier) {
-        case SERIAL_PORT_SOFTSERIAL2:
+            case SERIAL_PORT_SOFTSERIAL1:
-            break;
+            case SERIAL_PORT_SOFTSERIAL2:
-        default:
+                break;
-            serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
+            default:
-            break;
+                serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit
-    }
+                break;
+        }
 
-    for (int i = 0 ; i < len ; i++) {
+        for (int i = 0 ; i < len ; i++) {
-        serialWrite(smartAudioSerialPort, buf[i]);
+            serialWrite(smartAudioSerialPort, buf[i]);
+        }
+
+        saStat.pktsent++;
+    } else {
+        sa_outstanding = SA_CMD_NONE;
     }
 
     sa_lastTransmissionMs = millis();
-    saStat.pktsent++;
 }
 
 /*
@@ -525,7 +536,7 @@ static void saQueueCmd(uint8_t *buf, int len)
 static void saSendQueue(void)
 {
     if (saQueueEmpty()) {
-         return;
+        return;
     }
 
     saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len);
@@ -703,7 +714,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 
     while (serialRxBytesWaiting(smartAudioSerialPort) > 0) {
         uint8_t c = serialRead(smartAudioSerialPort);
-        saReceiveFramer((uint16_t)c);
+        saReceiveFrame((uint16_t)c);
     }
 
     // Re-evaluate baudrate after each frame reception

src/main/io/vtx_tramp.c

@@ -120,9 +120,9 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
     return cksum;
 }
 
-void trampCmdU16(uint8_t cmd, uint16_t param)
+static void trampCmdU16(uint8_t cmd, uint16_t param)
 {
-    if (!trampSerialPort) {
+    if (!trampSerialPort || IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
         return;
     }
 
@@ -217,7 +217,7 @@ void trampSetPitMode(uint8_t onoff)
 }
 
 // returns completed response code
-char trampHandleResponse(void)
+static char trampHandleResponse(void)
 {
     const uint8_t respCode = trampRespBuffer[1];
 
@@ -333,7 +333,7 @@ static char trampReceive(uint32_t currentTimeUs)
 
                 trampResetReceiver();
 
-                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
+                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0) && !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
                     return trampHandleResponse();
                 }
             }