diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 7a16b490f..0e93eb9e1 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -97,4 +97,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "GYRO_SAMPLE", "RX_TIMING", "D_LPF", + "VTX_TRAMP", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index c7ca477bf..b24061937 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index 16782c90f..0b0b0beff 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/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) - tfp_sprintf(&trampCmsStatusString[5], "%4d", trampCurFreq); - else + uint16_t freq; + if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) { tfp_sprintf(&trampCmsStatusString[5], "----"); + } else { + tfp_sprintf(&trampCmsStatusString[5], "%4d", freq); + } - if (trampPower) { - tfp_sprintf(&trampCmsStatusString[9], " %c%3d", (trampPower == trampConfiguredPower) ? ' ' : '*', trampPower); + uint16_t actualPower = vtxTrampGetCurrentActualPower(); + 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,12 +211,14 @@ 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; - } + if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &trampCmsPower)) { + trampCmsPower = 1; } trampCmsEntBand.val = &trampCmsBand; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 27b072714..48c1d6847 100644 --- a/src/main/io/vtx_tramp.c +++ b/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, - TRAMP_STATUS_SET_FREQ_PW, - TRAMP_STATUS_CHECK_FREQ_PW + // Init - fetching current settings from device + TRAMP_STATUS_INIT, + // 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; -static uint32_t trampLastFreq = 0; -uint16_t trampPower = 0; // Actual transmitting power -uint16_t trampConfiguredPower = 0; // Configured transmitting power -static uint16_t trampLastPower = 0; -int16_t trampTemperature = 0; -uint8_t trampPitMode = 0; -static uint8_t trampControlMode = 0; +// Device status, read from device periodically +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 int16_t trampCurTemp = 0; +static uint8_t trampCurControlMode = 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 -// 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 +// 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; -uint32_t trampConfFreq = 0; -uint8_t trampFreqRetries = 0; +// Retry count +static uint8_t trampRetryCount = TRAMP_MAX_RETRIES; -uint16_t trampConfPower = 0; -uint8_t trampPowerRetries = 0; +// Receive state machine +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) -{ - serialWriteBuf(trampSerialPort, buf, 16); -} +static trampReceiveState_e trampReceiveState = S_WAIT_LEN; +// 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) -{ - return !IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE); -} - +// Check if race lock is enabled 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) -{ - 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 +// Process response and return code if valid else 0 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; } - 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) { - trampFreqRetries = TRAMP_MAX_RETRIES; - } - trampLastFreq = trampCurFreq; - - 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); + trampCurConfPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); + trampCurControlMode = trampRespBuffer[6]; // Currently only used for race lock + trampCurPitMode = trampRespBuffer[7]; + trampCurActPower = 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; } - 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; } - break; } + // Likely reading a request, return zero to indicate not accepted return 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 trampReceiveState_e trampReceiveState = S_WAIT_LEN; -static int trampReceivePos = 0; - +// Reset receiver state machine 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) { - trampReceiveState = S_WAIT_CODE; - } else { - trampReceivePos = 0; - } - break; - - case S_WAIT_CODE: - if (trampIsValidResponseCode(c)) { - trampReceiveState = S_DATA; - } else { - trampResetReceiver(); - } - break; - - case S_DATA: - if (trampReceivePos == 16) { - uint8_t cksum = trampChecksum(trampRespBuffer); - - trampResetReceiver(); - - if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) { - return trampHandleResponse(); + { + if (c == 0x0F) { + // Found header byte, advance to wait for code + trampReceiveState = S_WAIT_CODE; + } else { + // Unexpected header, reset state machine + trampResetReceiver(); } + break; } - 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)) { + // Checksum is correct, process response + char r = trampHandleResponse(); + + // Check response valid else keep on reading + if (r != 0) { + return r; + } + } + } + break; + } default: - trampResetReceiver(); - break; + { + // 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; - static bool initSettingsDoneFlag = false; - -#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; - } - - switch (trampStatus) { + // Read response from device + const char replyCode = trampReceive(); + // Act on state + switch(trampStatus) { case TRAMP_STATUS_OFFLINE: - case TRAMP_STATUS_ONLINE: - if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s + { + // Offline, check for response + if (replyCode == 'r') { + // Device replied to reset? request, enter init + trampStatus = TRAMP_STATUS_INIT; + } else if (cmp32(currentTimeUs, trampLastTimeUs) >= TRAMP_MIN_REQUEST_PERIOD_US) { + // Min request period exceeded, issue another reset? + trampQuery('r'); - if (trampStatus == TRAMP_STATUS_OFFLINE) { - trampQueryR(); - } else { - static unsigned int cnt = 0; - if (((cnt++) & 1) == 0) { - trampQueryV(); + // Update last time + trampLastTimeUs = currentTimeUs; + } + break; + } + case TRAMP_STATUS_INIT: + { + // 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 (configUpdateRequired) { + // Update required, decrement retry count + trampRetryCount--; + + // Update last time + trampLastTimeUs = currentTimeUs; + + // Advance state + trampStatus = TRAMP_STATUS_ONLINE_CONFIG; } else { - trampQueryS(); + // No update required, reset retry count + trampRetryCount = TRAMP_MAX_RETRIES; } } - lastQueryTimeUs = currentTimeUs; - } - break; + /* 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'); - case TRAMP_STATUS_SET_FREQ_PW: + // 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_ONLINE_MONITOR_TEMP: { - bool done = true; - if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampCurFreq)) { - trampSendFreq(trampConfFreq); - trampFreqRetries--; -#ifdef TRAMP_DEBUG - debugFreqReqCounter++; -#endif - done = false; - } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) { - trampSendRFPower(trampConfPower); - trampPowerRetries--; -#ifdef TRAMP_DEBUG - debugPowReqCounter++; -#endif - done = false; + // Check request time + 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'); - if (!done) { - trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; + // Advance state + trampStatus = TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT; - // delay next status query by 300ms - lastQueryTimeUs = currentTimeUs + 300 * 1000; - } else { - // everything has been done, let's return to original state - trampStatus = TRAMP_STATUS_ONLINE; - // reset configuration value in case it failed (no more retries) - trampConfFreq = trampCurFreq; - trampConfPower = trampConfiguredPower; - trampFreqRetries = trampPowerRetries = 0; + // Update last time + trampLastTimeUs = currentTimeUs; } + break; } - break; - - case TRAMP_STATUS_CHECK_FREQ_PW: - if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) { - trampQueryV(); - lastQueryTimeUs = currentTimeUs; - } - break; - default: - break; + { + // Invalid state, reset + trampStatus = TRAMP_STATUS_OFFLINE; + break; + } } -#ifdef TRAMP_DEBUG - debug[1] = debugFreqReqCounter; - debug[2] = debugPowReqCounter; - debug[3] = 0; -#endif + DEBUG_SET(DEBUG_VTX_TRAMP, 0, trampStatus); #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); - trampCommitChanges(); + // Value found, apply + 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); - trampCommitChanges(); + + uint8_t freqValid; + + // 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) { - for (uint8_t i = 0; i < vtxTablePowerLevels; i++) { - if (trampConfiguredPower <= vtxTablePowerValues[i]) { - *pIndex = i + 1; - break; - } + // Special case, power not set + if (trampConfPower == 0) { + *pIndex = 0; + return true; + } + + // 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) - | ((trampControlMode & TRAMP_CONTROL_RACE_LOCK) ? VTX_STATUS_LOCKED : 0); + // 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 ? 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 diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 27adc8d91..a56410490 100644 --- a/src/main/io/vtx_tramp.h +++ b/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); -void trampSetBandAndChannel(uint8_t band, uint8_t channel); -void trampSetRFPower(uint16_t level); + +uint16_t vtxTrampGetCurrentActualPower(); +uint16_t vtxTrampGetCurrentTemp();