piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity
Files
4aa9a258eec663054cd78e1789400847a43a6c21
edgetx-gx12/radio/src/switches.cpp
T

1230 lines
33 KiB
C++
Raw Blame History

/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "hal/switch_driver.h"
#include "hal/adc_driver.h"
#include "hal/rgbleds.h"
#include "myeeprom.h"
#include "edgetx.h"
#include "edgetx_constants.h"
#include "os/sleep.h"
#include "switches.h"
#if defined(VOICE_CONTROL_SENSOR)
#include "drivers/CI1302_voice_integration.h"
#endif
#include "input_mapping.h"
#include "inactivity_timer.h"
#include "tasks/mixer_task.h"
#if defined(RADIO_GX12)
#include "targets/taranis/gx12/bsp_io.h"
#endif
#define CS_LAST_VALUE_INIT -32768
#if defined(COLORLCD)
#define SWITCH_WARNING_LIST_X WARNING_LINE_X
#define SWITCH_WARNING_LIST_Y WARNING_LINE_Y+3*FH
#elif LCD_W >= 212
#define SWITCH_WARNING_LIST_X 60
#define SWITCH_WARNING_LIST_Y 4*FH+4
#else
#define SWITCH_WARNING_LIST_X 4
#define SWITCH_WARNING_LIST_Y 4*FH+4
#endif
enum LogicalSwitchContextState {
SWITCH_START,
SWITCH_DELAY,
SWITCH_ENABLE
};
PACK(struct LogicalSwitchContext {
uint8_t state:1;
uint8_t timerState:2;
uint8_t spare:1;
uint8_t deltaTimer:4; // Timer for holding delta function state change active
uint8_t timer;
int16_t lastValue;
});
PACK(struct LogicalSwitchesFlightModeContext {
LogicalSwitchContext lsw[MAX_LOGICAL_SWITCHES];
});
LogicalSwitchesFlightModeContext lswFm[MAX_FLIGHT_MODES];
CircularBuffer<uint8_t, 8> luaSetStickySwitchBuffer;
#define LS_LAST_VALUE(fm, idx) lswFm[fm].lsw[idx].lastValue
tmr10ms_t switchesMidposStart[MAX_SWITCHES];
uint64_t switchesPos = 0;
static_assert(sizeof(uint64_t) * 8 >= ((MAX_SWITCHES - 1) / 2) + 1,
"MAX_SWITCHES too big for uint64_t position state");
tmr10ms_t potsLastposStart[MAX_POTS];
uint8_t potsPos[MAX_POTS];
#define SWITCH_POSITION(sw) (switchesPos & ((MASK_CFN_TYPE)1 << (sw)))
#define POT_POSITION(sw) \
((potsPos[(sw) / XPOTS_MULTIPOS_COUNT] & 0x0f) == \
((sw) % XPOTS_MULTIPOS_COUNT))
#if defined(FUNCTION_SWITCHES)
// Customizable switches
//
// Non pushed : SWSRC_Sx0 = -1024 = Sx(up) = state 0
// Pushed : SWSRC_Sx2 = +1024 = Sx(down) = state 1
uint32_t fsPreviousState = 0;
uint8_t isSwitch3Pos(uint8_t idx)
{
return IS_CONFIG_3POS(idx);
}
#if defined(SIMU)
bool evalFSok = false;
#endif
void setFSStartupPosition()
{
for (uint8_t i = 0; i < switchGetMaxSwitches(); i++) {
if (switchIsCustomSwitch(i)) {
uint8_t startPos = g_model.getSwitchStart(i);
if (g_model.getSwitchType(i) == SWITCH_TOGGLE)
startPos = FS_START_OFF;
switch(startPos) {
case FS_START_OFF:
g_model.cfsSetState(i, 0);
break;
case FS_START_ON:
g_model.cfsSetState(i, 1);
break;
case FS_START_PREVIOUS:
default:
// Do nothing, use existing value
break;
}
}
}
#if defined(SIMU)
evalFSok = true;
#endif
}
void setFSLogicalState(uint8_t index, uint8_t value)
{
g_model.cfsSetState(index, value ? 1 : 0);
}
bool getFSPhysicalState(uint8_t index)
{
#if defined(FUNCTION_SWITCHES)
if (switchIsCustomSwitch(index) && g_model.cfsSFState(index))
return true;
#endif
return switchGetPosition(index) != SWITCH_HW_UP;
}
static bool getFSPreviousPhysicalState(uint8_t index)
{
return (fsPreviousState >> index) & 1;
}
uint8_t getSwitchCountInFSGroup(uint8_t index)
{
uint8_t count = 0;
for (uint8_t i = 0; i < switchGetMaxSwitches(); i++) {
if (switchIsCustomSwitch(i) && g_model.getSwitchGroup(i) == index)
count++;
}
return count;
}
bool isFSGroupUsed(uint8_t index)
{
return getSwitchCountInFSGroup(index) != 0;
}
void evalFunctionSwitches()
{
#if defined(SIMU)
if (!evalFSok) return;
#endif
for (uint8_t i = 0; i < switchGetMaxSwitches(); i++) {
if (switchIsCustomSwitch(i)) {
if (g_model.getSwitchType(i) == SWITCH_NONE) {
continue;
}
bool physicalState = getFSPhysicalState(i);
if (physicalState != getFSPreviousPhysicalState(i)) {
// FS was moved
inactivityTimerReset(ActivitySource::MainControls);
if ((g_model.getSwitchType(i) == SWITCH_2POS && physicalState) ||
(g_model.getSwitchType(i) == SWITCH_TOGGLE)) {
if (g_model.cfsGroupAlwaysOn(g_model.getSwitchGroup(i)) != 0) {
// In an always on group
g_model.cfsSetState(i, 1);
} else {
g_model.cfsSetState(i, g_model.cfsState(i) ^ 1); // Toggle bit
}
}
if (g_model.getSwitchGroup(i) && physicalState) {
// switch is in a group, other in group need to be turned off
for (uint8_t j = 0; j < switchGetMaxSwitches(); j++) {
if ((i != j) && switchIsCustomSwitch(j)) {
if (g_model.getSwitchGroup(j) == g_model.getSwitchGroup(i)) {
g_model.cfsSetState(j, 0);
}
}
}
}
fsPreviousState ^= uint32_t(1) << i; // Toggle state
storageDirty(EE_MODEL);
}
if (!pwrPressed()) {
if (g_model.cfsState(i))
setFSLedON(i);
else
setFSLedOFF(i);
}
}
}
}
bool groupHasSwitchOn(uint8_t group)
{
for (int j = 0; j < switchGetMaxSwitches(); j += 1)
if (switchIsCustomSwitch(j) && g_model.getSwitchGroup(j) == group && g_model.cfsState(j))
return true;
return false;
}
int firstSwitchInGroup(uint8_t group)
{
for (int j = 0; j < switchGetMaxSwitches(); j += 1)
if (switchIsCustomSwitch(j) && g_model.getSwitchGroup(j) == group)
return j;
return -1;
}
int groupDefaultSwitch(uint8_t group)
{
bool allOff = true;
for (int j = 0; j < switchGetMaxSwitches(); j += 1) {
if (switchIsCustomSwitch(j)) {
if (g_model.getSwitchGroup(j) == group) {
if (g_model.getSwitchStart(j) == FS_START_ON)
return j;
if (g_model.getSwitchStart(j) != FS_START_OFF)
allOff = false;
}
}
}
if (allOff)
return switchGetMaxSwitches();
return -1;
}
void setGroupSwitchState(uint8_t group)
{
// Check rules for always on group
// - Toggle switch type not valid, change all switches to 2POS
// - One switch must be turned on, turn on first switch if needed
if (g_model.cfsGroupAlwaysOn(group)) {
for (int j = 0; j < switchGetMaxSwitches(); j += 1) {
if (switchIsCustomSwitch(j) && g_model.getSwitchGroup(j) == group) {
g_model.setSwitchType(j, SWITCH_2POS); // Toggle not valid
}
}
if (!groupHasSwitchOn(group)) {
int sw = firstSwitchInGroup(group);
if (sw >= 0)
setFSLogicalState(sw, 1); // Make sure a switch is on
}
if (groupDefaultSwitch(group) == switchGetMaxSwitches()) {
// Start state for all switches is off - set all to 'last'
for (int j = 0; j < switchGetMaxSwitches(); j += 1)
if (switchIsCustomSwitch(j) && g_model.getSwitchGroup(j) == group)
g_model.setSwitchStart(j, FS_START_PREVIOUS);
}
}
}
#endif // FUNCTION_SWITCHES
div_t switchInfo(int switchPosition)
{
return div(switchPosition - SWSRC_FIRST_SWITCH, 3);
}
int switchLookupIdx(char c)
{
uint8_t idx = 1; // Sx
if (c >= '1' && c <= '9') {
idx = 2; // SWx
}
auto max_switches = switchGetMaxAllSwitches();
for (int i = 0; i < max_switches; i++) {
const char *name = switchGetDefaultName(i);
if (name[idx] == c) return i;
}
return -1;
}
int switchLookupIdx(const char* name, size_t len)
{
if (len < 2) return -1;
if (name[0] != 'S' && name[0] != 'F'
#if defined(VOICE_CONTROL_SENSOR)
&& name[0] != 'V' // VGR/VFL
#endif
) return -1;
auto max_switches = switchGetMaxAllSwitches();
for (int i = 0; i < max_switches; i++) {
const char *sw_name = switchGetDefaultName(i);
if (strlen(sw_name) == len && strncmp(sw_name, name, len) == 0) return i;
}
return -1;
}
char switchGetLetter(uint8_t idx)
{
if (idx >= switchGetMaxAllSwitches() + MAX_FLEX_SWITCHES)
return -1;
const char* name = switchGetDefaultName(idx);
if (!name) return -1;
return name[strlen(name) - 1];
}
const char* fsSwitchGroupGetCanonicalName(uint8_t idx)
{
static const char fsgroupname[3][4] = {"GR1", "GR2", "GR3"};
return &fsgroupname[idx][0];
}
SwitchConfig switchGetMaxType(uint8_t idx)
{
auto hw_type = switchGetHwType(idx);
if (hw_type == SWITCH_HW_2POS) {
return SWITCH_2POS;
} else {
return SWITCH_3POS;
}
}
static uint64_t checkSwitchPosition(uint8_t idx, bool startup)
{
uint64_t result = 0;
uint32_t index = idx * 3;
auto pos = switchGetPosition(idx);
switch(pos) {
case SWITCH_HW_UP:
result = ((MASK_CFN_TYPE)1 << index);
switchesMidposStart[idx] = 0;
break;
case SWITCH_HW_DOWN:
index += 2;
result = ((MASK_CFN_TYPE)1 << index);
switchesMidposStart[idx] = 0;
break;
case SWITCH_HW_MID:
if (startup || SWITCH_POSITION(index + 1) ||
g_eeGeneral.switchesDelay == SWITCHES_DELAY_NONE ||
(switchesMidposStart[idx] &&
(tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) >
SWITCHES_DELAY())) {
index += 1;
result = ((MASK_CFN_TYPE)1 << index);
switchesMidposStart[idx] = 0;
} else {
result = (switchesPos & ((MASK_CFN_TYPE)0x7 << index));
if (!switchesMidposStart[idx]) {
switchesMidposStart[idx] = get_tmr10ms();
}
}
break;
}
if (!(switchesPos & result)) {
#if defined(VOICE_CONTROL_SENSOR) && !defined(SIMU)
if (!CI1302_voiceSwitchSuppressMovedAudio(idx, startup))
#endif
PLAY_SWITCH_MOVED(index);
}
return result;
}
void getSwitchesPosition(bool startup)
{
uint64_t newPos = 0;
for (unsigned i = 0; i < switchGetMaxAllSwitches(); i++) {
if (!SWITCH_EXISTS(i)) continue;
newPos |= checkSwitchPosition(i, startup);
}
switchesPos = newPos;
auto max_pots = adcGetMaxInputs(ADC_INPUT_FLEX);
auto offset = adcGetInputOffset(ADC_INPUT_FLEX);
for (int i = 0; i < max_pots; i++) {
if (IS_POT_MULTIPOS(i)) {
auto analog_idx = offset + i;
StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[analog_idx];
#if defined(SIMU)
{
uint8_t count = XPOTS_MULTIPOS_COUNT - 1;
#else
if (IS_MULTIPOS_CALIBRATED(calib)) {
uint8_t count = calib->count;
#endif
uint8_t pos = anaIn(analog_idx) / (2 * RESX / count);
uint8_t previousPos = potsPos[i] >> 4;
uint8_t previousStoredPos = potsPos[i] & 0x0F;
if (startup) {
potsPos[i] = (pos << 4) | pos;
}
else if (pos != previousPos) {
potsLastposStart[i] = get_tmr10ms();
potsPos[i] = (pos << 4) | previousStoredPos;
} else if (g_eeGeneral.switchesDelay == SWITCHES_DELAY_NONE ||
(tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) >
SWITCHES_DELAY()) {
potsLastposStart[i] = 0;
potsPos[i] = (pos << 4) | pos;
if (previousStoredPos != pos) {
PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH + i * XPOTS_MULTIPOS_COUNT + pos);
}
}
}
}
}
}
uint8_t getSwitchCount()
{
int count = 0;
for (int i = 0; i < switchGetMaxAllSwitches(); ++i) {
if (SWITCH_EXISTS(i)) {
++count;
}
}
return count;
}
#if !defined(COLORLCD)
uint8_t switchGetMaxRow(uint8_t col)
{
uint8_t lastrow = 0;
for (int i = 0; i < switchGetMaxAllSwitches(); ++i) {
if (SWITCH_EXISTS(i)) {
auto switch_display = switchGetDisplayPosition(i);
if (switch_display.col == col)
lastrow = switch_display.row > lastrow ? switch_display.row : lastrow;
}
}
return lastrow;
}
#endif
getvalue_t getValueForLogicalSwitch(mixsrc_t i)
{
getvalue_t result = getValue(i);
if (i>=MIXSRC_FIRST_INPUT && i<=MIXSRC_LAST_INPUT) {
int8_t trimIdx = virtualInputsTrims[i-MIXSRC_FIRST_INPUT];
if (trimIdx >= 0) {
int16_t trim = trims[trimIdx];
if (trimIdx == inputMappingConvertMode(inputMappingGetThrottle()) && g_model.throttleReversed)
result -= trim;
else
result += trim;
}
}
return result;
}
PACK(typedef struct {
uint8_t state;
uint8_t last;
}) ls_sticky_struct;
PACK(typedef struct {
uint16_t state:1;
uint16_t duration:15;
}) ls_stay_struct;
bool getLSStickyState(uint8_t idx)
{
return lswFm[mixerCurrentFlightMode].lsw[idx].lastValue & 1;
}
void logicalSwitchesInit(bool force)
{
for (unsigned int idx=0; idx<MAX_LOGICAL_SWITCHES; idx++) {
LogicalSwitchData * ls = lswAddress(idx);
if (ls->func == LS_FUNC_STICKY && (force || ls->lsPersist)) {
lswFm[mixerCurrentFlightMode].lsw[idx].lastValue = ls->lsState;
}
}
}
bool getLogicalSwitch(uint8_t idx)
{
LogicalSwitchData * ls = lswAddress(idx);
LogicalSwitchContext &context = lswFm[mixerCurrentFlightMode].lsw[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
context.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 = (context.lastValue <= 0);
}
else if (s == LS_FAMILY_STICKY) {
result = (context.lastValue & (1<<0));
}
else if (s == LS_FAMILY_EDGE) {
result = (context.lastValue & (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;
}
}
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_FIRST_GVAR) {
y = ls->v2;
}
else {
y = calc100toRESX(ls->v2);
}
switch (ls->func) {
case LS_FUNC_VEQUAL:
result = (x==y);
break;
case LS_FUNC_VALMOSTEQUAL:
#if defined(GVARS)
if (v1 >= MIXSRC_FIRST_GVAR && 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 (context.lastValue == CS_LAST_VALUE_INIT) {
context.lastValue = x;
}
int16_t diff = x - context.lastValue;
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) {
context.deltaTimer = 10;
} else if (context.deltaTimer > 0) {
// Hold active state for 100ms to ensure state change is seen
context.deltaTimer -= 1;
result = true;
}
if (result || update) {
context.lastValue = x;
}
break;
}
}
}
}
DurationAndDelayProcessing:
if (ls->delay || ls->duration) {
if (result) {
if (context.timerState == SWITCH_START) {
// set delay timer
context.timerState = SWITCH_DELAY;
context.timer = (ls->func == LS_FUNC_EDGE ? 0 : ls->delay);
}
if (context.timerState == SWITCH_DELAY) {
if (context.timer) {
result = false; // return false while delay timer running
}
else {
// set duration timer
context.timerState = SWITCH_ENABLE;
context.timer = ls->duration;
}
}
if (context.timerState == SWITCH_ENABLE) {
result = (ls->duration==0 || context.timer>0); // return false after duration timer runs out
if (!result && ls->func == LS_FUNC_STICKY) {
ls_sticky_struct & lastValue = (ls_sticky_struct &)context.lastValue;
lastValue.state = 0;
}
}
}
else if (context.timerState == SWITCH_ENABLE && ls->duration > 0 && context.timer > 0) {
result = true;
}
else {
context.timerState = SWITCH_START;
context.timer = 0;
}
}
return result;
}
bool getSwitch(swsrc_t swtch, uint8_t flags)
{
bool result;
if (swtch == SWSRC_NONE)
return true;
uint16_t cs_idx = abs(swtch);
if (cs_idx == SWSRC_ONE) {
result = !s_mixer_first_run_done;
}
else if (cs_idx == SWSRC_ON) {
result = true;
}
#if defined(DEBUG_LATENCY)
else if (cs_idx == SWSRC_LATENCY_TOGGLE) {
result = latencyToggleSwitch;
}
#endif
else if (cs_idx <= SWSRC_LAST_SWITCH) {
cs_idx -= SWSRC_FIRST_SWITCH;
#if defined(FUNCTION_SWITCHES)
div_t qr = div(cs_idx, 3);
if (switchIsCustomSwitch(qr.quot)) {
auto value = g_model.cfsState(qr.quot);
result = qr.rem == 0 ? !value : (qr.rem == 2 ? value : false);
} else
#endif
{
div_t qr = div(cs_idx, 3);
if (SWITCH_EXISTS(qr.quot)) {
auto sw_cfg = g_model.getSwitchType(qr.quot);
if (flags & GETSWITCH_MIDPOS_DELAY) {
result = SWITCH_POSITION(cs_idx);
// Handle 2POS switch installed in 3POS slot
if (!result && qr.rem == SWITCH_HW_DOWN && (sw_cfg == SWITCH_2POS || sw_cfg == SWITCH_TOGGLE))
result = SWITCH_POSITION(cs_idx - 1);
} else {
result = switchState(cs_idx);
// Handle 2POS switch installed in 3POS slot
if (!result && qr.rem == SWITCH_HW_DOWN && (sw_cfg == SWITCH_2POS || sw_cfg == SWITCH_TOGGLE))
result = switchState(cs_idx - 1);
}
} else {
result = false;
}
}
}
else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) {
result = POT_POSITION(cs_idx - SWSRC_FIRST_MULTIPOS_SWITCH);
}
else if (cs_idx <= SWSRC_LAST_TRIM) {
uint8_t idx = cs_idx - SWSRC_FIRST_TRIM;
idx = (inputMappingConvertMode(idx/2) << 1) + (idx & 1);
result = trimDown(idx);
}
else if (cs_idx == SWSRC_RADIO_ACTIVITY) {
result = (inactivity.counter < 2);
}
else if (cs_idx == SWSRC_TRAINER_CONNECTED) {
result = isTrainerConnected();
}
else if (cs_idx >= SWSRC_FIRST_SENSOR) {
result = !telemetryItems[cs_idx-SWSRC_FIRST_SENSOR].isOld();
}
else if (cs_idx == SWSRC_TELEMETRY_STREAMING) {
result = TELEMETRY_STREAMING();
}
else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) {
#if defined(FLIGHT_MODES)
uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE;
if (flags & GETSWITCH_MIDPOS_DELAY)
result = (idx == flightModeTransitionLast);
else
result = (idx == mixerCurrentFlightMode);
#else
result = false;
#endif
}
else {
cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
result = lswFm[mixerCurrentFlightMode].lsw[cs_idx].state;
}
return swtch > 0 ? result : !result;
}
uint8_t getXPotPosition(uint8_t idx)
{
if (idx >= MAX_POTS || !IS_POT_MULTIPOS(idx)) return 0;
return potsPos[idx] & 0x0F;
}
/**
@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);
}
}
context.state = result;
if ((g_model.logicalSw[idx].func == LS_FUNC_STICKY) && (g_model.logicalSw[idx].lsState != result)) {
g_model.logicalSw[idx].lsState = result;
storageDirty(EE_MODEL);
}
}
}
static inline uint8_t _bits_set(uint8_t val, uint8_t bits)
{
uint8_t bits_set = 0;
do {
if (val & 1) ++bits_set;
val >>= 1;
} while (--bits);
return bits_set;
}
swarnstate_t switches_states = 0;
swsrc_t getMovedSwitch()
{
static tmr10ms_t s_move_last_time = 0;
swsrc_t result = 0;
// Switches
auto max_switches = switchGetMaxAllSwitches();
for (uint8_t i = 0; i < max_switches; i++) {
if (SWITCH_EXISTS(i)) {
swarnstate_t mask = ((swarnstate_t) 0x03 << (i * 2));
uint8_t prev = (switches_states & mask) >> (i * 2);
uint8_t next;
#if defined(FUNCTION_SWITCHES)
if (switchIsCustomSwitch(i))
next = g_model.cfsState(i) ? 3 : 1;
else
next = (1024 + getValue(MIXSRC_FIRST_SWITCH + i)) / 1024 + 1;
#else
next = (1024 + getValue(MIXSRC_FIRST_SWITCH + i)) / 1024 + 1;
#endif
if (prev != next) {
switches_states =
(switches_states & (~mask)) | ((swarnstate_t)(next) << (i * 2));
result = (3 * i) + next;
}
}
}
// Multipos
for (int i = 0; i < MAX_POTS; i++) {
if (IS_POT_MULTIPOS(i)) {
StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[MAX_STICKS + i];
#if defined(SIMU)
{
uint8_t count = XPOTS_MULTIPOS_COUNT - 1;
#else
if (IS_MULTIPOS_CALIBRATED(calib)) {
uint8_t count = calib->count;
#endif
uint8_t prev = potsPos[i] & 0x0F;
uint8_t next = anaIn(MAX_STICKS + i) / (2 * RESX / count);
if (prev != next) {
result = SWSRC_FIRST_MULTIPOS_SWITCH + i * XPOTS_MULTIPOS_COUNT + next;
}
}
}
}
// Trims: only detect trims configured as 3P
for (int i = 0; i < keysGetMaxTrims(); i++) {
if (getRawTrimValue(mixerCurrentFlightMode, i).mode == TRIM_MODE_3POS) {
uint8_t tidx = inputMappingConvertMode(i) * 2;
if (trimDown(tidx)) result = SWSRC_FIRST_TRIM + i * 2;
else if (trimDown(tidx+1)) result = SWSRC_FIRST_TRIM + i * 2 + 1;
}
}
if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 100)
result = 0;
s_move_last_time = get_tmr10ms();
return result;
}
bool isSwitchWarningRequired(uint16_t &bad_pots)
{
if (!mixerTaskRunning()) getADC();
getMovedSwitch();
bool warn = false;
for (int i = 0; i < switchGetMaxAllSwitches(); i++) {
if (SWITCH_WARNING_ALLOWED(i)) {
uint8_t warnState = g_model.getSwitchWarning(i);
if (warnState) {
swarnstate_t swState = g_model.getSwitchStateForWarning(i);
if (warnState != swState) {
warn = true;
}
}
}
}
if (g_model.potsWarnMode) {
evalFlightModeMixes(e_perout_mode_normal, 0);
bad_pots = 0;
for (int i = 0; i < adcGetMaxInputs(ADC_INPUT_FLEX); i++) {
if (!IS_POT_SLIDER_AVAILABLE(i)) continue;
if ((g_model.potsWarnEnabled & (1 << i)) &&
(abs(g_model.potsWarnPosition[i] - GET_LOWRES_POT_POSITION(i)) > 1)) {
warn = true;
bad_pots |= (1 << i);
}
}
}
return warn;
}
#if defined(COLORLCD)
#include "switch_warn_dialog.h"
void checkSwitches()
{
uint16_t bad_pots = 0;
if (!isSwitchWarningRequired(bad_pots))
return;
LED_ERROR_BEGIN();
auto dialog = new SwitchWarnDialog();
MainWindow::instance()->blockUntilClose(true, [=]() {
return dialog->deleted();
});
LED_ERROR_END();
}
#elif defined(GUI)
void checkSwitches()
{
swarnstate_t last_bad_switches = 0xff;
uint16_t bad_pots = 0, last_bad_pots = 0xff;
#if defined(PWR_BUTTON_PRESS)
bool refresh = false;
#endif
while (true) {
#if defined(RADIO_GX12)
_poll_switches();
#endif
if (!isSwitchWarningRequired(bad_pots))
break;
cancelSplash();
LED_ERROR_BEGIN();
resetBacklightTimeout();
// first - display warning
if (last_bad_switches != switches_states || last_bad_pots != bad_pots) {
drawAlertBox(STR_SWITCHWARN, nullptr, STR_PRESS_ANY_KEY_TO_SKIP);
if (last_bad_switches == 0xff || last_bad_pots == 0xff) {
AUDIO_ERROR_MESSAGE(AU_SWITCH_ALERT);
}
int x = SWITCH_WARNING_LIST_X;
int y = SWITCH_WARNING_LIST_Y;
int numWarnings = 0;
for (int i = 0; i < switchGetMaxAllSwitches(); ++i) {
if (SWITCH_WARNING_ALLOWED(i)) {
uint8_t warnState = g_model.getSwitchWarning(i);
if (warnState) {
swarnstate_t swState = g_model.getSwitchStateForWarning(i);
if (warnState != swState) {
if (++numWarnings < 6) {
const char* s = getSwitchWarnSymbol(warnState);
drawSource(x, y, MIXSRC_FIRST_SWITCH + i, INVERS);
lcdDrawText(lcdNextPos, y, s, INVERS);
x = lcdNextPos + 3;
}
}
}
}
}
if (g_model.potsWarnMode) {
for (int i = 0; i < MAX_POTS; i++) {
if (!IS_POT_SLIDER_AVAILABLE(i)) continue;
if (g_model.potsWarnEnabled & (1 << i)) {
if (abs(g_model.potsWarnPosition[i] - GET_LOWRES_POT_POSITION(i)) > 1) {
if (++numWarnings < 6) {
drawSource(x, y, MIXSRC_FIRST_POT + i, INVERS);
const char* symbol;
auto warn_pos = g_model.potsWarnPosition[i];
if (IS_SLIDER(i)) {
symbol = warn_pos > GET_LOWRES_POT_POSITION(i)
? CHAR_UP
: CHAR_DOWN;
} else {
symbol = warn_pos > GET_LOWRES_POT_POSITION(i)
? CHAR_RIGHT
: CHAR_LEFT;
}
lcdDrawText(lcdNextPos, y, symbol, INVERS);
x = lcdNextPos + 3;
}
}
}
}
}
if (numWarnings >= 6) {
lcdDrawText(x, y, "...", 0);
}
last_bad_pots = bad_pots;
lcdRefresh();
lcdSetContrast();
waitKeysReleased();
last_bad_switches = switches_states;
}
if (keyDown())
break;
#if defined(PWR_BUTTON_PRESS)
uint32_t power = pwrCheck();
if (power == e_power_off) {
drawSleepBitmap();
boardOff();
break;
}
else if (power == e_power_press) {
refresh = true;
}
else if (power == e_power_on && refresh) {
last_bad_switches = 0xff;
last_bad_pots = 0xff;
refresh = false;
}
#else
if (pwrCheck() == e_power_off) {
break;
}
#endif
checkBacklight();
WDG_RESET();
sleep_ms(10);
}
LED_ERROR_END();
}
#endif // GUI
void logicalSwitchesTimerTick()
{
#if (MAX_LOGICAL_SWITCHES != 64)
#warning "The following code assumes that MAX_LOGICAL_SWITCHES == 64!"
#endif
// Read messages from Lua in the buffer and flick switches
uint8_t msg = luaSetStickySwitchBuffer.read();
while(msg) {
uint8_t i = msg & 0x3F;
uint8_t s = msg >> 7;
LogicalSwitchData * ls = lswAddress(i);
if (ls->func == LS_FUNC_STICKY) {
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i);
lastValue.state = s;
bool now;
if (s)
now = getSwitch(ls->v2);
else
now = getSwitch(ls->v1);
if (now)
lastValue.last |= 1;
else
lastValue.last &= ~1;
}
}
msg = luaSetStickySwitchBuffer.read();
}
// Update logical switches
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) {
*lastValue = -lswTimerValue(ls->v1);
} else if (*lastValue < 0) {
if (++(*lastValue) == 0) *lastValue = lswTimerValue(ls->v2);
} else { // if (*lastValue > 0)
if (--(*lastValue) == 0) *lastValue = -lswTimerValue(ls->v1);
}
} else if (ls->func == LS_FUNC_STICKY) {
ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i);
bool before = lastValue.last & 0x01;
if (lastValue.state) {
if (ls->v2 != SWSRC_NONE) { // only if used / source set
bool now = getSwitch(ls->v2);
if (now != before) {
lastValue.last ^= 1;
if (!before) {
lastValue.state = 0;
}
}
}
}
else {
if (ls->v1 != SWSRC_NONE) { // only if used / source set
bool now = getSwitch(ls->v1);
if (before != now) {
lastValue.last ^= 1;
if (!before) {
lastValue.state = 1;
}
}
}
}
} 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)
// when it is unpacked into ls_stay_struct the lastValue.duration will have a value of 0x4000
// this will produce an instant true for edge logical switch if the second parameter is big enough.
// So we reset it here.
if (LS_LAST_VALUE(fm, i) == CS_LAST_VALUE_INIT) {
lastValue.duration = 0;
}
lastValue.state = false;
bool state = getSwitch(ls->v1);
if (state) {
if (ls->v3 == -1 && lastValue.duration == lswTimerValue(ls->v2))
lastValue.state = true;
if (lastValue.duration < 1000)
lastValue.duration++;
}
else {
if (lastValue.duration > lswTimerValue(ls->v2) && (ls->v3 == 0 || lastValue.duration <= lswTimerValue(ls->v2+ls->v3)))
lastValue.state = true;
lastValue.duration = 0;
}
}
// decrement delay/duration timer
LogicalSwitchContext &context = lswFm[fm].lsw[i];
if (context.timer) {
context.timer--;
}
}
}
}
LogicalSwitchData * lswAddress(uint8_t idx)
{
return &g_model.logicalSw[idx];
}
uint8_t lswFamily(uint8_t func)
{
if (func <= LS_FUNC_ANEG)
return LS_FAMILY_OFS;
else if (func <= LS_FUNC_XOR)
return LS_FAMILY_BOOL;
else if (func == LS_FUNC_EDGE)
return LS_FAMILY_EDGE;
else if (func <= LS_FUNC_LESS)
return LS_FAMILY_COMP;
else if (func <= LS_FUNC_ADIFFEGREATER)
return LS_FAMILY_DIFF;
else
return LS_FAMILY_TIMER+func-LS_FUNC_TIMER;
}
// val = [-129,-110] => [0,19] (step 1)
// val = [-109,6] => [20,595] (step 5)
// val = [7,122] => [600,1750] (step 10)
//
int16_t lswTimerValue(delayval_t val)
{
return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10));
}
void logicalSwitchesReset()
{
memset(lswFm, 0, sizeof(lswFm));
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
for (uint8_t i=0; i<MAX_LOGICAL_SWITCHES; i++) {
LS_LAST_VALUE(fm, i) = CS_LAST_VALUE_INIT;
}
}
luaSetStickySwitchBuffer.clear();
}
getvalue_t convertLswTelemValue(LogicalSwitchData * ls)
{
getvalue_t val;
val = convert16bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, ls->v2);
return val;
}
void logicalSwitchesCopyState(uint8_t src, uint8_t dst)
{
lswFm[dst] = lswFm[src];
}
void setAllPreflightSwitchStates()
{
getMovedSwitch();
// Mask switches enabled for warnings
swarnstate_t sw_mask = 0;
for(uint8_t i = 0; i < switchGetMaxAllSwitches(); i++) {
if (SWITCH_WARNING_ALLOWED(i))
if (g_model.getSwitchWarning(i))
sw_mask |= (0x03 << (2 * i));
}
g_model.switchWarning = switches_states & sw_mask;
AUDIO_WARNING1();
storageDirty(EE_MODEL);
}
Reference in New Issue View Git Blame Copy Permalink