diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 38008a062b..9c4bd1c8f4 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -423,6 +423,7 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) .channel = vtxSettingsConfig()->channel, .freq = vtxSettingsConfig()->freq, .power = vtxSettingsConfig()->power, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, }; vtxSettingsConfig_t newSettings = prevSettings; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 901e813af0..45fa58228b 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -212,11 +212,11 @@ static void taskTelemetry(timeUs_t currentTimeUs) // Everything that listens to VTX devices void taskVtxControl(timeUs_t currentTime) { - if (ARMING_FLAG(ARMED) || cliMode) + if (cliMode) return; #ifdef VTX_COMMON - vtxProcess(currentTime); + vtxProcessSchedule(currentTime); #endif } #endif diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 5316c9761d..9a91fd4326 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -761,6 +761,7 @@ const clivalue_t valueTable[] = { { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_BAND }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, band) }, { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, channel) }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_POWER_COUNT }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, power) }, + { "vtx_low_power_disarm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, lowPowerDisarm) }, #ifdef VTX_SETTINGS_FREQCMD { "vtx_freq", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, VTX_SETTINGS_MAX_FREQUENCY_MHZ }, PG_VTX_SETTINGS_CONFIG, offsetof(vtxSettingsConfig_t, freq) }, #endif diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index e1ca7afec4..13d69ddea9 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -29,6 +29,7 @@ #include "drivers/vtx_common.h" #include "fc/config.h" +#include "fc/runtime_config.h" #include "io/vtx.h" #include "io/vtx_string.h" @@ -40,7 +41,8 @@ PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, .band = VTX_SETTINGS_DEFAULT_BAND, .channel = VTX_SETTINGS_DEFAULT_CHANNEL, .power = VTX_SETTINGS_DEFAULT_POWER, - .freq = VTX_SETTINGS_DEFAULT_FREQ + .freq = VTX_SETTINGS_DEFAULT_FREQ, + .lowPowerDisarm = 0, ); #define VTX_PARAM_CYCLE_TIME_US 100000 // 10Hz @@ -69,7 +71,48 @@ void vtxInit(void) } } -void vtxProcess(timeUs_t currentTimeUs) +static void vtxProcessBandAndChannel(timeUs_t currentTimeUs) { + if(!ARMING_FLAG(ARMED)) { + uint8_t vtxBand; + uint8_t vtxChan; + if (vtxCommonGetBandAndChannel(&vtxBand, &vtxChan)) { + if (vtxSettingsConfig()->band != vtxBand || vtxSettingsConfig()->channel != vtxChan) { + vtxCommonSetBandAndChannel(vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + vtxCommonProcess(currentTimeUs); + } + } + } +} + +#if defined(VTX_SETTINGS_FREQCMD) +static void vtxProcessFrequency(timeUs_t currentTimeUs) { + if(!ARMING_FLAG(ARMED)) { + uint16_t vtxFreq; + if (vtxCommonGetFrequency(&vtxFreq)) { + if (vtxSettingsConfig()->freq != vtxFreq) { + vtxCommonSetFrequency(vtxSettingsConfig()->freq); + vtxCommonProcess(currentTimeUs); + } + } + } +} +#endif + +static void vtxProcessPower(timeUs_t currentTimeUs) { + uint8_t vtxPower; + uint8_t newPower = vtxSettingsConfig()->power; + if (vtxCommonGetPowerIndex(&vtxPower)) { + if (!ARMING_FLAG(ARMED) && vtxSettingsConfig()->lowPowerDisarm) { + newPower = VTX_SETTINGS_MIN_POWER; + } + if (vtxPower != newPower) { + vtxCommonSetPowerByIndex(newPower); + vtxCommonProcess(currentTimeUs); + } + } +} + +void vtxProcessSchedule(timeUs_t currentTimeUs) { static timeUs_t lastCycleTimeUs; static uint8_t scheduleIndex; @@ -81,31 +124,15 @@ void vtxProcess(timeUs_t currentTimeUs) switch (currentSchedule) { case VTX_PARAM_BANDCHAN: if (vtxSettingsConfig()->band) { - uint8_t vtxBand; - uint8_t vtxChan; - if (vtxCommonGetBandAndChannel(&vtxBand, &vtxChan)) { - if (vtxSettingsConfig()->band != vtxBand || vtxSettingsConfig()->channel != vtxChan) { - vtxCommonSetBandAndChannel(vtxSettingsConfig()->band, vtxSettingsConfig()->channel); - } - } + vtxProcessBandAndChannel(currentTimeUs); #if defined(VTX_SETTINGS_FREQCMD) } else { - uint16_t vtxFreq; - if (vtxCommonGetFrequency(&vtxFreq)) { - if (vtxSettingsConfig()->freq != vtxFreq) { - vtxCommonSetFrequency(vtxSettingsConfig()->freq); - } - } + vtxProcessFrequency(currentTimeUs); #endif } break; - case VTX_PARAM_POWER: ; - uint8_t vtxPower; - if (vtxCommonGetPowerIndex(&vtxPower)) { - if (vtxSettingsConfig()->power != vtxPower) { - vtxCommonSetPowerByIndex(vtxSettingsConfig()->power); - } - } + case VTX_PARAM_POWER: + vtxProcessPower(currentTimeUs); break; default: break; @@ -113,7 +140,6 @@ void vtxProcess(timeUs_t currentTimeUs) lastCycleTimeUs = currentTimeUs; scheduleIndex = (scheduleIndex + 1) % vtxParamScheduleCount; } - vtxCommonProcess(currentTimeUs); } } diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 3029011af7..3673c59efe 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -28,9 +28,10 @@ typedef struct vtxSettingsConfig_s { uint8_t channel; // 1-8 uint8_t power; // 0 = lowest uint16_t freq; // sets freq in MHz if band=0 + uint8_t lowPowerDisarm; // min power while disarmed } vtxSettingsConfig_t; PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig); void vtxInit(void); -void vtxProcess(timeUs_t currentTimeUs); +void vtxProcessSchedule(timeUs_t currentTimeUs); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 05e8c6830e..94d1f980ad 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -265,6 +265,7 @@ const uint8_t vtxTrampPi[] = { // Spektrum Spec Tx menu Tx sends .channel = vtxSettingsConfig()->channel, .freq = vtxSettingsConfig()->freq, .power = vtxSettingsConfig()->power, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, }; vtxSettingsConfig_t newSettings = prevSettings;