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