Fix issue entering pitmode.

Make as many variables as possible private.
Re-work settings application into state machine.
Remove legacy interfaces.
Update OSD interface following implementation rework.
Review updates.

src/main/build/debug.c

@@ -97,4 +97,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "GYRO_SAMPLE",
     "RX_TIMING",
     "D_LPF",
+    "VTX_TRAMP",
 };

src/main/build/debug.h

@@ -113,6 +113,7 @@ typedef enum {
     DEBUG_GYRO_SAMPLE,
     DEBUG_RX_TIMING,
     DEBUG_D_LPF,
+    DEBUG_VTX_TRAMP,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cms/cms_menu_vtx_tramp.c

@@ -45,6 +45,9 @@ char trampCmsStatusString[31] = "- -- ---- ----";
 //                               m bc ffff tppp
 //                               01234567890123
 
+static int16_t trampCmsTemp;
+static OSD_INT16_t trampCmsEntTemp = { &trampCmsTemp, -100, 300, 0 };
+
 void trampCmsUpdateStatusString(void)
 {
     vtxDevice_t *vtxDevice = vtxCommonDevice();
@@ -67,16 +70,23 @@ void trampCmsUpdateStatusString(void)
     }
     trampCmsStatusString[4] = ' ';
 
-    if (trampCurFreq)
+    uint16_t freq;
-        tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq);
+    if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) {
-    else
         tfp_sprintf(&trampCmsStatusString[5], "----");
+    } else {
+        tfp_sprintf(&trampCmsStatusString[5], "%4d", freq);
+    }
 
-    if (trampPower) {
+    uint16_t actualPower = vtxTrampGetCurrentActualPower();
-        tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower);
+    uint8_t powerIndex;
+    uint16_t powerValue;
+    if (actualPower > 0 && vtxCommonGetPowerIndex(vtxDevice, &powerIndex) && vtxCommonLookupPowerValue(vtxDevice, powerIndex, &powerValue)) {
+        tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (actualPower == powerValue) ? ' ' : '*', actualPower);
     } else {
         tfp_sprintf(&trampCmsStatusString[9], " ----");
     }
+
+    trampCmsTemp = vtxTrampGetCurrentTemp();
 }
 
 uint8_t trampCmsPitMode = 0;
@@ -105,11 +115,12 @@ static const void *trampCmsConfigBand(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    if (trampCmsBand == 0)
+    if (trampCmsBand == 0) {
         // Bounce back
         trampCmsBand = 1;
-    else
+    } else {
         trampCmsUpdateFreqRef();
+    }
 
     return NULL;
 }
@@ -119,11 +130,12 @@ static const void *trampCmsConfigChan(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    if (trampCmsChan == 0)
+    if (trampCmsChan == 0) {
         // Bounce back
         trampCmsChan = 1;
-    else
+    } else {
         trampCmsUpdateFreqRef();
+    }
 
     return NULL;
 }
@@ -133,14 +145,17 @@ static const void *trampCmsConfigPower(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    if (trampCmsPower == 0)
+    if (trampCmsPower == 0) {
         // Bounce back
         trampCmsPower = 1;
+    }
 
     return NULL;
 }
 
-static OSD_INT16_t trampCmsEntTemp = { &trampTemperature, -100, 300, 0 };
+#define TRAMP_PIT_STATUS_NA (0)
+#define TRAMP_PIT_STATUS_OFF (1)
+#define TRAMP_PIT_STATUS_ON (2)
 
 static const char * const trampCmsPitModeNames[] = {
     "---", "OFF", "ON "
@@ -153,11 +168,12 @@ static const void *trampCmsSetPitMode(displayPort_t *pDisp, const void *self)
     UNUSED(pDisp);
     UNUSED(self);
 
-    if (trampCmsPitMode == 0) {
+    if (trampCmsPitMode == TRAMP_PIT_STATUS_NA) {
         // Bouce back
-        trampCmsPitMode = 1;
+        trampCmsPitMode = TRAMP_PIT_STATUS_OFF;
     } else {
-        trampSetPitMode(trampCmsPitMode - 1);
+        vtxCommonSetPitMode(vtxCommonDevice(),
+                            (trampCmsPitMode == TRAMP_PIT_STATUS_OFF) ? 0 : 1);
     }
 
     return NULL;
@@ -172,9 +188,6 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
     vtxCommonSetBandAndChannel(device, trampCmsBand, trampCmsChan);
     vtxCommonSetPowerByIndex(device, trampCmsPower);
 
-    // If it fails, the user should retry later
-    trampCommitChanges();
-
     // update'vtx_' settings
     vtxSettingsConfigMutable()->band = trampCmsBand;
     vtxSettingsConfigMutable()->channel = trampCmsChan;
@@ -189,6 +202,7 @@ static const void *trampCmsCommence(displayPort_t *pDisp, const void *self)
 static bool trampCmsInitSettings(void)
 {
     vtxDevice_t *device = vtxCommonDevice();
+    unsigned vtxStatus;
 
     if (!device) {
         return false;
@@ -197,13 +211,15 @@ static bool trampCmsInitSettings(void)
     vtxCommonGetBandAndChannel(device, &trampCmsBand, &trampCmsChan);
 
     trampCmsUpdateFreqRef();
-    trampCmsPitMode = trampPitMode + 1;
+    if (vtxCommonGetStatus(device, &vtxStatus)) {
+        trampCmsPitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? TRAMP_PIT_STATUS_ON : TRAMP_PIT_STATUS_OFF;
+    } else {
+        trampCmsPitMode = TRAMP_PIT_STATUS_NA;
+    }
 
-    if (trampConfiguredPower > 0) {
     if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &trampCmsPower)) {
         trampCmsPower = 1;
     }
-    }
 
     trampCmsEntBand.val = &trampCmsBand;
     trampCmsEntBand.max = vtxTableBandCount;

src/main/io/vtx_tramp.c

@@ -44,6 +44,18 @@
 #include "io/vtx_control.h"
 #include "io/vtx.h"
 
+// Maximum number of requests sent to try a config change
+// Some VTX fail to respond to every request (like Matek FCHUB-VTX) so
+// we sometimes need multiple retries to get the VTX to respond.
+#define TRAMP_MAX_RETRIES (20)
+
+// Race lock - settings can't be changed
+#define TRAMP_CONTROL_RACE_LOCK (0x01)
+
+// Define periods between requests
+#define TRAMP_MIN_REQUEST_PERIOD_US (200 * 1000) // 200ms
+#define TRAMP_STATUS_REQUEST_PERIOD_US (1000 * 1000) // 1s
+
 #if (defined(USE_CMS) || defined(USE_VTX_COMMON)) && !defined(USE_VTX_TABLE)
 const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
     25, 100, 200, 400, 600
@@ -61,52 +73,72 @@ static vtxDevice_t vtxTramp = {
 };
 #endif
 
+// Device serial port instance
 static serialPort_t *trampSerialPort = NULL;
 
+// Serial transmit and receive buffers
 static uint8_t trampReqBuffer[16];
 static uint8_t trampRespBuffer[16];
 
+// Module state machine
 typedef enum {
-    TRAMP_STATUS_BAD_DEVICE = -1,
+    // Offline - device hasn't responded yet
     TRAMP_STATUS_OFFLINE = 0,
-    TRAMP_STATUS_ONLINE,
+    // Init - fetching current settings from device
-    TRAMP_STATUS_SET_FREQ_PW,
+    TRAMP_STATUS_INIT,
-    TRAMP_STATUS_CHECK_FREQ_PW
+    // Online - device is ready and being monitored - freq/power/pitmode
+    TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT,
+    // Online - device is ready and being monitored - temperature
+    TRAMP_STATUS_ONLINE_MONITOR_TEMP,
+    // Online - device is ready and config has just been updated
+    TRAMP_STATUS_ONLINE_CONFIG
 } trampStatus_e;
 
-trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
+static trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
 
+// Device limits, read from device during init
 static uint32_t trampRFFreqMin = 0;
 static uint32_t trampRFFreqMax = 0;
 static uint32_t trampRFPowerMax;
 
-uint32_t trampCurFreq = 0;
+// Device status, read from device periodically
-static uint32_t trampLastFreq = 0;
+static uint32_t trampCurFreq = 0;
-uint16_t trampPower = 0;       // Actual transmitting power
+static uint16_t trampCurConfPower = 0; // Configured power
-uint16_t trampConfiguredPower = 0; // Configured transmitting power
+static uint16_t trampCurActPower = 0; // Actual power
-static uint16_t trampLastPower = 0;
+static uint8_t trampCurPitMode = 0;
-int16_t trampTemperature = 0;
+static int16_t trampCurTemp = 0;
-uint8_t trampPitMode = 0;
+static uint8_t trampCurControlMode = 0;
-static uint8_t trampControlMode = 0;
 
-#define TRAMP_CONTROL_RACE_LOCK 0x01
+// Device configuration, desired state of device
+static uint32_t trampConfFreq = 0;
+static uint16_t trampConfPower = 0;
+static uint8_t trampConfPitMode = 0;
 
-// Maximum number of requests sent to try a config change
+// Last device configuration, last desired state of device - used to reset
-// Some VTX fail to respond to every request (like Matek FCHUB-VTX) so
+// retry count
-// we sometimes need multiple retries to get the VTX to respond.
+static uint32_t trampLastConfFreq = 0;
-#define TRAMP_MAX_RETRIES 20
+static uint16_t trampLastConfPower = 0;
+static uint8_t trampLastConfPitMode = 0;
 
-uint32_t trampConfFreq = 0;
+// Retry count
-uint8_t  trampFreqRetries = 0;
+static uint8_t trampRetryCount = TRAMP_MAX_RETRIES;
 
-uint16_t trampConfPower = 0;
+// Receive state machine
-uint8_t  trampPowerRetries = 0;
+typedef enum {
+    S_WAIT_LEN = 0,   // Waiting for a packet len
+    S_WAIT_CODE,      // Waiting for a response code
+    S_DATA,           // Waiting for rest of the packet.
+} trampReceiveState_e;
 
-static void trampWriteBuf(uint8_t *buf)
+static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
-{
-    serialWriteBuf(trampSerialPort, buf, 16);
-}
 
+// Receive buffer index
+static int trampReceivePos = 0;
+
+// Last action time
+static timeUs_t trampLastTimeUs = 0;
+
+// Calculate tramp protocol checksum of provided buffer
 static uint8_t trampChecksum(uint8_t *trampBuf)
 {
     uint8_t cksum = 0;
@@ -118,16 +150,13 @@ static uint8_t trampChecksum(uint8_t *trampBuf)
     return cksum;
 }
 
-static bool trampVtxControlEnabled(void)
+// Check if race lock is enabled
-{
-    return !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE);
-}
-
 static bool trampVtxRaceLockEnabled(void)
 {
-    return trampControlMode & TRAMP_CONTROL_RACE_LOCK;
+    return trampCurControlMode & TRAMP_CONTROL_RACE_LOCK;
 }
 
+// Send tramp protocol frame to device
 static void trampSendU16(uint8_t cmd, uint16_t param)
 {
     if (!trampSerialPort) {
@@ -135,82 +164,25 @@ static void trampSendU16(uint8_t cmd, uint16_t param)
     }
 
     memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
-    trampReqBuffer[0] = 15;
+    trampReqBuffer[0] = 0x0F;
     trampReqBuffer[1] = cmd;
     trampReqBuffer[2] = param & 0xff;
     trampReqBuffer[3] = (param >> 8) & 0xff;
     trampReqBuffer[14] = trampChecksum(trampReqBuffer);
-    trampWriteBuf(trampReqBuffer);
+    serialWriteBuf(trampSerialPort, trampReqBuffer, 16);
 }
 
+// Send command to device
 static void trampSendCommand(uint8_t cmd, uint16_t param)
 {
-    if (trampVtxControlEnabled()) {
+    // Is VTX control enabled?
+    if (!IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
+        // Yes, send command
         trampSendU16(cmd, param);
     }
 }
 
-static bool trampValidateFreq(uint16_t freq)
+// Process response and return code if valid else 0
-{
-    if (trampRFFreqMin != 0 && trampRFFreqMax != 0) {
-        return (freq >= trampRFFreqMin && freq <= trampRFFreqMax);
-    } else {
-        return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
-    }
-}
-
-static void trampDevSetFreq(uint16_t freq)
-{
-    trampConfFreq = freq;
-    if (trampConfFreq != trampCurFreq) {
-        trampFreqRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSetFreq(uint16_t freq)
-{
-    trampDevSetFreq(freq);
-}
-
-void trampSendFreq(uint16_t freq)
-{
-    if (!trampVtxRaceLockEnabled()) {
-        trampSendCommand('F', freq);
-    }
-}
-
-void trampSetRFPower(uint16_t level)
-{
-    trampConfPower = level;
-    if (trampConfPower != trampConfiguredPower) {
-        trampPowerRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSendRFPower(uint16_t level)
-{
-    if (!trampVtxRaceLockEnabled()) {
-        trampSendCommand('P', level);
-    }
-}
-
-// return false if error
-bool trampCommitChanges(void)
-{
-    if (trampStatus != TRAMP_STATUS_ONLINE) {
-        return false;
-    }
-
-    trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-    return true;
-}
-
-void trampSetPitMode(uint8_t onoff)
-{
-    trampSendCommand('I', onoff ? 0 : 1);
-}
-
-// returns completed response code
 static char trampHandleResponse(void)
 {
     const uint8_t respCode = trampRespBuffer[1];
@@ -219,95 +191,66 @@ static char trampHandleResponse(void)
     case 'r':
         {
             const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            // Check we're not reading the request (indicated by freq zero)
             if (min_freq != 0) {
+                // Got response, update device limits
                 trampRFFreqMin = min_freq;
                 trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
                 trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
                 return 'r';
             }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
             break;
-
+        }
     case 'v':
         {
             const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
+            // Check we're not reading the request (indicated by freq zero)
             if (freq != 0) {
+                // Got response, update device status
                 trampCurFreq = freq;
-                if (trampLastFreq != trampCurFreq) {
+                trampCurConfPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                    trampFreqRetries = TRAMP_MAX_RETRIES;
+                trampCurControlMode = trampRespBuffer[6]; // Currently only used for race lock
-                }
+                trampCurPitMode = trampRespBuffer[7];
-                trampLastFreq = trampCurFreq;
+                trampCurActPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
-
-                trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                if (trampConfiguredPower != trampLastPower) {
-                    trampPowerRetries = TRAMP_MAX_RETRIES;
-                }
-                trampLastPower = trampConfiguredPower;
-
-                trampControlMode = trampRespBuffer[6]; // Currently only used for race lock
-
-                trampPitMode = trampRespBuffer[7];
-                trampPower = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
 
+                // Init config with current status if not set
                 if (trampConfFreq == 0) {
                     trampConfFreq  = trampCurFreq;
                 }
                 if (trampConfPower == 0) {
-                    trampConfPower = trampConfiguredPower;
+                    trampConfPower = trampCurConfPower;
                 }
-
                 return 'v';
             }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
             break;
-
+        }
     case 's':
         {
             const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
+            // Check we're not reading the request (indicated by temp zero)
             if (temp != 0) {
-                trampTemperature = temp;
+                // Got response, update device status
+                trampCurTemp = temp;
                 return 's';
             }
-        }
             break;
         }
+    }
 
+    // Likely reading a request, return zero to indicate not accepted
     return 0;
 }
 
-typedef enum {
+// Reset receiver state machine
-    S_WAIT_LEN = 0,   // Waiting for a packet len
-    S_WAIT_CODE,      // Waiting for a response code
-    S_DATA,           // Waiting for rest of the packet.
-} trampReceiveState_e;
-
-static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
-static int trampReceivePos = 0;
-
 static void trampResetReceiver(void)
 {
     trampReceiveState = S_WAIT_LEN;
     trampReceivePos = 0;
 }
 
-static bool trampIsValidResponseCode(uint8_t code)
-{
-    if (code == 'r' || code == 'v' || code == 's') {
-        return true;
-    } else {
-        return false;
-    }
-}
-
 // returns completed response code or 0
-static char trampReceive(uint32_t currentTimeUs)
+static char trampReceive()
 {
-    UNUSED(currentTimeUs);
-
     if (!trampSerialPort) {
         return 0;
     }
@@ -318,183 +261,214 @@ static char trampReceive(uint32_t currentTimeUs)
 
         switch (trampReceiveState) {
         case S_WAIT_LEN:
+            {
                 if (c == 0x0F) {
+                    // Found header byte, advance to wait for code
                     trampReceiveState = S_WAIT_CODE;
                 } else {
-                trampReceivePos = 0;
+                    // Unexpected header, reset state machine
-            }
-            break;
-
-        case S_WAIT_CODE:
-            if (trampIsValidResponseCode(c)) {
-                trampReceiveState = S_DATA;
-            } else {
                     trampResetReceiver();
                 }
                 break;
-
+            }
+        case S_WAIT_CODE:
+            {
+                if (c == 'r' || c == 'v' || c == 's') {
+                    // Code is for response is one we're interested in, advance to data
+                    trampReceiveState = S_DATA;
+                } else {
+                    // Unexpected code, reset state machine
+                    trampResetReceiver();
+                }
+                break;
+            }
         case S_DATA:
+            {
                 if (trampReceivePos == 16) {
+                    // Buffer is full, calculate checksum
                     uint8_t cksum = trampChecksum(trampRespBuffer);
 
+                    // Reset state machine ready for next response
                     trampResetReceiver();
 
                     if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
-                    return trampHandleResponse();
+                        // Checksum is correct, process response
+                        char r = trampHandleResponse();
+
+                        // Check response valid else keep on reading
+                        if (r != 0) {
+                            return r;
+                        }
                     }
                 }
                 break;
-
+            }
         default:
+            {
+                // Invalid state, reset state machine
                 trampResetReceiver();
                 break;
             }
         }
+    }
 
     return 0;
 }
 
-void trampQuery(uint8_t cmd)
+static void trampQuery(uint8_t cmd)
 {
+    // Reset receive buffer and issue command
     trampResetReceiver();
     trampSendU16(cmd, 0);
 }
 
-void trampQueryR(void)
-{
-    trampQuery('r');
-}
-
-void trampQueryV(void)
-{
-    trampQuery('v');
-}
-
-void trampQueryS(void)
-{
-    trampQuery('s');
-}
-
 static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 {
     UNUSED(vtxDevice);
 
-    static timeUs_t lastQueryTimeUs = 0;
+    // Read response from device
-    static bool initSettingsDoneFlag = false;
+    const char replyCode = trampReceive();
-
-#ifdef TRAMP_DEBUG
-    static uint16_t debugFreqReqCounter = 0;
-    static uint16_t debugPowReqCounter = 0;
-#endif
-
-    if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
-        return;
-    }
-
-    const char replyCode = trampReceive(currentTimeUs);
-
-#ifdef TRAMP_DEBUG
-    debug[0] = trampStatus;
-#endif
-
-    switch (replyCode) {
-    case 'r':
-        if (trampStatus <= TRAMP_STATUS_OFFLINE) {
-            trampStatus = TRAMP_STATUS_ONLINE;
-
-            // once device is ready enter vtx settings
-            if (!initSettingsDoneFlag) {
-                initSettingsDoneFlag = true;
-                // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
-            }
-        }
-        break;
-
-    case 'v':
-        trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-
-        break;
-    }
 
+    // Act on state
     switch(trampStatus) {
-
     case TRAMP_STATUS_OFFLINE:
-    case TRAMP_STATUS_ONLINE:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
-
-            if (trampStatus == TRAMP_STATUS_OFFLINE) {
-                trampQueryR();
-            } else {
-                static unsigned int cnt = 0;
-                if (((cnt++) & 1) == 0) {
-                    trampQueryV();
-                } else {
-                    trampQueryS();
-                }
-            }
-
-            lastQueryTimeUs = currentTimeUs;
-        }
-        break;
-
-    case TRAMP_STATUS_SET_FREQ_PW:
         {
-            bool done = true;
+            // Offline, check for response
-            if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) {
+            if (replyCode == 'r') {
-                trampSendFreq(trampConfFreq);
+                // Device replied to reset? request, enter init
-                trampFreqRetries--;
+                trampStatus = TRAMP_STATUS_INIT;
-#ifdef TRAMP_DEBUG
+            } else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
-                debugFreqReqCounter++;
+                // Min request period exceeded, issue another reset?
-#endif
+                trampQuery('r');
-                done = false;
+
-            } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) {
+                // Update last time
-                trampSendRFPower(trampConfPower);
+                trampLastTimeUs = currentTimeUs;
-                trampPowerRetries--;
+            }
-#ifdef TRAMP_DEBUG
+            break;
-                debugPowReqCounter++;
+        }
-#endif
+    case TRAMP_STATUS_INIT:
-                done = false;
+        {
+            // Initializing, check for response
+            if (replyCode == 'v') {
+                // Device replied to freq / power / pit query, enter online
+                trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
+            } else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
+                // Min request period exceeded, issue another query
+                trampQuery('v');
+
+                // Update last time
+                trampLastTimeUs = currentTimeUs;
+            }
+            break;
+        }
+    case TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT:
+        {
+            uint8_t configUpdateRequired = 0;
+
+            // Note after config a status update request is made, a new status
+            // request is made, this request is handled above and should prevent
+            // subsiquent config updates if the config is now correct
+            if (trampRetryCount > 0 && (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US)) {
+                // Config retries remain and min request period exceeded, check freq
+                if (!trampVtxRaceLockEnabled() && (trampConfFreq != trampCurFreq)) {
+                    // Freq can be and needs to be updated, issue request
+                    trampSendCommand('F', trampConfFreq);
+
+                    // Set flag
+                    configUpdateRequired = 1;
+                } else if (!trampVtxRaceLockEnabled() && (trampConfPower != trampCurConfPower)) {
+                    // Power can be and needs to be updated, issue request
+                    trampSendCommand('P', trampConfPower);
+
+                    // Set flag
+                    configUpdateRequired = 1;
+                } else if (trampConfPitMode != trampCurPitMode) {
+                    // Pit mode needs to be updated, issue request
+                    trampSendCommand('I', trampConfPitMode);
+
+                    // Set flag
+                    configUpdateRequired = 1;
                 }
 
-            if (!done) {
+                if (configUpdateRequired) {
-                trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
+                    // Update required, decrement retry count
+                    trampRetryCount--;
 
-                // delay next status query by 300ms
+                    // Update last time
-                lastQueryTimeUs = currentTimeUs + 300 * 1000;
+                    trampLastTimeUs = currentTimeUs;
+
+                    // Advance state
+                    trampStatus = TRAMP_STATUS_ONLINE_CONFIG;
                 } else {
-                // everything has been done, let's return to original state
+                    // No update required, reset retry count
-                trampStatus = TRAMP_STATUS_ONLINE;
+                    trampRetryCount = TRAMP_MAX_RETRIES;
-                // reset configuration value in case it failed (no more retries)
+                }
-                trampConfFreq  = trampCurFreq;
+            }
-                trampConfPower = trampConfiguredPower;
+
-                trampFreqRetries = trampPowerRetries = 0;
+            /* Was a config update made? */
+            if (!configUpdateRequired) {
+                /* No, look to continue monitoring */
+                if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_STATUS_REQUEST_PERIOD_US) {
+                    // Request period exceeded, issue freq/power/pit query
+                    trampQuery('v');
+
+                    // Update last time
+                    trampLastTimeUs = currentTimeUs;
+                } else if (replyCode == 'v') {
+                    // Got reply, issue temp query
+                    trampQuery('s');
+
+                    // Wait for reply
+                    trampStatus = TRAMP_STATUS_ONLINE_MONITOR_TEMP;
+
+                    // Update last time
+                    trampLastTimeUs = currentTimeUs;
                 }
             }
             break;
-
+        }
-    case TRAMP_STATUS_CHECK_FREQ_PW:
+    case TRAMP_STATUS_ONLINE_MONITOR_TEMP:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
+        {
-            trampQueryV();
+            // Check request time
-            lastQueryTimeUs = currentTimeUs;
+            if (replyCode == 's') {
+                // Got reply, return to request freq/power/pit
+                trampStatus = TRAMP_STATUS_ONLINE_MONITOR_TEMP;
+            } else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
+                // Timed out after min request period, return to request freq/power/pit query
+                trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
             }
             break;
+        }
+    case TRAMP_STATUS_ONLINE_CONFIG:
+        {
+            // Param should now be set, check time
+            if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) {
+                // Min request period exceeded, re-query
+                trampQuery('v');
 
+                // Advance state
+                trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
+
+                // Update last time
+                trampLastTimeUs = currentTimeUs;
+            }
+            break;
+        }
     default:
+        {
+            // Invalid state, reset
+            trampStatus = TRAMP_STATUS_OFFLINE;
             break;
         }
+    }
 
-#ifdef TRAMP_DEBUG
+    DEBUG_SET(DEBUG_VTX_TRAMP, 0, trampStatus);
-    debug[1] = debugFreqReqCounter;
-    debug[2] = debugPowReqCounter;
-    debug[3] = 0;
-#endif
 
 #ifdef USE_CMS
     trampCmsUpdateStatusString();
 #endif
 }
 
-
 #ifdef USE_VTX_COMMON
 
 // Interface to common VTX API
@@ -507,7 +481,7 @@ static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
 
 static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice != NULL && trampStatus > TRAMP_STATUS_OFFLINE;
+    return vtxDevice != NULL && trampStatus >= TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
 }
 
 static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
@@ -520,25 +494,54 @@ static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint
 
 static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    uint16_t powerValue = 0;
+    uint16_t powerValue;
+
+    // Lookup power level value
     if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
-        trampSetRFPower(powerValue);
+        // Value found, apply
-        trampCommitChanges();
+        trampConfPower = powerValue;
+        if (trampConfPower != trampLastConfPower) {
+            // Requested power changed, reset retry count
+            trampRetryCount = TRAMP_MAX_RETRIES;
+            trampLastConfPower = trampConfPower;
+        }
     }
 }
 
 static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
     UNUSED(vtxDevice);
-    trampSetPitMode(onoff);
+
+    trampConfPitMode = onoff ? 0 : 1; // note inverted values
+    if (trampConfPitMode != trampLastConfPitMode) {
+        // Requested pitmode changed, reset retry count
+        trampRetryCount = TRAMP_MAX_RETRIES;
+        trampLastConfPitMode = trampConfPitMode;
+    }
 }
 
 static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 {
     UNUSED(vtxDevice);
-    if (trampValidateFreq(freq)) {
+
-        trampSetFreq(freq);
+    uint8_t freqValid;
-        trampCommitChanges();
+
+    // Check frequency valid
+    if (trampRFFreqMin != 0 && trampRFFreqMax != 0) {
+        freqValid = (freq >= trampRFFreqMin && freq <= trampRFFreqMax);
+    } else {
+        freqValid = (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
+    }
+
+    // Is frequency valid?
+    if (freqValid) {
+        // Yes, set freq
+        trampConfFreq = freq;
+        if (trampConfFreq != trampLastConfFreq) {
+            // Requested freq changed, reset retry count
+            trampRetryCount = TRAMP_MAX_RETRIES;
+            trampLastConfFreq = trampConfFreq;
+        }
     }
 }
 
@@ -560,16 +563,24 @@ static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
         return false;
     }
 
-    if (trampConfiguredPower > 0) {
+    // Special case, power not set
-        for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
+    if (trampConfPower == 0) {
-            if (trampConfiguredPower <= vtxTablePowerValues[i]) {
+        *pIndex = 0;
-                *pIndex = i + 1;
+        return true;
-                break;
     }
+
+    // Lookup value in table
+    for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
+        // Find value that matches current configured power level
+        if (trampConfPower == vtxTablePowerValues[i]) {
+            // Value found, return index
+            *pIndex = i + 1;
+            return true;
         }
     }
 
-    return true;
+    // Value not found in table
+    return false;
 }
 
 static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
@@ -588,8 +599,14 @@ static bool vtxTrampGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
         return false;
     }
 
-    *status = (trampPitMode ? VTX_STATUS_PIT_MODE : 0)
+    // Mirror configued pit mode state rather than use current pitmode as we
-        | ((trampControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
+    // should, otherwise the logic in vtxProcessPitMode may not get us to the
+    // correct state if pitmode is toggled quickly
+    *status = (trampConfPitMode ? 0 : VTX_STATUS_PIT_MODE);
+
+    // Check VTX is not locked
+    *status |= ((trampCurControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0);
+
     return true;
 }
 
@@ -650,7 +667,6 @@ bool vtxTrampInit(void)
     for (int i = 0; i < VTX_TRAMP_POWER_COUNT; i++) {
         vtxTablePowerValues[i] = trampPowerTable[i];
     }
-
 #endif
 
 #endif
@@ -660,4 +676,14 @@ bool vtxTrampInit(void)
     return true;
 }
 
+uint16_t vtxTrampGetCurrentActualPower()
+{
+    return trampCurActPower;
+}
+
+uint16_t vtxTrampGetCurrentTemp()
+{
+    return trampCurTemp;
+}
+
 #endif // VTX_TRAMP

src/main/io/vtx_tramp.h

@@ -27,16 +27,7 @@
 #define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000             //min freq in MHz
 #define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999             //max freq in MHz
 
-extern uint8_t trampBand;
-extern uint8_t trampChannel;
-extern uint16_t trampPower;       // Actual transmitting power
-extern uint8_t trampPitMode;
-extern uint32_t trampCurFreq;
-extern uint16_t trampConfiguredPower; // Configured transmitting power
-extern int16_t trampTemperature;
-
 bool vtxTrampInit(void);
-bool trampCommitChanges(void);
+
-void trampSetPitMode(uint8_t onoff);
+uint16_t vtxTrampGetCurrentActualPower();
-void trampSetBandAndChannel(uint8_t band, uint8_t channel);
+uint16_t vtxTrampGetCurrentTemp();
-void trampSetRFPower(uint16_t level);