diff --git a/docs/Cli.md b/docs/Cli.md index a1a1d2f2a7..1ab8c2bbd5 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -283,6 +283,7 @@ Click on a variable to jump to the relevant documentation page. | `vtx_channel` | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | 1 | 8 | 1 | Master | UINT8 | | `vtx_freq` | Set the VTX frequency using raw MHz. This parameter is ignored unless `vtx_band` is 0. | 0 | 5900 | 5740 | Master | UINT16 | | `vtx_halfduplex` | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | OFF | ON | ON | Master | UINT8 | +| `vtx_tramp_pitmode_inverted` | Invert tramp pitmode status returned from device. May be required for some VTX's which don't implement the tramp protocol correctly (Matek are a known example). | OFF | ON | ON | Master | UINT8 | | `vtx_low_power_disarm` | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | | | OFF | Master | UINT8 | | `vtx_pit_mode_freq` | Frequency to use (in MHz) when the VTX is in pit mode. | 0 | 5900 | 0 | Master | UINT16 | | `vtx_power` | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | 0 | 3 | 1 | Master | UINT8 | diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 0c0383bd91..8cc20481f4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1464,6 +1464,9 @@ const clivalue_t valueTable[] = { #if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) { "vtx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) }, #endif +#ifdef USE_VTX_TRAMP + { "vtx_tramp_pitmode_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, trampPitModeInverted) }, +#endif // PG_VTX_IO #ifdef USE_VTX_RTC6705 diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 05c3d4f083..93b3654e94 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -185,11 +185,16 @@ static bool vtxProcessPower(vtxDevice_t *vtxDevice) static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) { + static bool prevPmSwitchState = false; + unsigned vtxStatus; - if (vtxCommonGetStatus(vtxDevice, &vtxStatus)) { + if (!ARMING_FLAG(ARMED) && vtxCommonGetStatus(vtxDevice, &vtxStatus)) { bool currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE); - if (currPmSwitchState) { - if (!ARMING_FLAG(ARMED)) { + + if (currPmSwitchState != prevPmSwitchState) { + prevPmSwitchState = currPmSwitchState; + + if (currPmSwitchState) { #if defined(VTX_SETTINGS_FREQCMD) if (vtxSettingsConfig()->pitModeFreq) { return false; @@ -200,12 +205,12 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) return true; } - } - } else { - if (vtxStatus & VTX_STATUS_PIT_MODE) { - vtxCommonSetPitMode(vtxDevice, false); + } else { + if (vtxStatus & VTX_STATUS_PIT_MODE) { + vtxCommonSetPitMode(vtxDevice, false); - return true; + return true; + } } } } diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index d83727fbc5..4adaf0b76f 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -41,6 +41,7 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; uint8_t halfDuplex; + uint8_t trampPitModeInverted; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index fd15b80fcf..ee67026680 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -918,8 +918,6 @@ static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - static bool lastOnOff = false; - if (!vtxSAIsReady(vtxDevice) || saDevice.version < 2) { return; } @@ -929,12 +927,6 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) return; } - // Only issue pit mode commands on status change - if (lastOnOff == onoff) { - return; - } - lastOnOff = onoff; - if (saDevice.version >= 3 && !saDevice.willBootIntoPitMode) { if (onoff) { // enable pitmode using SET_POWER command with 0 dbm. diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 561bd56b30..32e17b201b 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -94,6 +94,7 @@ typedef enum { TRAMP_STATUS_ONLINE_CONFIG } trampStatus_e; +// Module state static trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; // Device limits, read from device during init @@ -105,20 +106,20 @@ static uint32_t trampRFPowerMax; static uint32_t trampCurFreq = 0; static uint16_t trampCurConfPower = 0; // Configured power static uint16_t trampCurActPower = 0; // Actual power -static uint8_t trampCurPitMode = 0; +static uint8_t trampCurPitMode = 0; // Expect to startup out of pitmode static int16_t trampCurTemp = 0; static uint8_t trampCurControlMode = 0; // Device configuration, desired state of device static uint32_t trampConfFreq = 0; static uint16_t trampConfPower = 0; -static uint8_t trampConfPitMode = 0; +static uint8_t trampConfPitMode = 0; // Initially configured out of pitmode // Last device configuration, last desired state of device - used to reset // retry count static uint32_t trampLastConfFreq = 0; static uint16_t trampLastConfPower = 0; -static uint8_t trampLastConfPitMode = 0; +static uint8_t trampLastConfPitMode = 0; // Mirror trampConfPitMode // Retry count static uint8_t trampRetryCount = TRAMP_MAX_RETRIES; @@ -210,7 +211,13 @@ static char trampHandleResponse(void) trampCurFreq = freq; trampCurConfPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampCurControlMode = trampRespBuffer[6]; // Currently only used for race lock - trampCurPitMode = trampRespBuffer[7]; + if (vtxConfig()->trampPitModeInverted) { + // Invert pitmode (for devices which don't implement protocol correctly) + trampCurPitMode = !trampRespBuffer[7]; + } else { + // Use official pitmode value + trampCurPitMode = trampRespBuffer[7]; + } trampCurActPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8); // Init config with current status if not set @@ -325,7 +332,7 @@ static void trampQuery(uint8_t cmd) static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { UNUSED(vtxDevice); - uint8_t configUpdateRequired = 0; + bool configUpdateRequired = false; // Read response from device const char replyCode = trampReceive(); @@ -374,19 +381,19 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) trampSendCommand('F', trampConfFreq); // Set flag - configUpdateRequired = 1; + configUpdateRequired = true; } else if (!trampVtxRaceLockEnabled() && (trampConfPower != trampCurConfPower)) { // Power can be and needs to be updated, issue request trampSendCommand('P', trampConfPower); // Set flag - configUpdateRequired = 1; + configUpdateRequired = true; } else if (trampConfPitMode != trampCurPitMode) { // Pit mode needs to be updated, issue request - trampSendCommand('I', trampConfPitMode); + trampSendCommand('I', trampConfPitMode ? 0 : 1); // Set flag - configUpdateRequired = 1; + configUpdateRequired = true; } if (configUpdateRequired) { @@ -464,7 +471,14 @@ static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) DEBUG_SET(DEBUG_VTX_TRAMP, 0, trampStatus); DEBUG_SET(DEBUG_VTX_TRAMP, 1, replyCode); - DEBUG_SET(DEBUG_VTX_TRAMP, 2, configUpdateRequired); + DEBUG_SET(DEBUG_VTX_TRAMP, 2, ((trampConfPitMode << 14) & 0xC000) | + ((trampCurPitMode << 12) & 0x3000) | + ((trampConfPower << 8) & 0x0F00) | + ((trampCurConfPower << 4) & 0x00F0) | + ((trampConfFreq != trampCurFreq) ? 0x0008 : 0x0000) | + ((trampConfPower != trampCurConfPower) ? 0x0004 : 0x0000) | + ((trampConfPitMode != trampCurPitMode) ? 0x0002 : 0x0000) | + (configUpdateRequired ? 0x0001 : 0x0000)); DEBUG_SET(DEBUG_VTX_TRAMP, 3, trampRetryCount); #ifdef USE_CMS @@ -515,7 +529,7 @@ static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { UNUSED(vtxDevice); - trampConfPitMode = onoff ? 0 : 1; // note inverted values + trampConfPitMode = onoff; if (trampConfPitMode != trampLastConfPitMode) { // Requested pitmode changed, reset retry count trampRetryCount = TRAMP_MAX_RETRIES; @@ -602,8 +616,10 @@ static bool vtxTrampGetStatus(const vtxDevice_t *vtxDevice, unsigned *status) return false; } - // Return pitmode from latest query response - *status = (trampCurPitMode ? 0 : VTX_STATUS_PIT_MODE); + // Mirror configued pit mode state rather than use current pitmode as we + // should, otherwise the logic in vtxProcessPitMode may not get us to the + // correct state if pitmode is toggled quickly + *status = (trampConfPitMode ? VTX_STATUS_PIT_MODE : 0); // Check VTX is not locked *status |= ((trampCurControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);