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
This commit is contained in:
@@ -445,6 +445,7 @@ tmr10ms_t timeAutomaticPromptsSilence = 0;
|
|||||||
void playModelEvent(uint8_t category, uint8_t index, uint8_t event)
|
void playModelEvent(uint8_t category, uint8_t index, uint8_t event)
|
||||||
{
|
{
|
||||||
char filename[AUDIO_FILENAME_MAXLEN+1];
|
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)) {
|
if (IS_SILENCE_PERIOD_ELAPSED() && isAudioFileReferenced((category << 24) + (index << 16) + event, filename)) {
|
||||||
audioQueue.playFile(filename);
|
audioQueue.playFile(filename);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -53,8 +53,7 @@ void doMixerCalculations();
|
|||||||
memset(act, 0, sizeof(act)); \
|
memset(act, 0, sizeof(act)); \
|
||||||
memset(swOn, 0, sizeof(swOn)); \
|
memset(swOn, 0, sizeof(swOn)); \
|
||||||
int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \
|
int32_t lastAct = 0; lastAct = lastAct; /* to avoid a warning */ \
|
||||||
s_last_switch_used = 0; \
|
lswReset();
|
||||||
s_last_switch_value = 0;
|
|
||||||
|
|
||||||
uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 };
|
uint16_t anaInValues[NUM_STICKS+NUM_POTS] = { 0 };
|
||||||
uint16_t anaIn(uint8_t chan)
|
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[0] = { SWSRC_RUD, -SWSRC_SW2, LS_FUNC_OR };
|
||||||
g_model.logicalSw[1] = { SWSRC_ELE, -SWSRC_SW1, 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_SW1), false);
|
||||||
EXPECT_EQ(getSwitch(SWSRC_SW2), true);
|
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_SW1), false);
|
||||||
EXPECT_EQ(getSwitch(SWSRC_SW2), true);
|
EXPECT_EQ(getSwitch(SWSRC_SW2), true);
|
||||||
|
|
||||||
simuSetSwitch(1, 1);
|
simuSetSwitch(1, 1);
|
||||||
s_last_switch_used = 0;
|
evalLogicalSwitches(e_perout_mode_normal);
|
||||||
EXPECT_EQ(getSwitch(SWSRC_SW1), true);
|
EXPECT_EQ(getSwitch(SWSRC_SW1), true);
|
||||||
EXPECT_EQ(getSwitch(SWSRC_SW2), 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_SW1), true);
|
||||||
EXPECT_EQ(getSwitch(SWSRC_SW2), false);
|
EXPECT_EQ(getSwitch(SWSRC_SW2), false);
|
||||||
}
|
}
|
||||||
|
|||||||
+2
-4
@@ -524,6 +524,7 @@ uint8_t s_current_mixer_flight_mode;
|
|||||||
void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
||||||
{
|
{
|
||||||
evalInputs(mode);
|
evalInputs(mode);
|
||||||
|
evalLogicalSwitches(mode);
|
||||||
|
|
||||||
#if defined(MODULE_ALWAYS_SEND_PULSES)
|
#if defined(MODULE_ALWAYS_SEND_PULSES)
|
||||||
checkStartupWarnings();
|
checkStartupWarnings();
|
||||||
@@ -929,8 +930,6 @@ void evalMixes(uint8_t tick10ms)
|
|||||||
static ACTIVE_PHASES_TYPE s_fade_flight_phases = 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???
|
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();
|
uint8_t phase = getFlightPhase();
|
||||||
|
|
||||||
if (s_last_phase != phase) {
|
if (s_last_phase != phase) {
|
||||||
@@ -953,6 +952,7 @@ void evalMixes(uint8_t tick10ms)
|
|||||||
fp_act[phase] = MAX_ACT;
|
fp_act[phase] = MAX_ACT;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
copyLswState(s_last_phase, phase); //push last logical switches state from old to new phase
|
||||||
s_last_phase = phase;
|
s_last_phase = phase;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -960,7 +960,6 @@ void evalMixes(uint8_t tick10ms)
|
|||||||
if (s_fade_flight_phases) {
|
if (s_fade_flight_phases) {
|
||||||
memclear(sum_chans512, sizeof(sum_chans512));
|
memclear(sum_chans512, sizeof(sum_chans512));
|
||||||
for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
|
for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
|
||||||
s_last_switch_used = 0;
|
|
||||||
if (s_fade_flight_phases & ((ACTIVE_PHASES_TYPE)1 << p)) {
|
if (s_fade_flight_phases & ((ACTIVE_PHASES_TYPE)1 << p)) {
|
||||||
s_current_mixer_flight_mode = p;
|
s_current_mixer_flight_mode = p;
|
||||||
evalFlightModeMixes(p==phase ? e_perout_mode_normal : e_perout_mode_inactive_phase, p==phase ? tick10ms : 0);
|
evalFlightModeMixes(p==phase ? e_perout_mode_normal : e_perout_mode_inactive_phase, p==phase ? tick10ms : 0);
|
||||||
@@ -968,7 +967,6 @@ void evalMixes(uint8_t tick10ms)
|
|||||||
sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
|
sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
|
||||||
weight += fp_act[p];
|
weight += fp_act[p];
|
||||||
}
|
}
|
||||||
s_last_switch_used = 0;
|
|
||||||
}
|
}
|
||||||
assert(weight);
|
assert(weight);
|
||||||
s_current_mixer_flight_mode = phase;
|
s_current_mixer_flight_mode = phase;
|
||||||
|
|||||||
@@ -1521,7 +1521,6 @@ void flightReset()
|
|||||||
#endif
|
#endif
|
||||||
lswReset();
|
lswReset();
|
||||||
|
|
||||||
s_last_switch_value = 0;
|
|
||||||
s_mixer_first_run_done = false;
|
s_mixer_first_run_done = false;
|
||||||
|
|
||||||
START_SILENCE_PERIOD();
|
START_SILENCE_PERIOD();
|
||||||
|
|||||||
+2
-9
@@ -695,6 +695,8 @@ getvalue_t getValue(uint8_t i);
|
|||||||
bool getSwitch(int8_t swtch);
|
bool getSwitch(int8_t swtch);
|
||||||
void lswTimerTick();
|
void lswTimerTick();
|
||||||
void lswReset();
|
void lswReset();
|
||||||
|
void evalLogicalSwitches(uint8_t mode);
|
||||||
|
void copyLswState(uint8_t oldPhase, uint8_t newPhase);
|
||||||
|
|
||||||
#if defined(PCBTARANIS)
|
#if defined(PCBTARANIS)
|
||||||
void getSwitchesPosition(bool startup);
|
void getSwitchesPosition(bool startup);
|
||||||
@@ -817,15 +819,6 @@ extern int8_t safetyCh[NUM_CHNOUT];
|
|||||||
|
|
||||||
extern uint8_t trimsCheckTimer;
|
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_OFF 0
|
||||||
#define TMR_RUNNING 1
|
#define TMR_RUNNING 1
|
||||||
#define TMR_NEGATIVE 2
|
#define TMR_NEGATIVE 2
|
||||||
|
|||||||
+275
-258
@@ -36,21 +36,23 @@
|
|||||||
|
|
||||||
#include "opentx.h"
|
#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
|
#define CS_LAST_VALUE_INIT -32768
|
||||||
volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used = 0;
|
|
||||||
volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value = 0;
|
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t state:1;
|
||||||
#if defined(CPUARM)
|
#if defined(CPUARM)
|
||||||
uint32_t lswDelays[NUM_LOGICAL_SWITCH];
|
uint8_t internalState:1;
|
||||||
uint32_t lswDurations[NUM_LOGICAL_SWITCH];
|
uint32_t delay;
|
||||||
uint8_t lswStates[NUM_LOGICAL_SWITCH];
|
uint32_t duration;
|
||||||
#endif
|
#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)
|
#if defined(PCBTARANIS)
|
||||||
tmr10ms_t switchesMidposStart[6] = { 0 };
|
tmr10ms_t switchesMidposStart[6] = { 0 };
|
||||||
@@ -157,10 +159,6 @@ void getSwitchesPosition(bool startup)
|
|||||||
#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx)))
|
#define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx)))
|
||||||
#endif
|
#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 getSwitch(int8_t swtch)
|
||||||
{
|
{
|
||||||
bool result;
|
bool result;
|
||||||
@@ -221,192 +219,200 @@ bool getSwitch(int8_t swtch)
|
|||||||
#endif
|
#endif
|
||||||
else {
|
else {
|
||||||
cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
|
cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
|
||||||
|
result = lswFm[s_current_mixer_flight_mode].ls[cs_idx].state;
|
||||||
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 = (x<y);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
uint8_t v1 = ls->v1;
|
|
||||||
#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 = (x<y);
|
|
||||||
break;
|
|
||||||
case LS_FUNC_APOS:
|
|
||||||
result = (abs(x)>y);
|
|
||||||
break;
|
|
||||||
case LS_FUNC_ANEG:
|
|
||||||
result = (abs(x)<y);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
{
|
|
||||||
if (lsLastValue[cs_idx] == CS_LAST_VALUE_INIT)
|
|
||||||
lsLastValue[cs_idx] = x;
|
|
||||||
int16_t diff = x - lsLastValue[cs_idx];
|
|
||||||
if (ls->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;
|
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_idx<NUM_LOGICAL_SWITCH; cs_idx++) {
|
||||||
|
|
||||||
|
bool result = false;
|
||||||
|
|
||||||
|
LogicalSwitchData * ls = lswAddress(cs_idx);
|
||||||
|
lsw_struct * lsd = &lswFm[s_current_mixer_flight_mode].ls[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))) {
|
||||||
|
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 = (x<y);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
uint8_t v1 = ls->v1;
|
||||||
|
#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 = (x<y);
|
||||||
|
break;
|
||||||
|
case LS_FUNC_APOS:
|
||||||
|
result = (abs(x)>y);
|
||||||
|
break;
|
||||||
|
case LS_FUNC_ANEG:
|
||||||
|
result = (abs(x)<y);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
if (lsd->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;
|
swstate_t switches_states = 0;
|
||||||
int8_t getMovedSwitch()
|
int8_t getMovedSwitch()
|
||||||
{
|
{
|
||||||
@@ -597,74 +603,75 @@ void checkSwitches()
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void lswTimerTick() {
|
void lswTimerTick() {
|
||||||
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
|
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
|
||||||
LogicalSwitchData * ls = lswAddress(i);
|
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
|
||||||
if (ls->func == LS_FUNC_TIMER) {
|
LogicalSwitchData * ls = lswAddress(i);
|
||||||
int16_t *lastValue = &lsLastValue[i];
|
lsw_struct * lsd = &lswFm[fm].ls[i];
|
||||||
if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) {
|
if (ls->func == LS_FUNC_TIMER) {
|
||||||
*lastValue = -lswTimerValue(ls->v1);
|
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 (*lastValue < 0) {
|
else if (ls->func == LS_FUNC_STICKY) {
|
||||||
if (++(*lastValue) == 0)
|
PACK(typedef struct {
|
||||||
*lastValue = lswTimerValue(ls->v2);
|
uint8_t state;
|
||||||
}
|
uint8_t last;
|
||||||
else { // if (*lastValue > 0)
|
}) ls_sticky_struct;
|
||||||
*lastValue -= 1;
|
ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;;
|
||||||
}
|
bool before = lastValue.last & 0x01;
|
||||||
}
|
if (lastValue.state) {
|
||||||
else if (ls->func == LS_FUNC_STICKY) {
|
bool now = getSwitch(ls->v2);
|
||||||
PACK(typedef struct {
|
if (now != before) {
|
||||||
uint8_t state;
|
lastValue.last ^= 1;
|
||||||
uint8_t last;
|
if (!before) {
|
||||||
}) ls_sticky_struct;
|
lastValue.state = 0;
|
||||||
ls_sticky_struct & lastValue = (ls_sticky_struct &)lsLastValue[i];
|
}
|
||||||
bool before = lastValue.last & 0x01;
|
}
|
||||||
if (lastValue.state) {
|
}
|
||||||
bool now = getSwitch(ls->v2);
|
else {
|
||||||
if (now != before) {
|
bool now = getSwitch(ls->v1);
|
||||||
lastValue.last ^= 1;
|
if (before != now) {
|
||||||
if (!before) {
|
lastValue.last ^= 1;
|
||||||
lastValue.state = 0;
|
if (!before) {
|
||||||
|
lastValue.state = 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
#if defined(CPUARM)
|
||||||
bool now = getSwitch(ls->v1);
|
else if (ls->func == LS_FUNC_STAY) {
|
||||||
if (before != now) {
|
PACK(typedef struct {
|
||||||
lastValue.last ^= 1;
|
uint16_t state:1;
|
||||||
if (!before) {
|
uint16_t duration:15;
|
||||||
lastValue.state = 1;
|
}) ls_stay_struct;
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#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];
|
ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;;
|
||||||
lastValue.state = false;
|
lastValue.state = false;
|
||||||
bool state = getSwitch(ls->v1);
|
bool state = getSwitch(ls->v1);
|
||||||
if (state) {
|
if (state) {
|
||||||
if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2))
|
if (ls->v3 == 0 && lastValue.duration == lswTimerValue(ls->v2))
|
||||||
lastValue.state = true;
|
lastValue.state = true;
|
||||||
if (lastValue.duration < 1000)
|
if (lastValue.duration < 1000)
|
||||||
lastValue.duration++;
|
lastValue.duration++;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3))
|
if (lastValue.duration > lswTimerValue(ls->v2) && lastValue.duration <= lswTimerValue(ls->v2+ls->v3))
|
||||||
lastValue.state = true;
|
lastValue.state = true;
|
||||||
lastValue.duration = 0;
|
lastValue.duration = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
LogicalSwitchData *lswAddress(uint8_t idx)
|
LogicalSwitchData *lswAddress(uint8_t idx)
|
||||||
@@ -696,8 +703,13 @@ int16_t lswTimerValue(delayval_t val)
|
|||||||
}
|
}
|
||||||
|
|
||||||
void lswReset() {
|
void lswReset() {
|
||||||
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
|
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
|
||||||
lsLastValue[i] = CS_LAST_VALUE_INIT;
|
lsw_array * lsa = &lswFm[fm];
|
||||||
|
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
|
||||||
|
lsa->ls[i].lastValue = CS_LAST_VALUE_INIT;
|
||||||
|
//todo other values
|
||||||
|
lsa->ls[i].state = false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -715,3 +727,8 @@ getvalue_t convertLswTelemValue(LogicalSwitchData * ls)
|
|||||||
return val;
|
return val;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void copyLswState(uint8_t oldPhase, uint8_t newPhase) {
|
||||||
|
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
|
||||||
|
lswFm[newPhase].ls[i].state = lswFm[oldPhase].ls[i].state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user