diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 7a1a8fa175..da3c7ea7af 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -75,6 +75,7 @@ typedef enum { BOXUSER4, BOXPIDAUDIO, BOXACROTRAINER, + BOXVTXCONTROLDISABLE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index fa03a31110..00fe0144e8 100644 --- a/src/main/interface/msp_box.c +++ b/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); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 700df2a406..36ffc2aa82 100644 --- a/src/main/io/vtx.c +++ b/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) { - vtxCommonSetPitMode(vtxDevice, true); - return true; - } + if (!pitOnOff) { + vtxCommonSetPitMode(vtxDevice, true); + + return true; } } else { if (pitOnOff) { vtxCommonSetPitMode(vtxDevice, false); + return true; } } } } + return false; } diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index df424e45b2..07a59e1a5a 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/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) { - case SERIAL_PORT_SOFTSERIAL1: - case SERIAL_PORT_SOFTSERIAL2: - break; - default: - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - break; - } + if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) { + switch (smartAudioSerialPort->identifier) { + case SERIAL_PORT_SOFTSERIAL1: + case SERIAL_PORT_SOFTSERIAL2: + break; + default: + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + break; + } - for (int i = 0 ; i < len ; i++) { - serialWrite(smartAudioSerialPort, buf[i]); + for (int i = 0 ; i < len ; 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 diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 2c6086b063..591bdd4b89 100644 --- a/src/main/io/vtx_tramp.c +++ b/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(); } }