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:
parent
c5616f55c8
commit
08be646c9a
4 changed files with 73 additions and 42 deletions
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue