From ac761799429fb74a114eaa214a0d32d5b880553e Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 26 Jan 2018 13:52:29 +0100 Subject: [PATCH] Spektrum VTX control moved to VTX control task. Added some missing compile conditionals and a few cosmetic updates too. --- make/source.mk | 1 + src/main/io/spektrum_vtx_control.c | 92 +++++++++++++++++++----------- src/main/io/spektrum_vtx_control.h | 4 +- src/main/io/vtx.c | 4 ++ src/main/io/vtx_control.c | 12 +++- src/main/io/vtx_control.h | 1 + src/main/rx/spektrum.c | 3 +- 7 files changed, 80 insertions(+), 37 deletions(-) diff --git a/make/source.mk b/make/source.mk index 0aed6fc8c..0caf1d809 100644 --- a/make/source.mk +++ b/make/source.mk @@ -321,6 +321,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ io/vtx_control.c \ + io/spektrum_vtx_control.c \ pg/pg.h # F4 and F7 optimizations diff --git a/src/main/io/spektrum_vtx_control.c b/src/main/io/spektrum_vtx_control.c index 67043ecfe..7691abb9e 100644 --- a/src/main/io/spektrum_vtx_control.c +++ b/src/main/io/spektrum_vtx_control.c @@ -54,6 +54,7 @@ const uint8_t spek2commonBand[]= { // RF Power Index translation tables. No generic power API available..... +#ifdef USE_VTX_TRAMP // Tramp "---", 25, 200, 400. 600 mW const uint8_t vtxTrampPi[] = { // Spektrum Spec Tx menu Tx sends To VTX Watt VTX_TRAMP_POWER_OFF, // Off INHIBIT 0 0 - @@ -65,7 +66,9 @@ const uint8_t vtxTrampPi[] = { // Spektrum Spec Tx menu Tx sends T VTX_TRAMP_POWER_600, // 601 - max 601+ mW 6 5 600mW Slightly outside range VTX_TRAMP_POWER_200 // Manual - - - - }; +#endif // USE_VTX_TRAMP +#ifdef USE_VTX_RTC6705 // RTC6705 "---", 25 or 200 mW const uint8_t vtxRTC6705Pi[] = { VTX_6705_POWER_OFF, // Off @@ -77,7 +80,9 @@ const uint8_t vtxRTC6705Pi[] = { VTX_6705_POWER_200, // 601 - max VTX_6705_POWER_200 // Manual }; +#endif //USE_VTX_RTC6705 +#ifdef USE_VTX_SMARTAUDIO // SmartAudio "---", 25, 200, 500. 800 mW const uint8_t vtxSaPi[] = { VTX_SA_POWER_OFF, // Off @@ -89,23 +94,30 @@ const uint8_t vtxSaPi[] = { VTX_SA_POWER_800, // 601 - max VTX_SA_POWER_200 // Manual }; +#endif // USE_VTX_SMARTAUDIO uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower) { uint8_t devicePower = 0; switch (vtxCommonGetDeviceType()) { +#ifdef USE_VTX_RTC6705 case VTXDEV_RTC6705: devicePower = vtxRTC6705Pi[sPower]; break; +#endif // USE_VTX_RTC6705 +#ifdef USE_VTX_SMARTAUDIO case VTXDEV_SMARTAUDIO: devicePower = vtxSaPi[sPower]; break; +#endif // USE_VTX_SMARTAUDIO +#ifdef USE_VTX_TRAMP case VTXDEV_TRAMP: devicePower = vtxTrampPi[sPower]; break; +#endif // USE_VTX_TRAMP case VTXDEV_UNKNOWN: case VTXDEV_UNSUPPORTED: @@ -116,59 +128,71 @@ uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower) return devicePower; } -void spektrumHandleVtxControl(uint32_t vtxControl) -{ - stru_vtx vtx; +// Mark an inital invalid VTX ctrl frame to force first VTX settings cheange to actually come from Tx/Rx. +static uint32_t vtxControl_ipc = ~(SPEKTRUM_VTX_CONTROL_FRAME); - vtx.pitMode = (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT;; - vtx.region = (vtxControl & SPEKTRUM_VTX_REGION_MASK) >> SPEKTRUM_VTX_REGION_SHIFT; - vtx.power = (vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT; - vtx.band = (vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT; - vtx.channel = (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT; +// ############ RX task ###################### +void spektrumHandleVtxControl(uint32_t vtxCntrl) +{ + vtxControl_ipc = vtxCntrl; +} +// ########################################### + + +// ############ VTX_CONTROL task ############# +void spektrumVtxControl(void) +{ + // Check for invalid VTX ctrl frames + if ((vtxControl_ipc & SPEKTRUM_VTX_CONTROL_FRAME_MASK) != SPEKTRUM_VTX_CONTROL_FRAME) return; + + uint32_t vtxControl = vtxControl_ipc; + + spektrumVtx_t vtx = { + .pitMode = (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT, + .region = (vtxControl & SPEKTRUM_VTX_REGION_MASK) >> SPEKTRUM_VTX_REGION_SHIFT, + .power = (vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT, + .band = (vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT, + .channel = (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT, + }; const vtxSettingsConfig_t prevSettings = { - .band = vtxSettingsConfig()->band, + .band = vtxSettingsConfig()->band, .channel = vtxSettingsConfig()->channel, - .freq = vtxSettingsConfig()->freq, - .power = vtxSettingsConfig()->power, + .freq = vtxSettingsConfig()->freq, + .power = vtxSettingsConfig()->power, .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, }; vtxSettingsConfig_t newSettings = prevSettings; -#ifdef USE_VTX_COMMON_FREQ_API - uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel]; if (vtxCommonDeviceRegistered()) { + +#ifdef USE_VTX_COMMON_FREQ_API + uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel]; if (prevSettings.freq != freq) { - newSettings.band = VTX_COMMON_BAND_USER; + newSettings.band = VTX_COMMON_BAND_USER; newSettings.channel = vtx.channel; - newSettings.freq = freq; + 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 - if (vtxCommonDeviceRegistered()) { + // Convert to the internal Common Band index + uint8_t band = spek2commonBand[vtx.band]; + uint8_t channel = vtx.channel +1; // 0 based to 1 based if ((prevSettings.band != band) || (prevSettings.channel != channel)) { - newSettings.band = band; + newSettings.band = band; newSettings.channel = channel; - newSettings.freq = vtx58_Bandchan2Freq(band, channel); + newSettings.freq = vtx58_Bandchan2Freq(band, channel); } - } #endif - // Seems to be no unified internal VTX API std for popwer levels/indexes, VTX device brand specific. - uint8_t power = convertSpektrumVtxPowerIndex(vtx.power); - if (vtxCommonDeviceRegistered()) { + // Seems to be no unified internal VTX API std for popwer levels/indexes, VTX device brand specific. + uint8_t power = convertSpektrumVtxPowerIndex(vtx.power); if (prevSettings.power != power) { - newSettings.power = power; + newSettings.power = power; } - } - // Everyone seems to agree on what PIT ON/OFF means - uint8_t currentPitMode = 0; - if (vtxCommonDeviceRegistered()) { + // Everyone seems to agree on what PIT ON/OFF means + uint8_t currentPitMode = 0; vtxCommonGetPitMode(¤tPitMode); if (currentPitMode != vtx.pitMode) { vtxCommonSetPitMode(vtx.pitMode); @@ -176,10 +200,10 @@ void spektrumHandleVtxControl(uint32_t vtxControl) } if(memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) { - vtxSettingsConfigMutable()->band = newSettings.band; + vtxSettingsConfigMutable()->band = newSettings.band; vtxSettingsConfigMutable()->channel = newSettings.channel; - vtxSettingsConfigMutable()->power = newSettings.power; - vtxSettingsConfigMutable()->freq = newSettings.freq; + vtxSettingsConfigMutable()->power = newSettings.power; + vtxSettingsConfigMutable()->freq = newSettings.freq; saveConfigAndNotify(); } } diff --git a/src/main/io/spektrum_vtx_control.h b/src/main/io/spektrum_vtx_control.h index 9c7f0be25..ca97d0b17 100644 --- a/src/main/io/spektrum_vtx_control.h +++ b/src/main/io/spektrum_vtx_control.h @@ -82,9 +82,11 @@ typedef struct uint8_t power; uint8_t region; uint8_t pitMode; -} stru_vtx; +} spektrumVtx_t; + void spektrumHandleVtxControl(uint32_t vtxControl); +void spektrumVtxControl(void); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 8eeb61af2..ccdae705b 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -36,6 +36,7 @@ #include "io/vtx.h" #include "io/vtx_string.h" +#include "io/vtx_control.h" #include "interface/cli.h" @@ -208,6 +209,9 @@ void vtxUpdate(timeUs_t currentTimeUs) return; } + // Check input sources for config updates + vtxControlInputPoll(); + if (vtxCommonDeviceRegistered()) { bool vtxUpdatePending = false; const vtxSettingsConfig_t settings = vtxGetSettings(); diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 5871cfdbb..8cb8375b5 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -37,11 +37,12 @@ #include "fc/config.h" #include "fc/runtime_config.h" -#include "io/beeper.h" #include "io/osd.h" #include "io/vtx_control.h" #include "io/vtx.h" +#include "io/spektrum_vtx_control.h" + PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); @@ -58,6 +59,15 @@ void vtxControlInit(void) // NOTHING TO DO } +void vtxControlInputPoll(void) +{ + // Check variuos input sources for VTX config updates +#if defined(USE_SPEKTRUM_VTX_CONTROL) + // Get VTX updates + spektrumVtxControl(); +#endif +} + static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) { if (ARMING_FLAG(ARMED)) { diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 9820973a6..9fceb480a 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -42,6 +42,7 @@ typedef struct vtxConfig_s { PG_DECLARE(vtxConfig_t, vtxConfig); void vtxControlInit(void); +void vtxControlInputPoll(void); void vtxIncrementBand(void); void vtxDecrementBand(void); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index d08dbf23d..d27e803bf 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -93,6 +93,7 @@ static void spektrumDataReceive(uint16_t c, void *data) } } + uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch}; @@ -129,7 +130,7 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) (spekFrame[SPEKTRUM_VTX_CONTROL_4] << 0) ); // Handle VTX control frame, - if ( (vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME ) { + if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME) { spektrumHandleVtxControl(vtxControl); }