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