diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index d04c80d34..baee0aed7 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -30,114 +30,86 @@ #include "drivers/vtx_common.h" -vtxDevice_t *vtxDevice = NULL; +static vtxDevice_t *vtxDevice = NULL; void vtxCommonInit(void) { } -void vtxCommonRegisterDevice(vtxDevice_t *pDevice) +void vtxCommonSetDevice(vtxDevice_t *pDevice) { vtxDevice = pDevice; } -bool vtxCommonDeviceRegistered(void) +vtxDevice_t *vtxCommonDevice(void) { return vtxDevice; } -vtxDevType_e vtxCommonGetDeviceType(void) +vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice) { - if (!vtxDevice || !vtxDevice->vTable->getDeviceType) { + if (!vtxDevice) { return VTXDEV_UNKNOWN; } - - return vtxDevice->vTable->getDeviceType(); + return vtxDevice->vTable->getDeviceType(vtxDevice); } -void vtxCommonProcess(timeUs_t currentTimeUs) { - if (vtxDevice && vtxDevice->vTable->process) { - vtxDevice->vTable->process(currentTimeUs); +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + if (vtxDevice) { + vtxDevice->vTable->process(vtxDevice, currentTimeUs); } } // band and channel are 1 origin -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - if (vtxDevice && (band <= vtxDevice->capability.bandCount) && (channel <= vtxDevice->capability.channelCount)) { - if (vtxDevice->vTable->setBandAndChannel) { - vtxDevice->vTable->setBandAndChannel(band, channel); - } + if (band <= vtxDevice->capability.bandCount && channel <= vtxDevice->capability.channelCount) { + vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel); } } // index is zero origin, zero = power off completely -void vtxCommonSetPowerByIndex(uint8_t index) +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - if (vtxDevice && (index < vtxDevice->capability.powerCount)) { - if (vtxDevice->vTable->setPowerByIndex) { - vtxDevice->vTable->setPowerByIndex(index); - } + if (index < vtxDevice->capability.powerCount) { + vtxDevice->vTable->setPowerByIndex(vtxDevice, index); } } // on = 1, off = 0 -void vtxCommonSetPitMode(uint8_t onoff) +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { - if (vtxDevice && vtxDevice->vTable->setPitMode) { - vtxDevice->vTable->setPitMode(onoff); - } + vtxDevice->vTable->setPitMode(vtxDevice, onOff); } -void vtxCommonSetFrequency(uint16_t freq) +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency) { - if (vtxDevice && vtxDevice->vTable->setFrequency) { - vtxDevice->vTable->setFrequency(freq); - } + vtxDevice->vTable->setFrequency(vtxDevice, frequency); } -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (vtxDevice && vtxDevice->vTable->getBandAndChannel) { - return vtxDevice->vTable->getBandAndChannel(pBand, pChannel); - } else { - return false; - } + return vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel); } -bool vtxCommonGetPowerIndex(uint8_t *pIndex) +bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (vtxDevice && vtxDevice->vTable->getPowerIndex) { - return vtxDevice->vTable->getPowerIndex(pIndex); - } else { - return false; - } + return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex); } -bool vtxCommonGetPitMode(uint8_t *pOnOff) +bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (vtxDevice && vtxDevice->vTable->getPitMode) { - return vtxDevice->vTable->getPitMode(pOnOff); - } else { - return false; - } + return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff); } -bool vtxCommonGetFrequency(uint16_t *pFreq) +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency) { - if (vtxDevice && vtxDevice->vTable->getFrequency) { - return vtxDevice->vTable->getFrequency(pFreq); - } else { - return false; - } + return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency); } -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability) +bool vtxCommonGetDeviceCapability(const vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability) { - if (!vtxDevice) { - return false; - } - memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); return true; } diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 60acbca72..95573e05e 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -110,30 +110,12 @@ typedef struct vtxDeviceCapability_s { uint8_t bandCount; uint8_t channelCount; uint8_t powerCount; + uint8_t filler; } vtxDeviceCapability_t; -// {set,get}BandAndChannel: band and channel are 1 origin -// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent -// {set,get}PitMode: 0 = OFF, 1 = ON - -typedef struct vtxVTable_s { - void (*process)(timeUs_t currentTimeUs); - vtxDevType_e (*getDeviceType)(void); - bool (*isReady)(void); - - void (*setBandAndChannel)(uint8_t band, uint8_t channel); - void (*setPowerByIndex)(uint8_t level); - void (*setPitMode)(uint8_t onoff); - void (*setFrequency)(uint16_t freq); - - bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel); - bool (*getPowerIndex)(uint8_t *pIndex); - bool (*getPitMode)(uint8_t *pOnOff); - bool (*getFrequency)(uint16_t *pFreq); -} vtxVTable_t; - +struct vtxVTable_s; typedef struct vtxDevice_s { - const vtxVTable_t * const vTable; + const struct vtxVTable_s * const vTable; vtxDeviceCapability_t capability; @@ -142,6 +124,7 @@ typedef struct vtxDevice_s { char **channelNames; // char *channelNames[channelCount] char **powerNames; // char *powerNames[powerCount] + uint16_t frequency; uint8_t band; // Band = 1, 1-based uint8_t channel; // CH1 = 1, 1-based uint8_t powerIndex; // Lowest/Off = 0 @@ -149,24 +132,44 @@ typedef struct vtxDevice_s { } vtxDevice_t; +// {set,get}BandAndChannel: band and channel are 1 origin +// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent +// {set,get}PitMode: 0 = OFF, 1 = ON + +typedef struct vtxVTable_s { + void (*process)(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); + vtxDevType_e (*getDeviceType)(const vtxDevice_t *vtxDevice); + bool (*isReady)(const vtxDevice_t *vtxDevice); + + void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); + void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level); + void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff); + void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq); + + bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); + bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex); + bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); + bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); +} vtxVTable_t; + // 3.1.0 // PIT mode is defined as LOWEST POSSIBLE RF POWER. // - It can be a dedicated mode, or lowest RF power possible. // - It is *NOT* RF on/off control ? void vtxCommonInit(void); -void vtxCommonRegisterDevice(vtxDevice_t *pDevice); -bool vtxCommonDeviceRegistered(void); +void vtxCommonSetDevice(vtxDevice_t *vtxDevice); +vtxDevice_t *vtxCommonDevice(void); // VTable functions -void vtxCommonProcess(timeUs_t currentTimeUs); -uint8_t vtxCommonGetDeviceType(void); -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel); -void vtxCommonSetPowerByIndex(uint8_t level); -void vtxCommonSetPitMode(uint8_t onoff); -void vtxCommonSetFrequency(uint16_t freq); -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel); -bool vtxCommonGetPowerIndex(uint8_t *pIndex); -bool vtxCommonGetPitMode(uint8_t *pOnOff); -bool vtxCommonGetFrequency(uint16_t *pFreq); -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability); +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); +uint8_t vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice); +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t level); +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t freq); +bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); +bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex); +bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); +bool vtxCommonGetDeviceCapability(const vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index dbddefc8d..92ecf2472 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -704,7 +704,7 @@ void init(void) #ifdef USE_VTX_RTC6705 #ifdef VTX_RTC6705_OPTIONAL - if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured. + if (!vtxCommonDevice()) // external VTX takes precedence when configured. #endif { vtxRTC6705Init(); diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 1977408eb..65b6dc584 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1213,11 +1213,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) #if defined(USE_VTX_COMMON) case MSP_VTX_CONFIG: { - uint8_t deviceType = vtxCommonGetDeviceType(); - if (deviceType != VTXDEV_UNKNOWN) { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { uint8_t pitmode=0; - vtxCommonGetPitMode(&pitmode); - sbufWriteU8(dst, deviceType); + vtxCommonGetPitMode(vtxDevice, &pitmode); + sbufWriteU8(dst, vtxCommonGetDeviceType(vtxDevice)); sbufWriteU8(dst, vtxSettingsConfig()->band); sbufWriteU8(dst, vtxSettingsConfig()->channel); sbufWriteU8(dst, vtxSettingsConfig()->power); @@ -1730,8 +1730,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_VTX_COMMON case MSP_SET_VTX_CONFIG: { - if (vtxCommonDeviceRegistered()) { - if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { uint16_t newFrequency = sbufReadU16(src); if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel const uint8_t newBand = (newFrequency / 8) + 1; @@ -1749,9 +1750,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) // Delegate pitmode to vtx directly const uint8_t newPitmode = sbufReadU8(src); uint8_t currentPitmode = 0; - vtxCommonGetPitMode(¤tPitmode); + vtxCommonGetPitMode(vtxDevice, ¤tPitmode); if (currentPitmode != newPitmode) { - vtxCommonSetPitMode(newPitmode); + vtxCommonSetPitMode(vtxDevice, newPitmode); } } } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index a501525a1..473c9bcac 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -588,7 +588,8 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) static uint16_t lastCheck = 0; static bool blink = false; - if (!vtxCommonDeviceRegistered()) { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice) { return; } @@ -597,9 +598,9 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer) if (updateNow) { // keep counter running, so it stays in sync with vtx - vtxCommonGetBandAndChannel(&band, &channel); - vtxCommonGetPowerIndex(&power); - vtxCommonGetPitMode(&pit); + vtxCommonGetBandAndChannel(vtxDevice, &band, &channel); + vtxCommonGetPowerIndex(vtxDevice, &power); + vtxCommonGetPitMode(vtxDevice, &pit); frequency = vtx58frequencyTable[band - 1][channel - 1]; //subtracting 1 from band and channel so that correct frequency is returned. //might not be correct for tramp but should fix smart audio. diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 976568987..d35e6e446 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -549,8 +549,9 @@ static bool osdDrawSingleElement(uint8_t item) const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band]; const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel]; uint8_t vtxPower = vtxSettingsConfig()->power; - if (vtxSettingsConfig()->lowPowerDisarm) { - vtxCommonGetPowerIndex(&vtxPower); + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) { + vtxCommonGetPowerIndex(vtxDevice, &vtxPower); } tfp_sprintf(buff, "%c:%s:%2d", vtxBandLetter, vtxChannelName, vtxPower); break; diff --git a/src/main/io/spektrum_vtx_control.c b/src/main/io/spektrum_vtx_control.c index e0fc84f68..6ef339a1c 100644 --- a/src/main/io/spektrum_vtx_control.c +++ b/src/main/io/spektrum_vtx_control.c @@ -101,7 +101,8 @@ uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower) { uint8_t devicePower = 0; - switch (vtxCommonGetDeviceType()) { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + switch (vtxCommonGetDeviceType(vtxDevice)) { #ifdef USE_VTX_RTC6705 case VTXDEV_RTC6705: devicePower = vtxRTC6705Pi[sPower]; @@ -174,8 +175,8 @@ void spektrumVtxControl(void) }; vtxSettingsConfig_t newSettings = prevSettings; - if (vtxCommonDeviceRegistered()) { - + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { #ifdef USE_VTX_COMMON_FREQ_API uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel]; if (prevSettings.freq != freq) { @@ -183,34 +184,31 @@ void spektrumVtxControl(void) newSettings.channel = vtx.channel; newSettings.freq = freq; } - #else // Convert to the internal Common Band index - uint8_t band = spek2commonBand[vtx.band]; - uint8_t channel = vtx.channel +1; // 0 based to 1 based + const uint8_t band = spek2commonBand[vtx.band]; + const uint8_t channel = vtx.channel +1; // 0 based to 1 based if ((prevSettings.band != band) || (prevSettings.channel != channel)) { newSettings.band = band; newSettings.channel = channel; newSettings.freq = vtx58_Bandchan2Freq(band, channel); } #endif - - // Seems to be no unified internal VTX API std for power levels/indexes, VTX device brand specific. - uint8_t power = convertSpektrumVtxPowerIndex(vtx.power); + // Seems to be no unified internal VTX API standard for power levels/indexes, VTX device brand specific. + const uint8_t power = convertSpektrumVtxPowerIndex(vtx.power); if (prevSettings.power != power) { newSettings.power = power; } - // Everyone seems to agree on what PIT ON/OFF means uint8_t currentPitMode = 0; - vtxCommonGetPitMode(¤tPitMode); + vtxCommonGetPitMode(vtxDevice, ¤tPitMode); if (currentPitMode != vtx.pitMode) { - vtxCommonSetPitMode(vtx.pitMode); + vtxCommonSetPitMode(vtxDevice, vtx.pitMode); } } - if(memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) { - vtxSettingsConfigMutable()->band = newSettings.band; + if (memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) { + vtxSettingsConfigMutable()->band = newSettings.band; vtxSettingsConfigMutable()->channel = newSettings.channel; vtxSettingsConfigMutable()->power = newSettings.power; vtxSettingsConfigMutable()->freq = newSettings.freq; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index ea69855b5..a9342c9ce 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -22,17 +22,14 @@ #if defined(USE_VTX_COMMON) -#include "common/time.h" #include "common/maths.h" - -#include "pg/pg.h" -#include "pg/pg_ids.h" +#include "common/time.h" #include "drivers/vtx_common.h" #include "fc/config.h" -#include "fc/runtime_config.h" #include "fc/rc_modes.h" +#include "fc/runtime_config.h" #include "io/vtx.h" #include "io/vtx_string.h" @@ -40,6 +37,9 @@ #include "interface/cli.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" + PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0); @@ -87,7 +87,8 @@ void vtxInit(void) } } -static vtxSettingsConfig_t vtxGetSettings(void) { +static vtxSettingsConfig_t vtxGetSettings(void) +{ vtxSettingsConfig_t settings = { .band = vtxSettingsConfig()->band, .channel = vtxSettingsConfig()->channel, @@ -112,14 +113,15 @@ static vtxSettingsConfig_t vtxGetSettings(void) { return settings; } -static bool vtxProcessBandAndChannel(void) { +static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice) +{ if(!ARMING_FLAG(ARMED)) { uint8_t vtxBand; uint8_t vtxChan; - if (vtxCommonGetBandAndChannel(&vtxBand, &vtxChan)) { + if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) { const vtxSettingsConfig_t settings = vtxGetSettings(); if (vtxBand != settings.band || vtxChan != settings.channel) { - vtxCommonSetBandAndChannel(settings.band, settings.channel); + vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel); return true; } } @@ -128,13 +130,14 @@ static bool vtxProcessBandAndChannel(void) { } #if defined(VTX_SETTINGS_FREQCMD) -static bool vtxProcessFrequency(void) { +static bool vtxProcessFrequency(vtxDevice_t *vtxDevice) +{ if(!ARMING_FLAG(ARMED)) { uint16_t vtxFreq; - if (vtxCommonGetFrequency(&vtxFreq)) { + if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) { const vtxSettingsConfig_t settings = vtxGetSettings(); if (vtxFreq != settings.freq) { - vtxCommonSetFrequency(settings.freq); + vtxCommonSetFrequency(vtxDevice, settings.freq); return true; } } @@ -143,21 +146,23 @@ static bool vtxProcessFrequency(void) { } #endif -static bool vtxProcessPower(void) { +static bool vtxProcessPower(vtxDevice_t *vtxDevice) +{ uint8_t vtxPower; - if (vtxCommonGetPowerIndex(&vtxPower)) { + if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { const vtxSettingsConfig_t settings = vtxGetSettings(); if (vtxPower != settings.power) { - vtxCommonSetPowerByIndex(settings.power); + vtxCommonSetPowerByIndex(vtxDevice, settings.power); return true; } } return false; } -static bool vtxProcessPitMode(void) { +static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) +{ uint8_t pitOnOff; - if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(&pitOnOff)) { + if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) { if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE)) { #if defined(VTX_SETTINGS_FREQCMD) if (vtxSettingsConfig()->pitModeFreq) { @@ -166,12 +171,12 @@ static bool vtxProcessPitMode(void) { #endif if (isModeActivationConditionPresent(BOXVTXPITMODE)) { if (!pitOnOff) { - vtxCommonSetPitMode(true); + vtxCommonSetPitMode(vtxDevice, true); return true; } } else { if (pitOnOff) { - vtxCommonSetPitMode(false); + vtxCommonSetPitMode(vtxDevice, false); return true; } } @@ -180,22 +185,22 @@ static bool vtxProcessPitMode(void) { return false; } -static bool vtxProcessStateUpdate(void) +static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice) { const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings(); vtxSettingsConfig_t vtxState = vtxSettingsState; if (vtxSettingsState.band) { - vtxCommonGetBandAndChannel(&vtxState.band, &vtxState.channel); + vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel); #if defined(VTX_SETTINGS_FREQCMD) } else { - vtxCommonGetFrequency(&vtxState.freq); + vtxCommonGetFrequency(vtxDevice, &vtxState.freq); #endif } - vtxCommonGetPowerIndex(&vtxState.power); + vtxCommonGetPowerIndex(vtxDevice, &vtxState.power); - return (bool) memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); + return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); } void vtxUpdate(timeUs_t currentTimeUs) @@ -206,36 +211,37 @@ void vtxUpdate(timeUs_t currentTimeUs) return; } - // Check input sources for config updates - vtxControlInputPoll(); + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + // Check input sources for config updates + vtxControlInputPoll(); - if (vtxCommonDeviceRegistered()) { bool vtxUpdatePending = false; switch (currentSchedule) { case VTX_PARAM_POWER: - vtxUpdatePending = vtxProcessPower(); + vtxUpdatePending = vtxProcessPower(vtxDevice); break; case VTX_PARAM_BANDCHAN: if (vtxGetSettings().band) { - vtxUpdatePending = vtxProcessBandAndChannel(); + vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice); #if defined(VTX_SETTINGS_FREQCMD) } else { - vtxUpdatePending = vtxProcessFrequency(); + vtxUpdatePending = vtxProcessFrequency(vtxDevice); #endif } break; case VTX_PARAM_PITMODE: - vtxUpdatePending = vtxProcessPitMode(); + vtxUpdatePending = vtxProcessPitMode(vtxDevice); break; case VTX_PARAM_CONFIRM: - vtxUpdatePending = vtxProcessStateUpdate(); + vtxUpdatePending = vtxProcessStateUpdate(vtxDevice); break; default: break; } currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { - vtxCommonProcess(currentTimeUs); + vtxCommonProcess(vtxDevice, currentTimeUs); } } } diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 8cb8375b5..53d38f3cd 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -26,8 +26,6 @@ #include "common/maths.h" #include "config/config_eeprom.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" #include "drivers/buttons.h" #include "drivers/light_led.h" @@ -38,11 +36,12 @@ #include "fc/runtime_config.h" #include "io/osd.h" -#include "io/vtx_control.h" -#include "io/vtx.h" - #include "io/spektrum_vtx_control.h" +#include "io/vtx.h" +#include "io/vtx_control.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); @@ -54,6 +53,7 @@ PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, static uint8_t locked = 0; + void vtxControlInit(void) { // NOTHING TO DO @@ -74,7 +74,7 @@ static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) locked = 1; } - if (!locked && vtxCommonDeviceRegistered()) { + if (!locked && vtxCommonDevice()) { vtxSettingsConfigMutable()->band += bandStep; vtxSettingsConfigMutable()->channel += channelStep; } @@ -106,7 +106,7 @@ void vtxUpdateActivatedChannel(void) locked = 1; } - if (!locked && vtxCommonDeviceRegistered()) { + if (!locked && vtxCommonDevice()) { static uint8_t lastIndex = -1; for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { @@ -126,11 +126,12 @@ void vtxUpdateActivatedChannel(void) void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) { - if (vtxCommonDeviceRegistered()) { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { uint8_t band = 0, channel = 0; vtxDeviceCapability_t capability; - bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability); + const bool haveAllNeededInfo = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -156,11 +157,11 @@ void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) void vtxCyclePower(const uint8_t powerStep) { - if (vtxCommonDeviceRegistered()) { + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { uint8_t power = 0; vtxDeviceCapability_t capability; - - bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability); + const bool haveAllNeededInfo = vtxCommonGetPowerIndex(vtxDevice, &power) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -196,15 +197,15 @@ void handleVTXControlButton(void) { #if defined(USE_VTX_RTC6705) && defined(BUTTON_A_PIN) bool buttonWasPressed = false; - uint32_t start = millis(); - uint32_t ledToggleAt = start; + const timeMs_t start = millis(); + timeMs_t ledToggleAt = start; bool ledEnabled = false; uint8_t flashesDone = 0; uint8_t actionCounter = 0; bool buttonHeld; while ((buttonHeld = buttonAPressed())) { - uint32_t end = millis(); + const timeMs_t end = millis(); int32_t diff = cmp32(end, start); if (diff > 25 && diff <= 1000) { diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index 6dfcbe9f7..3a26521ea 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -24,33 +24,19 @@ #if defined(USE_VTX_RTC6705) && defined(USE_VTX_CONTROL) -#include "build/build_config.h" -#include "build/debug.h" - -#include "cms/cms.h" -#include "cms/cms_types.h" - #include "common/maths.h" #include "common/utils.h" #include "config/feature.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" #include "drivers/max7456.h" -#include "drivers/system.h" #include "drivers/time.h" #include "drivers/vtx_rtc6705.h" -#include "drivers/vtx_common.h" -#include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "io/vtx.h" #include "io/vtx_rtc6705.h" #include "io/vtx_string.h" + #if defined(USE_CMS) || defined(USE_VTX_COMMON) const char * const rtc6705PowerNames[] = { "OFF", "MIN", "MAX" @@ -70,21 +56,16 @@ static vtxDevice_t vtxRTC6705 = { }; #endif -static void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel); +static void vtxRTC6705SetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); +static void vtxRTC6705SetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency); bool vtxRTC6705Init(void) { - vtxCommonRegisterDevice(&vtxRTC6705); + vtxCommonSetDevice(&vtxRTC6705); return true; } -void vtxRTC6705Configure(void) -{ - rtc6705SetRFPower(vtxRTC6705.powerIndex); - vtxRTC6705SetBandAndChannel(vtxRTC6705.band, vtxRTC6705.channel); -} - bool vtxRTC6705CanUpdate(void) { #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705) @@ -96,7 +77,13 @@ bool vtxRTC6705CanUpdate(void) } #ifdef RTC6705_POWER_PIN -static void vtxRTC6705EnableAndConfigure(void) +static void vtxRTC6705Configure(vtxDevice_t *vtxDevice) +{ + rtc6705SetRFPower(vtxDevice->powerIndex); + vtxRTC6705SetBandAndChannel(vtxDevice, vtxDevice->band, vtxDevice->channel); +} + +static void vtxRTC6705EnableAndConfigure(vtxDevice_t *vtxDevice) { while (!vtxRTC6705CanUpdate()); @@ -104,51 +91,53 @@ static void vtxRTC6705EnableAndConfigure(void) delay(VTX_RTC6705_BOOT_DELAY); - vtxRTC6705Configure(); + vtxRTC6705Configure(vtxDevice); } #endif -void vtxRTC6705Process(timeUs_t now) +static void vtxRTC6705Process(vtxDevice_t *vtxDevice, timeUs_t now) { + UNUSED(vtxDevice); UNUSED(now); } #ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxRTC6705GetDeviceType(void) +static vtxDevType_e vtxRTC6705GetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_RTC6705; } -bool vtxRTC6705IsReady(void) +static bool vtxRTC6705IsReady(const vtxDevice_t *vtxDevice) { - return true; + return vtxDevice != NULL; } -static void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxRTC6705SetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { while (!vtxRTC6705CanUpdate()); if (band >= 1 && band <= VTX_SETTINGS_BAND_COUNT && channel >= 1 && channel <= VTX_SETTINGS_CHANNEL_COUNT) { - if (vtxRTC6705.powerIndex > 0) { - vtxRTC6705.band = band; - vtxRTC6705.channel = channel; - rtc6705SetFrequency(vtx58frequencyTable[vtxRTC6705.band-1][ vtxRTC6705.channel-1]); + if (vtxDevice->powerIndex > 0) { + vtxDevice->band = band; + vtxDevice->channel = channel; + vtxRTC6705SetFrequency(vtxDevice, vtx58frequencyTable[band-1][channel-1]); } } } -void vtxRTC6705SetPowerByIndex(uint8_t index) +static void vtxRTC6705SetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { while (!vtxRTC6705CanUpdate()); #ifdef RTC6705_POWER_PIN if (index == 0) { // power device off - if (vtxRTC6705.powerIndex > 0) { + if (vtxDevice->powerIndex > 0) { // on, power it off - vtxRTC6705.powerIndex = index; + vtxDevice->powerIndex = index; rtc6705Disable(); return; } else { @@ -156,58 +145,62 @@ void vtxRTC6705SetPowerByIndex(uint8_t index) } } else { // change rf power and maybe turn the device on first - if (vtxRTC6705.powerIndex == 0) { + if (vtxDevice->powerIndex == 0) { // if it's powered down, power it up, wait and configure channel, band and power. - vtxRTC6705.powerIndex = index; - vtxRTC6705EnableAndConfigure(); + vtxDevice->powerIndex = index; + vtxRTC6705EnableAndConfigure(vtxDevice); return; } else { // if it's powered up, just set the rf power - vtxRTC6705.powerIndex = index; + vtxDevice->powerIndex = index; rtc6705SetRFPower(index); } } #else - vtxRTC6705.powerIndex = MAX(index, VTX_RTC6705_MIN_POWER); + vtxDevice->powerIndex = MAX(index, VTX_RTC6705_MIN_POWER); rtc6705SetRFPower(index); #endif } -void vtxRTC6705SetPitMode(uint8_t onoff) +static void vtxRTC6705SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { + UNUSED(vtxDevice); UNUSED(onoff); - return; } -void vtxRTC6705SetFreq(uint16_t freq) +static void vtxRTC6705SetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency) { - UNUSED(freq); - return; + if (frequency >= VTX_RTC6705_FREQ_MIN && frequency <= VTX_RTC6705_FREQ_MAX) { + frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX); + vtxDevice->frequency = frequency; + rtc6705SetFrequency(frequency); + } } -bool vtxRTC6705GetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static bool vtxRTC6705GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - *pBand = vtxRTC6705.band; - *pChannel = vtxRTC6705.channel; + *pBand = vtxDevice->band; + *pChannel = vtxDevice->channel; return true; } -bool vtxRTC6705GetPowerIndex(uint8_t *pIndex) +static bool vtxRTC6705GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - *pIndex = vtxRTC6705.powerIndex; + *pIndex = vtxDevice->powerIndex; return true; } -bool vtxRTC6705GetPitMode(uint8_t *pOnOff) +static bool vtxRTC6705GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { + UNUSED(vtxDevice); UNUSED(pOnOff); return false; } -bool vtxRTC6705GetFreq(uint16_t *pFreq) +static bool vtxRTC6705GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFrequency) { - UNUSED(pFreq); - return false; + *pFrequency = vtxDevice->frequency; + return true; } static vtxVTable_t rtc6705VTable = { @@ -217,7 +210,7 @@ static vtxVTable_t rtc6705VTable = { .setBandAndChannel = vtxRTC6705SetBandAndChannel, .setPowerByIndex = vtxRTC6705SetPowerByIndex, .setPitMode = vtxRTC6705SetPitMode, - .setFrequency = vtxRTC6705SetFreq, + .setFrequency = vtxRTC6705SetFrequency, .getBandAndChannel = vtxRTC6705GetBandAndChannel, .getPowerIndex = vtxRTC6705GetPowerIndex, .getPitMode = vtxRTC6705GetPitMode, diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h index 6ddba8bf3..e496201d9 100644 --- a/src/main/io/vtx_rtc6705.h +++ b/src/main/io/vtx_rtc6705.h @@ -24,6 +24,5 @@ extern const char * const rtc6705PowerNames[]; -void vtxRTC6705Configure(void); bool vtxRTC6705CanUpdate(void); bool vtxRTC6705Init(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index a79b6d369..3681b9fb8 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -26,35 +26,25 @@ #if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL) -#include "build/build_config.h" #include "build/debug.h" #include "cms/cms.h" -#include "cms/cms_types.h" #include "cms/cms_menu_vtx_smartaudio.h" #include "common/maths.h" #include "common/printf.h" #include "common/utils.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" - -#include "drivers/serial.h" #include "drivers/time.h" #include "drivers/vtx_common.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" - #include "io/serial.h" -#include "io/vtx_control.h" #include "io/vtx.h" +#include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_string.h" + // Timing parameters // Note that vtxSAProcess() is normally called at 200ms interval #define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost @@ -693,7 +683,7 @@ bool vtxSmartAudioInit(void) return false; } - vtxCommonRegisterDevice(&vtxSmartAudio); + vtxCommonSetDevice(&vtxSmartAudio); return true; } @@ -703,8 +693,9 @@ bool vtxSmartAudioInit(void) #define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply. #define SA_INITPHASE_DONE 3 -void vtxSAProcess(timeUs_t currentTimeUs) +static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { + UNUSED(vtxDevice); UNUSED(currentTimeUs); static char initPhase = SA_INITPHASE_START; @@ -777,25 +768,28 @@ void vtxSAProcess(timeUs_t currentTimeUs) #ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxSAGetDeviceType(void) +vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_SMARTAUDIO; } -bool vtxSAIsReady(void) +static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return !(saDevice.version == 0); + return vtxDevice!=NULL && !(saDevice.version == 0); } -void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { + UNUSED(vtxDevice); if (saValidateBandAndChannel(band, channel)) { saSetBandAndChannel(band - 1, channel - 1); } } -void vtxSASetPowerByIndex(uint8_t index) +static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { + UNUSED(vtxDevice); if (index == 0) { // SmartAudio doesn't support power off. return; @@ -804,9 +798,9 @@ void vtxSASetPowerByIndex(uint8_t index) saSetPowerByIndex(index - 1); } -void vtxSASetPitMode(uint8_t onoff) +static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) { + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return; } @@ -830,17 +824,18 @@ void vtxSASetPitMode(uint8_t onoff) return; } -void vtxSASetFreq(uint16_t freq) +static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq) { + UNUSED(vtxDevice); if (saValidateFreq(freq)) { saSetMode(0); //need to be in FREE mode to set freq saSetFreq(freq); } } -bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxSAIsReady()) { + if (!vtxSAIsReady(vtxDevice)) { return false; } @@ -851,9 +846,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) return true; } -bool vtxSAGetPowerIndex(uint8_t *pIndex) +static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxSAIsReady()) { + if (!vtxSAIsReady(vtxDevice)) { return false; } @@ -861,9 +856,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex) return true; } -bool vtxSAGetPitMode(uint8_t *pOnOff) +static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) { + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return false; } @@ -871,9 +866,9 @@ bool vtxSAGetPitMode(uint8_t *pOnOff) return true; } -bool vtxSAGetFreq(uint16_t *pFreq) +static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - if (!vtxSAIsReady()) { + if (!vtxSAIsReady(vtxDevice)) { return false; } diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index c759c7ef3..55efcd1e6 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -366,8 +366,10 @@ void trampQueryS(void) trampQuery('s'); } -void vtxTrampProcess(timeUs_t currentTimeUs) +static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { + UNUSED(vtxDevice); + static timeUs_t lastQueryTimeUs = 0; static bool initSettingsDoneFlag = false; @@ -489,45 +491,50 @@ void vtxTrampProcess(timeUs_t currentTimeUs) // Interface to common VTX API -vtxDevType_e vtxTrampGetDeviceType(void) +static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_TRAMP; } -bool vtxTrampIsReady(void) +static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice) { - return trampStatus > TRAMP_STATUS_OFFLINE; + return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE; } -void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { + UNUSED(vtxDevice); if (trampValidateBandAndChannel(band, channel)) { trampSetBandAndChannel(band, channel); trampCommitChanges(); } } -void vtxTrampSetPowerByIndex(uint8_t index) +static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { + UNUSED(vtxDevice); trampDevSetPowerByIndex(index); } -void vtxTrampSetPitMode(uint8_t onoff) +static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { + UNUSED(vtxDevice); trampSetPitMode(onoff); } -void vtxTrampSetFreq(uint16_t freq) +static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) { + UNUSED(vtxDevice); if (trampValidateFreq(freq)) { trampSetFreq(freq); trampCommitChanges(); } } -bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxTrampIsReady()) { + if (!vtxTrampIsReady(vtxDevice)) { return false; } @@ -537,9 +544,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) return true; } -bool vtxTrampGetPowerIndex(uint8_t *pIndex) +static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxTrampIsReady()) { + if (!vtxTrampIsReady(vtxDevice)) { return false; } @@ -555,9 +562,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) return true; } -bool vtxTrampGetPitMode(uint8_t *pOnOff) +static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!vtxTrampIsReady()) { + if (!vtxTrampIsReady(vtxDevice)) { return false; } @@ -565,9 +572,9 @@ bool vtxTrampGetPitMode(uint8_t *pOnOff) return true; } -bool vtxTrampGetFreq(uint16_t *pFreq) +static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - if (!vtxTrampIsReady()) { + if (!vtxTrampIsReady(vtxDevice)) { return false; } @@ -611,7 +618,7 @@ bool vtxTrampInit(void) } #if defined(USE_VTX_COMMON) - vtxCommonRegisterDevice(&vtxTramp); + vtxCommonSetDevice(&vtxTramp); #endif return true; diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index feaa539cd..3f094b020 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -267,14 +267,15 @@ bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs) static uint8_t vtxDeviceType; -void collectVtxTmData(spektrumVtx_t * vtx) +static void collectVtxTmData(spektrumVtx_t * vtx) { - vtxDeviceType = vtxCommonGetDeviceType(); + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + vtxDeviceType = vtxCommonGetDeviceType(vtxDevice); // Collect all data from VTX, if VTX is ready - if ( !(vtxCommonGetBandAndChannel(&(vtx->band), &(vtx->channel)) && - vtxCommonGetPitMode(&(vtx->pitMode)) && - vtxCommonGetPowerIndex(&(vtx->power))) ) + if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) && + vtxCommonGetPitMode(vtxDevice, &vtx->pitMode) && + vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) ) { vtx->band = 0; vtx->channel = 0; @@ -291,7 +292,7 @@ void collectVtxTmData(spektrumVtx_t * vtx) } // Reverse lookup, device power index to Spektrum power range index. -void convertVtxPower(spektrumVtx_t * vtx) +static void convertVtxPower(spektrumVtx_t * vtx) { uint8_t const * powerIndexTable = NULL; @@ -333,7 +334,7 @@ void convertVtxPower(spektrumVtx_t * vtx) } } -void convertVtxTmData(spektrumVtx_t * vtx) +static void convertVtxTmData(spektrumVtx_t * vtx) { // Convert from internal band indexes to Spektrum indexes for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) { @@ -368,7 +369,7 @@ typedef struct #define STRU_TELE_VTX_RESERVE_COUNT 7 #define VTX_KEEPALIVE_TIME_OUT 2000000 // uS -bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) +static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) { static timeUs_t lastTimeSentVtx = 0; static spektrumVtx_t vtxSent;