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