Skip to content

Commit

Permalink
evaluation logic for LogicSwitchFunctionSafe: disassociates the AndSw…
Browse files Browse the repository at this point in the history 
…itch from affecting the new Logic Switch Type
  • Loading branch information
@shane-droid
shane-droid committed Jun 14, 2022
1 parent c3f1a32 commit 62a3fd1
Showing 1 changed file with 163 additions and 124 deletions.
287 changes: 163 additions & 124 deletions radio/src/switches.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -377,136 +377,144 @@ bool getLogicalSwitch(uint8_t idx)
LogicalSwitchData * ls = lswAddress(idx);
bool result;

swsrc_t s = ls->andsw;

if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) {
if (ls->func != LS_FUNC_STICKY && ls->func != LS_FUNC_EDGE ) {
// AND switch must not affect STICKY and EDGE processing
LS_LAST_VALUE(mixerCurrentFlightMode, 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(mixerCurrentFlightMode, idx) <= 0);
}
else if (s == LS_FAMILY_STICKY) {
result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));
}
else if (s == LS_FAMILY_EDGE) {
result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));

if(ls->func ==LS_FAMILY_SAFE){

result = lswFm[mixerCurrentFlightMode].lsw[idx].state;
}
else {
getvalue_t x = getValueForLogicalSwitch(ls->v1);
getvalue_t y;
if (s == LS_FAMILY_COMP) {
y = getValueForLogicalSwitch(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{
swsrc_t s = ls->andsw;
if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) {
if (ls->func != LS_FUNC_STICKY && ls->func != LS_FUNC_EDGE && ls->func != LS_FUNC_SAFE ) {
// AND switch must not affect STICKY and EDGE processing
LS_LAST_VALUE(mixerCurrentFlightMode, idx) = CS_LAST_VALUE_INIT;
}
result = false;
}
}
else {
mixsrc_t v1 = ls->v1;
// Telemetry
if (v1 >= MIXSRC_FIRST_TELEM) {
if (!TELEMETRY_STREAMING() || IS_FAI_FORBIDDEN(v1-1)) {
result = false;
goto DurationAndDelayProcessing;
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;
}

y = convertLswTelemValue(ls);


}
else if (v1 >= MIXSRC_GVAR1) {
y = ls->v2;
else if (s == LS_FAMILY_TIMER) {
result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) <= 0);
}
else {
y = calc100toRESX(ls->v2);
// else if (s == LS_FAMILY_STICKY) {
// result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));
// }
else if (s == LS_FAMILY_SAFE) {
result = LS_LAST_VALUE(mixerCurrentFlightMode, idx);
}

switch (ls->func) {
case LS_FUNC_VEQUAL:
result = (x==y);
break;
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 (LS_LAST_VALUE(mixerCurrentFlightMode, idx) == CS_LAST_VALUE_INIT) {
LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;
else if (s == LS_FAMILY_EDGE) {
result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));
}
else {
getvalue_t x = getValueForLogicalSwitch(ls->v1);
getvalue_t y;
if (s == LS_FAMILY_COMP) {
y = getValueForLogicalSwitch(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;
}
int16_t diff = x - LS_LAST_VALUE(mixerCurrentFlightMode, idx);
bool update = false;
if (ls->func == LS_FUNC_DIFFEGREATER) {
if (y >= 0) {
result = (diff >= y);
if (diff < 0)
update = true;
}
else {
result = (diff <= y);
if (diff > 0)
update = true;
}
else {
mixsrc_t v1 = ls->v1;
// Telemetry
if (v1 >= MIXSRC_FIRST_TELEM) {
if (!TELEMETRY_STREAMING() || IS_FAI_FORBIDDEN(v1-1)) {
result = false;
goto DurationAndDelayProcessing;
}

y = convertLswTelemValue(ls);


}
else if (v1 >= MIXSRC_GVAR1) {
y = ls->v2;
}
else {
result = (abs(diff) >= y);
y = calc100toRESX(ls->v2);
}
if (result || update) {
LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;

switch (ls->func) {
case LS_FUNC_VEQUAL:
result = (x==y);
break;
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 (LS_LAST_VALUE(mixerCurrentFlightMode, idx) == CS_LAST_VALUE_INIT) {
LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;
}
int16_t diff = x - LS_LAST_VALUE(mixerCurrentFlightMode, idx);
bool update = false;
if (ls->func == LS_FUNC_DIFFEGREATER) {
if (y >= 0) {
result = (diff >= y);
if (diff < 0)
update = true;
}
else {
result = (diff <= y);
if (diff > 0)
update = true;
}
}
else {
result = (abs(diff) >= y);
}
if (result || update) {
LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;
}
break;
}
}
break;
}
}
}
}

DurationAndDelayProcessing:
DurationAndDelayProcessing:

if (ls->delay || ls->duration) {
if (ls->delay || ls->duration) {
LogicalSwitchContext &context = lswFm[mixerCurrentFlightMode].lsw[idx];
if (result) {
if (context.timerState == SWITCH_START) {
Expand Down Expand Up @@ -542,8 +550,9 @@ bool getLogicalSwitch(uint8_t idx)
context.timer = 0;
}
}

}
return result;

}

bool getSwitch(swsrc_t swtch, uint8_t flags)
Expand Down Expand Up @@ -620,27 +629,50 @@ bool getSwitch(swsrc_t swtch, uint8_t flags)

return swtch > 0 ? result : !result;
}

void evalLogicalSwitch_FUNC_SAFE(LogicalSwitchData * ls, LogicalSwitchContext & context){
if(ls->v1 != SWSRC_NONE){
if(getSwitch(ls->v2)){
//ON
if(getSwitch(ls->v1)){
context.state = 1;
}
//OFF
if(!getSwitch(ls->v1)){
context.state = 0;
}
}
}
else{//no source set therfore switch is off
context.state = 0;
}
}
/**
@brief Calculates new state of logical switches for mixerCurrentFlightMode
*/
void evalLogicalSwitches(bool isCurrentFlightmode)
{
for (unsigned int idx=0; idx<MAX_LOGICAL_SWITCHES; idx++) {
LogicalSwitchContext & context = lswFm[mixerCurrentFlightMode].lsw[idx];
bool result = getLogicalSwitch(idx);
if (isCurrentFlightmode) {
if (result) {
if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx);
}
else {
if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx);
LogicalSwitchData * ls = lswAddress(idx);
if(ls->func == LS_FUNC_SAFE){
evalLogicalSwitch_FUNC_SAFE(ls, context);
}
else{
bool result = getLogicalSwitch(idx);
if (isCurrentFlightmode) {
if (result) {
if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx);
}
else {
if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx);
}
}
context.state = result;
}
context.state = result;
}
}


swarnstate_t switches_states = 0;
uint8_t fsswitches_states = 0;
swsrc_t getMovedSwitch()
Expand Down Expand Up @@ -902,6 +934,7 @@ void logicalSwitchesTimerTick()
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
for (uint8_t i=0; i<MAX_LOGICAL_SWITCHES; i++) {
LogicalSwitchData * ls = lswAddress(i);

if (ls->func == LS_FUNC_TIMER) {
int16_t * lastValue = &LS_LAST_VALUE(fm, i);
if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) {
Expand All @@ -916,6 +949,7 @@ void logicalSwitchesTimerTick()
*lastValue -= 1;
}
}

else if (ls->func == LS_FUNC_STICKY) {
ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i);
bool before = lastValue.last & 0x01;
Expand All @@ -942,6 +976,11 @@ void logicalSwitchesTimerTick()
}
}
}
else if (ls->func == LS_FUNC_SAFE) {
LogicalSwitchContext & context = lswFm[mixerCurrentFlightMode].lsw[i];
evalLogicalSwitch_FUNC_SAFE(ls, context);
}

else if (ls->func == LS_FUNC_EDGE) {
ls_stay_struct & lastValue = (ls_stay_struct &)LS_LAST_VALUE(fm, i);
// if this ls was reset by the logicalSwitchesReset() the lastValue will be set to CS_LAST_VALUE_INIT(0x8000)
Expand Down

0 comments on commit 62a3fd1

Please sign in to comment.