Add bitarray support; Migrage rcModeActivationMask to bitArray

Makefile

@@ -541,6 +541,7 @@ COMMON_SRC = \
             build/build_config.c \
             build/debug.c \
             build/version.c \
+            common/bitarray.c \
             common/encoding.c \
             common/filter.c \
             common/maths.c \
@@ -589,6 +590,7 @@ COMMON_SRC = \
             fc/rc_adjustments.c \
             fc/rc_controls.c \
             fc/rc_curves.c \
+            fc/rc_modes.c \
             fc/runtime_config.c \
             fc/stats.c \
             flight/failsafe.c \

src/main/blackbox/blackbox.c

@@ -49,6 +49,7 @@
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"
@@ -383,7 +384,7 @@ typedef struct blackboxGpsState_s {
 
 // This data is updated really infrequently:
 typedef struct blackboxSlowState_s {
-    uint16_t flightModeFlags;
+    uint32_t flightModeFlags; // extend this data size (from uint16_t)
     uint8_t stateFlags;
     uint8_t failsafePhase;
     bool rxSignalReceived;
@@ -391,9 +392,13 @@ typedef struct blackboxSlowState_s {
     int32_t hwHealthStatus;
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
+//From rc_controls.c
+extern boxBitmask_t rcModeActivationMask;
+
 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 
 static uint32_t blackboxLastArmingBeep = 0;
+static uint32_t blackboxLastFlightModeFlags = 0;
 
 static struct {
     uint32_t headerIndex;
@@ -898,7 +903,7 @@ static void writeSlowFrame(void)
  */
 static void loadSlowState(blackboxSlowState_t *slow)
 {
-    slow->flightModeFlags = flightModeFlags;
+    memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
     slow->stateFlags = stateFlags;
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
@@ -1026,6 +1031,7 @@ void blackboxStart(void)
      * it finally plays the beep for this arming event.
      */
     blackboxLastArmingBeep = getArmingBeepTimeMicros();
+    memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
 
     blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
 }
@@ -1418,6 +1424,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
     case FLIGHT_LOG_EVENT_SYNC_BEEP:
         blackboxWriteUnsignedVB(data->syncBeep.time);
         break;
+    case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
+        blackboxWriteUnsignedVB(data->flightMode.flags);
+        blackboxWriteUnsignedVB(data->flightMode.lastFlags);
+        break;
     case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
         if (data->inflightAdjustment.floatFlag) {
             blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
@@ -1453,6 +1463,19 @@ static void blackboxCheckAndLogArmingBeep(void)
     }
 }
 
+/* monitor the flight mode event status and trigger an event record if the state changes */
+static void blackboxCheckAndLogFlightMode(void)
+{
+    // Use != so that we can still detect a change if the counter wraps
+    if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
+        flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
+        eventData.lastFlags = blackboxLastFlightModeFlags;
+        memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
+        memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
+        blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
+    }
+}
+
 /*
  * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
  * the portion of logged loop iterations.
@@ -1498,6 +1521,7 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
         writeIntraframe();
     } else {
         blackboxCheckAndLogArmingBeep();
+        blackboxCheckAndLogFlightMode();
 
         if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
             /*

src/main/blackbox/blackbox_fielddefs.h

@@ -111,6 +111,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
     FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
+    FLIGHT_LOG_EVENT_FLIGHTMODE = 30, // Add new event type for flight mode status.
     FLIGHT_LOG_EVENT_IMU_FAILURE = 40,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
@@ -119,6 +120,11 @@ typedef struct flightLogEvent_syncBeep_s {
     uint32_t time;
 } flightLogEvent_syncBeep_t;
 
+typedef struct flightLogEvent_flightMode_s { // New Event Data type
+    uint32_t flags;
+    uint32_t lastFlags;
+} flightLogEvent_flightMode_t;
+
 typedef struct flightLogEvent_inflightAdjustment_s {
     uint8_t adjustmentFunction;
     bool floatFlag;
@@ -135,6 +141,7 @@ typedef struct flightLogEvent_loggingResume_s {
 
 typedef union flightLogEventData_u {
     flightLogEvent_syncBeep_t syncBeep;
+    flightLogEvent_flightMode_t flightMode; // New event data
     flightLogEvent_inflightAdjustment_t inflightAdjustment;
     flightLogEvent_loggingResume_t loggingResume;
 } flightLogEventData_t;

src/main/common/bitarray.c

@@ -0,0 +1,38 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bitarray.h"
+
+#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8))))
+
+bool bitArrayGet(const void *array, unsigned bit)
+{
+    return BITARRAY_BIT_OP((uint32_t*)array, bit, &);
+}
+
+void bitArraySet(void *array, unsigned bit)
+{
+    BITARRAY_BIT_OP((uint32_t*)array, bit, |=);
+}
+
+void bitArrayClr(void *array, unsigned bit)
+{
+    BITARRAY_BIT_OP((uint32_t*)array, bit, &=~);
+}

src/main/common/bitarray.h

@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+bool bitArrayGet(const void *array, unsigned bit);
+void bitArraySet(void *array, unsigned bit);
+void bitArrayClr(void *array, unsigned bit);

src/main/fc/cli.c

@@ -71,6 +71,7 @@ extern uint8_t __config_end;
 #include "fc/controlrate_profile.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"

src/main/fc/config.c

@@ -72,6 +72,7 @@
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "navigation/navigation.h"

src/main/fc/fc_core.c

@@ -53,6 +53,7 @@
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "io/beeper.h"

src/main/fc/fc_msp.c

@@ -48,6 +48,7 @@
 #include "fc/fc_msp.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"

src/main/fc/rc_adjustments.h

@@ -20,6 +20,7 @@
 #include "config/parameter_group.h"
 
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 
 //#define USE_INFLIGHT_PROFILE_ADJUSTMENT - not currently enabled
 

src/main/fc/rc_controls.c

@@ -42,6 +42,7 @@
 #include "fc/fc_core.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/pid.h"
@@ -63,23 +64,10 @@
 #define AIRMODE_DEADBAND 25
 #define MIN_RC_TICK_INTERVAL_MS     20
 
-// true if arming is done via the sticks (as opposed to a switch)
-static bool isUsingSticksToArm = true;
-
-// Count of mode activation ranged (per box mode)
-static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
-
-#ifdef NAV
-// true if pilot has any of GPS modes configured
-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
-
 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
@@ -97,21 +85,6 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
     .auto_disarm_delay = 5
 );
 
-PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
-PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
-
-bool isUsingSticksForArming(void)
-{
-    return isUsingSticksToArm;
-}
-
-#if defined(NAV)
-bool isUsingNavigationModes(void)
-{
-    return isUsingNAVModes;
-}
-#endif
-
 bool areSticksInApModePosition(uint16_t ap_mode)
 {
     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
@@ -197,7 +170,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
     rcSticks = stTmp;
 
     // perform actions
-    if (!isUsingSticksToArm) {
+    if (!isUsingSticksForArming()) {
         if (IS_RC_MODE_ACTIVE(BOXARM)) {
             rcDisarmTicks = 0;
             // Arming via ARM BOX
@@ -235,7 +208,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
         return;
     }
 
-   if (isUsingSticksToArm) {
+   if (isUsingSticksForArming()) {
         // Disarm on throttle down + yaw
         if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
             // Dont disarm if fixedwing and motorstop
@@ -300,7 +273,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
 
 
     // Arming by sticks
-    if (isUsingSticksToArm) {
+    if (isUsingSticksForArming()) {
         if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
             // Auto arm on throttle when using fixedwing and motorstop
             if (throttleStatus != THROTTLE_LOW) {
@@ -352,94 +325,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
     }
 }
 
-bool isModeActivationConditionPresent(boxId_e modeId)
-{
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
-            return true;
-        }
-    }
-
-    return false;
-}
-
-bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
-    if (!IS_RANGE_USABLE(range)) {
-        return false;
-    }
-
-    uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
-    return (channelValue >= 900 + (range->startStep * 25) &&
-            channelValue < 900 + (range->endStep * 25));
-}
-
-void updateActivatedModes(void)
-{
-    // Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
-    // We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
-
-    uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
-    memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
-
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
-            // Increment the number of valid conditions for this mode
-            activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
-        }
-    }
-
-    // Disable all modes to begin with
-    rcModeActivationMask = 0;
-
-    // Now see which modes should be enabled
-    for (int modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) {
-        // only modes with conditions specified are considered
-        if (specifiedConditionCountPerMode[modeIndex] > 0) {
-            // For AND logic, the specified condition count and valid condition count must be the same.
-            // For OR logic, the valid condition count must be greater than zero.
-
-            if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
-                // AND the conditions
-                if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
-                    ACTIVATE_RC_MODE(modeIndex);
-                }
-            }
-            else {
-                // OR the conditions
-                if (activeConditionCountPerMode[modeIndex] > 0) {
-                    ACTIVATE_RC_MODE(modeIndex);
-                }
-            }
-        }
-    }
-}
-
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
     return MIN(ABS(rcData[axis] - midrc), 500);
 }
 
-void updateUsedModeActivationConditionFlags(void)
-{
-    memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
-            specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
-        }
-    }
-
-    isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
-
-#ifdef NAV
-    isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
-                        isModeActivationConditionPresent(BOXNAVRTH) ||
-                        isModeActivationConditionPresent(BOXNAVWP);
-#endif
-}
-
-void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm)
-{
-    modeActivationConditionsMutable(macIndex)->modeId = modeId;
-    modeActivationConditionsMutable(macIndex)->auxChannelIndex = auxChannelIndex;
-    modeActivationConditionsMutable(macIndex)->range.startStep = CHANNEL_VALUE_TO_STEP(startPwm);
-    modeActivationConditionsMutable(macIndex)->range.endStep = CHANNEL_VALUE_TO_STEP(endPwm);
-}

src/main/fc/rc_controls.h

@@ -19,47 +19,6 @@
 
 #include "config/parameter_group.h"
 
-typedef enum {
-    BOXARM          = 0,
-    BOXANGLE        = 1,
-    BOXHORIZON      = 2,
-    BOXNAVALTHOLD   = 3,    // old BOXBARO
-    BOXHEADINGHOLD  = 4,    // old MAG
-    BOXHEADFREE     = 5,
-    BOXHEADADJ      = 6,
-    BOXCAMSTAB      = 7,
-    BOXNAVRTH       = 8,    // old GPSHOME
-    BOXNAVPOSHOLD   = 9,    // old GPSHOLD
-    BOXPASSTHRU     = 10,
-    BOXBEEPERON     = 11,
-    BOXLEDLOW       = 12,
-    BOXLLIGHTS      = 13,
-    BOXNAVLAUNCH    = 14,
-    BOXOSD          = 15,
-    BOXTELEMETRY    = 16,
-    BOXBLACKBOX     = 17,
-    BOXFAILSAFE     = 18,
-    BOXNAVWP        = 19,
-    BOXAIRMODE      = 20,
-    BOXHOMERESET    = 21,
-    BOXGCSNAV       = 22,
-    BOXKILLSWITCH   = 23,   // old HEADING LOCK
-    BOXSURFACE      = 24,
-    BOXFLAPERON     = 25,
-    BOXTURNASSIST   = 26,
-    BOXAUTOTRIM     = 27,
-    BOXAUTOTUNE     = 28,
-    BOXCAMERA1      = 29,
-    BOXCAMERA2      = 30,
-    BOXCAMERA3      = 31,
-    CHECKBOX_ITEM_COUNT
-} boxId_e;
-
-extern uint32_t rcModeActivationMask;
-
-#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask)
-#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId))
-
 typedef enum rc_alias {
     ROLL = 0,
     PITCH,
@@ -103,48 +62,6 @@ typedef enum {
     THR_HI = (2 << (2 * THROTTLE))
 } stickPositions_e;
 
-#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
-
-#define CHANNEL_RANGE_MIN 900
-#define CHANNEL_RANGE_MAX 2100
-
-#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
-#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
-
-#define MIN_MODE_RANGE_STEP 0
-#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
-
-
-// steps are 25 apart
-// a value of 0 corresponds to a channel value of 900 or less
-// a value of 48 corresponds to a channel value of 2100 or more
-// 48 steps between 900 and 1200
-typedef struct channelRange_s {
-    uint8_t startStep;
-    uint8_t endStep;
-} channelRange_t;
-
-typedef struct modeActivationCondition_s {
-    boxId_e modeId;
-    uint8_t auxChannelIndex;
-    channelRange_t range;
-} modeActivationCondition_t;
-
-#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
-
-PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
-
-typedef enum {
-    MODE_OPERATOR_OR, // default
-    MODE_OPERATOR_AND
-} modeActivationOperator_e;
-
-typedef struct modeActivationOperatorConfig_s {
-    modeActivationOperator_e modeActivationOperator;
-} modeActivationOperatorConfig_t;
-
-PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig);
-
 extern int16_t rcCommand[4];
 
 typedef struct rcControlsConfig_s {
@@ -168,21 +85,9 @@ PG_DECLARE(armingConfig_t, armingConfig);
 stickPositions_e getRcStickPositions(void);
 bool checkStickPosition(stickPositions_e stickPos);
 
-bool areUsingSticksToArm(void);
-
 bool areSticksInApModePosition(uint16_t ap_mode);
 throttleStatus_e calculateThrottleStatus(void);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
 
-bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
-void updateActivatedModes(void);
-
-bool isUsingSticksForArming(void);
-bool isUsingNavigationModes(void);
-
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
-bool isModeActivationConditionPresent(boxId_e modeId);
-void updateUsedModeActivationConditionFlags(void);
-
-void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

src/main/fc/rc_modes.c

@@ -0,0 +1,159 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "rc_modes.h"
+
+#include "common/bitarray.h"
+#include "common/maths.h"
+
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+
+#include "rx/rx.h"
+
+static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
+static bool isUsingSticksToArm = true;
+#ifdef NAV
+static bool isUsingNAVModes = false;
+#endif
+
+boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
+
+PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
+PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
+
+bool isUsingSticksForArming(void)
+{
+    return isUsingSticksToArm;
+}
+
+#if defined(NAV)
+bool isUsingNavigationModes(void)
+{
+    return isUsingNAVModes;
+}
+#endif
+
+
+bool IS_RC_MODE_ACTIVE(boxId_e boxId)
+{
+    return bitArrayGet(&rcModeActivationMask, boxId);
+}
+
+void rcModeUpdate(boxBitmask_t *newState)
+{
+    rcModeActivationMask = *newState;
+}
+
+bool isModeActivationConditionPresent(boxId_e modeId)
+{
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
+{
+    if (!IS_RANGE_USABLE(range)) {
+        return false;
+    }
+
+    uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
+    return (channelValue >= 900 + (range->startStep * 25) &&
+            channelValue < 900 + (range->endStep * 25));
+}
+
+void updateActivatedModes(void)
+{
+    // Disable all modes to begin with
+    boxBitmask_t newMask;
+    memset(&newMask, 0, sizeof(newMask));
+
+    // Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
+    // We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
+    uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
+    memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
+
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
+            // Increment the number of valid conditions for this mode
+            activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
+        }
+    }
+
+    // Now see which modes should be enabled
+    for (int modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) {
+        // only modes with conditions specified are considered
+        if (specifiedConditionCountPerMode[modeIndex] > 0) {
+            // For AND logic, the specified condition count and valid condition count must be the same.
+            // For OR logic, the valid condition count must be greater than zero.
+
+            if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
+                // AND the conditions
+                if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
+                    bitArraySet(&newMask, modeIndex);
+                }
+            }
+            else {
+                // OR the conditions
+                if (activeConditionCountPerMode[modeIndex] > 0) {
+                    bitArraySet(&newMask, modeIndex);
+                }
+            }
+        }
+    }
+
+    rcModeUpdate(&newMask);
+}
+
+void updateUsedModeActivationConditionFlags(void)
+{
+    memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
+            specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
+        }
+    }
+
+    isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
+
+#ifdef NAV
+    isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
+                        isModeActivationConditionPresent(BOXNAVRTH) ||
+                        isModeActivationConditionPresent(BOXNAVWP);
+#endif
+}
+
+void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm)
+{
+    modeActivationConditionsMutable(macIndex)->modeId = modeId;
+    modeActivationConditionsMutable(macIndex)->auxChannelIndex = auxChannelIndex;
+    modeActivationConditionsMutable(macIndex)->range.startStep = CHANNEL_VALUE_TO_STEP(startPwm);
+    modeActivationConditionsMutable(macIndex)->range.endStep = CHANNEL_VALUE_TO_STEP(endPwm);
+}

src/main/fc/rc_modes.h

@@ -0,0 +1,114 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#include "config/parameter_group.h"
+
+typedef enum {
+    BOXARM          = 0,
+    BOXANGLE        = 1,
+    BOXHORIZON      = 2,
+    BOXNAVALTHOLD   = 3,    // old BOXBARO
+    BOXHEADINGHOLD  = 4,    // old MAG
+    BOXHEADFREE     = 5,
+    BOXHEADADJ      = 6,
+    BOXCAMSTAB      = 7,
+    BOXNAVRTH       = 8,    // old GPSHOME
+    BOXNAVPOSHOLD   = 9,    // old GPSHOLD
+    BOXPASSTHRU     = 10,
+    BOXBEEPERON     = 11,
+    BOXLEDLOW       = 12,
+    BOXLLIGHTS      = 13,
+    BOXNAVLAUNCH    = 14,
+    BOXOSD          = 15,
+    BOXTELEMETRY    = 16,
+    BOXBLACKBOX     = 17,
+    BOXFAILSAFE     = 18,
+    BOXNAVWP        = 19,
+    BOXAIRMODE      = 20,
+    BOXHOMERESET    = 21,
+    BOXGCSNAV       = 22,
+    BOXKILLSWITCH   = 23,   // old HEADING LOCK
+    BOXSURFACE      = 24,
+    BOXFLAPERON     = 25,
+    BOXTURNASSIST   = 26,
+    BOXAUTOTRIM     = 27,
+    BOXAUTOTUNE     = 28,
+    BOXCAMERA1      = 29,
+    BOXCAMERA2      = 30,
+    BOXCAMERA3      = 31,
+    CHECKBOX_ITEM_COUNT
+} boxId_e;
+
+// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior
+typedef struct boxBitmask_s { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t;
+
+#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20
+
+#define CHANNEL_RANGE_MIN 900
+#define CHANNEL_RANGE_MAX 2100
+
+#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step)
+#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25)
+
+#define MIN_MODE_RANGE_STEP 0
+#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25)
+
+#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
+
+// steps are 25 apart
+// a value of 0 corresponds to a channel value of 900 or less
+// a value of 48 corresponds to a channel value of 2100 or more
+// 48 steps between 900 and 1200
+typedef struct channelRange_s {
+    uint8_t startStep;
+    uint8_t endStep;
+} channelRange_t;
+
+typedef struct modeActivationCondition_s {
+    boxId_e modeId;
+    uint8_t auxChannelIndex;
+    channelRange_t range;
+} modeActivationCondition_t;
+
+typedef enum {
+    MODE_OPERATOR_OR, // default
+    MODE_OPERATOR_AND
+} modeActivationOperator_e;
+
+typedef struct modeActivationOperatorConfig_s {
+    modeActivationOperator_e modeActivationOperator;
+} modeActivationOperatorConfig_t;
+
+PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
+PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig);
+
+bool IS_RC_MODE_ACTIVE(boxId_e boxId);
+void rcModeUpdate(boxBitmask_t *newState);
+
+bool isModeActivationConditionPresent(boxId_e modeId);
+
+bool isUsingSticksForArming(void);
+bool isUsingNavigationModes(void);
+bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
+
+void updateActivatedModes(void);
+void updateUsedModeActivationConditionFlags(void);
+void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

src/main/flight/failsafe.c

@@ -37,6 +37,7 @@
 #include "fc/fc_core.h"
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 #include "fc/controlrate_profile.h"
 

src/main/flight/servos.c

@@ -41,6 +41,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"

src/main/io/beeper.c

@@ -40,6 +40,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "config/feature.h"

src/main/io/ledstrip.c

@@ -44,6 +44,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/failsafe.h"

src/main/navigation/navigation.c

@@ -37,6 +37,7 @@
 #include "fc/fc_core.h"
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"

src/main/rx/rx.c

@@ -42,6 +42,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 
 #include "flight/failsafe.h"
 

src/main/telemetry/telemetry.c

@@ -32,6 +32,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "io/serial.h"