Skip to content

Commit

Permalink
refactor(mixer): Improve mixer "delay" option (#4256)
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelcoeffic @pfeerick
raphaelcoeffic authored and pfeerick committed Nov 7, 2023
1 parent 3fd0c35 commit e7cd978
Show file tree
Hide file tree
Showing 2 changed files with 129 additions and 68 deletions.
157 changes: 95 additions & 62 deletions radio/src/mixer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
*/

#include "opentx.h"
#include "opentx_types.h"
#include "timers.h"
#include "switches.h"
#include "mixes.h"
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
}
}
}

Expand All @@ -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<int16_t>(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;
}

Expand Down
40 changes: 34 additions & 6 deletions radio/src/tests/mixer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down

0 comments on commit e7cd978

Please sign in to comment.