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 "build_config.h"
  27
  28 #include "common/axis.h"
  29 #include "common/maths.h"
  30
  31 #include "config/config.h"
  32 #include "config/runtime_config.h"
  33
  34 #include "drivers/system.h"
  35 #include "drivers/sensor.h"
  36 #include "drivers/accgyro.h"
  37
  38 #include "sensors/barometer.h"
  39 #include "sensors/battery.h"
  40 #include "sensors/sensors.h"
  41 #include "sensors/gyro.h"
  42 #include "sensors/acceleration.h"
  43
  44 #include "rx/rx.h"
  45
  46 #include "io/gps.h"
  47 #include "io/beeper.h"
  48 #include "io/escservo.h"
  49 #include "io/rc_controls.h"
  50 #include "io/rc_curves.h"
  51
  52 #include "io/display.h"
  53
  54 #include "flight/pid.h"
  55 #include "flight/navigation_rewrite.h"
  56 #include "flight/failsafe.h"
  57
  58 #include "blackbox/blackbox.h"
  59
  60 #include "mw.h"
  61
  62
  63 static escAndServoConfig_t *escAndServoConfig;
  64 static pidProfile_t *pidProfile;
  65
  66 // true if arming is done via the sticks (as opposed to a switch)
  67 static bool isUsingSticksToArm = true;
  68
  69 #ifdef NAV
  70 // true if pilot has any of GPS modes configured
  71 static bool isUsingNAVModes = false;
  72 #endif
  73
  74 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  75
  76 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
  77
  78
  79 void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
  80 #ifndef BLACKBOX
  81     UNUSED(adjustmentFunction);
  82     UNUSED(newValue);
  83 #else
  84     if (feature(FEATURE_BLACKBOX)) {
  85         flightLogEvent_inflightAdjustment_t eventData;
  86         eventData.adjustmentFunction = adjustmentFunction;
  87         eventData.newValue = newValue;
  88         eventData.floatFlag = false;
  89         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
  90     }
  91 #endif
  92 }
  93
  94 void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
  95 #ifndef BLACKBOX
  96     UNUSED(adjustmentFunction);
  97     UNUSED(newFloatValue);
  98 #else
  99     if (feature(FEATURE_BLACKBOX)) {
 100         flightLogEvent_inflightAdjustment_t eventData;
 101         eventData.adjustmentFunction = adjustmentFunction;
 102         eventData.newFloatValue = newFloatValue;
 103         eventData.floatFlag = true;
 104         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 105     }
 106 #endif
 107 }
 108
 109 bool isUsingSticksForArming(void)
 110 {
 111     return isUsingSticksToArm;
 112 }
 113
 114 #if defined(NAV)
 115 bool isUsingNavigationModes(void)
 116 {
 117     return isUsingNAVModes;
 118 }
 119 #endif
 120
 121 bool areSticksInApModePosition(uint16_t ap_mode)
 122 {
 123     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 124 }
 125
 126 throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 127 {
 128     if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
 129         return THROTTLE_LOW;
 130     else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->mincheck))
 131         return THROTTLE_LOW;
 132
 133     return THROTTLE_HIGH;
 134 }
 135
 136 void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
 137 {
 138     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
 139     static uint8_t rcSticks;            // this hold sticks position for command combos
 140     uint8_t stTmp = 0;
 141     int i;
 142
 143     // ------------------ STICKS COMMAND HANDLER --------------------
 144     // checking sticks positions
 145     for (i = 0; i < 4; i++) {
 146         stTmp >>= 2;
 147         if (rcData[i] > rxConfig->mincheck)
 148             stTmp |= 0x80;  // check for MIN
 149         if (rcData[i] < rxConfig->maxcheck)
 150             stTmp |= 0x40;  // check for MAX
 151     }
 152     if (stTmp == rcSticks) {
 153         if (rcDelayCommand < 250)
 154             rcDelayCommand++;
 155     } else
 156         rcDelayCommand = 0;
 157     rcSticks = stTmp;
 158
 159     // perform actions
 160     if (!isUsingSticksToArm) {
 161
 162         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 163             // Arming via ARM BOX
 164             if (throttleStatus == THROTTLE_LOW) {
 165                 if (ARMING_FLAG(OK_TO_ARM)) {
 166                     mwArm();
 167                 }
 168             }
 169         } else {
 170             // Disarming via ARM BOX
 171
 172             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
 173                 if (disarm_kill_switch) {
 174                     mwDisarm();
 175                 } else if (throttleStatus == THROTTLE_LOW) {
 176                     mwDisarm();
 177                 }
 178             }
 179         }
 180     }
 181
 182     if (rcDelayCommand != 20) {
 183         return;
 184     }
 185
 186     if (isUsingSticksToArm) {
 187         // Disarm on throttle down + yaw
 188         if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 189             if (ARMING_FLAG(ARMED))
 190                 mwDisarm();
 191             else {
 192                 beeper(BEEPER_DISARM_REPEAT);    // sound tone while stick held
 193                 rcDelayCommand = 0;              // reset so disarm tone will repeat
 194             }
 195         }
 196     }
 197
 198     if (ARMING_FLAG(ARMED)) {
 199         // actions during armed
 200         return;
 201     }
 202
 203     // actions during not armed
 204     i = 0;
 205
 206     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 207         // GYRO calibration
 208         gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
 209         return;
 210     }
 211
 212     // Multiple configuration profiles
 213     if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 214         i = 1;
 215     else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 216         i = 2;
 217     else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 218         i = 3;
 219     if (i) {
 220         changeProfile(i - 1);
 221         return;
 222     }
 223
 224     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 225         saveConfigAndNotify();
 226     }
 227
 228     if (isUsingSticksToArm) {
 229
 230         if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
 231             // Arm via YAW
 232             mwArm();
 233             return;
 234         }
 235     }
 236
 237     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 238         // Calibrating Acc
 239         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 240         return;
 241     }
 242
 243
 244     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 245         // Calibrating Mag
 246         ENABLE_STATE(CALIBRATE_MAG);
 247         return;
 248     }
 249
 250
 251     // Accelerometer Trim
 252     if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
 253         applyAndSaveBoardAlignmentDelta(0, -2);
 254         rcDelayCommand = 10;
 255         return;
 256     } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
 257         applyAndSaveBoardAlignmentDelta(0, 2);
 258         rcDelayCommand = 10;
 259         return;
 260     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
 261         applyAndSaveBoardAlignmentDelta(-2, 0);
 262         rcDelayCommand = 10;
 263         return;
 264     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
 265         applyAndSaveBoardAlignmentDelta(2, 0);
 266         rcDelayCommand = 10;
 267         return;
 268     }
 269
 270 #ifdef DISPLAY
 271     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 272         displayDisablePageCycling();
 273     }
 274
 275     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 276         displayEnablePageCycling();
 277     }
 278 #endif
 279
 280 }
 281
 282 bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
 283 {
 284     uint8_t index;
 285
 286     for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 287         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 288
 289         if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
 290             return true;
 291         }
 292     }
 293
 294     return false;
 295 }
 296
 297 bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
 298     if (!IS_RANGE_USABLE(range)) {
 299         return false;
 300     }
 301
 302     uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
 303     return (channelValue >= 900 + (range->startStep * 25) &&
 304             channelValue < 900 + (range->endStep * 25));
 305 }
 306
 307 void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
 308 {
 309     rcModeActivationMask = 0;
 310
 311     uint8_t index;
 312
 313     for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 314         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 315
 316         if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) {
 317             ACTIVATE_RC_MODE(modeActivationCondition->modeId);
 318         }
 319     }
 320 }
 321
 322 uint8_t adjustmentStateMask = 0;
 323
 324 #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
 325 #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
 326
 327 #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
 328
 329 // sync with adjustmentFunction_e
 330 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
 331     {
 332         .adjustmentFunction = ADJUSTMENT_RC_RATE,
 333         .mode = ADJUSTMENT_MODE_STEP,
 334         .data = { .stepConfig = { .step = 1 }}
 335     },
 336     {
 337         .adjustmentFunction = ADJUSTMENT_RC_EXPO,
 338         .mode = ADJUSTMENT_MODE_STEP,
 339         .data = { .stepConfig = { .step = 1 }}
 340     },
 341     {
 342         .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
 343         .mode = ADJUSTMENT_MODE_STEP,
 344         .data = { .stepConfig = { .step = 1 }}
 345     },
 346     {
 347         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
 348         .mode = ADJUSTMENT_MODE_STEP,
 349         .data = { .stepConfig = { .step = 1 }}
 350     },
 351     {
 352         .adjustmentFunction = ADJUSTMENT_YAW_RATE,
 353         .mode = ADJUSTMENT_MODE_STEP,
 354         .data = { .stepConfig = { .step = 1 }}
 355     },
 356     {
 357         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
 358         .mode = ADJUSTMENT_MODE_STEP,
 359         .data = { .stepConfig = { .step = 1 }}
 360     },
 361     {
 362         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
 363         .mode = ADJUSTMENT_MODE_STEP,
 364         .data = { .stepConfig = { .step = 1 }}
 365     },
 366     {
 367         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
 368         .mode = ADJUSTMENT_MODE_STEP,
 369         .data = { .stepConfig = { .step = 1 }}
 370     },
 371     {
 372         .adjustmentFunction = ADJUSTMENT_YAW_P,
 373         .mode = ADJUSTMENT_MODE_STEP,
 374         .data = { .stepConfig = { .step = 1 }}
 375     },
 376     {
 377         .adjustmentFunction = ADJUSTMENT_YAW_I,
 378         .mode = ADJUSTMENT_MODE_STEP,
 379         .data = { .stepConfig = { .step = 1 }}
 380     },
 381     {
 382         .adjustmentFunction = ADJUSTMENT_YAW_D,
 383         .mode = ADJUSTMENT_MODE_STEP,
 384         .data = { .stepConfig = { .step = 1 }}
 385     },
 386     {
 387         .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
 388         .mode = ADJUSTMENT_MODE_SELECT,
 389         .data = { .selectConfig = { .switchPositions = 3 }}
 390     },
 391     {
 392         .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
 393         .mode = ADJUSTMENT_MODE_STEP,
 394         .data = { .stepConfig = { .step = 1 }}
 395     },
 396     {
 397         .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
 398         .mode = ADJUSTMENT_MODE_STEP,
 399         .data = { .stepConfig = { .step = 1 }}
 400     },
 401     {
 402         .adjustmentFunction = ADJUSTMENT_PITCH_P,
 403         .mode = ADJUSTMENT_MODE_STEP,
 404         .data = { .stepConfig = { .step = 1 }}
 405     },
 406     {
 407         .adjustmentFunction = ADJUSTMENT_PITCH_I,
 408         .mode = ADJUSTMENT_MODE_STEP,
 409         .data = { .stepConfig = { .step = 1 }}
 410     },
 411     {
 412         .adjustmentFunction = ADJUSTMENT_PITCH_D,
 413         .mode = ADJUSTMENT_MODE_STEP,
 414         .data = { .stepConfig = { .step = 1 }}
 415     },
 416     {
 417         .adjustmentFunction = ADJUSTMENT_ROLL_P,
 418         .mode = ADJUSTMENT_MODE_STEP,
 419         .data = { .stepConfig = { .step = 1 }}
 420     },
 421     {
 422         .adjustmentFunction = ADJUSTMENT_ROLL_I,
 423         .mode = ADJUSTMENT_MODE_STEP,
 424         .data = { .stepConfig = { .step = 1 }}
 425     },
 426     {
 427         .adjustmentFunction = ADJUSTMENT_ROLL_D,
 428         .mode = ADJUSTMENT_MODE_STEP,
 429         .data = { .stepConfig = { .step = 1 }}
 430     }
 431 };
 432
 433 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 434
 435 adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
 436
 437 void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) {
 438     adjustmentState_t *adjustmentState = &adjustmentStates[index];
 439
 440     if (adjustmentState->config == adjustmentConfig) {
 441         // already configured
 442         return;
 443     }
 444     adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
 445     adjustmentState->config = adjustmentConfig;
 446     adjustmentState->timeoutAt = 0;
 447
 448     MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
 449 }
 450
 451 void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
 452     int newValue;
 453     float newFloatValue;
 454
 455     if (delta > 0) {
 456         beeperConfirmationBeeps(2);
 457     } else {
 458         beeperConfirmationBeeps(1);
 459     }
 460     switch(adjustmentFunction) {
 461         case ADJUSTMENT_RC_RATE:
 462             newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c
 463             controlRateConfig->rcRate8 = newValue;
 464             generatePitchRollCurve(controlRateConfig);
 465             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
 466         break;
 467         case ADJUSTMENT_RC_EXPO:
 468             newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 469             controlRateConfig->rcExpo8 = newValue;
 470             generatePitchRollCurve(controlRateConfig);
 471             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
 472         break;
 473         case ADJUSTMENT_THROTTLE_EXPO:
 474             newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
 475             controlRateConfig->thrExpo8 = newValue;
 476             generateThrottleCurve(controlRateConfig, escAndServoConfig);
 477             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
 478         break;
 479         case ADJUSTMENT_PITCH_ROLL_RATE:
 480         case ADJUSTMENT_PITCH_RATE:
 481             newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
 482             controlRateConfig->rates[FD_PITCH] = newValue;
 483             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
 484             if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
 485                 break;
 486             }
 487             // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
 488         case ADJUSTMENT_ROLL_RATE:
 489             newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
 490             controlRateConfig->rates[FD_ROLL] = newValue;
 491             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
 492             break;
 493         case ADJUSTMENT_YAW_RATE:
 494             newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
 495             controlRateConfig->rates[FD_YAW] = newValue;
 496             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
 497             break;
 498         case ADJUSTMENT_PITCH_ROLL_P:
 499         case ADJUSTMENT_PITCH_P:
 500             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 501                 newFloatValue = constrainf(pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 502                 pidProfile->P_f[PIDPITCH] = newFloatValue;
 503                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_P, newFloatValue);
 504             } else {
 505                 newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 506                 pidProfile->P8[PIDPITCH] = newValue;
 507                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
 508             }
 509             if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
 510                 break;
 511             }
 512             // follow though for combined ADJUSTMENT_PITCH_ROLL_P
 513         case ADJUSTMENT_ROLL_P:
 514             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 515                 newFloatValue = constrainf(pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 516                 pidProfile->P_f[PIDROLL] = newFloatValue;
 517                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_P, newFloatValue);
 518             } else {
 519                 newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 520                 pidProfile->P8[PIDROLL] = newValue;
 521                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
 522             }
 523             break;
 524         case ADJUSTMENT_PITCH_ROLL_I:
 525         case ADJUSTMENT_PITCH_I:
 526             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 527                 newFloatValue = constrainf(pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 528                 pidProfile->I_f[PIDPITCH] = newFloatValue;
 529                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_I, newFloatValue);
 530             } else {
 531                 newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 532                 pidProfile->I8[PIDPITCH] = newValue;
 533                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
 534             }
 535             if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
 536                 break;
 537             }
 538             // follow though for combined ADJUSTMENT_PITCH_ROLL_I
 539         case ADJUSTMENT_ROLL_I:
 540             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 541                 newFloatValue = constrainf(pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 542                 pidProfile->I_f[PIDROLL] = newFloatValue;
 543                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_I, newFloatValue);
 544             } else {
 545                 newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 546                 pidProfile->I8[PIDROLL] = newValue;
 547                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
 548             }
 549             break;
 550         case ADJUSTMENT_PITCH_ROLL_D:
 551         case ADJUSTMENT_PITCH_D:
 552             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 553                 newFloatValue = constrainf(pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 554                 pidProfile->D_f[PIDPITCH] = newFloatValue;
 555                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_PITCH_D, newFloatValue);
 556             } else {
 557                 newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 558                 pidProfile->D8[PIDPITCH] = newValue;
 559                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
 560             }
 561             if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
 562                 break;
 563             }
 564             // follow though for combined ADJUSTMENT_PITCH_ROLL_D
 565         case ADJUSTMENT_ROLL_D:
 566             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 567                 newFloatValue = constrainf(pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 568                 pidProfile->D_f[PIDROLL] = newFloatValue;
 569                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_ROLL_D, newFloatValue);
 570             } else {
 571                 newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 572                 pidProfile->D8[PIDROLL] = newValue;
 573                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
 574             }
 575             break;
 576         case ADJUSTMENT_YAW_P:
 577             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 578                 newFloatValue = constrainf(pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 579                 pidProfile->P_f[PIDYAW] = newFloatValue;
 580                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_P, newFloatValue);
 581             } else {
 582                 newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 583                 pidProfile->P8[PIDYAW] = newValue;
 584                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
 585             }
 586             break;
 587         case ADJUSTMENT_YAW_I:
 588             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 589                 newFloatValue = constrainf(pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 590                 pidProfile->I_f[PIDYAW] = newFloatValue;
 591                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_I, newFloatValue);
 592             } else {
 593                 newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 594                 pidProfile->I8[PIDYAW] = newValue;
 595                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
 596             }
 597             break;
 598         case ADJUSTMENT_YAW_D:
 599             if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
 600                 newFloatValue = constrainf(pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f), 0, 100); // FIXME magic numbers repeated in serial_cli.c
 601                 pidProfile->D_f[PIDYAW] = newFloatValue;
 602                 blackboxLogInflightAdjustmentEventFloat(ADJUSTMENT_YAW_D, newFloatValue);
 603             } else {
 604                 newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 605                 pidProfile->D8[PIDYAW] = newValue;
 606                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
 607             }
 608             break;
 609         default:
 610             break;
 611     };
 612 }
 613
 614 void changeControlRateProfile(uint8_t profileIndex);
 615
 616 void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
 617 {
 618     bool applied = false;
 619
 620     switch(adjustmentFunction) {
 621         case ADJUSTMENT_RATE_PROFILE:
 622             if (getCurrentControlRateProfile() != position) {
 623                 changeControlRateProfile(position);
 624                 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
 625                 applied = true;
 626             }
 627             break;
 628     }
 629
 630     if (applied) {
 631         beeperConfirmationBeeps(position + 1);
 632     }
 633 }
 634
 635 #define RESET_FREQUENCY_2HZ (1000 / 2)
 636
 637 void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
 638 {
 639     uint8_t adjustmentIndex;
 640     uint32_t now = millis();
 641
 642     bool canUseRxData = rxIsReceivingSignal();
 643
 644
 645     for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
 646         adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
 647
 648         if (!adjustmentState->config) {
 649             continue;
 650         }
 651         uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
 652         if (adjustmentFunction == ADJUSTMENT_NONE) {
 653             continue;
 654         }
 655
 656         int32_t signedDiff = now - adjustmentState->timeoutAt;
 657         bool canResetReadyStates = signedDiff >= 0L;
 658
 659         if (canResetReadyStates) {
 660             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 661             MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 662         }
 663
 664         if (!canUseRxData) {
 665             continue;
 666         }
 667
 668         uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
 669
 670         if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
 671             int delta;
 672             if (rcData[channelIndex] > rxConfig->midrc + 200) {
 673                 delta = adjustmentState->config->data.stepConfig.step;
 674             } else if (rcData[channelIndex] < rxConfig->midrc - 200) {
 675                 delta = 0 - adjustmentState->config->data.stepConfig.step;
 676             } else {
 677                 // returning the switch to the middle immediately resets the ready state
 678                 MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 679                 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 680                 continue;
 681             }
 682             if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
 683                 continue;
 684             }
 685
 686             applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
 687         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
 688             uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
 689             uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
 690
 691             applySelectAdjustment(adjustmentFunction, position);
 692         }
 693         MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
 694     }
 695 }
 696
 697 void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
 698 {
 699     uint8_t index;
 700
 701     for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
 702         adjustmentRange_t *adjustmentRange = &adjustmentRanges[index];
 703
 704         if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
 705
 706             const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 707
 708             configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
 709         }
 710     }
 711 }
 712
 713 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 714     return MIN(ABS(rcData[axis] - midrc), 500);
 715 }
 716
 717 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
 718 {
 719     escAndServoConfig = escAndServoConfigToUse;
 720     pidProfile = pidProfileToUse;
 721
 722     isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
 723
 724 #ifdef NAV
 725     isUsingNAVModes = isModeActivationConditionPresent(modeActivationConditions, BOXNAVPOSHOLD) ||
 726                         isModeActivationConditionPresent(modeActivationConditions, BOXNAVRTH) ||
 727                         isModeActivationConditionPresent(modeActivationConditions, BOXNAVWP);
 728 #endif
 729 }
 730
 731 void resetAdjustmentStates(void)
 732 {
 733     memset(adjustmentStates, 0, sizeof(adjustmentStates));
 734 }
 735