diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 565fa1330..6e431ff81 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -233,18 +233,6 @@ void updateRcOptions(modeActivationCondition_t *modeActivationConditions)
     rcModeActivationMask = 0; // FIXME implement, use rcData & modeActivationConditions
 
     uint8_t index;
-    uint8_t auxChannelSteps[MAX_AUX_CHANNEL_COUNT];
-    for (index = 0; index < MAX_AUX_CHANNEL_COUNT; index++) {
-        uint16_t channelValue = rcData[index + NON_AUX_CHANNEL_COUNT];
-        uint16_t normalizedChannelValue = (constrain(channelValue, 900, 2100) - 900);
-
-        auxChannelSteps[index] = normalizedChannelValue / 25;
-
-        if (normalizedChannelValue > 0 && normalizedChannelValue % 25 == 0) {
-            auxChannelSteps[index]--;
-        }
-        printf("%d\n", auxChannelSteps[index]);
-    }
 
     for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
@@ -254,9 +242,9 @@ void updateRcOptions(modeActivationCondition_t *modeActivationConditions)
             continue;
         }
 
-        uint8_t auxChannelStep = auxChannelSteps[modeActivationCondition->auxChannelIndex];
-        if (auxChannelStep >= modeActivationCondition->rangeStartStep &&
-                auxChannelStep < modeActivationCondition->rangeEndStep) {
+        uint16_t channelValue = constrain(rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
+        if (channelValue >= 900 + (modeActivationCondition->rangeStartStep * 25) &&
+                channelValue < 900 + (modeActivationCondition->rangeEndStep * 25)) {
             ACTIVATE_RC_MODE(modeActivationCondition->modeId);
         }
     }
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 0814bfea6..12959982f 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -89,9 +89,11 @@ typedef enum {
 // this leaves plenty of 'slots' free for cases where you enable multiple modes for a switch
 // position (like gps rth + horizon + baro + beeper)
 
+#define CHANNEL_RANGE_MIN 900
+#define CHANNEL_RANGE_MAX 2100
 
-#define MODE_STEP_TO_CHANNEL_VALUE(step) (900 + 25 * step)
-#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, 900, 2100) - 900) / 25)
+#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)
 
 typedef struct modeActivationCondition_s {
     boxId_e modeId;
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index 7f85cff59..74012b433 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -133,9 +133,9 @@ TEST(RcControlsTest, updateRcOptionsUsingValidAuxConfigurationAndRXValues)
     rcData[AUX2] = PWM_RANGE_MIDDLE;
     rcData[AUX3] = PWM_RANGE_MIN;
     rcData[AUX4] = PWM_RANGE_MAX;
-    rcData[AUX5] = 899;
-    rcData[AUX6] = 2101;
-    rcData[AUX7] = 925;
+    rcData[AUX5] = 899; // value lower that range minimum should be treated the same as the lowest range value
+    rcData[AUX6] = 2101; // value higher than the range maximum should be treated the same as the highest range value
+    rcData[AUX7] = 950; // value equal to range step upper boundary should not activate the mode
 
     // and
     uint32_t expectedMask = 0;