From ce7adabfbeda0fe224a749572037a8210959793a Mon Sep 17 00:00:00 2001 From: bsongis Date: Tue, 4 Dec 2012 14:26:41 +0000 Subject: [PATCH] [X9D] Switches working on simu [stock / gruvin9x] N-pass mixer --- src/board_sky9x.cpp | 74 +++++++-- src/gtests.cpp | 192 +++++++++++++++++++--- src/main_views.cpp | 8 +- src/menus.cpp | 2 +- src/myeeprom.h | 2 +- src/open9x.cpp | 380 ++++++++++++++++++++++++-------------------- src/open9x.h | 6 + src/simpgmspace.cpp | 171 +++++++++++++++++--- src/simu.cpp | 125 ++++++--------- 9 files changed, 659 insertions(+), 301 deletions(-) diff --git a/src/board_sky9x.cpp b/src/board_sky9x.cpp index f7d3f253b..56041a004 100644 --- a/src/board_sky9x.cpp +++ b/src/board_sky9x.cpp @@ -898,49 +898,103 @@ uint8_t keyDown() #if defined(PCBX9D) extern uint32_t keyState(EnumKeys enuk) { - register uint32_t a ; register uint32_t c ; CPU_UINT xxx = 0; if (enuk < (int) DIM(keys)) return keys[enuk].state() ? 1 : 0; - a = PIOA->PIO_PDSR ; c = PIOC->PIO_PDSR ; // printf("c=%x\n", c); fflush(stdout); switch ((uint8_t) enuk) { case SW_SA0: - xxx = c & 0x80000000; + xxx = !(c & 0x80000000); break; case SW_SA2: - xxx = c & 0x40000000; + xxx = c & 0x80000000; break; case SW_SB0: - xxx = c & 0x08000000; + xxx = !(c & 0x08000000) && !(c & 0x04000000); break; case SW_SB1: - xxx = c & 0x04000000; + xxx = (c & 0x08000000) && !(c & 0x04000000); break; case SW_SB2: - xxx = c & 0x02000000; + xxx = !(c & 0x08000000) && (c & 0x04000000); break; case SW_SC0: - xxx = c & 0x00800000; + xxx = !(c & 0x00800000) && !(c & 0x00400000); break; case SW_SC1: - xxx = c & 0x00400000; + xxx = (c & 0x00800000) && !(c & 0x00400000); break; case SW_SC2: - xxx = c & 0x00200000; + xxx = !(c & 0x00800000) && (c & 0x00400000); + break; + + case SW_SD0: + xxx = !(c & 0x00200000) && !(c & 0x00100000); + break; + + case SW_SD1: + xxx = (c & 0x00200000) && !(c & 0x00100000); + break; + + case SW_SD2: + xxx = !(c & 0x00200000) && (c & 0x00100000); + break; + + case SW_SE0: + xxx = !(c & 0x00080000) && !(c & 0x00040000); + break; + + case SW_SE1: + xxx = (c & 0x00080000) && !(c & 0x00040000); + break; + + case SW_SE2: + xxx = !(c & 0x00080000) && (c & 0x00040000); + break; + + case SW_SF0: + xxx = !(c & 0x00020000) && !(c & 0x00010000); + break; + + case SW_SF1: + xxx = (c & 0x00020000) && !(c & 0x00010000); + break; + + case SW_SF2: + xxx = !(c & 0x00020000) && (c & 0x00010000); + break; + + case SW_SG0: + xxx = !(c & 0x00008000) && !(c & 0x00004000); + break; + + case SW_SG1: + xxx = (c & 0x00008000) && !(c & 0x00004000); + break; + + case SW_SG2: + xxx = !(c & 0x00008000) && (c & 0x00004000); + break; + + case SW_SH0: + xxx = !(c & 0x00002000); + break; + + case SW_SH2: + xxx = c & 0x00002000; break; default: diff --git a/src/gtests.cpp b/src/gtests.cpp index 558115b3a..b2a5209c3 100644 --- a/src/gtests.cpp +++ b/src/gtests.cpp @@ -34,36 +34,44 @@ #include #include "open9x.h" +void doMixerCalculations(); + +#define MODEL_RESET() memset(&g_model, 0, sizeof(g_model)) + +#define MIXER_RESET() \ + memset(g_chans512, 0, sizeof(g_chans512)); \ + memset(ex_chans, 0, sizeof(ex_chans)) + uint16_t anaIn(uint8_t chan) { return 0; } -TEST(trims, greaterTrimLink) +TEST(Trims, greaterTrimLink) { - memset(&g_model, 0, sizeof(g_model)); - setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim - setTrimValue(3, 0, 32); - EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(1, 0), 0), 32); + MODEL_RESET(); + setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim + setTrimValue(3, 0, 32); + EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(1, 0), 0), 32); } -TEST(trims, chainedTrims) +TEST(Trims, chainedTrims) { - memset(&g_model, 0, sizeof(g_model)); - setTrimValue(0, 0, 32); - setTrimValue(1, 0, TRIM_EXTENDED_MAX+1); // link to FP0 trim - setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim - EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32); + MODEL_RESET(); + setTrimValue(0, 0, 32); + setTrimValue(1, 0, TRIM_EXTENDED_MAX+1); // link to FP0 trim + setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim + EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32); } -TEST(trims, infiniteChainedTrims) +TEST(Trims, infiniteChainedTrims) { - memset(&g_model, 0, sizeof(g_model)); - setTrimValue(0, 0, 32); - setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim - setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim - setTrimValue(3, 0, TRIM_EXTENDED_MAX+3); // link to FP2 trim - EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32); + MODEL_RESET(); + setTrimValue(0, 0, 32); + setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim + setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim + setTrimValue(3, 0, TRIM_EXTENDED_MAX+3); // link to FP2 trim + EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32); } TEST(outdezNAtt, test_unsigned) @@ -259,14 +267,14 @@ TEST(FrSky, dateNtime) TEST(getSwitch, undefCSW) { - memset(&g_model, 0, sizeof(g_model)); + MODEL_RESET(); EXPECT_EQ(getSwitch(MAX_PSWITCH, 0), false); EXPECT_EQ(getSwitch(-MAX_PSWITCH, 0), true); // no good answer there! } TEST(getSwitch, circularCSW) { - memset(&g_model, 0, sizeof(g_model)); + MODEL_RESET(); g_model.customSw[0] = { MAX_SWITCH-NUM_CSW, MAX_SWITCH-NUM_CSW, CS_OR }; g_model.customSw[1] = { MAX_SWITCH-NUM_CSW, MAX_SWITCH-NUM_CSW, CS_AND }; EXPECT_EQ(getSwitch(MAX_SWITCH-NUM_CSW, 0), false); @@ -277,15 +285,15 @@ TEST(getSwitch, circularCSW) TEST(getSwitch, nullSW) { - memset(&g_model, 0, sizeof(g_model)); + MODEL_RESET(); EXPECT_EQ(getSwitch(0, 0), false); EXPECT_EQ(getSwitch(0, true), true); EXPECT_EQ(getSwitch(0, 0), false); } -TEST(phases, nullFadeOut_posFadeIn) +TEST(Phases, nullFadeOut_posFadeIn) { - memset(&g_model, 0, sizeof(g_model)); + MODEL_RESET(); g_model.phaseData[1].swtch = DSW(SW_ID1); g_model.phaseData[1].fadeIn = 15; perMain(); @@ -293,6 +301,144 @@ TEST(phases, nullFadeOut_posFadeIn) perMain(); } +TEST(Mixer, Cascaded3Channels) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].srcRaw = MIXSRC_CH2; + g_model.mixData[0].weight = 100; + g_model.mixData[1].destCh = 1; + g_model.mixData[1].srcRaw = MIXSRC_CH3; + g_model.mixData[1].weight = 100; + g_model.mixData[2].destCh = 2; + g_model.mixData[2].srcRaw = MIXSRC_ID1; + g_model.mixData[2].weight = 100; + setSwitch(DSW(SW_ID1)); + perOut(e_perout_mode_normal, 0); + EXPECT_EQ(chans[0], 102400); + EXPECT_EQ(chans[1], 102400); + EXPECT_EQ(chans[2], 102400); +} + +TEST(Mixer, CascadedOrderedChannels) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].srcRaw = MIXSRC_ID1; + g_model.mixData[0].weight = 100; + g_model.mixData[1].destCh = 1; + g_model.mixData[1].srcRaw = MIXSRC_CH1; + g_model.mixData[1].weight = 100; + setSwitch(DSW(SW_ID1)); + perOut(e_perout_mode_normal, 0); + EXPECT_EQ(chans[0], 102400); + EXPECT_EQ(chans[1], 102400); +} + +TEST(Mixer, Cascaded5Channels) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].srcRaw = MIXSRC_CH2; + g_model.mixData[0].weight = 100; + g_model.mixData[1].destCh = 1; + g_model.mixData[1].srcRaw = MIXSRC_CH3; + g_model.mixData[1].weight = 100; + g_model.mixData[2].destCh = 2; + g_model.mixData[2].srcRaw = MIXSRC_CH4; + g_model.mixData[2].weight = 100; + g_model.mixData[3].destCh = 3; + g_model.mixData[3].srcRaw = MIXSRC_CH5; + g_model.mixData[3].weight = 100; + g_model.mixData[4].destCh = 4; + g_model.mixData[4].srcRaw = MIXSRC_ID1; + g_model.mixData[4].weight = 100; + int32_t lastVal = 0; + for (uint8_t i=0; i<10; i++) { + setSwitch(DSW(SW_ID1)); + doMixerCalculations(); + EXPECT_EQ(chans[0], lastVal); + EXPECT_EQ(chans[1], lastVal); + EXPECT_EQ(chans[2], 102400); + EXPECT_EQ(chans[3], 102400); + EXPECT_EQ(chans[4], 102400); + doMixerCalculations(); + EXPECT_EQ(chans[0], 102400); + EXPECT_EQ(chans[1], 102400); + EXPECT_EQ(chans[2], 102400); + EXPECT_EQ(chans[3], 102400); + EXPECT_EQ(chans[4], 102400); + setSwitch(DSW(SW_ID0)); + doMixerCalculations(); + EXPECT_EQ(chans[0], 102400); + EXPECT_EQ(chans[1], 102400); + EXPECT_EQ(chans[2], -102400); + EXPECT_EQ(chans[3], -102400); + EXPECT_EQ(chans[4], -102400); + doMixerCalculations(); + EXPECT_EQ(chans[0], -102400); + EXPECT_EQ(chans[1], -102400); + EXPECT_EQ(chans[2], -102400); + EXPECT_EQ(chans[3], -102400); + EXPECT_EQ(chans[4], -102400); + lastVal = -102400; + } +} + +TEST(Mixer, InfiniteRecursiveChannels) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].srcRaw = MIXSRC_CH2; + g_model.mixData[0].weight = 100; + g_model.mixData[1].destCh = 1; + g_model.mixData[1].srcRaw = MIXSRC_CH3; + g_model.mixData[1].weight = 100; + g_model.mixData[2].destCh = 2; + g_model.mixData[2].srcRaw = MIXSRC_CH1; + g_model.mixData[2].weight = 100; + perOut(e_perout_mode_normal, 0); + EXPECT_EQ(chans[2], 0); + EXPECT_EQ(chans[1], 0); + EXPECT_EQ(chans[0], 0); +} + +TEST(Mixer, BlockingChannel) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].srcRaw = MIXSRC_CH1; + g_model.mixData[0].weight = 100; + perOut(e_perout_mode_normal, 0); + EXPECT_EQ(chans[0], 0); +} + +TEST(Mixer, RecursiveAddChannel) +{ + MODEL_RESET(); + MIXER_RESET(); + g_model.mixData[0].destCh = 0; + g_model.mixData[0].mltpx = MLTPX_ADD; + g_model.mixData[0].srcRaw = MIXSRC_MAX; + g_model.mixData[0].weight = 50; + g_model.mixData[1].destCh = 0; + g_model.mixData[1].mltpx = MLTPX_ADD; + g_model.mixData[1].srcRaw = MIXSRC_CH2; + g_model.mixData[1].weight = 100; + g_model.mixData[2].destCh = 1; + g_model.mixData[2].srcRaw = MIXSRC_Rud; + g_model.mixData[2].weight = 100; + perOut(e_perout_mode_normal, 0); + EXPECT_EQ(chans[0], 102400/2); + EXPECT_EQ(chans[1], 0); +} + + int main(int argc, char **argv) { StartEepromThread(NULL); g_menuStackPtr = 0; diff --git a/src/main_views.cpp b/src/main_views.cpp index f2f061a82..62ba281d9 100644 --- a/src/main_views.cpp +++ b/src/main_views.cpp @@ -433,15 +433,15 @@ void menuMainView(uint8_t event) uint8_t sw; switch(i) { case 0: - sw = getValue(MIXSRC_SA) > 0 ? 2 : 1; + sw = getValue(MIXSRC_SA-1) > 0 ? 2 : 1; break; case 7: - sw = getValue(MIXSRC_SH) > 0 ? 2+2+3*6 : 1+3*6; + sw = getValue(MIXSRC_SH-1) > 0 ? 2+2+3*6 : 1+2+3*6; break; default: { - int8_t val = getValue(MIXSRC_SA+i); - sw = ((val > 0) ? 3*i+2 : ((val < 0) ? 3*i+1 : 3*i)); + int16_t val = getValue(MIXSRC_SA+i-1); + sw = ((val < 0) ? 3*i : ((val == 0) ? 3*i+1 : 3*i+2)); break; } } diff --git a/src/menus.cpp b/src/menus.cpp index 9de552a90..e1ec10392 100644 --- a/src/menus.cpp +++ b/src/menus.cpp @@ -164,7 +164,7 @@ int16_t checkIncDec(uint8_t event, int16_t val, int16_t i_min, int16_t i_max, ui } if (newval != val) { - if ((~i_flags & NO_INCDEC_MARKS) && (newval != i_max) && (newval != i_min) && (newval==0 || newval==-100 || newval==+100)) { + if (!(i_flags & NO_INCDEC_MARKS) && (newval != i_max) && (newval != i_min) && (newval==0 || newval==-100 || newval==+100)) { pauseEvents(event); // delay before auto-repeat continues if (newval>val) // without AUDIO it's optimized, because the 2 sounds are the same AUDIO_KEYPAD_UP(); diff --git a/src/myeeprom.h b/src/myeeprom.h index 171ae96b2..902154cd7 100644 --- a/src/myeeprom.h +++ b/src/myeeprom.h @@ -669,7 +669,7 @@ enum MixSources { MIXSRC_P2, MIXSRC_P3, #endif -#if defined(PCBSKY9X) +#if defined(PCBSKY9X) && !defined(PCBX9D) MIXSRC_REa, #elif defined(PCBGRUVIN9X) MIXSRC_REa, diff --git a/src/open9x.cpp b/src/open9x.cpp index 561c3f8ff..538667975 100644 --- a/src/open9x.cpp +++ b/src/open9x.cpp @@ -88,7 +88,7 @@ const pm_uchar asterisk_lbm[] PROGMEM = { EEGeneral g_eeGeneral; ModelData g_model; -#if defined(PCBX9D) +#if defined(PCBX9D) && defined(SDCARD) uint8_t modelBitmap[64*32/8]; void loadModelBitmap() { @@ -619,9 +619,14 @@ int16_t getValue(uint8_t i) else if(ifunc] = FSW_PARAM(sd); } - if (~activeFunctions & function_mask) { + if (!(activeFunctions & function_mask)) { if (sd->func == FUNC_INSTANT_TRIM) { if (g_menuStack[0] == menuMainView #if defined(FRSKY) @@ -2092,7 +2097,7 @@ void evalFunctions() } #endif - if (~activeFunctionSwitches & switch_mask) { + if (!(activeFunctionSwitches & switch_mask)) { if (sd->func == FUNC_RESET) { switch (FSW_PARAM(sd)) { case 0: @@ -2120,7 +2125,7 @@ void evalFunctions() else #endif - if (!COMPLEX_SWITCH || (~activeFunctionSwitches & switch_mask)) { + if (!COMPLEX_SWITCH || !(activeFunctionSwitches & switch_mask)) { if (sd->func == FUNC_PLAY_SOUND) { AUDIO_PLAY(AU_FRSKY_FIRST+FSW_PARAM(sd)); } @@ -2201,8 +2206,8 @@ void evalFunctions() } if (COMPLEX_SWITCH) { - if (~activeFunctionSwitches & switch_mask) { - if (~activeFunctions & function_mask) { + if (!(activeFunctionSwitches & switch_mask)) { + if (!(activeFunctions & function_mask)) { newActiveFunctions |= function_mask; } } @@ -2317,202 +2322,231 @@ void perOut(uint8_t mode, uint8_t tick10ms) } #endif - memclear(chans, sizeof(chans)); // All outputs to 0 - //========== MIXER LOOP =============== uint8_t lv_mixWarning = 0; - for (uint8_t i=0; isrcRaw==0) break; + do { - if (md->phases & (1<swtch, 1); + bitfield_channels_t passDirtyChannels = 0; - //========== VALUE =============== - //Notice 0 = NC switch means not used -> always on line - uint8_t k = md->srcRaw-1; - int16_t v = 0; - if (mode != e_perout_mode_normal) { - if (!sw || k >= NUM_STICKS || (k == THR_STICK && g_model.thrTrim)) { - continue; + for (uint8_t i = 0; i < MAX_MIXERS; i++) { + + MixData *md = mixaddress(i); + + if (md->srcRaw == 0) break; + + uint8_t k = md->srcRaw - 1; + + if (!(dirtyChannels & ((bitfield_channels_t) 1 << md->destCh))) continue; + + if (md->phases & (1 << s_perout_flight_phase)) continue; + + //========== SWITCH =============== + bool sw = getSwitch(md->swtch, 1); + + //========== VALUE =============== + int16_t v = 0; + if (mode != e_perout_mode_normal) { + if (!sw || k >= NUM_STICKS || (k == THR_STICK && g_model.thrTrim)) { + continue; + } + else { + if (!(mode & e_perout_mode_nosticks)) v = anas[k]; + } } else { - if (!(mode & e_perout_mode_nosticks)) - v = anas[k]; - } - } - else { - if (k < NUM_STICKS) - v = md->noExpo ? rawAnas[k] : anas[k]; //Switch is on. MAX=FULL=512 or value. - else if (k>=MIXSRC_CH1-1 && k<=MIXSRC_CH16-1 && k-MIXSRC_CH1+1destCh) // if we've already calculated the value - take it instead - v = chans[k-MIXSRC_CH1+1] / 100; + if (k < NUM_STICKS) + v = md->noExpo ? rawAnas[k] : anas[k]; //Switch is on. MAX=FULL=512 or value. #if defined(PCBX9D) - // TODO + // TODO #else - else if (k>=MIXSRC_THR-1 && k<=MIXSRC_SWC-1) { - v = getSwitch(k-MIXSRC_THR+1+1, 0) ? +1024 : -1024; - if (v<0 && !md->swtch) - sw = false; + else if (k >= MIXSRC_THR - 1 && k <= MIXSRC_SWC - 1) { + v = getSwitch(k - MIXSRC_THR + 1 + 1, 0) ? +1024 : -1024; + if (v < 0 && !md->swtch) sw = false; + } + else { + v = getValue(k <= MIXSRC_3POS ? k : k - MAX_SWITCH); + if (k >= MIXSRC_CH1 - 1 && k <= MIXSRC_CHMAX - 1 + && md->destCh != k - MIXSRC_CH1 + 1) { + if (dirtyChannels + & ((bitfield_channels_t) 1 << (k - MIXSRC_CH1 + 1)) + & (passDirtyChannels + | ~(((bitfield_channels_t) 1 << md->destCh) - 1))) passDirtyChannels |= + (bitfield_channels_t) 1 << md->destCh; + if (k - MIXSRC_CH1 + 1 < md->destCh || pass > 0) v = chans[k + - MIXSRC_CH1 + 1] / 100; + } + } +#endif + } + + //========== DELAYS =============== + uint8_t swTog; + bool apply_offset = true; + if (sw) { // switch on? (if no switch selected => on) + swTog = !swOn[i]; + if (mode == e_perout_mode_normal) { + swOn[i] = true; + if (md->delayUp) { + if (swTog) { + if (sDelay[i]) + sDelay[i] = 0; + else + sDelay[i] = md->delayUp * 50; + } + if (sDelay[i] > 0) { // perform delay + sDelay[i] = max(0, sDelay[i] - tick10ms); + if (!md->swtch) { + v = -1024; + } + else { + continue; + } + } + } + if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); // Mix warning + } } else { - v = getValue(k <= MIXSRC_3POS ? k : k-MAX_SWITCH); - } -#endif - } - - //========== DELAYS =============== - uint8_t swTog; - bool apply_offset = true; - if (sw) { // switch on? (if no switch selected => on) - swTog = !swOn[i]; - if (mode == e_perout_mode_normal) { - swOn[i] = true; - if (md->delayUp) { + bool has_delay = false; + swTog = swOn[i]; + swOn[i] = false; + if (md->delayDown) { if (swTog) { if (sDelay[i]) sDelay[i] = 0; else - sDelay[i] = md->delayUp * 50; + sDelay[i] = md->delayDown * 50; } if (sDelay[i] > 0) { // perform delay - sDelay[i] = max(0, sDelay[i]-tick10ms); - if (!md->swtch) { - v = -1024; - } - else { - continue; - } + sDelay[i] = max(0, sDelay[i] - tick10ms); + if (!md->swtch) v = +1024; + has_delay = true; + } + else if (!md->swtch) { + v = -1024; } } - if (md->mixWarn) lv_mixWarning |= 1<<(md->mixWarn-1); // Mix warning - } - } - else { - bool has_delay = false; - swTog = swOn[i]; - swOn[i] = false; - if (md->delayDown) { - if (swTog) { - if (sDelay[i]) - sDelay[i] = 0; - else - sDelay[i] = md->delayDown * 50; - } - if (sDelay[i] > 0) { // perform delay - sDelay[i] = max(0, sDelay[i]-tick10ms); - if (!md->swtch) v = +1024; - has_delay = true; - } - else if (!md->swtch) { - v = -1024; + if (!has_delay) { + if (md->speedDown) { + if (md->mltpx == MLTPX_REP) continue; + if (md->swtch) { + v = 0; + apply_offset = false; + } + } + else if (md->swtch) { + continue; + } } } - if (!has_delay) { - if (md->speedDown) { - if (md->mltpx==MLTPX_REP) continue; - if (md->swtch) { v = 0; apply_offset = false; } - } - else if (md->swtch) { - continue; - } - } - } #ifdef BOLD_FONT - activeMixes |= ((ACTIVE_MIXES_TYPE)1 << i); + activeMixes |= ((ACTIVE_MIXES_TYPE) 1 << i); #endif - //========== OFFSET =============== - if (apply_offset) { - int8_t offset = GET_GVAR(md->offset, -125, 125, s_perout_flight_phase); - if (offset) v += calc100toRESX(offset); - } + //========== OFFSET =============== + if (apply_offset) { + int8_t offset = GET_GVAR(md->offset, -125, 125, s_perout_flight_phase); + if (offset) v += calc100toRESX(offset); + } - //========== TRIMS =============== - if (!(mode & e_perout_mode_notrims)) { - int8_t mix_trim = md->carryTrim; - if (mix_trim < TRIM_ON) - mix_trim = -mix_trim-1; - else if (mix_trim == TRIM_ON && k < NUM_STICKS) - mix_trim = k; - else - mix_trim = -1; - if (mix_trim >= 0) - v += trims[mix_trim]; - } + //========== TRIMS =============== + if (!(mode & e_perout_mode_notrims)) { + int8_t mix_trim = md->carryTrim; + if (mix_trim < TRIM_ON) + mix_trim = -mix_trim - 1; + else if (mix_trim == TRIM_ON && k < NUM_STICKS) + mix_trim = k; + else + mix_trim = -1; + if (mix_trim >= 0) v += trims[mix_trim]; + } - int16_t weight = GET_GVAR(md->weight, -500, 500, s_perout_flight_phase); + int16_t weight = GET_GVAR(md->weight, -500, 500, s_perout_flight_phase); - //========== SPEED =============== - if (mode == e_perout_mode_normal && (md->speedUp || md->speedDown)) // there are delay values - { + //========== SPEED =============== + if (mode == e_perout_mode_normal && (md->speedUp || md->speedDown)) { // there are delay values + #define DEL_MULT 256 - int16_t diff = v-act[i]/DEL_MULT; + int16_t diff = v - act[i] / DEL_MULT; - if (diff) { - //rate = steps/sec => 32*1024/100*md->speedUp/Down - //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown); - //-100..100 => 32768 -> 100*83886/256 = 32768, For MAX we divide by 2 since it's asymmetrical - if (tick10ms) { - int32_t rate = (int32_t)DEL_MULT*2048*100*tick10ms; + if (diff) { + //rate = steps/sec => 32*1024/100*md->speedUp/Down + //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown); + //-100..100 => 32768 -> 100*83886/256 = 32768, For MAX we divide by 2 since it's asymmetrical + if (tick10ms) { + int32_t rate = (int32_t) DEL_MULT * 2048 * 100 * tick10ms; if (weight) rate /= abs(weight); act[i] = (diff>0) ? ((md->speedUp>0) ? act[i]+(rate)/((int16_t)50*md->speedUp) : (int32_t)v*DEL_MULT) : ((md->speedDown>0) ? act[i]-(rate)/((int16_t)50*md->speedDown) : (int32_t)v*DEL_MULT) ; - } + } - { - int32_t tmp = act[i]/DEL_MULT ; - if(((diff>0) && (vtmp))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow - } + { + int32_t tmp = act[i] / DEL_MULT; + if (((diff > 0) && (v < tmp)) || ((diff < 0) && (v > tmp))) act[i] = (int32_t) v * DEL_MULT; //deal with overflow + } - v = act[i]/DEL_MULT; + v = act[i] / DEL_MULT; + } } - } - //========== CURVES =============== - if (md->curveParam && md->curveMode == MODE_CURVE) { - v = applyCurve(v, md->curveParam); - } + //========== CURVES =============== + if (md->curveParam && md->curveMode == MODE_CURVE) { + v = applyCurve(v, md->curveParam); + } - //========== WEIGHT =============== - int32_t dv = (int32_t)v*weight; + //========== WEIGHT =============== + int32_t dv = (int32_t) v * weight; - //========== DIFFERENTIAL ========= - if (md->curveMode == MODE_DIFFERENTIAL) { - int8_t curveParam = GET_GVAR(md->curveParam, -100, 100, s_perout_flight_phase); - if (curveParam>0 && dv<0) - dv = (dv * (100-curveParam)) / 100; - else if (curveParam<0 && dv>0) - dv = (dv * (100+curveParam)) / 100; - } + //========== DIFFERENTIAL ========= + if (md->curveMode == MODE_DIFFERENTIAL) { + int8_t curveParam = + GET_GVAR(md->curveParam, -100, 100, s_perout_flight_phase); + if (curveParam > 0 && dv < 0) + dv = (dv * (100 - curveParam)) / 100; + else if (curveParam < 0 && dv > 0) dv = (dv * (100 + curveParam)) / 100; + } - int32_t *ptr = &chans[md->destCh]; // Save calculating address several times - switch(md->mltpx){ - case MLTPX_REP: - *ptr = dv; + int32_t *ptr = &chans[md->destCh]; // Save calculating address several times + + if (i == 0 || md->destCh != (md - 1)->destCh) { + *ptr = 0; + } + + switch (md->mltpx) { + case MLTPX_REP: + *ptr = dv; #ifdef BOLD_FONT - for (uint8_t m=i-1; mdestCh == md->destCh; m--) - activeMixes &= ~((ACTIVE_MIXES_TYPE)1 << m); + for (uint8_t m = i - 1; + m < MAX_MIXERS && mixaddress(m)->destCh == md->destCh; m--) + activeMixes &= ~((ACTIVE_MIXES_TYPE) 1 << m); #endif - break; - case MLTPX_MUL: - dv /= 100; - dv *= *ptr; - dv /= RESXl; - *ptr = dv; - break; - default: // MLTPX_ADD - *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100); - break; + break; + case MLTPX_MUL: + dv /= 100; + dv *= *ptr; + dv /= RESXl; + *ptr = dv; + break; + default: // MLTPX_ADD + *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100); + break; } - } + } + + tick10ms = 0; + dirtyChannels &= passDirtyChannels; + + } while (++pass < 3 && dirtyChannels); mixWarning = lv_mixWarning; } @@ -2529,9 +2563,9 @@ ACTIVE_MIXES_TYPE activeMixes; int32_t sum_chans512[NUM_CHNOUT] = {0}; #if defined(PCBSKY9X) -inline bool doMixerCalculations() +FORCEINLINE bool doMixerCalculations() #else -inline void doMixerCalculations() +FORCEINLINE void doMixerCalculations() #endif { #if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) @@ -2584,14 +2618,16 @@ inline void doMixerCalculations() } else { uint8_t fadeTime = max(g_model.phaseData[s_last_phase].fadeOut, g_model.phaseData[phase].fadeIn); + uint8_t transitionMask = (1< 1024*100 // later we multiply by the limit (up to 100) and then we need to normalize // at the end chans[i] = chans[i]/100 => -1024..1024 @@ -2627,6 +2663,7 @@ inline void doMixerCalculations() int32_t q = (s_fade_flight_phases ? (sum_chans512[i] / weight) * 16 : chans[i]); ex_chans[i] = q / 100; // for the next perMain + int16_t value = applyLimits(i, q); cli(); @@ -2817,21 +2854,24 @@ inline void doMixerCalculations() if (s_fade_flight_phases) { for (uint8_t p=0; p delta * tick10ms) - fp_act[p] += delta * tick10ms; + if (MAX_ACT - fp_act[p] > delta) + fp_act[p] += delta; else { fp_act[p] = MAX_ACT; - s_fade_flight_phases -= (1< delta * tick10ms) - fp_act[p] -= delta * tick10ms; + if (fp_act[p] > delta) + fp_act[p] -= delta; else { fp_act[p] = 0; - s_fade_flight_phases -= (1<PIO_PDSR &= ~(1<<31); + break; + case DSW(SW_SA2): + PIOC->PIO_PDSR |= (1<<31); + break; + case DSW(SW_SB0): + PIOC->PIO_PDSR &= ~(1<<26); + PIOC->PIO_PDSR &= ~(1<<27); + break; + case DSW(SW_SB1): + PIOC->PIO_PDSR |= (1<<27); + PIOC->PIO_PDSR &= ~(1<<26); + break; + case DSW(SW_SB2): + PIOC->PIO_PDSR |= (1<<26); + PIOC->PIO_PDSR &= ~(1<<27); + break; + case DSW(SW_SC0): + PIOC->PIO_PDSR &= ~(1<<22); + PIOC->PIO_PDSR &= ~(1<<23); + break; + case DSW(SW_SC1): + PIOC->PIO_PDSR |= (1<<23); + PIOC->PIO_PDSR &= ~(1<<22); + break; + case DSW(SW_SC2): + PIOC->PIO_PDSR |= (1<<22); + PIOC->PIO_PDSR &= ~(1<<23); + break; + case DSW(SW_SD0): + PIOC->PIO_PDSR &= ~(1<<20); + PIOC->PIO_PDSR &= ~(1<<21); + break; + case DSW(SW_SD1): + PIOC->PIO_PDSR |= (1<<21); + PIOC->PIO_PDSR &= ~(1<<20); + break; + case DSW(SW_SD2): + PIOC->PIO_PDSR |= (1<<20); + PIOC->PIO_PDSR &= ~(1<<21); + break; + case DSW(SW_SE0): + PIOC->PIO_PDSR &= ~(1<<18); + PIOC->PIO_PDSR &= ~(1<<19); + break; + case DSW(SW_SE1): + PIOC->PIO_PDSR |= (1<<19); + PIOC->PIO_PDSR &= ~(1<<18); + break; + case DSW(SW_SE2): + PIOC->PIO_PDSR |= (1<<18); + PIOC->PIO_PDSR &= ~(1<<19); + break; + case DSW(SW_SF0): + PIOC->PIO_PDSR &= ~(1<<16); + PIOC->PIO_PDSR &= ~(1<<17); + break; + case DSW(SW_SF1): + PIOC->PIO_PDSR |= (1<<17); + PIOC->PIO_PDSR &= ~(1<<16); + break; + case DSW(SW_SF2): + PIOC->PIO_PDSR |= (1<<16); + PIOC->PIO_PDSR &= ~(1<<17); + break; + case DSW(SW_SG0): + PIOC->PIO_PDSR &= ~(1<<14); + PIOC->PIO_PDSR &= ~(1<<15); + break; + case DSW(SW_SG1): + PIOC->PIO_PDSR |= (1<<15); + PIOC->PIO_PDSR &= ~(1<<14); + break; + case DSW(SW_SG2): + PIOC->PIO_PDSR |= (1<<14); + PIOC->PIO_PDSR &= ~(1<<15); + break; + case DSW(SW_SH0): + PIOC->PIO_PDSR &= ~(1<<13); + break; + case DSW(SW_SH2): + PIOC->PIO_PDSR |= (1<<13); + break; +#elif defined(PCBSKY9X) case DSW(SW_ID0): -#if defined(PCBSKY9X) PIOC->PIO_PDSR &= ~0x00004000; PIOC->PIO_PDSR |= 0x00000800; -#elif defined(PCBGRUVIN9X) - ping |= (1<PIO_PDSR |= 0x00004800; -#elif defined(PCBGRUVIN9X) - ping &= ~(1<PIO_PDSR &= ~0x00000800; PIOC->PIO_PDSR |= 0x00004000; + break; + + SWITCH_CASE(SW_THR, PIOC->PIO_PDSR, 20) + SWITCH_CASE(SW_RUD, PIOA->PIO_PDSR, 15) + SWITCH_CASE(SW_ELE, PIOC->PIO_PDSR, 31) + SWITCH_CASE(SW_AIL, PIOA->PIO_PDSR, 2) + SWITCH_CASE(SW_GEA, PIOC->PIO_PDSR, 16) + SWITCH_CASE(SW_TRN, PIOC->PIO_PDSR, 8) #elif defined(PCBGRUVIN9X) + SWITCH_CASE(SW_THR, ping, INP_G_ThrCt) + SWITCH_CASE(SW_RUD, ping, INP_G_RuddDR) + SWITCH_CASE(SW_ELE, pinc, INP_C_ElevDR) + + case DSW(SW_ID0): + ping |= (1<PIO_PDSR, 31, 0 }, - { KEY_B, PIOC->PIO_PDSR, 27, 0 }, - { KEY_C, PIOC->PIO_PDSR, 23, 0 }, - { KEY_D, PIOC->PIO_PDSR, 17, 0 }, - { KEY_E, PIOC->PIO_PDSR, 16, 0 }, - { KEY_F, PIOC->PIO_PDSR, 15, 0 }, - { KEY_G, PIOC->PIO_PDSR, 15, 0 }, - { KEY_H, PIOC->PIO_PDSR, 15, 0 } }; -#elif defined(PCBSKY9X) - { KEY_1, PIOC->PIO_PDSR, 20, 0 }, - { KEY_6, PIOA->PIO_PDSR, 2, 0 }, - { KEY_2, PIOA->PIO_PDSR, 15, 0 }, - { KEY_3, PIOC->PIO_PDSR, 31, 0 }, - { KEY_7, PIOC->PIO_PDSR, 16, 0 }, - { KEY_8, PIOC->PIO_PDSR, 8, 0 } }; +#define SWITCH_KEY(key, swtch, states) \ + static bool state##key = 0; \ + static uint8_t state##swtch = 0; \ + if (getApp()->getKeyState(KEY_##key)) { \ + if (!state##key) { \ + state##swtch = (state##swtch+1) % states; \ + setSwitch(DSW(SW_##swtch+state##swtch)); \ + state##key = true; \ + } \ + } \ + else { \ + state##key = false; \ + } #else -#if defined(PCBGRUVIN9X) || defined(JETI) || defined(FRSKY) || defined(NMEA) || defined(ARDUPILOT) - { KEY_1, pinc, INP_C_ThrCt, 0 }, - { KEY_6, pinc, INP_C_AileDR, 0 }, -#else - { KEY_1, pine, INP_E_ThrCt, 0 }, - { KEY_6, pine, INP_E_AileDR, 0 }, -#endif - { KEY_2, ping, INP_G_RuddDR, 0 }, - { KEY_3, pine, INP_E_ElevDR, 0 }, - //KEY_4, ping, INP_G_ID1, 0, - //KEY_5, pine, INP_E_ID2, 0, - { KEY_7, pine, INP_E_Gear, 0 }, - { KEY_8, pine, INP_E_Trainer, 0 } }; +#define SWITCH_KEY(key, swtch, states) \ + static bool state##key = 0; \ + static uint8_t state##swtch = 0; \ + if (getApp()->getKeyState(KEY_##key)) { \ + if (!state##key) { \ + state##swtch = (state##swtch+1) % states; \ + if (states > 2) \ + setSwitch(DSW(SW_##swtch+state##swtch)); \ + else \ + setSwitch(state##swtch ? DSW(SW_##swtch) : -DSW(SW_##swtch)); \ + state##key = true; \ + } \ + } \ + else { \ + state##key = false; \ + } #endif - for(unsigned i=0; igetKeyState(keys3[i].key); - if (ks != keys3[i].value) { - if (ks) keys3[i].pin ^= (1<getKeyState(KEY_4); - if(ks != k4st){ - if(ks && id>0) id--; - k4st = ks; - } - ks=getApp()->getKeyState(KEY_5); - if(ks != k5st){ - if(ks && id<2) id++; - k5st = ks; - } - - switch(id){ - case 0: setSwitch(DSW(SW_ID0)); break; - case 1: setSwitch(DSW(SW_ID1)); break; - case 2: setSwitch(DSW(SW_ID2)); break; - } +#if defined(PCBX9D) + SWITCH_KEY(A, SA0, 2); + SWITCH_KEY(B, SB0, 3); + SWITCH_KEY(C, SC0, 3); + SWITCH_KEY(D, SD0, 3); + SWITCH_KEY(E, SE0, 3); + SWITCH_KEY(F, SF0, 3); + SWITCH_KEY(G, SG0, 3); + SWITCH_KEY(H, SH0, 2); +#else + SWITCH_KEY(1, THR, 2); + SWITCH_KEY(2, RUD, 2); + SWITCH_KEY(3, ELE, 2); + SWITCH_KEY(4, ID0, 3); + SWITCH_KEY(5, AIL, 2); + SWITCH_KEY(6, GEA, 2); + SWITCH_KEY(7, TRN, 2); +#endif } - per10ms(); refreshDiplay(); getApp()->addTimeout(this,2,5); @@ -515,6 +488,10 @@ int main(int argc,char **argv) // so that persistent settings are now available. application.init(argc,argv); +#if !defined(PCBX9D) + setSwitch(DSW(SW_ID0)); +#endif + // This creates the main window. We pass in the title to be displayed // above the window, and possibly some icons for when its iconified. // The decorations determine stuff like the borders, close buttons,