Unify stick combo processing

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;
         }

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) {

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

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)