Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-25 17:25:13 +03:00
Code Issues Wiki Activity

Fixes #1039:

Browse source 
* 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:
Damjan Adamic 2014-05-17 17:45:53 +02:00
parent 6e5f064c31
commit 07a5333a08
6 changed files with 744 additions and 736 deletions
Download patch file Download diff file Expand all files Collapse all files

1
radio/src/audio_arm.cpp
View file

@ -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);
}

10
radio/src/gtests.cpp
View file

@ -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);
}

6
radio/src/mixer.cpp
View file

@ -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<MAX_FLIGHT_MODES; p++) {
s_last_switch_used = 0;
if (s_fade_flight_phases & ((ACTIVE_PHASES_TYPE)1 << p)) {
s_current_mixer_flight_mode = p;
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];
weight += fp_act[p];
}
s_last_switch_used = 0;
}
assert(weight);
s_current_mixer_flight_mode = phase;

1
radio/src/opentx.cpp
View file

@ -1521,7 +1521,6 @@ void flightReset()
#endif
lswReset();
s_last_switch_value = 0;
s_mixer_first_run_done = false;
START_SILENCE_PERIOD();

11
radio/src/opentx.h
View file

@ -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

533
radio/src/switches.cpp
View file

@ -36,21 +36,23 @@
#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;
typedef struct {
uint8_t state:1;
#if defined(CPUARM)
uint32_t lswDelays[NUM_LOGICAL_SWITCH];
uint32_t lswDurations[NUM_LOGICAL_SWITCH];
uint8_t lswStates[NUM_LOGICAL_SWITCH];
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 };
@ -157,10 +159,6 @@ void getSwitchesPosition(bool startup)
#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;
@ -221,192 +219,200 @@ bool getSwitch(int8_t swtch)
#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 = (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;
}
}
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_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;
int8_t getMovedSwitch()
{
@ -597,74 +603,75 @@ void checkSwitches()
#endif
}
void lswTimerTick() {
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
LogicalSwitchData * ls = lswAddress(i);
if (ls->func == LS_FUNC_TIMER) {
int16_t *lastValue = &lsLastValue[i];
if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) {
*lastValue = -lswTimerValue(ls->v1);
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
LogicalSwitchData * ls = lswAddress(i);
lsw_struct * lsd = &lswFm[fm].ls[i];
if (ls->func == 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 (*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 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;
}
}
}
}
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;
#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;
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
}
#endif
}
}
}
LogicalSwitchData *lswAddress(uint8_t idx)
@ -696,8 +703,13 @@ int16_t lswTimerValue(delayval_t val)
}
void lswReset() {
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
lsLastValue[i] = CS_LAST_VALUE_INIT;
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
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;
}
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;
}
}