fix(radio): Mix delay not working when mix line has a switch (#4364)
This commit is contained in:
+2
-4
@@ -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
@@ -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
@@ -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 {
|
||||||
|
|||||||
@@ -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();
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user