From e7cd9786c59f923712fb5bceed59a1b134a89a9b Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 7 Nov 2023 11:34:35 +1000 Subject: [PATCH] refactor(mixer): Improve mixer "delay" option (#4256) --- radio/src/mixer.cpp | 157 +++++++++++++++++++++++--------------- radio/src/tests/mixer.cpp | 40 ++++++++-- 2 files changed, 129 insertions(+), 68 deletions(-) diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 360b5c31e0e..9a9512465bf 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -20,6 +20,7 @@ */ #include "opentx.h" +#include "opentx_types.h" #include "timers.h" #include "switches.h" #include "mixes.h" @@ -633,6 +634,31 @@ int getSourceTrimValue(int source, int stickValue=0) } } +constexpr bitfield_channels_t all_channels_dirty = (bitfield_channels_t)-1; + +static inline bitfield_channels_t channel_bit(uint16_t ch) +{ + return (bitfield_channels_t)1 << ch; +} + +static inline bitfield_channels_t channel_dirty(bitfield_channels_t mask, uint16_t ch) +{ + return mask & channel_bit(ch); +} + +static inline bitfield_channels_t upper_channels_mask(uint16_t ch) +{ + // take the 2's complement to generate a bit pattern + // that has all bits of 'ch' order and above set + // + // Examples (mask for max 8 channels): + // - channel 0: 0b11111111 + // - channel 1: 0b11111110 + // - channel 2: 0b11111100 + + return ~(channel_bit(ch)) + 1; +} + uint8_t mixerCurrentFlightMode; void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) @@ -713,11 +739,10 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) memclear(chans, sizeof(chans)); // all outputs to 0 //========== MIXER LOOP =============== - uint8_t lv_mixWarning = 0; uint8_t pass = 0; - - bitfield_channels_t dirtyChannels = (bitfield_channels_t)-1; // all dirty when mixer starts + uint8_t lv_mixWarning = 0; + bitfield_channels_t dirtyChannels = all_channels_dirty; do { bitfield_channels_t passDirtyChannels = 0; @@ -728,62 +753,76 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) MixData * md = mixAddress(i); - if (md->srcRaw == 0) + if (md->srcRaw == 0) { #if defined(COLORLCD) continue; #else break; #endif + } - mixsrc_t stickIndex = md->srcRaw - MIXSRC_Rud; - - if (!(dirtyChannels & ((bitfield_channels_t)1 << md->destCh))) + if (!channel_dirty(dirtyChannels, md->destCh)) continue; - // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random) - if (i == 0 || md->destCh != (md-1)->destCh) + // if this is the first calculation for the destination channel, + // initialize it with 0 (otherwise would be random) + if (i == 0 || md->destCh != (md - 1)->destCh) chans[md->destCh] = 0; //========== FLIGHT MODE && SWITCH ===== - bool mixCondition = (md->flightModes != 0 || md->swtch); - delayval_t mixEnabled = (!(md->flightModes & (1 << mixerCurrentFlightMode)) && getSwitch(md->swtch)) ? DELAY_POS_MARGIN+1 : 0; - -#define MIXER_LINE_DISABLE() (mixCondition = true, mixEnabled = 0) - - if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !IS_TRAINER_INPUT_VALID()) { - MIXER_LINE_DISABLE(); - } + bool fmEnabled = (md->flightModes & (1 << mixerCurrentFlightMode)) == 0; + bool mixLineActive = fmEnabled && getSwitch(md->swtch); + + if (mixLineActive) { + // disable mixer using trainer channels if not connected + if (md->srcRaw >= MIXSRC_FIRST_TRAINER && + md->srcRaw <= MIXSRC_LAST_TRAINER && !IS_TRAINER_INPUT_VALID()) { + mixLineActive = false; + } #if defined(LUA_MODEL_SCRIPTS) - // disable mixer if Lua script is used as source and script was killed - if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_LUA && md->srcRaw <= MIXSRC_LAST_LUA) { - div_t qr = div(md->srcRaw-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS); - if (scriptInternalData[qr.quot].state != SCRIPT_OK) { - MIXER_LINE_DISABLE(); + // disable mixer if Lua script is used as source and script was killed + if (md->srcRaw >= MIXSRC_FIRST_LUA && md->srcRaw <= MIXSRC_LAST_LUA) { + div_t qr = div(md->srcRaw - MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS); + if (scriptInternalData[qr.quot].state != SCRIPT_OK) { + mixLineActive = false; + } } - } #endif + } //========== VALUE =============== getvalue_t v = 0; + if (mode > e_perout_mode_inactive_flight_mode) { - if (mixEnabled) - v = getValue(md->srcRaw); - else - continue; - } - else { - mixsrc_t srcRaw = MIXSRC_Rud + stickIndex; + if (!mixLineActive) continue; + v = getValue(md->srcRaw); + } else { + mixsrc_t srcRaw = md->srcRaw; v = getValue(srcRaw); - srcRaw -= MIXSRC_CH1; - if (srcRaw <= MIXSRC_LAST_CH-MIXSRC_CH1 && md->destCh != srcRaw) { - if (dirtyChannels & ((bitfield_channels_t)1 << srcRaw) & (passDirtyChannels|~(((bitfield_channels_t) 1 << md->destCh)-1))) - passDirtyChannels |= (bitfield_channels_t) 1 << md->destCh; - if (srcRaw < md->destCh || pass > 0) - v = chans[srcRaw] >> 8; - } - if (!mixCondition) { - mixEnabled = v; + + if (srcRaw >= MIXSRC_FIRST_CH) { + + auto srcChan = srcRaw - MIXSRC_FIRST_CH; + if (srcChan <= MAX_OUTPUT_CHANNELS && md->destCh != srcChan) { + + // check whether we need to recompute the current channel later + bitfield_channels_t upperChansMask = upper_channels_mask(md->destCh); + bitfield_channels_t srcChanDirtyMask = channel_dirty(dirtyChannels, srcChan); + + // if the source is any of the channels marked as dirty + // or contained in [ destCh, MAX_OUTPUT_CHANNELS [ + if (srcChanDirtyMask & (passDirtyChannels | upperChansMask)) { + passDirtyChannels |= channel_bit(md->destCh); + } + + // if the source has already be computed, + // then use it! + if (srcChan < md->destCh || pass > 0) { + // channels are in [ -1024 * 256, 1024 * 256 ] + v = chans[srcChan] >> 8; + } + } } } @@ -792,41 +831,35 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) //========== DELAYS =============== delayval_t _swOn = swOn[i].now; delayval_t _swPrev = swOn[i].prev; - bool swTog = (mixEnabled > _swOn+DELAY_POS_MARGIN || mixEnabled < _swOn-DELAY_POS_MARGIN); + + delayval_t v_active = mixLineActive ? v : 0; + + bool swTog = (v_active > _swOn + DELAY_POS_MARGIN || v_active < _swOn - DELAY_POS_MARGIN); if (mode == e_perout_mode_normal && swTog) { - if (!swOn[i].delay) - _swPrev = _swOn; - swOn[i].delay = (mixEnabled > _swOn ? md->delayUp : md->delayDown) * 10; - swOn[i].now = mixEnabled; - swOn[i].prev = _swPrev; + if (!swOn[i].delay) { swOn[i].prev = _swOn; } + swOn[i].now = v_active; + swOn[i].delay = (v_active > _swOn ? md->delayUp : md->delayDown) * 10; } if (mode == e_perout_mode_normal && swOn[i].delay > 0) { swOn[i].delay = max(0, (int16_t)swOn[i].delay - tick10ms); - if (!mixCondition) - v = _swPrev; - else if (mixEnabled) - continue; + v = _swPrev; } else { - if (mode==e_perout_mode_normal) { - swOn[i].now = swOn[i].prev = mixEnabled; + if (mode == e_perout_mode_normal) { + swOn[i].now = swOn[i].prev = v_active; } - if (!mixEnabled) { - if ((md->speedDown || md->speedUp) && md->mltpx!=MLTPX_REPL) { - if (mixCondition) { - v = (md->mltpx == MLTPX_ADD ? 0 : RESX); - applyOffsetAndCurve = false; - } - } - else if (mixCondition) { + if (!mixLineActive) { + if ((md->speedDown || md->speedUp) && md->mltpx != MLTPX_REPL) { + v = (md->mltpx == MLTPX_ADD ? 0 : RESX); + applyOffsetAndCurve = false; + } else { continue; } } } - if (mode==e_perout_mode_normal && (!mixCondition || mixEnabled || swOn[i].delay)) { - if (md->mixWarn) - lv_mixWarning |= 1 << (md->mixWarn - 1); + if (mode == e_perout_mode_normal && (mixLineActive || swOn[i].delay)) { + if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); swOn[i].activeMix = true; } diff --git a/radio/src/tests/mixer.cpp b/radio/src/tests/mixer.cpp index e0d288e3039..82d31db56ea 100644 --- a/radio/src/tests/mixer.cpp +++ b/radio/src/tests/mixer.cpp @@ -624,16 +624,44 @@ TEST_F(MixerTest, DelayOnSwitch) g_model.mixData[0].mltpx = MLTPX_ADD; g_model.mixData[0].srcRaw = MIXSRC_MAX; g_model.mixData[0].weight = 100; -#if defined(PCBFRSKY) - g_model.mixData[0].swtch = SWSRC_SA2; + g_model.mixData[0].swtch = SWSRC_FIRST_SWITCH + 2; + g_model.mixData[0].delayUp = 50; + g_model.mixData[0].delayDown = 50; + int switch_index = 0; -#else - g_model.mixData[0].swtch = SWSRC_THR; - int switch_index = 1; -#endif + simuSetSwitch(switch_index, -1); + + evalFlightModeMixes(e_perout_mode_normal, 0); + EXPECT_EQ(chans[0], 0); + + simuSetSwitch(switch_index, 1); + CHECK_DELAY(0, 500); + + evalFlightModeMixes(e_perout_mode_normal, 1); + EXPECT_EQ(chans[0], CHANNEL_MAX); + + simuSetSwitch(switch_index, 0); + CHECK_DELAY(0, 500); + + evalFlightModeMixes(e_perout_mode_normal, 1); + EXPECT_EQ(chans[0], 0); +} + +TEST_F(MixerTest, DelayOnSwitch2) +{ + g_eeGeneral.switchConfig = SWITCH_3POS; + + g_model.mixData[0].destCh = 0; + g_model.mixData[0].mltpx = MLTPX_ADD; + g_model.mixData[0].srcRaw = MIXSRC_FIRST_SWITCH; + g_model.mixData[0].weight = 100; + g_model.mixData[0].swtch = SWSRC_ON; g_model.mixData[0].delayUp = 50; g_model.mixData[0].delayDown = 50; + int switch_index = 0; + simuSetSwitch(switch_index, -1); + evalFlightModeMixes(e_perout_mode_normal, 0); EXPECT_EQ(chans[0], 0);