fix(radio): Mix delay not working when mix line has a switch (#4364)

This commit is contained in:
philmoz
2023-12-21 11:35:18 +10:00
committed by GitHub
parent 178fabd522
commit b53ccfa515
5 changed files with 58 additions and 39 deletions
+2 -4
View File
@@ -61,9 +61,7 @@ enum MainRequest {
extern uint8_t mainRequestFlags; extern uint8_t mainRequestFlags;
#define DELAY_POS_MARGIN 3 PACK(struct MixState {
PACK(struct SwOn {
uint16_t delay:14; // max = 2550 uint16_t delay:14; // max = 2550
uint8_t activeMix:1; uint8_t activeMix:1;
uint8_t activeExpo:1; uint8_t activeExpo:1;
@@ -71,7 +69,7 @@ PACK(struct SwOn {
int16_t prev; int16_t prev;
}); });
extern SwOn swOn[MAX_MIXERS]; extern MixState mixState[MAX_MIXERS];
extern int32_t act[MAX_MIXERS]; extern int32_t act[MAX_MIXERS];
// static variables used in evalFlightModeMixes - moved here so they don't interfere with the stack // static variables used in evalFlightModeMixes - moved here so they don't interfere with the stack
+52 -31
View File
@@ -30,6 +30,8 @@
#include "hal/trainer_driver.h" #include "hal/trainer_driver.h"
#include "hal/switch_driver.h" #include "hal/switch_driver.h"
#define DELAY_POS_MARGIN 3
uint8_t s_mixer_first_run_done = false; uint8_t s_mixer_first_run_done = false;
int8_t virtualInputsTrims[MAX_INPUTS]; int8_t virtualInputsTrims[MAX_INPUTS];
@@ -38,8 +40,8 @@ int16_t trims[MAX_TRIMS] = {0};
int32_t chans[MAX_OUTPUT_CHANNELS] = {0}; int32_t chans[MAX_OUTPUT_CHANNELS] = {0};
BeepANACenter bpanaCenter = 0; BeepANACenter bpanaCenter = 0;
int32_t act [MAX_MIXERS] = {0}; int32_t act [MAX_MIXERS] = {0};
SwOn swOn [MAX_MIXERS]; // TODO better name later... MixState mixState [MAX_MIXERS];
uint8_t mixWarning; 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; int8_t cur_chn = -1;
for (uint8_t i=0; i<MAX_EXPOS; i++) { for (uint8_t i=0; i<MAX_EXPOS; i++) {
if (mode == e_perout_mode_normal) swOn[i].activeExpo = false; if (mode == e_perout_mode_normal) mixState[i].activeExpo = false;
ExpoData * ed = expoAddress(i); ExpoData * ed = expoAddress(i);
if (!EXPO_VALID(ed)) break; // end of list if (!EXPO_VALID(ed)) break; // end of list
if (ed->chn == cur_chn) if (ed->chn == cur_chn)
@@ -180,7 +182,7 @@ void applyExpos(int16_t * anas, uint8_t mode, uint8_t ovwrIdx, int16_t ovwrValue
v = limit<int32_t>(-1024, v, 1024); v = limit<int32_t>(-1024, v, 1024);
} }
if (EXPO_MODE_ENABLE(ed, v)) { 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; cur_chn = ed->chn;
//========== CURVE================= //========== 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) 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 // that has all bits of 'ch' order and above set
// //
// Examples (mask for max 8 channels): // Examples (mask for max 8 channels):
@@ -731,12 +733,15 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
uint8_t lv_mixWarning = 0; uint8_t lv_mixWarning = 0;
bitfield_channels_t dirtyChannels = all_channels_dirty; 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 { do {
bitfield_channels_t passDirtyChannels = 0; bitfield_channels_t passDirtyChannels = 0;
for (uint8_t i=0; i<MAX_MIXERS; i++) { for (uint8_t i=0; i<MAX_MIXERS; i++) {
if (mode == e_perout_mode_normal && pass == 0) if (mode == e_perout_mode_normal && pass == 0)
swOn[i].activeMix = 0; activeMixes[i] = 0;
MixData * md = mixAddress(i); MixData * md = mixAddress(i);
@@ -757,14 +762,17 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
chans[md->destCh] = 0; chans[md->destCh] = 0;
//========== FLIGHT MODE && SWITCH ===== //========== FLIGHT MODE && SWITCH =====
bool mixCondition = (md->flightModes != 0 || md->swtch);
bool fmEnabled = (md->flightModes & (1 << mixerCurrentFlightMode)) == 0; bool fmEnabled = (md->flightModes & (1 << mixerCurrentFlightMode)) == 0;
bool mixLineActive = fmEnabled && getSwitch(md->swtch); bool mixLineActive = fmEnabled && getSwitch(md->swtch);
delayval_t mixEnabled = (mixLineActive) ? DELAY_POS_MARGIN+1 : 0;
if (mixLineActive) { if (mixLineActive) {
// disable mixer using trainer channels if not connected // disable mixer using trainer channels if not connected
if (md->srcRaw >= MIXSRC_FIRST_TRAINER && if (md->srcRaw >= MIXSRC_FIRST_TRAINER &&
md->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) { md->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) {
mixLineActive = false; mixCondition = true;
mixEnabled = 0;
} }
#if defined(LUA_MODEL_SCRIPTS) #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) { if (md->srcRaw >= MIXSRC_FIRST_LUA && md->srcRaw <= MIXSRC_LAST_LUA) {
div_t qr = div(md->srcRaw - MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS); div_t qr = div(md->srcRaw - MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS);
if (scriptInternalData[qr.quot].state != SCRIPT_OK) { if (scriptInternalData[qr.quot].state != SCRIPT_OK) {
mixLineActive = false; mixCondition = true;
mixEnabled = 0;
} }
} }
#endif #endif
@@ -782,8 +791,10 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
getvalue_t v = 0; getvalue_t v = 0;
if (mode > e_perout_mode_inactive_flight_mode) { if (mode > e_perout_mode_inactive_flight_mode) {
if (!mixLineActive) continue; if (mixEnabled)
v = getValue(md->srcRaw); v = getValue(md->srcRaw);
else
continue;
} else { } else {
mixsrc_t srcRaw = md->srcRaw; mixsrc_t srcRaw = md->srcRaw;
v = getValue(srcRaw); v = getValue(srcRaw);
@@ -811,43 +822,51 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
} }
} }
} }
if (!mixCondition)
mixEnabled = v;
} }
bool applyOffsetAndCurve = true; bool applyOffsetAndCurve = true;
//========== DELAYS =============== //========== DELAYS ===============
delayval_t _swOn = swOn[i].now; delayval_t _swOn = mixState[i].now;
delayval_t _swPrev = swOn[i].prev; delayval_t _swPrev = mixState[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 (mode == e_perout_mode_normal && swTog) {
if (!swOn[i].delay) { swOn[i].prev = _swOn; } if (!mixState[i].delay)
swOn[i].now = v_active; _swPrev = _swOn;
swOn[i].delay = (v_active > _swOn ? md->delayUp : md->delayDown) * 10; 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) { if (mode == e_perout_mode_normal && mixState[i].delay > 0) {
swOn[i].delay = max<int16_t>(0, (int16_t)swOn[i].delay - tick10ms); mixState[i].delay = max<int16_t>(0, (int16_t)mixState[i].delay - tick10ms);
v = _swPrev; // Freeze value until delay expires
if (!mixCondition)
v = _swPrev;
else if (mixEnabled)
continue;
} }
else { else {
if (mode == e_perout_mode_normal) { 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) { if ((md->speedDown || md->speedUp) && md->mltpx != MLTPX_REPL) {
v = (md->mltpx == MLTPX_ADD ? 0 : RESX); if (mixCondition) {
applyOffsetAndCurve = false; v = (md->mltpx == MLTPX_ADD ? 0 : RESX);
} else { applyOffsetAndCurve = false;
}
} else if (mixCondition) {
continue; 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); if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1);
swOn[i].activeMix = true; activeMixes[i] = true;
} }
if (applyOffsetAndCurve) { if (applyOffsetAndCurve) {
@@ -932,8 +951,8 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
case MLTPX_REPL: case MLTPX_REPL:
*ptr = dv; *ptr = dv;
if (mode == e_perout_mode_normal) { if (mode == e_perout_mode_normal) {
for (uint8_t m=i-1; m<MAX_MIXERS && mixAddress(m)->destCh==md->destCh; m--) for (int8_t m = i - 1; m >= 0 && mixAddress(m)->destCh == md->destCh; m--)
swOn[m].activeMix = false; activeMixes[m] = false;
} }
break; break;
case MLTPX_MUL: 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(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 // *ptr=limit( int32_t((-32767+RESXl)<<8), *ptr, int32_t((32767-RESXl)<<8)); // limit code cost 80 bytes
#endif #endif
} //endfor mixers } //endfor mixers
tick10ms = 0; tick10ms = 0;
@@ -995,6 +1013,9 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
} while (++pass < 5 && dirtyChannels); } while (++pass < 5 && dirtyChannels);
for (uint8_t i=0; i<MAX_MIXERS; i++)
mixState[i].activeMix = activeMixes[i];
mixWarning = lv_mixWarning; mixWarning = lv_mixWarning;
} }
+2 -2
View File
@@ -461,12 +461,12 @@ void moveTrimsToOffsets();
inline bool isExpoActive(uint8_t expo) inline bool isExpoActive(uint8_t expo)
{ {
return swOn[expo].activeExpo; return mixState[expo].activeExpo;
} }
inline bool isMixActive(uint8_t mix) inline bool isMixActive(uint8_t mix)
{ {
return swOn[mix].activeMix; return mixState[mix].activeMix;
} }
enum FunctionsActive { enum FunctionsActive {
+1 -1
View File
@@ -82,7 +82,7 @@ inline void MIXER_RESET()
memset(chans, 0, sizeof(chans)); memset(chans, 0, sizeof(chans));
memset(ex_chans, 0, sizeof(ex_chans)); memset(ex_chans, 0, sizeof(ex_chans));
memset(act, 0, sizeof(act)); memset(act, 0, sizeof(act));
memset(swOn, 0, sizeof(swOn)); memset(mixState, 0, sizeof(mixState));
mixerCurrentFlightMode = lastFlightMode = 0; mixerCurrentFlightMode = lastFlightMode = 0;
lastAct = 0; lastAct = 0;
logicalSwitchesReset(); logicalSwitchesReset();
+1 -1
View File
@@ -692,7 +692,7 @@ TEST_F(MixerTest, DelayOnSwitch2)
g_model.mixData[0].mltpx = MLTPX_ADD; g_model.mixData[0].mltpx = MLTPX_ADD;
g_model.mixData[0].srcRaw = MIXSRC_FIRST_SWITCH; g_model.mixData[0].srcRaw = MIXSRC_FIRST_SWITCH;
g_model.mixData[0].weight = 100; g_model.mixData[0].weight = 100;
g_model.mixData[0].swtch = SWSRC_ON; // g_model.mixData[0].swtch = SWSRC_ON;
g_model.mixData[0].delayUp = 50; g_model.mixData[0].delayUp = 50;
g_model.mixData[0].delayDown = 50; g_model.mixData[0].delayDown = 50;