From 07a5333a080ce14e3c2a308552ce05ec4f59abc2 Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sat, 17 May 2014 17:45:53 +0200 Subject: [PATCH 01/18] Fixes #1039: * order of logical switches calculaton now predicable: always from L1 to L64 * each flight mode has its own logical switches context (RAM usage multiplied by number of flight modes) * resolved problem of non-reentrancy of getSwitch() function --- radio/src/audio_arm.cpp | 1 + radio/src/gtests.cpp | 10 +- radio/src/mixer.cpp | 6 +- radio/src/opentx.cpp | 1 - radio/src/opentx.h | 11 +- radio/src/switches.cpp | 1451 ++++++++++++++++++++------------------- 6 files changed, 744 insertions(+), 736 deletions(-) diff --git a/radio/src/audio_arm.cpp b/radio/src/audio_arm.cpp index b9cfd6eff..2ba4021d1 100644 --- a/radio/src/audio_arm.cpp +++ b/radio/src/audio_arm.cpp @@ -445,6 +445,7 @@ tmr10ms_t timeAutomaticPromptsSilence = 0; void playModelEvent(uint8_t category, uint8_t index, uint8_t event) { char filename[AUDIO_FILENAME_MAXLEN+1]; + TRACE("playModelEvent(): cat: %u, idx: %u, evt:%u", (uint32_t)category, (uint32_t)index, (uint32_t)event); if (IS_SILENCE_PERIOD_ELAPSED() && isAudioFileReferenced((category << 24) + (index << 16) + event, filename)) { audioQueue.playFile(filename); } diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index 7a0b4132f..6ff16890f 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -53,8 +53,7 @@ void doMixerCalculations(); memset(act, 0, sizeof(act)); \ memset(swOn, 0, sizeof(swOn)); \ int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \ - s_last_switch_used = 0; \ - s_last_switch_value = 0; + lswReset(); uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 }; uint16_t anaIn(uint8_t chan) @@ -330,19 +329,20 @@ TEST(getSwitch, recursiveSW) g_model.logicalSw[0] = { SWSRC_RUD, -SWSRC_SW2, LS_FUNC_OR }; g_model.logicalSw[1] = { SWSRC_ELE, -SWSRC_SW1, LS_FUNC_OR }; + evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), false); EXPECT_EQ(getSwitch(SWSRC_SW2), true); - s_last_switch_used = 0; + evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), false); EXPECT_EQ(getSwitch(SWSRC_SW2), true); simuSetSwitch(1, 1); - s_last_switch_used = 0; + evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), true); - s_last_switch_used = 0; + evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); } diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 39856c596..75f1f495d 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -524,6 +524,7 @@ uint8_t s_current_mixer_flight_mode; void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) { evalInputs(mode); + evalLogicalSwitches(mode); #if defined(MODULE_ALWAYS_SEND_PULSES) checkStartupWarnings(); @@ -929,8 +930,6 @@ void evalMixes(uint8_t tick10ms) static ACTIVE_PHASES_TYPE s_fade_flight_phases = 0; static uint8_t s_last_phase = 255; // TODO reinit everything here when the model changes, no??? - s_last_switch_used = 0; - uint8_t phase = getFlightPhase(); if (s_last_phase != phase) { @@ -953,6 +952,7 @@ void evalMixes(uint8_t tick10ms) fp_act[phase] = MAX_ACT; } } + copyLswState(s_last_phase, phase); //push last logical switches state from old to new phase s_last_phase = phase; } @@ -960,7 +960,6 @@ void evalMixes(uint8_t tick10ms) if (s_fade_flight_phases) { memclear(sum_chans512, sizeof(sum_chans512)); for (uint8_t p=0; p> 4) * fp_act[p]; weight += fp_act[p]; } - s_last_switch_used = 0; } assert(weight); s_current_mixer_flight_mode = phase; diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index b26c8144b..d07e28a5c 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1521,7 +1521,6 @@ void flightReset() #endif lswReset(); - s_last_switch_value = 0; s_mixer_first_run_done = false; START_SILENCE_PERIOD(); diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 49348366a..3ccbd0a2b 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -695,6 +695,8 @@ getvalue_t getValue(uint8_t i); bool getSwitch(int8_t swtch); void lswTimerTick(); void lswReset(); +void evalLogicalSwitches(uint8_t mode); +void copyLswState(uint8_t oldPhase, uint8_t newPhase); #if defined(PCBTARANIS) void getSwitchesPosition(bool startup); @@ -817,15 +819,6 @@ extern int8_t safetyCh[NUM_CHNOUT]; extern uint8_t trimsCheckTimer; -#if defined(CPUARM) - #define GETSWITCH_RECURSIVE_TYPE uint32_t -#else - #define GETSWITCH_RECURSIVE_TYPE uint16_t -#endif - -extern volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used; -extern volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value; - #define TMR_OFF 0 #define TMR_RUNNING 1 #define TMR_NEGATIVE 2 diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 5e4c970b2..7b564f0ce 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -1,717 +1,734 @@ -/* - * Authors (alphabetical order) - * - Andre Bernet - * - Andreas Weitl - * - Bertrand Songis - * - Bryan J. Rentoul (Gruvin) - * - Cameron Weeks - * - Erez Raviv - * - Gabriel Birkus - * - Jean-Pierre Parisy - * - Karl Szmutny - * - Michael Blandford - * - Michal Hlavinka - * - Pat Mackenzie - * - Philip Moss - * - Rob Thomson - * - Romolo Manfredini - * - Thomas Husterer - * - * opentx is based on code named - * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, - * er9x by Erez Raviv: http://code.google.com/p/er9x/, - * and the original (and ongoing) project by - * Thomas Husterer, th9x: http://code.google.com/p/th9x/ - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 2 as - * published by the Free Software Foundation. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - */ - -#include "opentx.h" - -#if defined(CPUARM) - #define GETSWITCH_RECURSIVE_TYPE uint32_t -#else - #define GETSWITCH_RECURSIVE_TYPE uint16_t -#endif - -#define CS_LAST_VALUE_INIT -32768 -volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used = 0; -volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value = 0; - -#if defined(CPUARM) -uint32_t lswDelays[NUM_LOGICAL_SWITCH]; -uint32_t lswDurations[NUM_LOGICAL_SWITCH]; -uint8_t lswStates[NUM_LOGICAL_SWITCH]; -#endif - -#if defined(PCBTARANIS) -tmr10ms_t switchesMidposStart[6] = { 0 }; -uint32_t switchesPos = 0; -tmr10ms_t potsLastposStart[NUM_XPOTS]; -uint8_t potsPos[NUM_XPOTS]; - -uint32_t check2PosSwitchPosition(EnumKeys sw) -{ - uint32_t result; - uint32_t index; - - if (switchState(sw)) - index = sw - SW_SA0; - else - index = sw - SW_SA0 + 1; - - result = (1 << index); - - if (!(switchesPos & result)) { - PLAY_SWITCH_MOVED(index); - } - - return result; -} - -#define DELAY_SWITCH_3POS 15/*150ms*/ -uint32_t check3PosSwitchPosition(uint8_t idx, EnumKeys sw, bool startup) -{ - uint32_t result; - uint32_t index; - - if (switchState(sw)) { - index = sw - SW_SA0; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else if (switchState(EnumKeys(sw+2))) { - index = sw - SW_SA0 + 2; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else if (startup || (switchesPos & (1 << (sw - SW_SA0 + 1))) || (switchesMidposStart[idx] && (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) > DELAY_SWITCH_3POS)) { - index = sw - SW_SA0 + 1; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else { - index = sw - SW_SA0 + 1; - if (!switchesMidposStart[idx]) { - switchesMidposStart[idx] = get_tmr10ms(); - } - result = (switchesPos & (0x7 << (sw - SW_SA0))); - } - - if (!(switchesPos & result)) { - PLAY_SWITCH_MOVED(index); - } - - return result; -} - -#define CHECK_2POS(sw) newPos |= check2PosSwitchPosition(sw ## 0) -#define CHECK_3POS(idx, sw) newPos |= check3PosSwitchPosition(idx, sw ## 0, startup) - -void getSwitchesPosition(bool startup) -{ - uint32_t newPos = 0; - CHECK_3POS(0, SW_SA); - CHECK_3POS(1, SW_SB); - CHECK_3POS(2, SW_SC); - CHECK_3POS(3, SW_SD); - CHECK_3POS(4, SW_SE); - CHECK_2POS(SW_SF); - CHECK_3POS(5, SW_SG); - CHECK_2POS(SW_SH); - switchesPos = newPos; - - for (int i=0; icount>0 && calib->countcount); - uint8_t previousPos = potsPos[i] >> 4; - uint8_t previousStoredPos = potsPos[i] & 0x0F; - if (pos != previousPos) { - potsLastposStart[i] = get_tmr10ms(); - potsPos[i] = (pos << 4) | previousStoredPos; - } - else if (startup || (tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) > DELAY_SWITCH_3POS) { - potsLastposStart[i] = 0; - potsPos[i] = (pos << 4) | pos; - if (previousStoredPos != pos) { - PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH+i*XPOTS_MULTIPOS_COUNT+pos); - } - } - } - } - } -} -#define SWITCH_POSITION(sw) (switchesPos & (1<<(sw))) -#define POT_POSITION(sw) ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT)) -#else -#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx))) -#endif - -int16_t lsLastValue[NUM_LOGICAL_SWITCH]; -#define CS_LAST_VALUE_INIT -32768 - -/* recursive function. stack as of today (16/03/2012) grows by 8bytes at each call, which is ok! */ -bool getSwitch(int8_t swtch) -{ - bool result; - - if (swtch == SWSRC_NONE) - return true; - - uint8_t cs_idx = abs(swtch); - - if (cs_idx == SWSRC_One) { - result = !s_mixer_first_run_done; - } - else if (cs_idx == SWSRC_ON) { - result = true; - } - else if (cs_idx <= SWSRC_LAST_SWITCH) { - result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH); -#if defined(MODULE_ALWAYS_SEND_PULSES) - if (startupWarningState < STARTUP_WARNING_DONE) { - // if throttle or switch warning is currently active, ignore actual stick position and use wanted values - if (cs_idx <= 3) { - if (!(g_model.nSwToWarn&1)) { // ID1 to ID3 is just one bit in nSwToWarn - result = (cs_idx)==((g_model.switchWarningStates&3)+1); // overwrite result with desired value - } - } - else if (!(g_model.nSwToWarn & (1<<(cs_idx-3)))) { - // current switch should not be ignored for warning - result = g_model.switchWarningStates & (1<<(cs_idx-2)); // overwrite result with desired value - } - } -#endif - } -#if defined(PCBTARANIS) - else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) { - result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH); - } -#endif - else if (cs_idx <= SWSRC_LAST_TRIM) { - uint8_t idx = cs_idx - SWSRC_FIRST_TRIM; - idx = (CONVERT_MODE(idx/2) << 1) + (idx & 1); - result = trimDown(idx); - } -#if ROTARY_ENCODERS > 0 - else if (cs_idx == SWSRC_REa) { - result = REA_DOWN(); - } -#endif -#if ROTARY_ENCODERS > 1 - else if (cs_idx == SWSRC_REb) { - result = REB_DOWN(); - } -#endif -#if defined(CPUARM) && defined(FLIGHT_MODES) - else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) { - uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE; - result = (idx == s_current_mixer_flight_mode); - } -#endif - else { - cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; - - GETSWITCH_RECURSIVE_TYPE mask = ((GETSWITCH_RECURSIVE_TYPE)1 << cs_idx); - if (s_last_switch_used & mask) { - result = (s_last_switch_value & mask); - } - else { - s_last_switch_used |= mask; - - LogicalSwitchData * ls = lswAddress(cs_idx); -#if defined(CPUARM) - int8_t s = ls->andsw; -#else - uint8_t s = ls->andsw; - if (s > SWSRC_LAST_SWITCH) { - s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; - } -#endif - if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { - lsLastValue[cs_idx] = CS_LAST_VALUE_INIT; - result = false; - } - else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { - bool res1 = getSwitch(ls->v1); - bool res2 = getSwitch(ls->v2); - switch (ls->func) { - case LS_FUNC_AND: - result = (res1 && res2); - break; - case LS_FUNC_OR: - result = (res1 || res2); - break; - // case LS_FUNC_XOR: - default: - result = (res1 ^ res2); - break; - } - } - else if (s == LS_FAMILY_TIMER) { - result = (lsLastValue[cs_idx] <= 0); - } - else if (s == LS_FAMILY_STICKY) { - result = (lsLastValue[cs_idx] & (1<<0)); - } -#if defined(CPUARM) - else if (s == LS_FAMILY_STAY) { - result = (lsLastValue[cs_idx] & (1<<0)); - } -#endif - else { - getvalue_t x = getValue(ls->v1); - getvalue_t y; - if (s == LS_FAMILY_COMP) { - y = getValue(ls->v2); - - switch (ls->func) { - case LS_FUNC_EQUAL: - result = (x==y); - break; - case LS_FUNC_GREATER: - result = (x>y); - break; - default: - result = (xv1; -#if defined(FRSKY) - // Telemetry - if (v1 >= MIXSRC_FIRST_TELEM) { - if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) - return swtch > 0 ? false : true; - - y = convertLswTelemValue(ls); - -#if defined(FRSKY_HUB) && defined(GAUGES) - if (s == LS_FAMILY_OFS) { - uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; - if (idx < THLD_MAX) { - // Fill the threshold array - barsThresholds[idx] = 128 + ls->v2; - } - } -#endif - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; - } - else { - y = calc100toRESX(ls->v2); - } -#else - if (v1 >= MIXSRC_FIRST_TELEM) { - y = (int16_t)3 * (128+ls->v2); // it's a Timer - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; // it's a GVAR - } - else { - y = calc100toRESX(ls->v2); - } -#endif - - switch (ls->func) { -#if defined(CPUARM) - case LS_FUNC_VEQUAL: - result = (x==y); - break; -#endif - case LS_FUNC_VALMOSTEQUAL: -#if defined(GVARS) - if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) - result = (x==y); - else -#endif - result = (abs(x-y) < (1024 / STICK_TOLERANCE)); - break; - case LS_FUNC_VPOS: - result = (x>y); - break; - case LS_FUNC_VNEG: - result = (xy); - break; - case LS_FUNC_ANEG: - result = (abs(x)func == LS_FUNC_DIFFEGREATER) - result = (y >= 0 ? (diff >= y) : (diff <= y)); - else - result = (abs(diff) >= y); - if (result) - lsLastValue[cs_idx] = x; - break; - } - } - } - } - -#if defined(CPUARM) - if (ls->delay) { - if (result) { - if (lswDelays[cs_idx] > get_tmr10ms()) - result = false; - } - else { - lswDelays[cs_idx] = get_tmr10ms() + (ls->delay*10); - } - } - - if (ls->duration) { - if (result && !lswStates[cs_idx]) { - lswDurations[cs_idx] = get_tmr10ms() + (ls->duration*10); - } - - lswStates[cs_idx] = result; - result = false; - - if (lswDurations[cs_idx] > get_tmr10ms()) { - result = true; - } - } -#endif - - if (result) { - if (!(s_last_switch_value&mask)) PLAY_LOGICAL_SWITCH_ON(cs_idx); - s_last_switch_value |= mask; - } - else { - if (s_last_switch_value&mask) PLAY_LOGICAL_SWITCH_OFF(cs_idx); - s_last_switch_value &= ~mask; - } - } - } - - return swtch > 0 ? result : !result; -} - -swstate_t switches_states = 0; -int8_t getMovedSwitch() -{ - static tmr10ms_t s_move_last_time = 0; - int8_t result = 0; - -#if defined(PCBTARANIS) - for (uint8_t i=0; i> (i*2); - uint8_t next = (1024+getValue(MIXSRC_SA+i)) / 1024; - if (prev != next) { - switches_states = (switches_states & (~mask)) | (next << (i*2)); - if (i<5) - result = 1+(3*i)+next; - else if (i==5) - result = 1+(3*5)+(next!=0); - else if (i==6) - result = 1+(3*5)+2+next; - else - result = 1+(3*5)+2+3+(next!=0); - } - } -#else - // return delivers 1 to 3 for ID1 to ID3 - // 4..8 for all other switches if changed to true - // -4..-8 for all other switches if changed to false - // 9 for Trainer switch if changed to true; Change to false is ignored - swstate_t mask = 0x80; - for (uint8_t i=NUM_PSWITCH; i>1; i--) { - bool prev; - prev = (switches_states & mask); - // don't use getSwitch here to always get the proper value, even getSwitch manipulates - bool next = switchState((EnumKeys)(SW_BASE+i-1)); - if (prev != next) { - if (((i3)) || next==true) - result = next ? i : -i; - if (i<=3 && result==0) result = 1; - switches_states ^= mask; - } - mask >>= 1; - } -#endif - - if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10) - result = 0; - - s_move_last_time = get_tmr10ms(); - return result; -} - -void checkSwitches() -{ -#if defined(MODULE_ALWAYS_SEND_PULSES) - static swstate_t last_bad_switches = 0xff; -#else - swstate_t last_bad_switches = 0xff; -#endif - swstate_t states = g_model.switchWarningStates; - -#if defined(PCBTARANIS) - uint8_t bad_pots = 0, last_bad_pots = 0xff; -#endif - -#if !defined(MODULE_ALWAYS_SEND_PULSES) - while (1) { - -#if defined(TELEMETRY_MOD_14051) || defined(PCBTARANIS) - getADC(); -#endif -#endif // !defined(MODULE_ALWAYS_SEND_PULSES) - - getMovedSwitch(); - - bool warn = false; -#if defined(PCBTARANIS) - for (uint8_t i=0; i> 6; - if (potMode) { - evalFlightModeMixes(e_perout_mode_normal, 0); - bad_pots = 0; - for (uint8_t i=0; i> 4)) > 1)) { - warn = true; - bad_pots |= (1<> (i*2)]; - lcd_putcAtt(60+i*(2*FW+FW/2), 4*FH+3, 'A'+i, attr); - lcd_putcAtt(60+i*(2*FW+FW/2)+FW, 4*FH+3, c, attr); - } - } - if (potMode) { - for (uint8_t i=0; i> 4)) > 1) { - switch (i) { - case 0: - case 1: - case 2: - lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? 126 : 127); - break; - case 3: - case 4: - lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? '\300' : '\301'); - break; - } - flags = INVERS; - } - lcd_putsiAtt(60+i*(5*FW), 6*FH-2, STR_VSRCRAW, NUM_STICKS+1+i, flags); - } - } - } - last_bad_pots = bad_pots; -#else - if (last_bad_switches != switches_states) { - MESSAGE(STR_SWITCHWARN, NULL, STR_PRESSANYKEYTOSKIP, last_bad_switches == 0xff ? AU_SWITCH_ALERT : AU_NONE); - uint8_t x = 2; - for (uint8_t i=0; i0?(i+3):(states&0x3)+1), attr); - x += 3*FW+FW/2; - } -#endif - lcdRefresh(); - last_bad_switches = switches_states; - } - -#if defined(MODULE_ALWAYS_SEND_PULSES) - if (pwrCheck()==e_power_off || keyDown()) { - startupWarningState = STARTUP_WARNING_SWITCHES+1; - last_bad_switches = 0xff; - } -#else - if (pwrCheck()==e_power_off || keyDown()) return; - - checkBacklight(); - - wdt_reset(); - - SIMU_SLEEP(1); - } -#endif -} - - - -void lswTimerTick() { - for (uint8_t i=0; ifunc == LS_FUNC_TIMER) { - int16_t *lastValue = &lsLastValue[i]; - if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { - *lastValue = -lswTimerValue(ls->v1); - } - else if (*lastValue < 0) { - if (++(*lastValue) == 0) - *lastValue = lswTimerValue(ls->v2); - } - else { // if (*lastValue > 0) - *lastValue -= 1; - } - } - else if (ls->func == LS_FUNC_STICKY) { - PACK(typedef struct { - uint8_t state; - uint8_t last; - }) ls_sticky_struct; - ls_sticky_struct & lastValue = (ls_sticky_struct &)lsLastValue[i]; - bool before = lastValue.last & 0x01; - if (lastValue.state) { - bool now = getSwitch(ls->v2); - if (now != before) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 0; - } - } - } - else { - bool now = getSwitch(ls->v1); - if (before != now) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 1; - } - } - } - } -#if defined(CPUARM) - else if (ls->func == LS_FUNC_STAY) { - PACK(typedef struct { - uint16_t state:1; - uint16_t duration:15; - }) ls_stay_struct; - - ls_stay_struct & lastValue = (ls_stay_struct &)lsLastValue[i]; - lastValue.state = false; - bool state = getSwitch(ls->v1); - if (state) { - if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2)) - lastValue.state = true; - if (lastValue.duration < 1000) - lastValue.duration++; - } - else { - if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3)) - lastValue.state = true; - lastValue.duration = 0; - } - } -#endif - } -} - -LogicalSwitchData *lswAddress(uint8_t idx) -{ - return &g_model.logicalSw[idx]; -} - -uint8_t lswFamily(uint8_t func) -{ - if (func <= LS_FUNC_ANEG) - return LS_FAMILY_OFS; - else if (func <= LS_FUNC_XOR) - return LS_FAMILY_BOOL; -#if defined(CPUARM) - else if (func == LS_FUNC_STAY) - return LS_FAMILY_STAY; -#endif - else if (func <= LS_FUNC_LESS) - return LS_FAMILY_COMP; - else if (func <= LS_FUNC_ADIFFEGREATER) - return LS_FAMILY_DIFF; - else - return LS_FAMILY_TIMER+func-LS_FUNC_TIMER; -} - -int16_t lswTimerValue(delayval_t val) -{ - return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); -} - -void lswReset() { - for (uint8_t i=0; iv1 - MIXSRC_FIRST_TELEM + 1, ls->v2); -#else - if (lswFamily(ls->func)==LS_FAMILY_OFS) - val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2); - else - val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2) - convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128); -#endif - return val; -} - +/* + * Authors (alphabetical order) + * - Andre Bernet + * - Andreas Weitl + * - Bertrand Songis + * - Bryan J. Rentoul (Gruvin) + * - Cameron Weeks + * - Erez Raviv + * - Gabriel Birkus + * - Jean-Pierre Parisy + * - Karl Szmutny + * - Michael Blandford + * - Michal Hlavinka + * - Pat Mackenzie + * - Philip Moss + * - Rob Thomson + * - Romolo Manfredini + * - Thomas Husterer + * + * opentx is based on code named + * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, + * er9x by Erez Raviv: http://code.google.com/p/er9x/, + * and the original (and ongoing) project by + * Thomas Husterer, th9x: http://code.google.com/p/th9x/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include "opentx.h" + +#define CS_LAST_VALUE_INIT -32768 + +typedef struct { + uint8_t state:1; +#if defined(CPUARM) + uint8_t internalState:1; + uint32_t delay; + uint32_t duration; +#endif + int16_t lastValue; +} lsw_struct; + +typedef struct { + lsw_struct ls[NUM_LOGICAL_SWITCH]; +} lsw_array; + +lsw_array lswFm[MAX_FLIGHT_MODES]; + +#if defined(PCBTARANIS) +tmr10ms_t switchesMidposStart[6] = { 0 }; +uint32_t switchesPos = 0; +tmr10ms_t potsLastposStart[NUM_XPOTS]; +uint8_t potsPos[NUM_XPOTS]; + +uint32_t check2PosSwitchPosition(EnumKeys sw) +{ + uint32_t result; + uint32_t index; + + if (switchState(sw)) + index = sw - SW_SA0; + else + index = sw - SW_SA0 + 1; + + result = (1 << index); + + if (!(switchesPos & result)) { + PLAY_SWITCH_MOVED(index); + } + + return result; +} + +#define DELAY_SWITCH_3POS 15/*150ms*/ +uint32_t check3PosSwitchPosition(uint8_t idx, EnumKeys sw, bool startup) +{ + uint32_t result; + uint32_t index; + + if (switchState(sw)) { + index = sw - SW_SA0; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else if (switchState(EnumKeys(sw+2))) { + index = sw - SW_SA0 + 2; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else if (startup || (switchesPos & (1 << (sw - SW_SA0 + 1))) || (switchesMidposStart[idx] && (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) > DELAY_SWITCH_3POS)) { + index = sw - SW_SA0 + 1; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else { + index = sw - SW_SA0 + 1; + if (!switchesMidposStart[idx]) { + switchesMidposStart[idx] = get_tmr10ms(); + } + result = (switchesPos & (0x7 << (sw - SW_SA0))); + } + + if (!(switchesPos & result)) { + PLAY_SWITCH_MOVED(index); + } + + return result; +} + +#define CHECK_2POS(sw) newPos |= check2PosSwitchPosition(sw ## 0) +#define CHECK_3POS(idx, sw) newPos |= check3PosSwitchPosition(idx, sw ## 0, startup) + +void getSwitchesPosition(bool startup) +{ + uint32_t newPos = 0; + CHECK_3POS(0, SW_SA); + CHECK_3POS(1, SW_SB); + CHECK_3POS(2, SW_SC); + CHECK_3POS(3, SW_SD); + CHECK_3POS(4, SW_SE); + CHECK_2POS(SW_SF); + CHECK_3POS(5, SW_SG); + CHECK_2POS(SW_SH); + switchesPos = newPos; + + for (int i=0; icount>0 && calib->countcount); + uint8_t previousPos = potsPos[i] >> 4; + uint8_t previousStoredPos = potsPos[i] & 0x0F; + if (pos != previousPos) { + potsLastposStart[i] = get_tmr10ms(); + potsPos[i] = (pos << 4) | previousStoredPos; + } + else if (startup || (tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) > DELAY_SWITCH_3POS) { + potsLastposStart[i] = 0; + potsPos[i] = (pos << 4) | pos; + if (previousStoredPos != pos) { + PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH+i*XPOTS_MULTIPOS_COUNT+pos); + } + } + } + } + } +} +#define SWITCH_POSITION(sw) (switchesPos & (1<<(sw))) +#define POT_POSITION(sw) ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT)) +#else +#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx))) +#endif + +bool getSwitch(int8_t swtch) +{ + bool result; + + if (swtch == SWSRC_NONE) + return true; + + uint8_t cs_idx = abs(swtch); + + if (cs_idx == SWSRC_One) { + result = !s_mixer_first_run_done; + } + else if (cs_idx == SWSRC_ON) { + result = true; + } + else if (cs_idx <= SWSRC_LAST_SWITCH) { + result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH); +#if defined(MODULE_ALWAYS_SEND_PULSES) + if (startupWarningState < STARTUP_WARNING_DONE) { + // if throttle or switch warning is currently active, ignore actual stick position and use wanted values + if (cs_idx <= 3) { + if (!(g_model.nSwToWarn&1)) { // ID1 to ID3 is just one bit in nSwToWarn + result = (cs_idx)==((g_model.switchWarningStates&3)+1); // overwrite result with desired value + } + } + else if (!(g_model.nSwToWarn & (1<<(cs_idx-3)))) { + // current switch should not be ignored for warning + result = g_model.switchWarningStates & (1<<(cs_idx-2)); // overwrite result with desired value + } + } +#endif + } +#if defined(PCBTARANIS) + else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) { + result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH); + } +#endif + else if (cs_idx <= SWSRC_LAST_TRIM) { + uint8_t idx = cs_idx - SWSRC_FIRST_TRIM; + idx = (CONVERT_MODE(idx/2) << 1) + (idx & 1); + result = trimDown(idx); + } +#if ROTARY_ENCODERS > 0 + else if (cs_idx == SWSRC_REa) { + result = REA_DOWN(); + } +#endif +#if ROTARY_ENCODERS > 1 + else if (cs_idx == SWSRC_REb) { + result = REB_DOWN(); + } +#endif +#if defined(CPUARM) && defined(FLIGHT_MODES) + else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) { + uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE; + result = (idx == s_current_mixer_flight_mode); + } +#endif + else { + cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; + result = lswFm[s_current_mixer_flight_mode].ls[cs_idx].state; + } + + return swtch > 0 ? result : !result; +} + +/** + @brief Calculates new state of logical switches for s_current_mixer_flight_mode +*/ +void evalLogicalSwitches(uint8_t mode) +{ + for(unsigned int cs_idx=0; cs_idxandsw; +#else + uint8_t s = ls->andsw; + if (s > SWSRC_LAST_SWITCH) { + s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; + } +#endif + if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { + lsd->lastValue = CS_LAST_VALUE_INIT; + result = false; + } + else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { + bool res1 = getSwitch(ls->v1); + bool res2 = getSwitch(ls->v2); + switch (ls->func) { + case LS_FUNC_AND: + result = (res1 && res2); + break; + case LS_FUNC_OR: + result = (res1 || res2); + break; + // case LS_FUNC_XOR: + default: + result = (res1 ^ res2); + break; + } + } + else if (s == LS_FAMILY_TIMER) { + result = (lsd->lastValue <= 0); + } + else if (s == LS_FAMILY_STICKY) { + result = (lsd->lastValue & (1<<0)); + } +#if defined(CPUARM) + else if (s == LS_FAMILY_STAY) { + result = (lsd->lastValue & (1<<0)); + } +#endif + else { + getvalue_t x = getValue(ls->v1); + getvalue_t y; + if (s == LS_FAMILY_COMP) { + y = getValue(ls->v2); + + switch (ls->func) { + case LS_FUNC_EQUAL: + result = (x==y); + break; + case LS_FUNC_GREATER: + result = (x>y); + break; + default: + result = (xv1; +#if defined(FRSKY) + // Telemetry + if (v1 >= MIXSRC_FIRST_TELEM) { + if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) { + lsd->state = false; + continue; + } + + + y = convertLswTelemValue(ls); + +#if defined(FRSKY_HUB) && defined(GAUGES) + if (s == LS_FAMILY_OFS) { + uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; + if (idx < THLD_MAX) { + // Fill the threshold array + barsThresholds[idx] = 128 + ls->v2; + } + } +#endif + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; + } + else { + y = calc100toRESX(ls->v2); + } +#else + if (v1 >= MIXSRC_FIRST_TELEM) { + y = (int16_t)3 * (128+ls->v2); // it's a Timer + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; // it's a GVAR + } + else { + y = calc100toRESX(ls->v2); + } +#endif + + switch (ls->func) { +#if defined(CPUARM) + case LS_FUNC_VEQUAL: + result = (x==y); + break; +#endif + case LS_FUNC_VALMOSTEQUAL: +#if defined(GVARS) + if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) + result = (x==y); + else +#endif + result = (abs(x-y) < (1024 / STICK_TOLERANCE)); + break; + case LS_FUNC_VPOS: + result = (x>y); + break; + case LS_FUNC_VNEG: + result = (xy); + break; + case LS_FUNC_ANEG: + result = (abs(x)lastValue == CS_LAST_VALUE_INIT) + lsd->lastValue = x; + int16_t diff = x - lsd->lastValue; + if (ls->func == LS_FUNC_DIFFEGREATER) + result = (y >= 0 ? (diff >= y) : (diff <= y)); + else + result = (abs(diff) >= y); + if (result) + lsd->lastValue = x; + break; + } + } + } + } + +#if defined(CPUARM) + if (ls->delay) { + if (result) { + if (lsd->delay > get_tmr10ms()) + result = false; + } + else { + lsd->delay = get_tmr10ms() + (ls->delay*10); + } + } + + if (ls->duration) { + if (result && !lsd->internalState) { + lsd->duration = get_tmr10ms() + (ls->duration*10); + } + + lsd->internalState = result; + result = false; + + if (lsd->duration > get_tmr10ms()) { + result = true; + } + } +#endif + + if (result) { + if (!lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_ON(cs_idx); + } + else { + if (lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_OFF(cs_idx); + } + lsd->state = result; + } +} + +swstate_t switches_states = 0; +int8_t getMovedSwitch() +{ + static tmr10ms_t s_move_last_time = 0; + int8_t result = 0; + +#if defined(PCBTARANIS) + for (uint8_t i=0; i> (i*2); + uint8_t next = (1024+getValue(MIXSRC_SA+i)) / 1024; + if (prev != next) { + switches_states = (switches_states & (~mask)) | (next << (i*2)); + if (i<5) + result = 1+(3*i)+next; + else if (i==5) + result = 1+(3*5)+(next!=0); + else if (i==6) + result = 1+(3*5)+2+next; + else + result = 1+(3*5)+2+3+(next!=0); + } + } +#else + // return delivers 1 to 3 for ID1 to ID3 + // 4..8 for all other switches if changed to true + // -4..-8 for all other switches if changed to false + // 9 for Trainer switch if changed to true; Change to false is ignored + swstate_t mask = 0x80; + for (uint8_t i=NUM_PSWITCH; i>1; i--) { + bool prev; + prev = (switches_states & mask); + // don't use getSwitch here to always get the proper value, even getSwitch manipulates + bool next = switchState((EnumKeys)(SW_BASE+i-1)); + if (prev != next) { + if (((i3)) || next==true) + result = next ? i : -i; + if (i<=3 && result==0) result = 1; + switches_states ^= mask; + } + mask >>= 1; + } +#endif + + if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10) + result = 0; + + s_move_last_time = get_tmr10ms(); + return result; +} + +void checkSwitches() +{ +#if defined(MODULE_ALWAYS_SEND_PULSES) + static swstate_t last_bad_switches = 0xff; +#else + swstate_t last_bad_switches = 0xff; +#endif + swstate_t states = g_model.switchWarningStates; + +#if defined(PCBTARANIS) + uint8_t bad_pots = 0, last_bad_pots = 0xff; +#endif + +#if !defined(MODULE_ALWAYS_SEND_PULSES) + while (1) { + +#if defined(TELEMETRY_MOD_14051) || defined(PCBTARANIS) + getADC(); +#endif +#endif // !defined(MODULE_ALWAYS_SEND_PULSES) + + getMovedSwitch(); + + bool warn = false; +#if defined(PCBTARANIS) + for (uint8_t i=0; i> 6; + if (potMode) { + evalFlightModeMixes(e_perout_mode_normal, 0); + bad_pots = 0; + for (uint8_t i=0; i> 4)) > 1)) { + warn = true; + bad_pots |= (1<> (i*2)]; + lcd_putcAtt(60+i*(2*FW+FW/2), 4*FH+3, 'A'+i, attr); + lcd_putcAtt(60+i*(2*FW+FW/2)+FW, 4*FH+3, c, attr); + } + } + if (potMode) { + for (uint8_t i=0; i> 4)) > 1) { + switch (i) { + case 0: + case 1: + case 2: + lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? 126 : 127); + break; + case 3: + case 4: + lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? '\300' : '\301'); + break; + } + flags = INVERS; + } + lcd_putsiAtt(60+i*(5*FW), 6*FH-2, STR_VSRCRAW, NUM_STICKS+1+i, flags); + } + } + } + last_bad_pots = bad_pots; +#else + if (last_bad_switches != switches_states) { + MESSAGE(STR_SWITCHWARN, NULL, STR_PRESSANYKEYTOSKIP, last_bad_switches == 0xff ? AU_SWITCH_ALERT : AU_NONE); + uint8_t x = 2; + for (uint8_t i=0; i0?(i+3):(states&0x3)+1), attr); + x += 3*FW+FW/2; + } +#endif + lcdRefresh(); + last_bad_switches = switches_states; + } + +#if defined(MODULE_ALWAYS_SEND_PULSES) + if (pwrCheck()==e_power_off || keyDown()) { + startupWarningState = STARTUP_WARNING_SWITCHES+1; + last_bad_switches = 0xff; + } +#else + if (pwrCheck()==e_power_off || keyDown()) return; + + checkBacklight(); + + wdt_reset(); + + SIMU_SLEEP(1); + } +#endif +} + +void lswTimerTick() { + for (uint8_t fm=0; fmfunc == LS_FUNC_TIMER) { + int16_t *lastValue = &lsd->lastValue; + if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { + *lastValue = -lswTimerValue(ls->v1); + } + else if (*lastValue < 0) { + if (++(*lastValue) == 0) + *lastValue = lswTimerValue(ls->v2); + } + else { // if (*lastValue > 0) + *lastValue -= 1; + } + } + else if (ls->func == LS_FUNC_STICKY) { + PACK(typedef struct { + uint8_t state; + uint8_t last; + }) ls_sticky_struct; + ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;; + bool before = lastValue.last & 0x01; + if (lastValue.state) { + bool now = getSwitch(ls->v2); + if (now != before) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 0; + } + } + } + else { + bool now = getSwitch(ls->v1); + if (before != now) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 1; + } + } + } + } + #if defined(CPUARM) + else if (ls->func == LS_FUNC_STAY) { + PACK(typedef struct { + uint16_t state:1; + uint16_t duration:15; + }) ls_stay_struct; + + ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;; + lastValue.state = false; + bool state = getSwitch(ls->v1); + if (state) { + if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2)) + lastValue.state = true; + if (lastValue.duration < 1000) + lastValue.duration++; + } + else { + if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3)) + lastValue.state = true; + lastValue.duration = 0; + } + } + #endif + } + } +} + +LogicalSwitchData *lswAddress(uint8_t idx) +{ + return &g_model.logicalSw[idx]; +} + +uint8_t lswFamily(uint8_t func) +{ + if (func <= LS_FUNC_ANEG) + return LS_FAMILY_OFS; + else if (func <= LS_FUNC_XOR) + return LS_FAMILY_BOOL; +#if defined(CPUARM) + else if (func == LS_FUNC_STAY) + return LS_FAMILY_STAY; +#endif + else if (func <= LS_FUNC_LESS) + return LS_FAMILY_COMP; + else if (func <= LS_FUNC_ADIFFEGREATER) + return LS_FAMILY_DIFF; + else + return LS_FAMILY_TIMER+func-LS_FUNC_TIMER; +} + +int16_t lswTimerValue(delayval_t val) +{ + return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); +} + +void lswReset() { + for (uint8_t fm=0; fmls[i].lastValue = CS_LAST_VALUE_INIT; + //todo other values + lsa->ls[i].state = false; + } + } +} + +getvalue_t convertLswTelemValue(LogicalSwitchData * ls) +{ + getvalue_t val; +#if defined(CPUARM) + val = convert16bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, ls->v2); +#else + if (lswFamily(ls->func)==LS_FAMILY_OFS) + val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2); + else + val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2) - convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128); +#endif + return val; +} + +void copyLswState(uint8_t oldPhase, uint8_t newPhase) { + for (uint8_t i=0; i Date: Sat, 17 May 2014 18:23:36 +0200 Subject: [PATCH 02/18] CRLF --- radio/src/switches.cpp | 1468 ++++++++++++++++++++-------------------- 1 file changed, 734 insertions(+), 734 deletions(-) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 7b564f0ce..dec33f1b3 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -1,734 +1,734 @@ -/* - * Authors (alphabetical order) - * - Andre Bernet - * - Andreas Weitl - * - Bertrand Songis - * - Bryan J. Rentoul (Gruvin) - * - Cameron Weeks - * - Erez Raviv - * - Gabriel Birkus - * - Jean-Pierre Parisy - * - Karl Szmutny - * - Michael Blandford - * - Michal Hlavinka - * - Pat Mackenzie - * - Philip Moss - * - Rob Thomson - * - Romolo Manfredini - * - Thomas Husterer - * - * opentx is based on code named - * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, - * er9x by Erez Raviv: http://code.google.com/p/er9x/, - * and the original (and ongoing) project by - * Thomas Husterer, th9x: http://code.google.com/p/th9x/ - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 2 as - * published by the Free Software Foundation. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - */ - -#include "opentx.h" - -#define CS_LAST_VALUE_INIT -32768 - -typedef struct { - uint8_t state:1; -#if defined(CPUARM) - uint8_t internalState:1; - uint32_t delay; - uint32_t duration; -#endif - int16_t lastValue; -} lsw_struct; - -typedef struct { - lsw_struct ls[NUM_LOGICAL_SWITCH]; -} lsw_array; - -lsw_array lswFm[MAX_FLIGHT_MODES]; - -#if defined(PCBTARANIS) -tmr10ms_t switchesMidposStart[6] = { 0 }; -uint32_t switchesPos = 0; -tmr10ms_t potsLastposStart[NUM_XPOTS]; -uint8_t potsPos[NUM_XPOTS]; - -uint32_t check2PosSwitchPosition(EnumKeys sw) -{ - uint32_t result; - uint32_t index; - - if (switchState(sw)) - index = sw - SW_SA0; - else - index = sw - SW_SA0 + 1; - - result = (1 << index); - - if (!(switchesPos & result)) { - PLAY_SWITCH_MOVED(index); - } - - return result; -} - -#define DELAY_SWITCH_3POS 15/*150ms*/ -uint32_t check3PosSwitchPosition(uint8_t idx, EnumKeys sw, bool startup) -{ - uint32_t result; - uint32_t index; - - if (switchState(sw)) { - index = sw - SW_SA0; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else if (switchState(EnumKeys(sw+2))) { - index = sw - SW_SA0 + 2; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else if (startup || (switchesPos & (1 << (sw - SW_SA0 + 1))) || (switchesMidposStart[idx] && (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) > DELAY_SWITCH_3POS)) { - index = sw - SW_SA0 + 1; - result = (1 << index); - switchesMidposStart[idx] = 0; - } - else { - index = sw - SW_SA0 + 1; - if (!switchesMidposStart[idx]) { - switchesMidposStart[idx] = get_tmr10ms(); - } - result = (switchesPos & (0x7 << (sw - SW_SA0))); - } - - if (!(switchesPos & result)) { - PLAY_SWITCH_MOVED(index); - } - - return result; -} - -#define CHECK_2POS(sw) newPos |= check2PosSwitchPosition(sw ## 0) -#define CHECK_3POS(idx, sw) newPos |= check3PosSwitchPosition(idx, sw ## 0, startup) - -void getSwitchesPosition(bool startup) -{ - uint32_t newPos = 0; - CHECK_3POS(0, SW_SA); - CHECK_3POS(1, SW_SB); - CHECK_3POS(2, SW_SC); - CHECK_3POS(3, SW_SD); - CHECK_3POS(4, SW_SE); - CHECK_2POS(SW_SF); - CHECK_3POS(5, SW_SG); - CHECK_2POS(SW_SH); - switchesPos = newPos; - - for (int i=0; icount>0 && calib->countcount); - uint8_t previousPos = potsPos[i] >> 4; - uint8_t previousStoredPos = potsPos[i] & 0x0F; - if (pos != previousPos) { - potsLastposStart[i] = get_tmr10ms(); - potsPos[i] = (pos << 4) | previousStoredPos; - } - else if (startup || (tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) > DELAY_SWITCH_3POS) { - potsLastposStart[i] = 0; - potsPos[i] = (pos << 4) | pos; - if (previousStoredPos != pos) { - PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH+i*XPOTS_MULTIPOS_COUNT+pos); - } - } - } - } - } -} -#define SWITCH_POSITION(sw) (switchesPos & (1<<(sw))) -#define POT_POSITION(sw) ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT)) -#else -#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx))) -#endif - -bool getSwitch(int8_t swtch) -{ - bool result; - - if (swtch == SWSRC_NONE) - return true; - - uint8_t cs_idx = abs(swtch); - - if (cs_idx == SWSRC_One) { - result = !s_mixer_first_run_done; - } - else if (cs_idx == SWSRC_ON) { - result = true; - } - else if (cs_idx <= SWSRC_LAST_SWITCH) { - result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH); -#if defined(MODULE_ALWAYS_SEND_PULSES) - if (startupWarningState < STARTUP_WARNING_DONE) { - // if throttle or switch warning is currently active, ignore actual stick position and use wanted values - if (cs_idx <= 3) { - if (!(g_model.nSwToWarn&1)) { // ID1 to ID3 is just one bit in nSwToWarn - result = (cs_idx)==((g_model.switchWarningStates&3)+1); // overwrite result with desired value - } - } - else if (!(g_model.nSwToWarn & (1<<(cs_idx-3)))) { - // current switch should not be ignored for warning - result = g_model.switchWarningStates & (1<<(cs_idx-2)); // overwrite result with desired value - } - } -#endif - } -#if defined(PCBTARANIS) - else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) { - result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH); - } -#endif - else if (cs_idx <= SWSRC_LAST_TRIM) { - uint8_t idx = cs_idx - SWSRC_FIRST_TRIM; - idx = (CONVERT_MODE(idx/2) << 1) + (idx & 1); - result = trimDown(idx); - } -#if ROTARY_ENCODERS > 0 - else if (cs_idx == SWSRC_REa) { - result = REA_DOWN(); - } -#endif -#if ROTARY_ENCODERS > 1 - else if (cs_idx == SWSRC_REb) { - result = REB_DOWN(); - } -#endif -#if defined(CPUARM) && defined(FLIGHT_MODES) - else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) { - uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE; - result = (idx == s_current_mixer_flight_mode); - } -#endif - else { - cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; - result = lswFm[s_current_mixer_flight_mode].ls[cs_idx].state; - } - - return swtch > 0 ? result : !result; -} - -/** - @brief Calculates new state of logical switches for s_current_mixer_flight_mode -*/ -void evalLogicalSwitches(uint8_t mode) -{ - for(unsigned int cs_idx=0; cs_idxandsw; -#else - uint8_t s = ls->andsw; - if (s > SWSRC_LAST_SWITCH) { - s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; - } -#endif - if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { - lsd->lastValue = CS_LAST_VALUE_INIT; - result = false; - } - else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { - bool res1 = getSwitch(ls->v1); - bool res2 = getSwitch(ls->v2); - switch (ls->func) { - case LS_FUNC_AND: - result = (res1 && res2); - break; - case LS_FUNC_OR: - result = (res1 || res2); - break; - // case LS_FUNC_XOR: - default: - result = (res1 ^ res2); - break; - } - } - else if (s == LS_FAMILY_TIMER) { - result = (lsd->lastValue <= 0); - } - else if (s == LS_FAMILY_STICKY) { - result = (lsd->lastValue & (1<<0)); - } -#if defined(CPUARM) - else if (s == LS_FAMILY_STAY) { - result = (lsd->lastValue & (1<<0)); - } -#endif - else { - getvalue_t x = getValue(ls->v1); - getvalue_t y; - if (s == LS_FAMILY_COMP) { - y = getValue(ls->v2); - - switch (ls->func) { - case LS_FUNC_EQUAL: - result = (x==y); - break; - case LS_FUNC_GREATER: - result = (x>y); - break; - default: - result = (xv1; -#if defined(FRSKY) - // Telemetry - if (v1 >= MIXSRC_FIRST_TELEM) { - if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) { - lsd->state = false; - continue; - } - - - y = convertLswTelemValue(ls); - -#if defined(FRSKY_HUB) && defined(GAUGES) - if (s == LS_FAMILY_OFS) { - uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; - if (idx < THLD_MAX) { - // Fill the threshold array - barsThresholds[idx] = 128 + ls->v2; - } - } -#endif - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; - } - else { - y = calc100toRESX(ls->v2); - } -#else - if (v1 >= MIXSRC_FIRST_TELEM) { - y = (int16_t)3 * (128+ls->v2); // it's a Timer - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; // it's a GVAR - } - else { - y = calc100toRESX(ls->v2); - } -#endif - - switch (ls->func) { -#if defined(CPUARM) - case LS_FUNC_VEQUAL: - result = (x==y); - break; -#endif - case LS_FUNC_VALMOSTEQUAL: -#if defined(GVARS) - if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) - result = (x==y); - else -#endif - result = (abs(x-y) < (1024 / STICK_TOLERANCE)); - break; - case LS_FUNC_VPOS: - result = (x>y); - break; - case LS_FUNC_VNEG: - result = (xy); - break; - case LS_FUNC_ANEG: - result = (abs(x)lastValue == CS_LAST_VALUE_INIT) - lsd->lastValue = x; - int16_t diff = x - lsd->lastValue; - if (ls->func == LS_FUNC_DIFFEGREATER) - result = (y >= 0 ? (diff >= y) : (diff <= y)); - else - result = (abs(diff) >= y); - if (result) - lsd->lastValue = x; - break; - } - } - } - } - -#if defined(CPUARM) - if (ls->delay) { - if (result) { - if (lsd->delay > get_tmr10ms()) - result = false; - } - else { - lsd->delay = get_tmr10ms() + (ls->delay*10); - } - } - - if (ls->duration) { - if (result && !lsd->internalState) { - lsd->duration = get_tmr10ms() + (ls->duration*10); - } - - lsd->internalState = result; - result = false; - - if (lsd->duration > get_tmr10ms()) { - result = true; - } - } -#endif - - if (result) { - if (!lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_ON(cs_idx); - } - else { - if (lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_OFF(cs_idx); - } - lsd->state = result; - } -} - -swstate_t switches_states = 0; -int8_t getMovedSwitch() -{ - static tmr10ms_t s_move_last_time = 0; - int8_t result = 0; - -#if defined(PCBTARANIS) - for (uint8_t i=0; i> (i*2); - uint8_t next = (1024+getValue(MIXSRC_SA+i)) / 1024; - if (prev != next) { - switches_states = (switches_states & (~mask)) | (next << (i*2)); - if (i<5) - result = 1+(3*i)+next; - else if (i==5) - result = 1+(3*5)+(next!=0); - else if (i==6) - result = 1+(3*5)+2+next; - else - result = 1+(3*5)+2+3+(next!=0); - } - } -#else - // return delivers 1 to 3 for ID1 to ID3 - // 4..8 for all other switches if changed to true - // -4..-8 for all other switches if changed to false - // 9 for Trainer switch if changed to true; Change to false is ignored - swstate_t mask = 0x80; - for (uint8_t i=NUM_PSWITCH; i>1; i--) { - bool prev; - prev = (switches_states & mask); - // don't use getSwitch here to always get the proper value, even getSwitch manipulates - bool next = switchState((EnumKeys)(SW_BASE+i-1)); - if (prev != next) { - if (((i3)) || next==true) - result = next ? i : -i; - if (i<=3 && result==0) result = 1; - switches_states ^= mask; - } - mask >>= 1; - } -#endif - - if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10) - result = 0; - - s_move_last_time = get_tmr10ms(); - return result; -} - -void checkSwitches() -{ -#if defined(MODULE_ALWAYS_SEND_PULSES) - static swstate_t last_bad_switches = 0xff; -#else - swstate_t last_bad_switches = 0xff; -#endif - swstate_t states = g_model.switchWarningStates; - -#if defined(PCBTARANIS) - uint8_t bad_pots = 0, last_bad_pots = 0xff; -#endif - -#if !defined(MODULE_ALWAYS_SEND_PULSES) - while (1) { - -#if defined(TELEMETRY_MOD_14051) || defined(PCBTARANIS) - getADC(); -#endif -#endif // !defined(MODULE_ALWAYS_SEND_PULSES) - - getMovedSwitch(); - - bool warn = false; -#if defined(PCBTARANIS) - for (uint8_t i=0; i> 6; - if (potMode) { - evalFlightModeMixes(e_perout_mode_normal, 0); - bad_pots = 0; - for (uint8_t i=0; i> 4)) > 1)) { - warn = true; - bad_pots |= (1<> (i*2)]; - lcd_putcAtt(60+i*(2*FW+FW/2), 4*FH+3, 'A'+i, attr); - lcd_putcAtt(60+i*(2*FW+FW/2)+FW, 4*FH+3, c, attr); - } - } - if (potMode) { - for (uint8_t i=0; i> 4)) > 1) { - switch (i) { - case 0: - case 1: - case 2: - lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? 126 : 127); - break; - case 3: - case 4: - lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? '\300' : '\301'); - break; - } - flags = INVERS; - } - lcd_putsiAtt(60+i*(5*FW), 6*FH-2, STR_VSRCRAW, NUM_STICKS+1+i, flags); - } - } - } - last_bad_pots = bad_pots; -#else - if (last_bad_switches != switches_states) { - MESSAGE(STR_SWITCHWARN, NULL, STR_PRESSANYKEYTOSKIP, last_bad_switches == 0xff ? AU_SWITCH_ALERT : AU_NONE); - uint8_t x = 2; - for (uint8_t i=0; i0?(i+3):(states&0x3)+1), attr); - x += 3*FW+FW/2; - } -#endif - lcdRefresh(); - last_bad_switches = switches_states; - } - -#if defined(MODULE_ALWAYS_SEND_PULSES) - if (pwrCheck()==e_power_off || keyDown()) { - startupWarningState = STARTUP_WARNING_SWITCHES+1; - last_bad_switches = 0xff; - } -#else - if (pwrCheck()==e_power_off || keyDown()) return; - - checkBacklight(); - - wdt_reset(); - - SIMU_SLEEP(1); - } -#endif -} - -void lswTimerTick() { - for (uint8_t fm=0; fmfunc == LS_FUNC_TIMER) { - int16_t *lastValue = &lsd->lastValue; - if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { - *lastValue = -lswTimerValue(ls->v1); - } - else if (*lastValue < 0) { - if (++(*lastValue) == 0) - *lastValue = lswTimerValue(ls->v2); - } - else { // if (*lastValue > 0) - *lastValue -= 1; - } - } - else if (ls->func == LS_FUNC_STICKY) { - PACK(typedef struct { - uint8_t state; - uint8_t last; - }) ls_sticky_struct; - ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;; - bool before = lastValue.last & 0x01; - if (lastValue.state) { - bool now = getSwitch(ls->v2); - if (now != before) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 0; - } - } - } - else { - bool now = getSwitch(ls->v1); - if (before != now) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 1; - } - } - } - } - #if defined(CPUARM) - else if (ls->func == LS_FUNC_STAY) { - PACK(typedef struct { - uint16_t state:1; - uint16_t duration:15; - }) ls_stay_struct; - - ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;; - lastValue.state = false; - bool state = getSwitch(ls->v1); - if (state) { - if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2)) - lastValue.state = true; - if (lastValue.duration < 1000) - lastValue.duration++; - } - else { - if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3)) - lastValue.state = true; - lastValue.duration = 0; - } - } - #endif - } - } -} - -LogicalSwitchData *lswAddress(uint8_t idx) -{ - return &g_model.logicalSw[idx]; -} - -uint8_t lswFamily(uint8_t func) -{ - if (func <= LS_FUNC_ANEG) - return LS_FAMILY_OFS; - else if (func <= LS_FUNC_XOR) - return LS_FAMILY_BOOL; -#if defined(CPUARM) - else if (func == LS_FUNC_STAY) - return LS_FAMILY_STAY; -#endif - else if (func <= LS_FUNC_LESS) - return LS_FAMILY_COMP; - else if (func <= LS_FUNC_ADIFFEGREATER) - return LS_FAMILY_DIFF; - else - return LS_FAMILY_TIMER+func-LS_FUNC_TIMER; -} - -int16_t lswTimerValue(delayval_t val) -{ - return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); -} - -void lswReset() { - for (uint8_t fm=0; fmls[i].lastValue = CS_LAST_VALUE_INIT; - //todo other values - lsa->ls[i].state = false; - } - } -} - -getvalue_t convertLswTelemValue(LogicalSwitchData * ls) -{ - getvalue_t val; -#if defined(CPUARM) - val = convert16bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, ls->v2); -#else - if (lswFamily(ls->func)==LS_FAMILY_OFS) - val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2); - else - val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2) - convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128); -#endif - return val; -} - -void copyLswState(uint8_t oldPhase, uint8_t newPhase) { - for (uint8_t i=0; i + * - Andreas Weitl + * - Bertrand Songis + * - Bryan J. Rentoul (Gruvin) + * - Cameron Weeks + * - Erez Raviv + * - Gabriel Birkus + * - Jean-Pierre Parisy + * - Karl Szmutny + * - Michael Blandford + * - Michal Hlavinka + * - Pat Mackenzie + * - Philip Moss + * - Rob Thomson + * - Romolo Manfredini + * - Thomas Husterer + * + * opentx is based on code named + * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, + * er9x by Erez Raviv: http://code.google.com/p/er9x/, + * and the original (and ongoing) project by + * Thomas Husterer, th9x: http://code.google.com/p/th9x/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include "opentx.h" + +#define CS_LAST_VALUE_INIT -32768 + +typedef struct { + uint8_t state:1; +#if defined(CPUARM) + uint8_t internalState:1; + uint32_t delay; + uint32_t duration; +#endif + int16_t lastValue; +} lsw_struct; + +typedef struct { + lsw_struct ls[NUM_LOGICAL_SWITCH]; +} lsw_array; + +lsw_array lswFm[MAX_FLIGHT_MODES]; + +#if defined(PCBTARANIS) +tmr10ms_t switchesMidposStart[6] = { 0 }; +uint32_t switchesPos = 0; +tmr10ms_t potsLastposStart[NUM_XPOTS]; +uint8_t potsPos[NUM_XPOTS]; + +uint32_t check2PosSwitchPosition(EnumKeys sw) +{ + uint32_t result; + uint32_t index; + + if (switchState(sw)) + index = sw - SW_SA0; + else + index = sw - SW_SA0 + 1; + + result = (1 << index); + + if (!(switchesPos & result)) { + PLAY_SWITCH_MOVED(index); + } + + return result; +} + +#define DELAY_SWITCH_3POS 15/*150ms*/ +uint32_t check3PosSwitchPosition(uint8_t idx, EnumKeys sw, bool startup) +{ + uint32_t result; + uint32_t index; + + if (switchState(sw)) { + index = sw - SW_SA0; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else if (switchState(EnumKeys(sw+2))) { + index = sw - SW_SA0 + 2; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else if (startup || (switchesPos & (1 << (sw - SW_SA0 + 1))) || (switchesMidposStart[idx] && (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) > DELAY_SWITCH_3POS)) { + index = sw - SW_SA0 + 1; + result = (1 << index); + switchesMidposStart[idx] = 0; + } + else { + index = sw - SW_SA0 + 1; + if (!switchesMidposStart[idx]) { + switchesMidposStart[idx] = get_tmr10ms(); + } + result = (switchesPos & (0x7 << (sw - SW_SA0))); + } + + if (!(switchesPos & result)) { + PLAY_SWITCH_MOVED(index); + } + + return result; +} + +#define CHECK_2POS(sw) newPos |= check2PosSwitchPosition(sw ## 0) +#define CHECK_3POS(idx, sw) newPos |= check3PosSwitchPosition(idx, sw ## 0, startup) + +void getSwitchesPosition(bool startup) +{ + uint32_t newPos = 0; + CHECK_3POS(0, SW_SA); + CHECK_3POS(1, SW_SB); + CHECK_3POS(2, SW_SC); + CHECK_3POS(3, SW_SD); + CHECK_3POS(4, SW_SE); + CHECK_2POS(SW_SF); + CHECK_3POS(5, SW_SG); + CHECK_2POS(SW_SH); + switchesPos = newPos; + + for (int i=0; icount>0 && calib->countcount); + uint8_t previousPos = potsPos[i] >> 4; + uint8_t previousStoredPos = potsPos[i] & 0x0F; + if (pos != previousPos) { + potsLastposStart[i] = get_tmr10ms(); + potsPos[i] = (pos << 4) | previousStoredPos; + } + else if (startup || (tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) > DELAY_SWITCH_3POS) { + potsLastposStart[i] = 0; + potsPos[i] = (pos << 4) | pos; + if (previousStoredPos != pos) { + PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH+i*XPOTS_MULTIPOS_COUNT+pos); + } + } + } + } + } +} +#define SWITCH_POSITION(sw) (switchesPos & (1<<(sw))) +#define POT_POSITION(sw) ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT)) +#else +#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx))) +#endif + +bool getSwitch(int8_t swtch) +{ + bool result; + + if (swtch == SWSRC_NONE) + return true; + + uint8_t cs_idx = abs(swtch); + + if (cs_idx == SWSRC_One) { + result = !s_mixer_first_run_done; + } + else if (cs_idx == SWSRC_ON) { + result = true; + } + else if (cs_idx <= SWSRC_LAST_SWITCH) { + result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH); +#if defined(MODULE_ALWAYS_SEND_PULSES) + if (startupWarningState < STARTUP_WARNING_DONE) { + // if throttle or switch warning is currently active, ignore actual stick position and use wanted values + if (cs_idx <= 3) { + if (!(g_model.nSwToWarn&1)) { // ID1 to ID3 is just one bit in nSwToWarn + result = (cs_idx)==((g_model.switchWarningStates&3)+1); // overwrite result with desired value + } + } + else if (!(g_model.nSwToWarn & (1<<(cs_idx-3)))) { + // current switch should not be ignored for warning + result = g_model.switchWarningStates & (1<<(cs_idx-2)); // overwrite result with desired value + } + } +#endif + } +#if defined(PCBTARANIS) + else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) { + result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH); + } +#endif + else if (cs_idx <= SWSRC_LAST_TRIM) { + uint8_t idx = cs_idx - SWSRC_FIRST_TRIM; + idx = (CONVERT_MODE(idx/2) << 1) + (idx & 1); + result = trimDown(idx); + } +#if ROTARY_ENCODERS > 0 + else if (cs_idx == SWSRC_REa) { + result = REA_DOWN(); + } +#endif +#if ROTARY_ENCODERS > 1 + else if (cs_idx == SWSRC_REb) { + result = REB_DOWN(); + } +#endif +#if defined(CPUARM) && defined(FLIGHT_MODES) + else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) { + uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE; + result = (idx == s_current_mixer_flight_mode); + } +#endif + else { + cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; + result = lswFm[s_current_mixer_flight_mode].ls[cs_idx].state; + } + + return swtch > 0 ? result : !result; +} + +/** + @brief Calculates new state of logical switches for s_current_mixer_flight_mode +*/ +void evalLogicalSwitches(uint8_t mode) +{ + for(unsigned int cs_idx=0; cs_idxandsw; +#else + uint8_t s = ls->andsw; + if (s > SWSRC_LAST_SWITCH) { + s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; + } +#endif + if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { + lsd->lastValue = CS_LAST_VALUE_INIT; + result = false; + } + else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { + bool res1 = getSwitch(ls->v1); + bool res2 = getSwitch(ls->v2); + switch (ls->func) { + case LS_FUNC_AND: + result = (res1 && res2); + break; + case LS_FUNC_OR: + result = (res1 || res2); + break; + // case LS_FUNC_XOR: + default: + result = (res1 ^ res2); + break; + } + } + else if (s == LS_FAMILY_TIMER) { + result = (lsd->lastValue <= 0); + } + else if (s == LS_FAMILY_STICKY) { + result = (lsd->lastValue & (1<<0)); + } +#if defined(CPUARM) + else if (s == LS_FAMILY_STAY) { + result = (lsd->lastValue & (1<<0)); + } +#endif + else { + getvalue_t x = getValue(ls->v1); + getvalue_t y; + if (s == LS_FAMILY_COMP) { + y = getValue(ls->v2); + + switch (ls->func) { + case LS_FUNC_EQUAL: + result = (x==y); + break; + case LS_FUNC_GREATER: + result = (x>y); + break; + default: + result = (xv1; +#if defined(FRSKY) + // Telemetry + if (v1 >= MIXSRC_FIRST_TELEM) { + if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) { + lsd->state = false; + continue; + } + + + y = convertLswTelemValue(ls); + +#if defined(FRSKY_HUB) && defined(GAUGES) + if (s == LS_FAMILY_OFS) { + uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; + if (idx < THLD_MAX) { + // Fill the threshold array + barsThresholds[idx] = 128 + ls->v2; + } + } +#endif + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; + } + else { + y = calc100toRESX(ls->v2); + } +#else + if (v1 >= MIXSRC_FIRST_TELEM) { + y = (int16_t)3 * (128+ls->v2); // it's a Timer + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; // it's a GVAR + } + else { + y = calc100toRESX(ls->v2); + } +#endif + + switch (ls->func) { +#if defined(CPUARM) + case LS_FUNC_VEQUAL: + result = (x==y); + break; +#endif + case LS_FUNC_VALMOSTEQUAL: +#if defined(GVARS) + if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) + result = (x==y); + else +#endif + result = (abs(x-y) < (1024 / STICK_TOLERANCE)); + break; + case LS_FUNC_VPOS: + result = (x>y); + break; + case LS_FUNC_VNEG: + result = (xy); + break; + case LS_FUNC_ANEG: + result = (abs(x)lastValue == CS_LAST_VALUE_INIT) + lsd->lastValue = x; + int16_t diff = x - lsd->lastValue; + if (ls->func == LS_FUNC_DIFFEGREATER) + result = (y >= 0 ? (diff >= y) : (diff <= y)); + else + result = (abs(diff) >= y); + if (result) + lsd->lastValue = x; + break; + } + } + } + } + +#if defined(CPUARM) + if (ls->delay) { + if (result) { + if (lsd->delay > get_tmr10ms()) + result = false; + } + else { + lsd->delay = get_tmr10ms() + (ls->delay*10); + } + } + + if (ls->duration) { + if (result && !lsd->internalState) { + lsd->duration = get_tmr10ms() + (ls->duration*10); + } + + lsd->internalState = result; + result = false; + + if (lsd->duration > get_tmr10ms()) { + result = true; + } + } +#endif + + if (result) { + if (!lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_ON(cs_idx); + } + else { + if (lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_OFF(cs_idx); + } + lsd->state = result; + } +} + +swstate_t switches_states = 0; +int8_t getMovedSwitch() +{ + static tmr10ms_t s_move_last_time = 0; + int8_t result = 0; + +#if defined(PCBTARANIS) + for (uint8_t i=0; i> (i*2); + uint8_t next = (1024+getValue(MIXSRC_SA+i)) / 1024; + if (prev != next) { + switches_states = (switches_states & (~mask)) | (next << (i*2)); + if (i<5) + result = 1+(3*i)+next; + else if (i==5) + result = 1+(3*5)+(next!=0); + else if (i==6) + result = 1+(3*5)+2+next; + else + result = 1+(3*5)+2+3+(next!=0); + } + } +#else + // return delivers 1 to 3 for ID1 to ID3 + // 4..8 for all other switches if changed to true + // -4..-8 for all other switches if changed to false + // 9 for Trainer switch if changed to true; Change to false is ignored + swstate_t mask = 0x80; + for (uint8_t i=NUM_PSWITCH; i>1; i--) { + bool prev; + prev = (switches_states & mask); + // don't use getSwitch here to always get the proper value, even getSwitch manipulates + bool next = switchState((EnumKeys)(SW_BASE+i-1)); + if (prev != next) { + if (((i3)) || next==true) + result = next ? i : -i; + if (i<=3 && result==0) result = 1; + switches_states ^= mask; + } + mask >>= 1; + } +#endif + + if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10) + result = 0; + + s_move_last_time = get_tmr10ms(); + return result; +} + +void checkSwitches() +{ +#if defined(MODULE_ALWAYS_SEND_PULSES) + static swstate_t last_bad_switches = 0xff; +#else + swstate_t last_bad_switches = 0xff; +#endif + swstate_t states = g_model.switchWarningStates; + +#if defined(PCBTARANIS) + uint8_t bad_pots = 0, last_bad_pots = 0xff; +#endif + +#if !defined(MODULE_ALWAYS_SEND_PULSES) + while (1) { + +#if defined(TELEMETRY_MOD_14051) || defined(PCBTARANIS) + getADC(); +#endif +#endif // !defined(MODULE_ALWAYS_SEND_PULSES) + + getMovedSwitch(); + + bool warn = false; +#if defined(PCBTARANIS) + for (uint8_t i=0; i> 6; + if (potMode) { + evalFlightModeMixes(e_perout_mode_normal, 0); + bad_pots = 0; + for (uint8_t i=0; i> 4)) > 1)) { + warn = true; + bad_pots |= (1<> (i*2)]; + lcd_putcAtt(60+i*(2*FW+FW/2), 4*FH+3, 'A'+i, attr); + lcd_putcAtt(60+i*(2*FW+FW/2)+FW, 4*FH+3, c, attr); + } + } + if (potMode) { + for (uint8_t i=0; i> 4)) > 1) { + switch (i) { + case 0: + case 1: + case 2: + lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? 126 : 127); + break; + case 3: + case 4: + lcd_putc(60+i*(5*FW)+2*FW+2, 6*FH-2, g_model.potPosition[i] > (getValue(MIXSRC_FIRST_POT+i) >> 4) ? '\300' : '\301'); + break; + } + flags = INVERS; + } + lcd_putsiAtt(60+i*(5*FW), 6*FH-2, STR_VSRCRAW, NUM_STICKS+1+i, flags); + } + } + } + last_bad_pots = bad_pots; +#else + if (last_bad_switches != switches_states) { + MESSAGE(STR_SWITCHWARN, NULL, STR_PRESSANYKEYTOSKIP, last_bad_switches == 0xff ? AU_SWITCH_ALERT : AU_NONE); + uint8_t x = 2; + for (uint8_t i=0; i0?(i+3):(states&0x3)+1), attr); + x += 3*FW+FW/2; + } +#endif + lcdRefresh(); + last_bad_switches = switches_states; + } + +#if defined(MODULE_ALWAYS_SEND_PULSES) + if (pwrCheck()==e_power_off || keyDown()) { + startupWarningState = STARTUP_WARNING_SWITCHES+1; + last_bad_switches = 0xff; + } +#else + if (pwrCheck()==e_power_off || keyDown()) return; + + checkBacklight(); + + wdt_reset(); + + SIMU_SLEEP(1); + } +#endif +} + +void lswTimerTick() { + for (uint8_t fm=0; fmfunc == LS_FUNC_TIMER) { + int16_t *lastValue = &lsd->lastValue; + if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { + *lastValue = -lswTimerValue(ls->v1); + } + else if (*lastValue < 0) { + if (++(*lastValue) == 0) + *lastValue = lswTimerValue(ls->v2); + } + else { // if (*lastValue > 0) + *lastValue -= 1; + } + } + else if (ls->func == LS_FUNC_STICKY) { + PACK(typedef struct { + uint8_t state; + uint8_t last; + }) ls_sticky_struct; + ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;; + bool before = lastValue.last & 0x01; + if (lastValue.state) { + bool now = getSwitch(ls->v2); + if (now != before) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 0; + } + } + } + else { + bool now = getSwitch(ls->v1); + if (before != now) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 1; + } + } + } + } + #if defined(CPUARM) + else if (ls->func == LS_FUNC_STAY) { + PACK(typedef struct { + uint16_t state:1; + uint16_t duration:15; + }) ls_stay_struct; + + ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;; + lastValue.state = false; + bool state = getSwitch(ls->v1); + if (state) { + if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2)) + lastValue.state = true; + if (lastValue.duration < 1000) + lastValue.duration++; + } + else { + if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3)) + lastValue.state = true; + lastValue.duration = 0; + } + } + #endif + } + } +} + +LogicalSwitchData *lswAddress(uint8_t idx) +{ + return &g_model.logicalSw[idx]; +} + +uint8_t lswFamily(uint8_t func) +{ + if (func <= LS_FUNC_ANEG) + return LS_FAMILY_OFS; + else if (func <= LS_FUNC_XOR) + return LS_FAMILY_BOOL; +#if defined(CPUARM) + else if (func == LS_FUNC_STAY) + return LS_FAMILY_STAY; +#endif + else if (func <= LS_FUNC_LESS) + return LS_FAMILY_COMP; + else if (func <= LS_FUNC_ADIFFEGREATER) + return LS_FAMILY_DIFF; + else + return LS_FAMILY_TIMER+func-LS_FUNC_TIMER; +} + +int16_t lswTimerValue(delayval_t val) +{ + return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); +} + +void lswReset() { + for (uint8_t fm=0; fmls[i].lastValue = CS_LAST_VALUE_INIT; + //todo other values + lsa->ls[i].state = false; + } + } +} + +getvalue_t convertLswTelemValue(LogicalSwitchData * ls) +{ + getvalue_t val; +#if defined(CPUARM) + val = convert16bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, ls->v2); +#else + if (lswFamily(ls->func)==LS_FAMILY_OFS) + val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2); + else + val = convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128+ls->v2) - convert8bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, 128); +#endif + return val; +} + +void copyLswState(uint8_t oldPhase, uint8_t newPhase) { + for (uint8_t i=0; i Date: Sat, 17 May 2014 18:31:15 +0200 Subject: [PATCH 03/18] gtest correction needed because of changed order of logical swithces calculation --- radio/src/gtests.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index 6ff16890f..b33f795d5 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -330,17 +330,17 @@ TEST(getSwitch, recursiveSW) g_model.logicalSw[1] = { SWSRC_ELE, -SWSRC_SW1, LS_FUNC_OR }; evalLogicalSwitches(e_perout_mode_normal); - EXPECT_EQ(getSwitch(SWSRC_SW1), false); - EXPECT_EQ(getSwitch(SWSRC_SW2), true); + EXPECT_EQ(getSwitch(SWSRC_SW1), true); + EXPECT_EQ(getSwitch(SWSRC_SW2), false); evalLogicalSwitches(e_perout_mode_normal); - EXPECT_EQ(getSwitch(SWSRC_SW1), false); - EXPECT_EQ(getSwitch(SWSRC_SW2), true); + EXPECT_EQ(getSwitch(SWSRC_SW1), true); + EXPECT_EQ(getSwitch(SWSRC_SW2), false); simuSetSwitch(1, 1); evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), true); - EXPECT_EQ(getSwitch(SWSRC_SW2), true); + EXPECT_EQ(getSwitch(SWSRC_SW2), false); evalLogicalSwitches(e_perout_mode_normal); EXPECT_EQ(getSwitch(SWSRC_SW1), true); From c0c824c0f404325418d7e63c6c3af4333602ed2d Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sat, 17 May 2014 19:34:53 +0200 Subject: [PATCH 04/18] Cosmetics --- radio/src/audio_arm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/radio/src/audio_arm.cpp b/radio/src/audio_arm.cpp index 2ba4021d1..29e2a309f 100644 --- a/radio/src/audio_arm.cpp +++ b/radio/src/audio_arm.cpp @@ -420,7 +420,7 @@ bool isAudioFileReferenced(uint32_t i, char * filename) } else if (category == PHASE_AUDIO_CATEGORY) { if (sdAvailablePhaseAudioFiles & MASK_PHASE_AUDIO_FILE(index, event)) { - getPhaseAudioFile(filename, index, event); + getPhaseAudioFile(filename, index, event); return true; } } @@ -432,7 +432,7 @@ bool isAudioFileReferenced(uint32_t i, char * filename) } else if (category == LOGICAL_SWITCH_AUDIO_CATEGORY) { if (sdAvailableLogicalSwitchAudioFiles & MASK_LOGICAL_SWITCH_AUDIO_FILE(index, event)) { - getLogicalSwitchAudioFile(filename, index, event); + getLogicalSwitchAudioFile(filename, index, event); return true; } } From 8ec706ad08aba00a913c9922b0e8d90464304b09 Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sat, 17 May 2014 19:35:34 +0200 Subject: [PATCH 05/18] Bugfix --- radio/src/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 75f1f495d..53cc718de 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -951,8 +951,8 @@ void evalMixes(uint8_t tick10ms) fp_act[s_last_phase] = 0; fp_act[phase] = MAX_ACT; } + copyLswState(s_last_phase, phase); //push last logical switches state from old to new phase } - copyLswState(s_last_phase, phase); //push last logical switches state from old to new phase s_last_phase = phase; } From 1491d3b23601c78e1dbef04557a4dabe04dcdd81 Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sat, 17 May 2014 19:38:23 +0200 Subject: [PATCH 06/18] Renamed lswReset() to logicalSwitchesReset() Added PACKED to logical switches data structures --- radio/src/gtests.cpp | 2 +- radio/src/opentx.cpp | 2 +- radio/src/opentx.h | 2 +- radio/src/switches.cpp | 10 +++++----- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index b33f795d5..a54110ee3 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -53,7 +53,7 @@ void doMixerCalculations(); memset(act, 0, sizeof(act)); \ memset(swOn, 0, sizeof(swOn)); \ int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \ - lswReset(); + logicalSwitchesReset(); uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 }; uint16_t anaIn(uint8_t chan) diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index d07e28a5c..b884d03d0 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1519,7 +1519,7 @@ void flightReset() #if defined(FRSKY) telemetryReset(); #endif - lswReset(); + logicalSwitchesReset(); s_mixer_first_run_done = false; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 3ccbd0a2b..604d82303 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -694,7 +694,7 @@ NOINLINE void per10ms(); getvalue_t getValue(uint8_t i); bool getSwitch(int8_t swtch); void lswTimerTick(); -void lswReset(); +void logicalSwitchesReset(); void evalLogicalSwitches(uint8_t mode); void copyLswState(uint8_t oldPhase, uint8_t newPhase); diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index dec33f1b3..cf8b9bccb 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -38,7 +38,7 @@ #define CS_LAST_VALUE_INIT -32768 -typedef struct { +PACK(typedef struct { uint8_t state:1; #if defined(CPUARM) uint8_t internalState:1; @@ -46,11 +46,11 @@ typedef struct { uint32_t duration; #endif int16_t lastValue; -} lsw_struct; +}) lsw_struct; -typedef struct { +PACK(typedef struct { lsw_struct ls[NUM_LOGICAL_SWITCH]; -} lsw_array; +}) lsw_array; lsw_array lswFm[MAX_FLIGHT_MODES]; @@ -702,7 +702,7 @@ int16_t lswTimerValue(delayval_t val) return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); } -void lswReset() { +void logicalSwitchesReset() { for (uint8_t fm=0; fm Date: Sat, 17 May 2014 19:55:53 +0200 Subject: [PATCH 07/18] Cosmetics --- radio/src/mixer.cpp | 2 +- radio/src/opentx.cpp | 2 +- radio/src/opentx.h | 4 ++-- radio/src/switches.cpp | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 53cc718de..b6b2b4c46 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -951,7 +951,7 @@ void evalMixes(uint8_t tick10ms) fp_act[s_last_phase] = 0; fp_act[phase] = MAX_ACT; } - copyLswState(s_last_phase, phase); //push last logical switches state from old to new phase + logicalSwitchesCopyState(s_last_phase, phase); //push last logical switches state from old to new phase } s_last_phase = phase; } diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index b884d03d0..cbc7d7e08 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -2219,7 +2219,7 @@ void doMixerCalculations() s_cnt_100ms -= 10; s_cnt_1s += 1; - lswTimerTick(); + logicalSwitchesTimerTick(); if (s_cnt_1s >= 10) { // 1sec s_cnt_1s -= 10; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 604d82303..d69411a71 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -693,10 +693,10 @@ NOINLINE void per10ms(); getvalue_t getValue(uint8_t i); bool getSwitch(int8_t swtch); -void lswTimerTick(); +void logicalSwitchesTimerTick(); void logicalSwitchesReset(); void evalLogicalSwitches(uint8_t mode); -void copyLswState(uint8_t oldPhase, uint8_t newPhase); +void logicalSwitchesCopyState(uint8_t oldPhase, uint8_t newPhase); #if defined(PCBTARANIS) void getSwitchesPosition(bool startup); diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index cf8b9bccb..bb7dacff3 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -603,7 +603,7 @@ void checkSwitches() #endif } -void lswTimerTick() { +void logicalSwitchesTimerTick() { for (uint8_t fm=0; fm Date: Sun, 18 May 2014 16:38:00 +0200 Subject: [PATCH 08/18] Delay and Duration using 1 x uint8_t instead of 2 x uint32_t. Timing for delay and duration is not as accurate as before, max error possible is +-100ms. Worst case example: duration set at 0.1s will produce output pulse of width between 10ms (min mixer loop time) and 200ms. But RAM saving is BIG, unfortunately only on ARM. --- radio/src/switches.cpp | 45 +++++++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index bb7dacff3..7a64e42a1 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -41,9 +41,8 @@ PACK(typedef struct { uint8_t state:1; #if defined(CPUARM) - uint8_t internalState:1; - uint32_t delay; - uint32_t duration; + uint8_t ddState:2; + uint8_t ddTimer; #endif int16_t lastValue; }) lsw_struct; @@ -379,26 +378,29 @@ void evalLogicalSwitches(uint8_t mode) } #if defined(CPUARM) - if (ls->delay) { + if (ls->delay || ls->duration) { if (result) { - if (lsd->delay > get_tmr10ms()) - result = false; + if (lsd->ddState==0) { + //set delay timer + lsd->ddTimer = ls->delay; + lsd->ddState++; + } + if (lsd->ddState==1) { + if (lsd->ddTimer) { + result = false; //return false while delay timer running + } + else if (ls->duration) { + //set duration timer + lsd->ddTimer = ls->duration; + lsd->ddState++; + } + } + else if (lsd->ddState==2) { + result = (lsd->ddTimer>0); //return false after duration timer runs out + } } else { - lsd->delay = get_tmr10ms() + (ls->delay*10); - } - } - - if (ls->duration) { - if (result && !lsd->internalState) { - lsd->duration = get_tmr10ms() + (ls->duration*10); - } - - lsd->internalState = result; - result = false; - - if (lsd->duration > get_tmr10ms()) { - result = true; + lsd->ddState = 0; } } #endif @@ -669,6 +671,9 @@ void logicalSwitchesTimerTick() { lastValue.duration = 0; } } + //decrement delay/duration timer + if (lsd->ddTimer) + lsd->ddTimer--; #endif } } From e14734c8af7b34be5c8d00b6bd70bb93370afe45 Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sun, 18 May 2014 18:08:41 +0200 Subject: [PATCH 09/18] Reduced number of logical switch context to 2. Now we have only active fm context (lswFM[0]) and other context that is used for all other fms (lswFm[1]). Much smaller RAM usage, but possible (I beleave unimportant) errors when trasitioning between more than two flight modes. --- radio/src/mixer.cpp | 6 +++--- radio/src/opentx.h | 5 +++-- radio/src/switches.cpp | 24 ++++++++++-------------- 3 files changed, 16 insertions(+), 19 deletions(-) diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index b6b2b4c46..8638a14bf 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -524,7 +524,7 @@ uint8_t s_current_mixer_flight_mode; void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) { evalInputs(mode); - evalLogicalSwitches(mode); + evalLogicalSwitches(); #if defined(MODULE_ALWAYS_SEND_PULSES) checkStartupWarnings(); @@ -918,6 +918,7 @@ int32_t sum_chans512[NUM_CHNOUT] = {0}; #define MAX_ACT 0xffff +uint8_t s_last_phase = 255; // TODO reinit everything here when the model changes, no??? void evalMixes(uint8_t tick10ms) { @@ -928,7 +929,6 @@ void evalMixes(uint8_t tick10ms) static uint16_t fp_act[MAX_FLIGHT_MODES] = {0}; static uint16_t delta = 0; static ACTIVE_PHASES_TYPE s_fade_flight_phases = 0; - static uint8_t s_last_phase = 255; // TODO reinit everything here when the model changes, no??? uint8_t phase = getFlightPhase(); @@ -951,7 +951,7 @@ void evalMixes(uint8_t tick10ms) fp_act[s_last_phase] = 0; fp_act[phase] = MAX_ACT; } - logicalSwitchesCopyState(s_last_phase, phase); //push last logical switches state from old to new phase + logicalSwitchesCopyState(); //push last logical switches state from old to new phase } s_last_phase = phase; } diff --git a/radio/src/opentx.h b/radio/src/opentx.h index d69411a71..13e5c681b 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -671,6 +671,7 @@ enum StartupWarningStates { #endif extern uint8_t s_current_mixer_flight_mode; +extern uint8_t s_last_phase; #if defined(CPUARM) #define bitfield_channels_t uint32_t @@ -695,8 +696,8 @@ getvalue_t getValue(uint8_t i); bool getSwitch(int8_t swtch); void logicalSwitchesTimerTick(); void logicalSwitchesReset(); -void evalLogicalSwitches(uint8_t mode); -void logicalSwitchesCopyState(uint8_t oldPhase, uint8_t newPhase); +void evalLogicalSwitches(); +void logicalSwitchesCopyState(); #if defined(PCBTARANIS) void getSwitchesPosition(bool startup); diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 7a64e42a1..63006b232 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -51,7 +51,7 @@ PACK(typedef struct { lsw_struct ls[NUM_LOGICAL_SWITCH]; }) lsw_array; -lsw_array lswFm[MAX_FLIGHT_MODES]; +lsw_array lswFm[2]; #if defined(PCBTARANIS) tmr10ms_t switchesMidposStart[6] = { 0 }; @@ -218,7 +218,7 @@ bool getSwitch(int8_t swtch) #endif else { cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; - result = lswFm[s_current_mixer_flight_mode].ls[cs_idx].state; + result = lswFm[(s_last_phase == s_current_mixer_flight_mode)?0:1].ls[cs_idx].state; } return swtch > 0 ? result : !result; @@ -227,14 +227,14 @@ bool getSwitch(int8_t swtch) /** @brief Calculates new state of logical switches for s_current_mixer_flight_mode */ -void evalLogicalSwitches(uint8_t mode) +void evalLogicalSwitches() { for(unsigned int cs_idx=0; cs_idxandsw; @@ -406,10 +406,10 @@ void evalLogicalSwitches(uint8_t mode) #endif if (result) { - if (!lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_ON(cs_idx); + if (!lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_ON(cs_idx); } else { - if (lsd->state && mode == e_perout_mode_normal) PLAY_LOGICAL_SWITCH_OFF(cs_idx); + if (lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_OFF(cs_idx); } lsd->state = result; } @@ -606,7 +606,7 @@ void checkSwitches() } void logicalSwitchesTimerTick() { - for (uint8_t fm=0; fmls[i].lastValue = CS_LAST_VALUE_INIT; - //todo other values - lsa->ls[i].state = false; } } } @@ -732,8 +730,6 @@ getvalue_t convertLswTelemValue(LogicalSwitchData * ls) return val; } -void logicalSwitchesCopyState(uint8_t oldPhase, uint8_t newPhase) { - for (uint8_t i=0; i Date: Sun, 18 May 2014 18:09:23 +0200 Subject: [PATCH 10/18] Cosmetics --- radio/src/switches.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 63006b232..d92713831 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -380,12 +380,12 @@ void evalLogicalSwitches() #if defined(CPUARM) if (ls->delay || ls->duration) { if (result) { - if (lsd->ddState==0) { + if (lsd->ddState == 0) { //set delay timer lsd->ddTimer = ls->delay; lsd->ddState++; } - if (lsd->ddState==1) { + if (lsd->ddState == 1) { if (lsd->ddTimer) { result = false; //return false while delay timer running } @@ -395,8 +395,8 @@ void evalLogicalSwitches() lsd->ddState++; } } - else if (lsd->ddState==2) { - result = (lsd->ddTimer>0); //return false after duration timer runs out + else if (lsd->ddState == 2) { + result = (lsd->ddTimer > 0); //return false after duration timer runs out } } else { From 0180e3b90dc0ffca793465e12ff4f1f037ebb78d Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sun, 18 May 2014 18:22:56 +0200 Subject: [PATCH 11/18] Gtests problems --- radio/src/gtests.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index a54110ee3..ccc761030 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -53,6 +53,7 @@ void doMixerCalculations(); memset(act, 0, sizeof(act)); \ memset(swOn, 0, sizeof(swOn)); \ int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \ + s_current_mixer_flight_mode = s_last_phase = 0; \ logicalSwitchesReset(); uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 }; @@ -308,6 +309,8 @@ TEST(getSwitch, circularCSW) MIXER_RESET(); g_model.logicalSw[0] = { SWSRC_SW1, SWSRC_SW1, LS_FUNC_OR }; g_model.logicalSw[1] = { SWSRC_SW1, SWSRC_SW1, LS_FUNC_AND }; + + evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), false); EXPECT_EQ(getSwitch(-SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); @@ -329,20 +332,20 @@ TEST(getSwitch, recursiveSW) g_model.logicalSw[0] = { SWSRC_RUD, -SWSRC_SW2, LS_FUNC_OR }; g_model.logicalSw[1] = { SWSRC_ELE, -SWSRC_SW1, LS_FUNC_OR }; - evalLogicalSwitches(e_perout_mode_normal); + evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); - evalLogicalSwitches(e_perout_mode_normal); + evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); simuSetSwitch(1, 1); - evalLogicalSwitches(e_perout_mode_normal); + evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); - evalLogicalSwitches(e_perout_mode_normal); + evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); } From dc2fcb26186fdfddaf9a20779297b50832faaa4d Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sun, 18 May 2014 18:30:30 +0200 Subject: [PATCH 12/18] Another gtests failure --- radio/src/gtests.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index ccc761030..6f7158a07 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -45,7 +45,8 @@ void doMixerCalculations(); #define MODEL_RESET() \ memset(&g_model, 0, sizeof(g_model)); \ extern uint8_t s_mixer_first_run_done; \ - s_mixer_first_run_done = false; + s_mixer_first_run_done = false; \ + s_last_phase = 255; #define MIXER_RESET() \ memset(channelOutputs, 0, sizeof(channelOutputs)); \ From df971b0e772af4e1ff36201303bbeafc9bbc89b0 Mon Sep 17 00:00:00 2001 From: Damjan Adamic Date: Sun, 18 May 2014 18:44:07 +0200 Subject: [PATCH 13/18] Cosmetics --- radio/src/switches.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index d92713831..fd9570e5e 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -649,7 +649,7 @@ void logicalSwitchesTimerTick() { } } } - #if defined(CPUARM) +#if defined(CPUARM) else if (ls->func == LS_FUNC_STAY) { PACK(typedef struct { uint16_t state:1; @@ -674,7 +674,7 @@ void logicalSwitchesTimerTick() { //decrement delay/duration timer if (lsd->ddTimer) lsd->ddTimer--; - #endif +#endif } } } From b9ee91b08d63fbe94b84a5f7724a77f7b2c1e589 Mon Sep 17 00:00:00 2001 From: bsongis Date: Mon, 19 May 2014 11:05:59 +0200 Subject: [PATCH 14/18] Indentation --- radio/src/audio_arm.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/radio/src/audio_arm.cpp b/radio/src/audio_arm.cpp index 29e2a309f..d727b9eaf 100644 --- a/radio/src/audio_arm.cpp +++ b/radio/src/audio_arm.cpp @@ -420,10 +420,10 @@ bool isAudioFileReferenced(uint32_t i, char * filename) } else if (category == PHASE_AUDIO_CATEGORY) { if (sdAvailablePhaseAudioFiles & MASK_PHASE_AUDIO_FILE(index, event)) { - getPhaseAudioFile(filename, index, event); - return true; - } + getPhaseAudioFile(filename, index, event); + return true; } + } else if (category == SWITCH_AUDIO_CATEGORY) { if (sdAvailableSwitchAudioFiles & MASK_SWITCH_AUDIO_FILE(index)) { getSwitchAudioFile(filename, index); @@ -432,10 +432,10 @@ bool isAudioFileReferenced(uint32_t i, char * filename) } else if (category == LOGICAL_SWITCH_AUDIO_CATEGORY) { if (sdAvailableLogicalSwitchAudioFiles & MASK_LOGICAL_SWITCH_AUDIO_FILE(index, event)) { - getLogicalSwitchAudioFile(filename, index, event); - return true; - } + getLogicalSwitchAudioFile(filename, index, event); + return true; } + } return false; } From dffb5a7a79892d62e49974f8e5625545d5ea2474 Mon Sep 17 00:00:00 2001 From: bsongis Date: Mon, 19 May 2014 11:07:51 +0200 Subject: [PATCH 15/18] Bad structure alignment (the compiler does the job, but it's better to write it so we see where are the lost bits) --- radio/src/switches.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index fd9570e5e..036cb9456 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -42,7 +42,10 @@ PACK(typedef struct { uint8_t state:1; #if defined(CPUARM) uint8_t ddState:2; + uint8_t spare:5; uint8_t ddTimer; +#else + uint8_t spare:7; #endif int16_t lastValue; }) lsw_struct; From 1a7ff9239da8a7a106380eb53b4fd4730f91e402 Mon Sep 17 00:00:00 2001 From: bsongis Date: Mon, 19 May 2014 14:52:07 +0200 Subject: [PATCH 16/18] A couple of changes on top of projectkkglider branch --- radio/src/mixer.cpp | 13 +- radio/src/opentx.h | 9 +- radio/src/switches.cpp | 464 +++++++++++++++++++++++------------------ 3 files changed, 277 insertions(+), 209 deletions(-) diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 8638a14bf..023c696bf 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -524,7 +524,10 @@ uint8_t s_current_mixer_flight_mode; void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) { evalInputs(mode); - evalLogicalSwitches(); + +#if defined(CPUARM) + evalLogicalSwitches(mode==e_perout_mode_normal); +#endif #if defined(MODULE_ALWAYS_SEND_PULSES) checkStartupWarnings(); @@ -930,6 +933,8 @@ void evalMixes(uint8_t tick10ms) static uint16_t delta = 0; static ACTIVE_PHASES_TYPE s_fade_flight_phases = 0; + LS_RECURSIVE_EVALUATION_RESET(); + uint8_t phase = getFlightPhase(); if (s_last_phase != phase) { @@ -951,7 +956,9 @@ void evalMixes(uint8_t tick10ms) fp_act[s_last_phase] = 0; fp_act[phase] = MAX_ACT; } - logicalSwitchesCopyState(); //push last logical switches state from old to new phase +#if defined(CPUARM) + logicalSwitchesCopyState(s_last_phase, phase); // push last logical switches state from old to new phase +#endif } s_last_phase = phase; } @@ -960,6 +967,7 @@ void evalMixes(uint8_t tick10ms) if (s_fade_flight_phases) { memclear(sum_chans512, sizeof(sum_chans512)); for (uint8_t p=0; p> 4) * fp_act[p]; weight += fp_act[p]; } + LS_RECURSIVE_EVALUATION_RESET(); } assert(weight); s_current_mixer_flight_mode = phase; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 13e5c681b..7c661dd0f 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -671,7 +671,6 @@ enum StartupWarningStates { #endif extern uint8_t s_current_mixer_flight_mode; -extern uint8_t s_last_phase; #if defined(CPUARM) #define bitfield_channels_t uint32_t @@ -696,8 +695,14 @@ getvalue_t getValue(uint8_t i); bool getSwitch(int8_t swtch); void logicalSwitchesTimerTick(); void logicalSwitchesReset(); -void evalLogicalSwitches(); + +#if defined(CPUARM) +void evalLogicalSwitches(bool isCurrentPhase=true); void logicalSwitchesCopyState(); +#define LS_RECURSIVE_EVALUATION_RESET() +#else +#define LS_RECURSIVE_EVALUATION_RESET() s_last_switch_used = 0 +#endif #if defined(PCBTARANIS) void getSwitchesPosition(bool startup); diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 036cb9456..2f40bc91a 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -38,23 +38,39 @@ #define CS_LAST_VALUE_INIT -32768 +#if defined(CPUARM) + +enum LogicalSwitchContextState { + SWITCH_START, + SWITCH_DELAY, + SWITCH_ENABLE +}; + PACK(typedef struct { uint8_t state:1; -#if defined(CPUARM) - uint8_t ddState:2; + uint8_t timerState:2; uint8_t spare:5; - uint8_t ddTimer; -#else - uint8_t spare:7; -#endif + uint8_t timer; int16_t lastValue; -}) lsw_struct; +}) LogicalSwitchContext; PACK(typedef struct { - lsw_struct ls[NUM_LOGICAL_SWITCH]; -}) lsw_array; + LogicalSwitchContext lsw[NUM_LOGICAL_SWITCH]; +}) LogicalSwitchesFlightModeContext; +LogicalSwitchesFlightModeContext lswFm[MAX_FLIGHT_MODES]; -lsw_array lswFm[2]; +#define LS_LAST_VALUE(fm, idx) lswFm[fm].lsw[idx].lastValue + +#else + +int16_t lsLastValue[NUM_LOGICAL_SWITCH]; +#define LS_LAST_VALUE(fm, idx) lsLastValue[idx] + +#define GETSWITCH_RECURSIVE_TYPE uint16_t +volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used = 0; +volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value = 0; + +#endif #if defined(PCBTARANIS) tmr10ms_t switchesMidposStart[6] = { 0 }; @@ -161,6 +177,182 @@ void getSwitchesPosition(bool startup) #define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx))) #endif +bool getLogicalSwitch(uint8_t idx) +{ + LogicalSwitchData * ls = lswAddress(idx); + bool result; + +#if defined(CPUARM) + int8_t s = ls->andsw; +#else + uint8_t s = ls->andsw; + if (s > SWSRC_LAST_SWITCH) { + s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; + } +#endif + + if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { + LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = CS_LAST_VALUE_INIT; + result = false; + } + else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { + bool res1 = getSwitch(ls->v1); + bool res2 = getSwitch(ls->v2); + switch (ls->func) { + case LS_FUNC_AND: + result = (res1 && res2); + break; + case LS_FUNC_OR: + result = (res1 || res2); + break; + // case LS_FUNC_XOR: + default: + result = (res1 ^ res2); + break; + } + } + else if (s == LS_FAMILY_TIMER) { + result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) <= 0); + } + else if (s == LS_FAMILY_STICKY) { + result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) & (1<<0)); + } +#if defined(CPUARM) + else if (s == LS_FAMILY_STAY) { + result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) & (1<<0)); + } +#endif + else { + getvalue_t x = getValue(ls->v1); + getvalue_t y; + if (s == LS_FAMILY_COMP) { + y = getValue(ls->v2); + + switch (ls->func) { + case LS_FUNC_EQUAL: + result = (x==y); + break; + case LS_FUNC_GREATER: + result = (x>y); + break; + default: + result = (xv1; +#if defined(FRSKY) + // Telemetry + if (v1 >= MIXSRC_FIRST_TELEM) { + if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) + return false; + + y = convertLswTelemValue(ls); + +#if defined(FRSKY_HUB) && defined(GAUGES) + if (s == LS_FAMILY_OFS) { + uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; + if (idx < THLD_MAX) { + // Fill the threshold array + barsThresholds[idx] = 128 + ls->v2; + } + } +#endif + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; + } + else { + y = calc100toRESX(ls->v2); + } +#else + if (v1 >= MIXSRC_FIRST_TELEM) { + y = (int16_t)3 * (128+ls->v2); // it's a Timer + } + else if (v1 >= MIXSRC_GVAR1) { + y = ls->v2; // it's a GVAR + } + else { + y = calc100toRESX(ls->v2); + } +#endif + + switch (ls->func) { +#if defined(CPUARM) + case LS_FUNC_VEQUAL: + result = (x==y); + break; +#endif + case LS_FUNC_VALMOSTEQUAL: +#if defined(GVARS) + if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) + result = (x==y); + else +#endif + result = (abs(x-y) < (1024 / STICK_TOLERANCE)); + break; + case LS_FUNC_VPOS: + result = (x>y); + break; + case LS_FUNC_VNEG: + result = (xy); + break; + case LS_FUNC_ANEG: + result = (abs(x)func == LS_FUNC_DIFFEGREATER) + result = (y >= 0 ? (diff >= y) : (diff <= y)); + else + result = (abs(diff) >= y); + if (result) + LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = x; + break; + } + } + } + } + +#if defined(CPUARM) + if (ls->delay || ls->duration) { + LogicalSwitchContext &context = lswFm[s_current_mixer_flight_mode].lsw[idx]; + if (result) { + if (context.timerState == SWITCH_START) { + // set delay timer + context.timerState = SWITCH_DELAY; + context.timer = ls->delay; + } + if (context.timerState == SWITCH_DELAY) { + if (context.timer) { + result = false; //return false while delay timer running + } + else if (ls->duration) { + // set duration timer + context.timerState = SWITCH_ENABLE; + context.timer = ls->duration; + } + } + else if (context.timerState == SWITCH_ENABLE) { + result = (context.timer > 0); //return false after duration timer runs out + } + } + else { + context.timerState = SWITCH_START; + } + } +#endif + + return result; +} + bool getSwitch(int8_t swtch) { bool result; @@ -221,202 +413,50 @@ bool getSwitch(int8_t swtch) #endif else { cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH; - result = lswFm[(s_last_phase == s_current_mixer_flight_mode)?0:1].ls[cs_idx].state; +#if defined(CPUARM) + result = lswFm[s_current_mixer_flight_mode].lsw[cs_idx].state; +#else + GETSWITCH_RECURSIVE_TYPE mask = ((GETSWITCH_RECURSIVE_TYPE)1 << cs_idx); + if (s_last_switch_used & mask) { + result = (s_last_switch_value & mask); + } + else { + s_last_switch_used |= mask; + result = getLogicalSwitch(cs_idx); + if (result) { + s_last_switch_value |= mask; + } + else { + s_last_switch_value &= ~mask; + } + } +#endif } return swtch > 0 ? result : !result; } +#if defined(CPUARM) /** @brief Calculates new state of logical switches for s_current_mixer_flight_mode */ -void evalLogicalSwitches() +void evalLogicalSwitches(bool isCurrentPhase) { - for(unsigned int cs_idx=0; cs_idxandsw; -#else - uint8_t s = ls->andsw; - if (s > SWSRC_LAST_SWITCH) { - s += SWSRC_SW1-SWSRC_LAST_SWITCH-1; - } -#endif - if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) { - lsd->lastValue = CS_LAST_VALUE_INIT; - result = false; - } - else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) { - bool res1 = getSwitch(ls->v1); - bool res2 = getSwitch(ls->v2); - switch (ls->func) { - case LS_FUNC_AND: - result = (res1 && res2); - break; - case LS_FUNC_OR: - result = (res1 || res2); - break; - // case LS_FUNC_XOR: - default: - result = (res1 ^ res2); - break; - } - } - else if (s == LS_FAMILY_TIMER) { - result = (lsd->lastValue <= 0); - } - else if (s == LS_FAMILY_STICKY) { - result = (lsd->lastValue & (1<<0)); - } -#if defined(CPUARM) - else if (s == LS_FAMILY_STAY) { - result = (lsd->lastValue & (1<<0)); - } -#endif - else { - getvalue_t x = getValue(ls->v1); - getvalue_t y; - if (s == LS_FAMILY_COMP) { - y = getValue(ls->v2); - - switch (ls->func) { - case LS_FUNC_EQUAL: - result = (x==y); - break; - case LS_FUNC_GREATER: - result = (x>y); - break; - default: - result = (xv1; -#if defined(FRSKY) - // Telemetry - if (v1 >= MIXSRC_FIRST_TELEM) { - if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) { - lsd->state = false; - continue; - } - - - y = convertLswTelemValue(ls); - -#if defined(FRSKY_HUB) && defined(GAUGES) - if (s == LS_FAMILY_OFS) { - uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT; - if (idx < THLD_MAX) { - // Fill the threshold array - barsThresholds[idx] = 128 + ls->v2; - } - } -#endif - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; - } - else { - y = calc100toRESX(ls->v2); - } -#else - if (v1 >= MIXSRC_FIRST_TELEM) { - y = (int16_t)3 * (128+ls->v2); // it's a Timer - } - else if (v1 >= MIXSRC_GVAR1) { - y = ls->v2; // it's a GVAR - } - else { - y = calc100toRESX(ls->v2); - } -#endif - - switch (ls->func) { -#if defined(CPUARM) - case LS_FUNC_VEQUAL: - result = (x==y); - break; -#endif - case LS_FUNC_VALMOSTEQUAL: -#if defined(GVARS) - if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR) - result = (x==y); - else -#endif - result = (abs(x-y) < (1024 / STICK_TOLERANCE)); - break; - case LS_FUNC_VPOS: - result = (x>y); - break; - case LS_FUNC_VNEG: - result = (xy); - break; - case LS_FUNC_ANEG: - result = (abs(x)lastValue == CS_LAST_VALUE_INIT) - lsd->lastValue = x; - int16_t diff = x - lsd->lastValue; - if (ls->func == LS_FUNC_DIFFEGREATER) - result = (y >= 0 ? (diff >= y) : (diff <= y)); - else - result = (abs(diff) >= y); - if (result) - lsd->lastValue = x; - break; - } - } - } - } - -#if defined(CPUARM) - if (ls->delay || ls->duration) { + for (unsigned int idx=0; idxddState == 0) { - //set delay timer - lsd->ddTimer = ls->delay; - lsd->ddState++; - } - if (lsd->ddState == 1) { - if (lsd->ddTimer) { - result = false; //return false while delay timer running - } - else if (ls->duration) { - //set duration timer - lsd->ddTimer = ls->duration; - lsd->ddState++; - } - } - else if (lsd->ddState == 2) { - result = (lsd->ddTimer > 0); //return false after duration timer runs out - } + if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx); } else { - lsd->ddState = 0; + if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx); } } -#endif - - if (result) { - if (!lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_ON(cs_idx); - } - else { - if (lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_OFF(cs_idx); - } - lsd->state = result; + context.state = result; } } +#endif swstate_t switches_states = 0; int8_t getMovedSwitch() @@ -608,13 +648,15 @@ void checkSwitches() #endif } -void logicalSwitchesTimerTick() { - for (uint8_t fm=0; fm<2; fm++) { +void logicalSwitchesTimerTick() +{ +#if defined(CPUARM) + for (uint8_t fm=0; fmfunc == LS_FUNC_TIMER) { - int16_t *lastValue = &lsd->lastValue; + int16_t *lastValue = &LS_LAST_VALUE(fm, i); if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { *lastValue = -lswTimerValue(ls->v1); } @@ -631,7 +673,7 @@ void logicalSwitchesTimerTick() { uint8_t state; uint8_t last; }) ls_sticky_struct; - ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;; + ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i); bool before = lastValue.last & 0x01; if (lastValue.state) { bool now = getSwitch(ls->v2); @@ -659,7 +701,7 @@ void logicalSwitchesTimerTick() { uint16_t duration:15; }) ls_stay_struct; - ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;; + ls_stay_struct & lastValue = (ls_stay_struct &)LS_LAST_VALUE(fm, i); lastValue.state = false; bool state = getSwitch(ls->v1); if (state) { @@ -674,12 +716,17 @@ void logicalSwitchesTimerTick() { lastValue.duration = 0; } } - //decrement delay/duration timer - if (lsd->ddTimer) - lsd->ddTimer--; + + // decrement delay/duration timer + LogicalSwitchContext &context = lswFm[fm].lsw[i]; + if (context.timer) { + context.timer--; + } #endif } +#if defined(CPUARM) } +#endif } LogicalSwitchData *lswAddress(uint8_t idx) @@ -710,13 +757,17 @@ int16_t lswTimerValue(delayval_t val) return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10)); } -void logicalSwitchesReset() { - for (uint8_t fm=0; fm<2; fm++) { - lsw_array * lsa = &lswFm[fm]; +void logicalSwitchesReset() +{ +#if defined(CPUARM) + for (uint8_t fm=0; fmls[i].lastValue = CS_LAST_VALUE_INIT; + LS_LAST_VALUE(fm, i) = CS_LAST_VALUE_INIT; } +#if defined(CPUARM) } +#endif } getvalue_t convertLswTelemValue(LogicalSwitchData * ls) @@ -733,6 +784,9 @@ getvalue_t convertLswTelemValue(LogicalSwitchData * ls) return val; } -void logicalSwitchesCopyState() { - lswFm[1] = lswFm[0]; +#if defined(CPUARM) +void logicalSwitchesCopyState(uint8_t src, uint8_t dst) +{ + lswFm[dst] = lswFm[src]; } +#endif From 49a4d5ecb176c3c3d80077e4e5cfbe7e74e7848b Mon Sep 17 00:00:00 2001 From: bsongis Date: Mon, 19 May 2014 15:12:23 +0200 Subject: [PATCH 17/18] Gtests KO --- radio/src/gtests.cpp | 35 +++++++++++++++++++++++------------ radio/src/mixer.cpp | 2 -- radio/src/opentx.h | 7 ++++++- radio/src/switches.cpp | 1 - 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/radio/src/gtests.cpp b/radio/src/gtests.cpp index 6f7158a07..ddfff10ad 100644 --- a/radio/src/gtests.cpp +++ b/radio/src/gtests.cpp @@ -48,14 +48,22 @@ void doMixerCalculations(); s_mixer_first_run_done = false; \ s_last_phase = 255; -#define MIXER_RESET() \ - memset(channelOutputs, 0, sizeof(channelOutputs)); \ - memset(ex_chans, 0, sizeof(ex_chans)); \ - memset(act, 0, sizeof(act)); \ - memset(swOn, 0, sizeof(swOn)); \ - int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \ - s_current_mixer_flight_mode = s_last_phase = 0; \ +int32_t lastAct = 0; +void MIXER_RESET() +{ + memset(channelOutputs, 0, sizeof(channelOutputs)); + memset(ex_chans, 0, sizeof(ex_chans)); + memset(act, 0, sizeof(act)); + memset(swOn, 0, sizeof(swOn)); +#if !defined(CPUARM) + s_last_switch_used = 0; + s_last_switch_value = 0; +#endif + s_current_mixer_flight_mode = s_last_phase = 0; + lastAct = 0; logicalSwitchesReset(); +} + uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 }; uint16_t anaIn(uint8_t chan) @@ -334,18 +342,21 @@ TEST(getSwitch, recursiveSW) g_model.logicalSw[1] = { SWSRC_ELE, -SWSRC_SW1, LS_FUNC_OR }; evalLogicalSwitches(); - EXPECT_EQ(getSwitch(SWSRC_SW1), true); - EXPECT_EQ(getSwitch(SWSRC_SW2), false); + EXPECT_EQ(getSwitch(SWSRC_SW1), false); + EXPECT_EQ(getSwitch(SWSRC_SW2), true); + LS_RECURSIVE_EVALUATION_RESET(); evalLogicalSwitches(); - EXPECT_EQ(getSwitch(SWSRC_SW1), true); - EXPECT_EQ(getSwitch(SWSRC_SW2), false); + EXPECT_EQ(getSwitch(SWSRC_SW1), false); + EXPECT_EQ(getSwitch(SWSRC_SW2), true); simuSetSwitch(1, 1); + LS_RECURSIVE_EVALUATION_RESET(); evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); - EXPECT_EQ(getSwitch(SWSRC_SW2), false); + EXPECT_EQ(getSwitch(SWSRC_SW2), true); + LS_RECURSIVE_EVALUATION_RESET(); evalLogicalSwitches(); EXPECT_EQ(getSwitch(SWSRC_SW1), true); EXPECT_EQ(getSwitch(SWSRC_SW2), false); diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 023c696bf..fd761f8ec 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -525,9 +525,7 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms) { evalInputs(mode); -#if defined(CPUARM) evalLogicalSwitches(mode==e_perout_mode_normal); -#endif #if defined(MODULE_ALWAYS_SEND_PULSES) checkStartupWarnings(); diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 7c661dd0f..2611bd1b0 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -671,6 +671,7 @@ enum StartupWarningStates { #endif extern uint8_t s_current_mixer_flight_mode; +extern uint8_t s_last_phase; #if defined(CPUARM) #define bitfield_channels_t uint32_t @@ -698,9 +699,13 @@ void logicalSwitchesReset(); #if defined(CPUARM) void evalLogicalSwitches(bool isCurrentPhase=true); -void logicalSwitchesCopyState(); +void logicalSwitchesCopyState(uint8_t src, uint8_t dst); #define LS_RECURSIVE_EVALUATION_RESET() #else +#define evalLogicalSwitches(xxx) +#define GETSWITCH_RECURSIVE_TYPE uint16_t +extern volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used; +extern volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value; #define LS_RECURSIVE_EVALUATION_RESET() s_last_switch_used = 0 #endif diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index 2f40bc91a..091e01d1a 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -66,7 +66,6 @@ LogicalSwitchesFlightModeContext lswFm[MAX_FLIGHT_MODES]; int16_t lsLastValue[NUM_LOGICAL_SWITCH]; #define LS_LAST_VALUE(fm, idx) lsLastValue[idx] -#define GETSWITCH_RECURSIVE_TYPE uint16_t volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used = 0; volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value = 0; From 788193f89c9b145d3f9d7d99ed5a76226c155b55 Mon Sep 17 00:00:00 2001 From: bsongis Date: Mon, 19 May 2014 15:20:48 +0200 Subject: [PATCH 18/18] This initialization was lost in the battle --- radio/src/opentx.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index cbc7d7e08..172d258b9 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1519,7 +1519,11 @@ void flightReset() #if defined(FRSKY) telemetryReset(); #endif + logicalSwitchesReset(); +#if !defined(CPUARM) + s_last_switch_value = 0; +#endif s_mixer_first_run_done = false;