1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00

Unify stick combo processing

This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-02-08 21:58:46 +10:00
parent c5616f55c8
commit 08be646c9a
4 changed files with 73 additions and 42 deletions

View file

@ -582,10 +582,6 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
// Stick/key detection and key codes // Stick/key detection and key codes
#define IS_HI(X) (rcData[X] > 1750)
#define IS_LO(X) (rcData[X] < 1250)
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
#define KEY_NONE 0 #define KEY_NONE 0
#define KEY_UP 1 #define KEY_UP 1
#define KEY_DOWN 2 #define KEY_DOWN 2
@ -810,7 +806,7 @@ static void cmsUpdate(timeUs_t currentTimeUs)
if (!cmsInMenu) { if (!cmsInMenu) {
// Detect menu invocation // Detect menu invocation
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { if (checkStickPosition(THR_CE + YAW_LO + PIT_HI) && !ARMING_FLAG(ARMED)) {
cmsMenuOpen(); cmsMenuOpen();
rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME
} }
@ -821,22 +817,22 @@ static void cmsUpdate(timeUs_t currentTimeUs)
uint8_t key = KEY_NONE; uint8_t key = KEY_NONE;
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { if (checkStickPosition(THR_CE + YAW_LO + PIT_HI) && !ARMING_FLAG(ARMED)) {
key = KEY_MENU; key = KEY_MENU;
} }
else if (IS_HI(PITCH)) { else if (checkStickPosition(PIT_HI)) {
key = KEY_UP; key = KEY_UP;
} }
else if (IS_LO(PITCH)) { else if (checkStickPosition(PIT_LO)) {
key = KEY_DOWN; key = KEY_DOWN;
} }
else if (IS_LO(ROLL)) { else if (checkStickPosition(ROL_LO)) {
key = KEY_LEFT; key = KEY_LEFT;
} }
else if (IS_HI(ROLL)) { else if (checkStickPosition(ROL_HI)) {
key = KEY_RIGHT; key = KEY_RIGHT;
} }
else if (IS_HI(YAW) || IS_LO(YAW)) else if (checkStickPosition(YAW_HI) || checkStickPosition(YAW_LO))
{ {
key = KEY_ESC; key = KEY_ESC;
} }

View file

@ -69,6 +69,8 @@ static bool isUsingSticksToArm = true;
static bool isUsingNAVModes = false; static bool isUsingNAVModes = false;
#endif #endif
stickPositions_e rcStickPositions;
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
@ -130,26 +132,54 @@ rollPitchStatus_e calculateRollPitchCenterStatus(void)
return NOT_CENTERED; return NOT_CENTERED;
} }
stickPositions_e getRcStickPositions(void)
{
return rcStickPositions;
}
bool checkStickPosition(stickPositions_e stickPos)
{
for (int i = 0; i < 4; i++) {
const uint32_t mask = (0x03 << i);
const stickPositions_e checkPos = stickPos & mask;
if ((checkPos != 0) && (checkPos != (rcStickPositions & mask))) {
return false;
}
}
return true;
}
static void updateRcStickPositions(void)
{
stickPositions_e tmp = 0;
tmp |= ((rcData[ROLL] > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2);
tmp |= ((rcData[ROLL] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2);
tmp |= ((rcData[PITCH] > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2);
tmp |= ((rcData[PITCH] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2);
tmp |= ((rcData[YAW] > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2);
tmp |= ((rcData[YAW] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2);
tmp |= ((rcData[THROTTLE] > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2);
tmp |= ((rcData[THROTTLE] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2);
rcStickPositions = tmp;
}
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm) void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm)
{ {
static timeMs_t lastTickTimeMs = 0; static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos static uint32_t rcSticks; // this hold sticks position for command combos
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
uint8_t stTmp = 0;
int i;
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
// ------------------ STICKS COMMAND HANDLER -------------------- updateRcStickPositions();
// checking sticks positions
for (i = 0; i < 4; i++) { uint32_t stTmp = getRcStickPositions();
stTmp >>= 2;
if (rcData[i] > rxConfig()->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig()->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) { if (stTmp == rcSticks) {
if (rcDelayCommand < 250) { if (rcDelayCommand < 250) {
if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) { if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) {
@ -159,6 +189,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
} }
} else } else
rcDelayCommand = 0; rcDelayCommand = 0;
rcSticks = stTmp; rcSticks = stTmp;
// perform actions // perform actions
@ -223,7 +254,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
} }
// actions during not armed // actions during not armed
i = 0; int i = 0;
// GYRO calibration // GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {

View file

@ -82,18 +82,23 @@ typedef enum {
CENTERED CENTERED
} rollPitchStatus_e; } rollPitchStatus_e;
#define ROL_LO (1 << (2 * ROLL)) typedef enum {
#define ROL_CE (3 << (2 * ROLL)) ROL_LO = (1 << (2 * ROLL)),
#define ROL_HI (2 << (2 * ROLL)) ROL_CE = (3 << (2 * ROLL)),
#define PIT_LO (1 << (2 * PITCH)) ROL_HI = (2 << (2 * ROLL)),
#define PIT_CE (3 << (2 * PITCH))
#define PIT_HI (2 << (2 * PITCH)) PIT_LO = (1 << (2 * PITCH)),
#define YAW_LO (1 << (2 * YAW)) PIT_CE = (3 << (2 * PITCH)),
#define YAW_CE (3 << (2 * YAW)) PIT_HI = (2 << (2 * PITCH)),
#define YAW_HI (2 << (2 * YAW))
#define THR_LO (1 << (2 * THROTTLE)) YAW_LO = (1 << (2 * YAW)),
#define THR_CE (3 << (2 * THROTTLE)) YAW_CE = (3 << (2 * YAW)),
#define THR_HI (2 << (2 * THROTTLE)) YAW_HI = (2 << (2 * YAW)),
THR_LO = (1 << (2 * THROTTLE)),
THR_CE = (3 << (2 * THROTTLE)),
THR_HI = (2 << (2 * THROTTLE))
} stickPositions_e;
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 #define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
@ -157,6 +162,9 @@ typedef struct armingConfig_s {
PG_DECLARE(armingConfig_t, armingConfig); PG_DECLARE(armingConfig_t, armingConfig);
stickPositions_e getRcStickPositions(void);
bool checkStickPosition(stickPositions_e stickPos);
bool areUsingSticksToArm(void); bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode); bool areSticksInApModePosition(uint16_t ap_mode);

View file

@ -90,10 +90,6 @@
// Things in both OSD and CMS // Things in both OSD and CMS
#define IS_HI(X) (rcData[X] > 1750)
#define IS_LO(X) (rcData[X] < 1250)
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
bool blinkState = true; bool blinkState = true;
//extern uint8_t RSSI; // TODO: not used? //extern uint8_t RSSI; // TODO: not used?
@ -858,7 +854,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
} }
if (refreshTimeout) { if (refreshTimeout) {
if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics if (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) // hide statistics
refreshTimeout = 1; refreshTimeout = 1;
refreshTimeout--; refreshTimeout--;
if (!refreshTimeout) if (!refreshTimeout)