src/main/io/rc_controls.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include <math.h>
  23
  24 #include "platform.h"
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28
  29 #include "config/config.h"
  30 #include "config/runtime_config.h"
  31
  32 #include "drivers/system.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro.h"
  36
  37 #include "sensors/barometer.h"
  38 #include "sensors/battery.h"
  39 #include "sensors/sensors.h"
  40 #include "sensors/gyro.h"
  41 #include "sensors/acceleration.h"
  42
  43 #include "io/gps.h"
  44 #include "io/beeper.h"
  45
  46 #include "rx/rx.h"
  47 #include "io/escservo.h"
  48 #include "io/rc_controls.h"
  49 #include "io/rc_curves.h"
  50
  51 #include "flight/flight.h"
  52 #include "flight/navigation.h"
  53
  54 #include "mw.h"
  55
  56
  57 static escAndServoConfig_t *escAndServoConfig;
  58 static pidProfile_t *pidProfile;
  59
  60 static bool isUsingSticksToArm = true;
  61
  62 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  63
  64 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
  65
  66 bool isUsingSticksForArming(void)
  67 {
  68     return isUsingSticksToArm;
  69 }
  70
  71 bool areSticksInApModePosition(uint16_t ap_mode)
  72 {
  73     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
  74 }
  75
  76 throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
  77 {
  78     if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
  79         return THROTTLE_LOW;
  80     else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->mincheck))
  81         return THROTTLE_LOW;
  82
  83     return THROTTLE_HIGH;
  84 }
  85
  86
  87
  88 void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
  89 {
  90     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
  91     static uint8_t rcSticks;            // this hold sticks position for command combos
  92     uint8_t stTmp = 0;
  93     int i;
  94
  95     // ------------------ STICKS COMMAND HANDLER --------------------
  96     // checking sticks positions
  97     for (i = 0; i < 4; i++) {
  98         stTmp >>= 2;
  99         if (rcData[i] > rxConfig->mincheck)
 100             stTmp |= 0x80;  // check for MIN
 101         if (rcData[i] < rxConfig->maxcheck)
 102             stTmp |= 0x40;  // check for MAX
 103     }
 104     if (stTmp == rcSticks) {
 105         if (rcDelayCommand < 250)
 106             rcDelayCommand++;
 107     } else
 108         rcDelayCommand = 0;
 109     rcSticks = stTmp;
 110
 111     // perform actions
 112     if (!isUsingSticksToArm) {
 113
 114         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 115             // Arming via ARM BOX
 116             if (throttleStatus == THROTTLE_LOW) {
 117                 if (ARMING_FLAG(OK_TO_ARM)) {
 118                     mwArm();
 119                 }
 120             }
 121         } else {
 122             // Disarming via ARM BOX
 123             if (ARMING_FLAG(ARMED)) {
 124                 if (disarm_kill_switch) {
 125                     mwDisarm();
 126                 } else if (throttleStatus == THROTTLE_LOW) {
 127                     mwDisarm();
 128                 }
 129             }
 130         }
 131     }
 132
 133     if (rcDelayCommand != 20) {
 134         return;
 135     }
 136
 137     if (ARMING_FLAG(ARMED)) {
 138         // actions during armed
 139
 140         if (isUsingSticksToArm) {
 141             // Disarm on throttle down + yaw
 142             if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)
 143                 mwDisarm();
 144
 145             // Disarm on roll (only when retarded_arm is enabled)
 146             if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
 147                 mwDisarm();
 148         }
 149         return;
 150     }
 151
 152     // actions during not armed
 153     i = 0;
 154
 155     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 156         // GYRO calibration
 157         gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
 158
 159 #ifdef GPS
 160         if (feature(FEATURE_GPS)) {
 161             GPS_reset_home_position();
 162         }
 163 #endif
 164
 165 #ifdef BARO
 166         if (sensors(SENSOR_BARO))
 167             baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 168 #endif
 169
 170         if (!sensors(SENSOR_MAG))
 171             heading = 0; // reset heading to zero after gyro calibration
 172
 173         return;
 174     }
 175
 176     if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 177         // Inflight ACC Calibration
 178         handleInflightCalibrationStickPosition();
 179         return;
 180     }
 181
 182     // Multiple configuration profiles
 183     if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 184         i = 1;
 185     else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 186         i = 2;
 187     else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 188         i = 3;
 189     if (i) {
 190         changeProfile(i - 1);
 191         return;
 192     }
 193
 194     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 195         saveConfigAndNotify();
 196     }
 197
 198     if (isUsingSticksToArm) {
 199
 200         if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
 201             // Arm via YAW
 202             mwArm();
 203             return;
 204         }
 205
 206         if (retarded_arm && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
 207             // Arm via ROLL
 208             mwArm();
 209             return;
 210         }
 211     }
 212
 213     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 214         // Calibrating Acc
 215         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 216         return;
 217     }
 218
 219
 220     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 221         // Calibrating Mag
 222         ENABLE_STATE(CALIBRATE_MAG);
 223         return;
 224     }
 225
 226
 227     // Accelerometer Trim
 228
 229     rollAndPitchTrims_t accelerometerTrimsDelta;
 230     memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
 231
 232     bool shouldApplyRollAndPitchTrimDelta = false;
 233     if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
 234         accelerometerTrimsDelta.values.pitch = 2;
 235         shouldApplyRollAndPitchTrimDelta = true;
 236     } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
 237         accelerometerTrimsDelta.values.pitch = -2;
 238         shouldApplyRollAndPitchTrimDelta = true;
 239     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
 240         accelerometerTrimsDelta.values.roll = 2;
 241         shouldApplyRollAndPitchTrimDelta = true;
 242     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
 243         accelerometerTrimsDelta.values.roll = -2;
 244         shouldApplyRollAndPitchTrimDelta = true;
 245     }
 246     if (shouldApplyRollAndPitchTrimDelta) {
 247         applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
 248         rcDelayCommand = 0; // allow autorepetition
 249         return;
 250     }
 251 }
 252
 253 bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
 254     if (!IS_RANGE_USABLE(range)) {
 255         return false;
 256     }
 257
 258     uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
 259     return (channelValue >= 900 + (range->startStep * 25) &&
 260             channelValue < 900 + (range->endStep * 25));
 261 }
 262
 263 void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
 264 {
 265     rcModeActivationMask = 0;
 266
 267     uint8_t index;
 268
 269     for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 270         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 271
 272         if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
 273             ACTIVATE_RC_MODE(modeActivationCondition->modeId);
 274         }
 275     }
 276 }
 277
 278 uint8_t adjustmentStateMask = 0;
 279
 280 #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
 281 #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
 282
 283 #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
 284
 285 // sync with adjustmentFunction_e
 286 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
 287     {
 288         .adjustmentFunction = ADJUSTMENT_RC_RATE,
 289         .mode = ADJUSTMENT_MODE_STEP,
 290         .data.stepConfig.step = 1
 291     },
 292     {
 293         .adjustmentFunction = ADJUSTMENT_RC_EXPO,
 294         .mode = ADJUSTMENT_MODE_STEP,
 295         .data.stepConfig.step = 1
 296     },
 297     {
 298         .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
 299         .mode = ADJUSTMENT_MODE_STEP,
 300         .data.stepConfig.step = 1
 301     },
 302     {
 303         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
 304         .mode = ADJUSTMENT_MODE_STEP,
 305         .data.stepConfig.step = 1
 306     },
 307     {
 308         .adjustmentFunction = ADJUSTMENT_YAW_RATE,
 309         .mode = ADJUSTMENT_MODE_STEP,
 310         .data.stepConfig.step = 1
 311     },
 312     {
 313         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
 314         .mode = ADJUSTMENT_MODE_STEP,
 315         .data.stepConfig.step = 1
 316     },
 317     {
 318         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
 319         .mode = ADJUSTMENT_MODE_STEP,
 320         .data.stepConfig.step = 1
 321     },
 322     {
 323         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
 324         .mode = ADJUSTMENT_MODE_STEP,
 325         .data.stepConfig.step = 1
 326     },
 327     {
 328         .adjustmentFunction = ADJUSTMENT_YAW_P,
 329         .mode = ADJUSTMENT_MODE_STEP,
 330         .data.stepConfig.step = 1
 331     },
 332     {
 333         .adjustmentFunction = ADJUSTMENT_YAW_I,
 334         .mode = ADJUSTMENT_MODE_STEP,
 335         .data.stepConfig.step = 1
 336     },
 337     {
 338         .adjustmentFunction = ADJUSTMENT_YAW_D,
 339         .mode = ADJUSTMENT_MODE_STEP,
 340         .data.stepConfig.step = 1
 341     },
 342     {
 343         .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
 344         .mode = ADJUSTMENT_MODE_SELECT,
 345         .data.selectConfig.switchPositions = 3
 346     }
 347 };
 348
 349 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 350
 351 adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
 352
 353 void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
 354     adjustmentState_t *adjustmentState = &adjustmentStates[index];
 355
 356     if (adjustmentState->config == adjustmentConfig) {
 357         // already configured
 358         return;
 359     }
 360     adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
 361     adjustmentState->config = adjustmentConfig;
 362     adjustmentState->timeoutAt = 0;
 363
 364     MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
 365 }
 366
 367 void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
 368     int newValue;
 369     float newFloatValue;
 370
 371     if (delta > 0) {
 372         queueConfirmationBeep(2);
 373     } else {
 374         queueConfirmationBeep(1);
 375     }
 376     switch(adjustmentFunction) {
 377         case ADJUSTMENT_RC_RATE:
 378             newValue = (int)controlRateConfig->rcRate8 + delta;
 379             controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c
 380             generatePitchRollCurve(controlRateConfig);
 381         break;
 382         case ADJUSTMENT_RC_EXPO:
 383             newValue = (int)controlRateConfig->rcExpo8 + delta;
 384             controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 385             generatePitchRollCurve(controlRateConfig);
 386             break;
 387         case ADJUSTMENT_THROTTLE_EXPO:
 388             newValue = (int)controlRateConfig->thrExpo8 + delta;
 389             controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 390             generateThrottleCurve(controlRateConfig, escAndServoConfig);
 391             break;
 392         case ADJUSTMENT_PITCH_ROLL_RATE:
 393             newValue = (int)controlRateConfig->rollPitchRate + delta;
 394             controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 395             break;
 396         case ADJUSTMENT_YAW_RATE:
 397             newValue = (int)controlRateConfig->yawRate + delta;
 398             controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 399             break;
 400         case ADJUSTMENT_PITCH_ROLL_P:
 401             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 402                 newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
 403                 pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 404                 newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
 405                 pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 406             } else {
 407                 newValue = (int)pidProfile->P8[PIDPITCH] + delta;
 408                 pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 409                 newValue = (int)pidProfile->P8[PIDROLL] + delta;
 410                 pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 411             }
 412             break;
 413         case ADJUSTMENT_PITCH_ROLL_I:
 414             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 415                 newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
 416                 pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 417                 newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
 418                 pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 419             } else {
 420                 newValue = (int)pidProfile->I8[PIDPITCH] + delta;
 421                 pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 422                 newValue = (int)pidProfile->I8[PIDROLL] + delta;
 423                 pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 424             }
 425             break;
 426         case ADJUSTMENT_PITCH_ROLL_D:
 427             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 428                 newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
 429                 pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 430                 newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
 431                 pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 432             } else {
 433                 newValue = (int)pidProfile->D8[PIDPITCH] + delta;
 434                 pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 435                 newValue = (int)pidProfile->D8[PIDROLL] + delta;
 436                 pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 437             }
 438             break;
 439         case ADJUSTMENT_YAW_P:
 440             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 441                 newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
 442                 pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 443             } else {
 444                 newValue = (int)pidProfile->P8[PIDYAW] + delta;
 445                 pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 446             }
 447             break;
 448         case ADJUSTMENT_YAW_I:
 449             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 450                 newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
 451                 pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 452             } else {
 453                 newValue = (int)pidProfile->I8[PIDYAW] + delta;
 454                 pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 455             }
 456             break;
 457         case ADJUSTMENT_YAW_D:
 458             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 459                 newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
 460                 pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 461             } else {
 462                 newValue = (int)pidProfile->D8[PIDYAW] + delta;
 463                 pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 464             }
 465             break;
 466         default:
 467             break;
 468     };
 469 }
 470
 471 void changeControlRateProfile(uint8_t profileIndex);
 472
 473 void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
 474 {
 475     bool applied = false;
 476
 477     switch(adjustmentFunction) {
 478         case ADJUSTMENT_RATE_PROFILE:
 479             if (getCurrentControlRateProfile() != position) {
 480                 changeControlRateProfile(position);
 481                 applied = true;
 482             }
 483             break;
 484     }
 485
 486     if (applied) {
 487         queueConfirmationBeep(position + 1);
 488     }
 489 }
 490
 491 #define RESET_FREQUENCY_2HZ (1000 / 2)
 492
 493 void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
 494 {
 495     uint8_t adjustmentIndex;
 496     uint32_t now = millis();
 497
 498     for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
 499         adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
 500
 501         if (!adjustmentState->config) {
 502             continue;
 503         }
 504         uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
 505         if (adjustmentFunction == ADJUSTMENT_NONE) {
 506             continue;
 507         }
 508
 509         int32_t signedDiff = now - adjustmentState->timeoutAt;
 510         bool canResetReadyStates = signedDiff >= 0L;
 511
 512         if (canResetReadyStates) {
 513             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 514             MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 515         }
 516
 517
 518         uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
 519
 520         if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
 521             int delta;
 522             if (rcData[channelIndex] > rxConfig->midrc + 200) {
 523                 delta = adjustmentState->config->data.stepConfig.step;
 524             } else if (rcData[channelIndex] < rxConfig->midrc - 200) {
 525                 delta = 0 - adjustmentState->config->data.stepConfig.step;
 526             } else {
 527                 // returning the switch to the middle immediately resets the ready state
 528                 MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 529                 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 530                 continue;
 531             }
 532             if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
 533                 continue;
 534             }
 535
 536             applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
 537         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
 538             uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
 539             uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
 540
 541             applySelectAdjustment(adjustmentFunction, position);
 542         }
 543         MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
 544     }
 545 }
 546
 547 void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
 548 {
 549     uint8_t index;
 550
 551     for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
 552         adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
 553
 554         if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
 555
 556             const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 557
 558             configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
 559         }
 560     }
 561 }
 562
 563 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 564     return MIN(ABS(rcData[axis] - midrc), 500);
 565 }
 566
 567 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
 568 {
 569     uint8_t index;
 570
 571     escAndServoConfig = escAndServoConfigToUse;
 572     pidProfile = pidProfileToUse;
 573
 574     isUsingSticksToArm = true;
 575
 576     for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 577         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 578         if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
 579             isUsingSticksToArm = false;
 580             break;
 581         }
 582     }
 583 }