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