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

A couple of changes on top of projectkkglider branch

Browse source
This commit is contained in:
bsongis 2014-05-19 14:52:07 +02:00
parent dffb5a7a79
commit 1a7ff9239d
3 changed files with 277 additions and 209 deletions
Download patch file Download diff file Expand all files Collapse all files

13
radio/src/mixer.cpp
View file

@ -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<MAX_FLIGHT_MODES; p++) {
LS_RECURSIVE_EVALUATION_RESET();
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);
@ -967,6 +975,7 @@ void evalMixes(uint8_t tick10ms)
sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
weight += fp_act[p];
}
LS_RECURSIVE_EVALUATION_RESET();
}
assert(weight);
s_current_mixer_flight_mode = phase;

9
radio/src/opentx.h
View file

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

464
radio/src/switches.cpp
View file

@ -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 = (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 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 = (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(s_current_mixer_flight_mode, idx) == CS_LAST_VALUE_INIT)
LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = x;
int16_t diff = x - LS_LAST_VALUE(s_current_mixer_flight_mode, idx);
if (ls->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,203 +413,51 @@ 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_idx<NUM_LOGICAL_SWITCH; cs_idx++) {
bool result = false;
LogicalSwitchData * ls = lswAddress(cs_idx);
lsw_struct * lsd = &lswFm[(s_last_phase == s_current_mixer_flight_mode)?0:1].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 || ls->duration) {
for (unsigned int idx=0; idx<NUM_LOGICAL_SWITCH; idx++) {
LogicalSwitchContext &context = lswFm[s_current_mixer_flight_mode].lsw[idx];
bool result = getLogicalSwitch(idx);
if (isCurrentPhase) {
if (result) {
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
}
if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx);
}
else {
lsd->ddState = 0;
if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx);
}
}
context.state = result;
}
}
#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;
}
}
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; fm<MAX_FLIGHT_MODES; fm++) {
#endif
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;
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--;
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; fm<MAX_FLIGHT_MODES; fm++) {
#endif
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
lsa->ls[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