diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index f3e1f7392b..1b286d3a84 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -582,10 +582,6 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) // 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_UP 1 #define KEY_DOWN 2 @@ -810,7 +806,7 @@ static void cmsUpdate(timeUs_t currentTimeUs) if (!cmsInMenu) { // 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(); rcDelayMs = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME } @@ -821,22 +817,22 @@ static void cmsUpdate(timeUs_t currentTimeUs) 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; } - else if (IS_HI(PITCH)) { + else if (checkStickPosition(PIT_HI)) { key = KEY_UP; } - else if (IS_LO(PITCH)) { + else if (checkStickPosition(PIT_LO)) { key = KEY_DOWN; } - else if (IS_LO(ROLL)) { + else if (checkStickPosition(ROL_LO)) { key = KEY_LEFT; } - else if (IS_HI(ROLL)) { + else if (checkStickPosition(ROL_HI)) { key = KEY_RIGHT; } - else if (IS_HI(YAW) || IS_LO(YAW)) + else if (checkStickPosition(YAW_HI) || checkStickPosition(YAW_LO)) { key = KEY_ESC; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 23beca60c0..0beceec050 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -69,6 +69,8 @@ static bool isUsingSticksToArm = true; static bool isUsingNAVModes = false; #endif +stickPositions_e rcStickPositions; + 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 @@ -130,26 +132,54 @@ rollPitchStatus_e calculateRollPitchCenterStatus(void) 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) { 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 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 - uint8_t stTmp = 0; - int i; - const timeMs_t currentTimeMs = millis(); - // ------------------ STICKS COMMAND HANDLER -------------------- - // checking sticks positions - for (i = 0; i < 4; i++) { - stTmp >>= 2; - if (rcData[i] > rxConfig()->mincheck) - stTmp |= 0x80; // check for MIN - if (rcData[i] < rxConfig()->maxcheck) - stTmp |= 0x40; // check for MAX - } + updateRcStickPositions(); + + uint32_t stTmp = getRcStickPositions(); if (stTmp == rcSticks) { if (rcDelayCommand < 250) { if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) { @@ -159,6 +189,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s } } else rcDelayCommand = 0; + rcSticks = stTmp; // perform actions @@ -223,7 +254,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s } // actions during not armed - i = 0; + int i = 0; // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 1a284496ec..04be40b619 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -82,18 +82,23 @@ typedef enum { CENTERED } rollPitchStatus_e; -#define ROL_LO (1 << (2 * ROLL)) -#define ROL_CE (3 << (2 * ROLL)) -#define ROL_HI (2 << (2 * ROLL)) -#define PIT_LO (1 << (2 * PITCH)) -#define PIT_CE (3 << (2 * PITCH)) -#define PIT_HI (2 << (2 * PITCH)) -#define YAW_LO (1 << (2 * YAW)) -#define YAW_CE (3 << (2 * YAW)) -#define YAW_HI (2 << (2 * YAW)) -#define THR_LO (1 << (2 * THROTTLE)) -#define THR_CE (3 << (2 * THROTTLE)) -#define THR_HI (2 << (2 * THROTTLE)) +typedef enum { + ROL_LO = (1 << (2 * ROLL)), + ROL_CE = (3 << (2 * ROLL)), + ROL_HI = (2 << (2 * ROLL)), + + PIT_LO = (1 << (2 * PITCH)), + PIT_CE = (3 << (2 * PITCH)), + PIT_HI = (2 << (2 * PITCH)), + + YAW_LO = (1 << (2 * YAW)), + YAW_CE = (3 << (2 * YAW)), + 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 @@ -157,6 +162,9 @@ typedef struct armingConfig_s { PG_DECLARE(armingConfig_t, armingConfig); +stickPositions_e getRcStickPositions(void); +bool checkStickPosition(stickPositions_e stickPos); + bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 28bfb95d4e..2775dbb1ae 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -90,10 +90,6 @@ // 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; //extern uint8_t RSSI; // TODO: not used? @@ -858,7 +854,7 @@ static void osdRefresh(timeUs_t currentTimeUs) } if (refreshTimeout) { - if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + if (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) // hide statistics refreshTimeout = 1; refreshTimeout--; if (!refreshTimeout)