diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 7e617b03ac..e43505d26c 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs) vtxDevType_e vtxCommonGetDeviceType(void) { - if (!vtxDevice) + if (!vtxDevice || !vtxDevice->vTable->getDeviceType) return VTXDEV_UNKNOWN; - return vtxDevice->devtype; + return vtxDevice->vTable->getDeviceType(); } // band and chan are 1 origin @@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan) if (!vtxDevice) return; + if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan)) + return; + if (vtxDevice->vTable->setBandChan) vtxDevice->vTable->setBandChan(band, chan); } @@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index) if (!vtxDevice) return; + if (index > vtxDevice->numPower) + return; + if (vtxDevice->vTable->setPowerByIndex) vtxDevice->vTable->setPowerByIndex(index); } diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index c7c521c666..e0899cc85a 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -18,9 +18,12 @@ /* Created by jflyper */ typedef enum { - VTXDEV_UNKNOWN = 0, + VTXDEV_UNSUPPORTED = 0, // reserved for MSP + // 1 reserved + // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, + VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; struct vtxVTable_s; @@ -28,8 +31,6 @@ struct vtxVTable_s; typedef struct vtxDevice_s { const struct vtxVTable_s *vTable; - vtxDevType_e devtype; // 3.1 only; eventually goes away - uint8_t numBand; uint8_t numChan; uint8_t numPower; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 749a0d40c3..34da6e97fa 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = { }; static const char * const trampPowerNames[] = { - "25 ", "100", "200", "400", "600" + "---", "25 ", "100", "200", "400", "600" }; #endif #if defined(VTX_COMMON) -static vtxVTable_t trampVTable; // Forward static vtxDevice_t vtxTramp = { - .vTable = &trampVTable, + .vTable = NULL, .numBand = 5, .numChan = 8, .numPower = sizeof(trampPowerTable), @@ -309,26 +308,6 @@ void trampQueryS(void) trampQuery('s'); } -bool trampInit() -{ - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); - - if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); - } - - if (!trampSerialPort) { - return false; - } - -#if defined(VTX_COMMON) - vtxTramp.vTable = &trampVTable; - vtxCommonRegisterDevice(&vtxTramp); -#endif - - return true; -} - void vtxTrampProcess(uint32_t currentTimeUs) { static uint32_t lastQueryTimeUs = 0; @@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL } static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; -static uint8_t trampCmsPower = 0; +static uint8_t trampCmsPower = 1; -static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL }; +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL }; static void trampCmsUpdateFreqRef(void) { @@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(self); trampSetBandChan(trampCmsBand, trampCmsChan); - trampSetRFPower(trampPowerTable[trampCmsPower]); + trampSetRFPower(trampPowerTable[trampCmsPower-1]); // If it fails, the user should retry later trampCommitChanges(); @@ -559,7 +538,7 @@ static void trampCmsInitSettings() if (trampCurConfigPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { if (trampCurConfigPower <= trampPowerTable[i]) { - trampCmsPower = i; + trampCmsPower = i + 1; break; } } @@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan) void vtxTrampSetPowerByIndex(uint8_t index) { if (index) { - trampSetRFPower(trampPowerTable[index]); + trampSetRFPower(trampPowerTable[index - 1]); trampCommitChanges(); } } @@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) if (trampCurConfigPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { if (trampCurConfigPower <= trampPowerTable[i]) { - *pIndex = i; + *pIndex = i + 1; break; } } @@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = { #endif +bool trampInit() +{ + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); + + if (portConfig) { + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + } + + if (!trampSerialPort) { + return false; + } + +#if defined(VTX_COMMON) + vtxTramp.vTable = &trampVTable; + vtxCommonRegisterDevice(&vtxTramp); +#endif + + return true; +} + #endif // VTX_TRAMP