From b53ccfa515b641bbe205fd625febe0a6e58d1517 Mon Sep 17 00:00:00 2001 From: philmoz Date: Thu, 21 Dec 2023 12:35:18 +1100 Subject: [PATCH] fix(radio): Mix delay not working when mix line has a switch (#4364) --- radio/src/globals.h | 6 +-- radio/src/mixer.cpp | 83 ++++++++++++++++++++++++--------------- radio/src/opentx.h | 4 +- radio/src/tests/gtests.h | 2 +- radio/src/tests/mixer.cpp | 2 +- 5 files changed, 58 insertions(+), 39 deletions(-) diff --git a/radio/src/globals.h b/radio/src/globals.h index a64e2057cd1..2271c4413df 100644 --- a/radio/src/globals.h +++ b/radio/src/globals.h @@ -61,9 +61,7 @@ enum MainRequest { extern uint8_t mainRequestFlags; -#define DELAY_POS_MARGIN 3 - -PACK(struct SwOn { +PACK(struct MixState { uint16_t delay:14; // max = 2550 uint8_t activeMix:1; uint8_t activeExpo:1; @@ -71,7 +69,7 @@ PACK(struct SwOn { int16_t prev; }); -extern SwOn swOn[MAX_MIXERS]; +extern MixState mixState[MAX_MIXERS]; extern int32_t act[MAX_MIXERS]; // static variables used in evalFlightModeMixes - moved here so they don't interfere with the stack diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 453e0c37f6b..90a22fb7659 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -30,6 +30,8 @@ #include "hal/trainer_driver.h" #include "hal/switch_driver.h" +#define DELAY_POS_MARGIN 3 + uint8_t s_mixer_first_run_done = false; int8_t virtualInputsTrims[MAX_INPUTS]; @@ -38,8 +40,8 @@ int16_t trims[MAX_TRIMS] = {0}; int32_t chans[MAX_OUTPUT_CHANNELS] = {0}; BeepANACenter bpanaCenter = 0; -int32_t act [MAX_MIXERS] = {0}; -SwOn swOn [MAX_MIXERS]; // TODO better name later... +int32_t act [MAX_MIXERS] = {0}; +MixState mixState [MAX_MIXERS]; uint8_t mixWarning; @@ -158,7 +160,7 @@ void applyExpos(int16_t * anas, uint8_t mode, uint8_t ovwrIdx, int16_t ovwrValue int8_t cur_chn = -1; for (uint8_t i=0; ichn == cur_chn) @@ -180,7 +182,7 @@ void applyExpos(int16_t * anas, uint8_t mode, uint8_t ovwrIdx, int16_t ovwrValue v = limit(-1024, v, 1024); } if (EXPO_MODE_ENABLE(ed, v)) { - if (mode == e_perout_mode_normal) swOn[i].activeExpo = true; + if (mode == e_perout_mode_normal) mixState[i].activeExpo = true; cur_chn = ed->chn; //========== CURVE================= @@ -635,7 +637,7 @@ static inline bitfield_channels_t channel_dirty(bitfield_channels_t mask, uint16 static inline bitfield_channels_t upper_channels_mask(uint16_t ch) { - // take the 2's complement to generate a bit pattern + // take the 1's complement to generate a bit pattern // that has all bits of 'ch' order and above set // // Examples (mask for max 8 channels): @@ -731,12 +733,15 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) uint8_t lv_mixWarning = 0; bitfield_channels_t dirtyChannels = all_channels_dirty; + // Calculate locally and then copy to mixState array - prevent UI seeing phantom values while calculating + bool activeMixes[MAX_MIXERS]; + do { bitfield_channels_t passDirtyChannels = 0; for (uint8_t i=0; idestCh] = 0; //========== FLIGHT MODE && SWITCH ===== + bool mixCondition = (md->flightModes != 0 || md->swtch); bool fmEnabled = (md->flightModes & (1 << mixerCurrentFlightMode)) == 0; bool mixLineActive = fmEnabled && getSwitch(md->swtch); + delayval_t mixEnabled = (mixLineActive) ? DELAY_POS_MARGIN+1 : 0; if (mixLineActive) { // disable mixer using trainer channels if not connected if (md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) { - mixLineActive = false; + mixCondition = true; + mixEnabled = 0; } #if defined(LUA_MODEL_SCRIPTS) @@ -772,7 +780,8 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) 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; + mixCondition = true; + mixEnabled = 0; } } #endif @@ -782,8 +791,10 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) getvalue_t v = 0; if (mode > e_perout_mode_inactive_flight_mode) { - if (!mixLineActive) continue; - v = getValue(md->srcRaw); + if (mixEnabled) + v = getValue(md->srcRaw); + else + continue; } else { mixsrc_t srcRaw = md->srcRaw; v = getValue(srcRaw); @@ -811,43 +822,51 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) } } } + if (!mixCondition) + mixEnabled = v; } bool applyOffsetAndCurve = true; //========== DELAYS =============== - delayval_t _swOn = swOn[i].now; - delayval_t _swPrev = swOn[i].prev; - - delayval_t v_active = mixLineActive ? v : 0; + delayval_t _swOn = mixState[i].now; + delayval_t _swPrev = mixState[i].prev; + bool swTog = (mixEnabled > _swOn+DELAY_POS_MARGIN || mixEnabled < _swOn-DELAY_POS_MARGIN); - 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) { swOn[i].prev = _swOn; } - swOn[i].now = v_active; - swOn[i].delay = (v_active > _swOn ? md->delayUp : md->delayDown) * 10; + if (!mixState[i].delay) + _swPrev = _swOn; + mixState[i].delay = (mixEnabled > _swOn ? md->delayUp : md->delayDown) * 10; + mixState[i].now = mixEnabled; + mixState[i].prev = _swPrev; } - if (mode == e_perout_mode_normal && swOn[i].delay > 0) { - swOn[i].delay = max(0, (int16_t)swOn[i].delay - tick10ms); - v = _swPrev; + if (mode == e_perout_mode_normal && mixState[i].delay > 0) { + mixState[i].delay = max(0, (int16_t)mixState[i].delay - tick10ms); + // Freeze value until delay expires + if (!mixCondition) + v = _swPrev; + else if (mixEnabled) + continue; } else { if (mode == e_perout_mode_normal) { - swOn[i].now = swOn[i].prev = v_active; + mixState[i].now = mixState[i].prev = mixEnabled; } - if (!mixLineActive) { + if (!mixEnabled) { if ((md->speedDown || md->speedUp) && md->mltpx != MLTPX_REPL) { - v = (md->mltpx == MLTPX_ADD ? 0 : RESX); - applyOffsetAndCurve = false; - } else { + if (mixCondition) { + v = (md->mltpx == MLTPX_ADD ? 0 : RESX); + applyOffsetAndCurve = false; + } + } else if (mixCondition) { continue; } } } - if (mode == e_perout_mode_normal && (mixLineActive || swOn[i].delay)) { + if (mode == e_perout_mode_normal && (!mixCondition || mixEnabled || mixState[i].delay)) { if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); - swOn[i].activeMix = true; + activeMixes[i] = true; } if (applyOffsetAndCurve) { @@ -932,8 +951,8 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) case MLTPX_REPL: *ptr = dv; if (mode == e_perout_mode_normal) { - for (uint8_t m=i-1; mdestCh==md->destCh; m--) - swOn[m].activeMix = false; + for (int8_t m = i - 1; m >= 0 && mixAddress(m)->destCh == md->destCh; m--) + activeMixes[m] = false; } break; case MLTPX_MUL: @@ -987,7 +1006,6 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) // *ptr=limit( int32_t(int32_t(-1)<<23), *ptr, int32_t(int32_t(1)<<23)); // limit code cost 72 bytes // *ptr=limit( int32_t((-32767+RESXl)<<8), *ptr, int32_t((32767-RESXl)<<8)); // limit code cost 80 bytes #endif - } //endfor mixers tick10ms = 0; @@ -995,6 +1013,9 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) } while (++pass < 5 && dirtyChannels); + for (uint8_t i=0; i