src/main/fc/rc_controls.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #include "blackbox/blackbox.h"
  30
  31 #include "build/build_config.h"
  32
  33 #include "common/axis.h"
  34 #include "common/maths.h"
  35
  36 #include "config/feature.h"
  37
  38 #include "drivers/camera_control_impl.h"
  39
  40 #include "config/config.h"
  41 #include "fc/core.h"
  42 #include "fc/rc.h"
  43 #include "fc/runtime_config.h"
  44
  45 #include "flight/pid.h"
  46 #include "flight/failsafe.h"
  47
  48 #include "io/beeper.h"
  49 #include "io/usb_cdc_hid.h"
  50 #include "io/dashboard.h"
  51 #include "io/gimbal_control.h"
  52 #include "io/gps.h"
  53 #include "io/vtx_control.h"
  54
  55 #include "pg/pg.h"
  56 #include "pg/pg_ids.h"
  57 #include "pg/rx.h"
  58
  59 #include "rx/rx.h"
  60
  61 #include "scheduler/scheduler.h"
  62
  63 #include "sensors/acceleration.h"
  64 #include "sensors/barometer.h"
  65 #include "sensors/battery.h"
  66 #include "sensors/compass.h"
  67 #include "sensors/gyro.h"
  68
  69 #include "rc_controls.h"
  70
  71 // true if arming is done via the sticks (as opposed to a switch)
  72 static bool isUsingSticksToArm = true;
  73
  74 float rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  75
  76 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1);
  77
  78 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
  79     .deadband = 0,
  80     .yaw_deadband = 0,
  81     .yaw_control_reversed = false,
  82 );
  83
  84 PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2);
  85
  86 PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
  87     .gyro_cal_on_first_arm = 0,
  88     .auto_disarm_delay = 5,
  89     .prearm_allow_rearm = 0,
  90 );
  91
  92 PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
  93 PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
  94     .deadband3d_low = 1406,
  95     .deadband3d_high = 1514,
  96     .neutral3d = 1460,
  97     .deadband3d_throttle = 50,
  98     .limit3d_low = 1000,
  99     .limit3d_high = 2000,
 100     .switched_mode3d = false
 101 );
 102
 103 bool isUsingSticksForArming(void)
 104 {
 105     return isUsingSticksToArm;
 106 }
 107
 108 throttleStatus_e calculateThrottleStatus(void)
 109 {
 110     if (featureIsEnabled(FEATURE_3D)) {
 111         if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
 112             if (rcData[THROTTLE] < rxConfig()->mincheck) {
 113                 return THROTTLE_LOW;
 114             }
 115         } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) {
 116             return THROTTLE_LOW;
 117         }
 118     } else if (rcData[THROTTLE] < rxConfig()->mincheck) {
 119         return THROTTLE_LOW;
 120     }
 121
 122     return THROTTLE_HIGH;
 123 }
 124
 125 #define ARM_DELAY_MS        500
 126 #define STICK_DELAY_MS      50
 127 #define STICK_AUTOREPEAT_MS 250
 128 #define repeatAfter(t) { \
 129     rcDelayMs -= (t); \
 130     doNotRepeat = false; \
 131 }
 132
 133 void processRcStickPositions(void)
 134 {
 135     // time the sticks are maintained
 136     static int16_t rcDelayMs;
 137     // hold sticks position for command combos
 138     static uint8_t rcSticks;
 139     // an extra guard for disarming through switch to prevent that one frame can disarm it
 140     static uint8_t rcDisarmTicks;
 141     static bool doNotRepeat;
 142     static bool pendingApplyRollAndPitchTrimDeltaSave = false;
 143
 144     // checking sticks positions
 145     uint8_t stTmp = 0;
 146     for (int i = 0; i < 4; i++) {
 147         stTmp >>= 2;
 148         if (rcData[i] > rxConfig()->mincheck) {
 149             stTmp |= 0x80;  // check for MIN
 150         }
 151         if (rcData[i] < rxConfig()->maxcheck) {
 152             stTmp |= 0x40;  // check for MAX
 153         }
 154     }
 155     if (stTmp == rcSticks) {
 156         if (rcDelayMs <= INT16_MAX - (getTaskDeltaTimeUs(TASK_SELF) / 1000)) {
 157             rcDelayMs += getTaskDeltaTimeUs(TASK_SELF) / 1000;
 158         }
 159     } else {
 160         rcDelayMs = 0;
 161         doNotRepeat = false;
 162     }
 163     rcSticks = stTmp;
 164
 165     // perform actions
 166     if (!isUsingSticksToArm) {
 167         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 168             rcDisarmTicks = 0;
 169             // Arming via ARM BOX
 170             tryArm();
 171         } else {
 172             resetTryingToArm();
 173             // Disarming via ARM BOX
 174             resetArmingDisabled();
 175             const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 176             if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || boxFailsafeSwitchIsOn)) {
 177                 // in a true signal loss situation, allow disarm only once we regain validated RxData (failsafeIsReceivingRxData = true),
 178                 // to avoid potentially false disarm signals soon after link recover
 179                 // Note that BOXFAILSAFE will also drive failsafeIsReceivingRxData false (immediately at start or end)
 180                 // That's why we explicitly allow disarm here if BOXFAILSAFE switch is active
 181                 // Note that BOXGPSRESCUE mode does not trigger failsafe - we can always disarm in that mode
 182                 rcDisarmTicks++;
 183                 if (rcDisarmTicks > 3) {
 184                     // require three duplicate disarm values in a row before we disarm
 185                     disarm(DISARM_REASON_SWITCH);
 186                 }
 187             }
 188         }
 189     } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 190         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 191             doNotRepeat = true;
 192             // Disarm on throttle down + yaw
 193             resetTryingToArm();
 194             if (ARMING_FLAG(ARMED))
 195                 disarm(DISARM_REASON_STICKS);
 196             else {
 197                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
 198                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
 199
 200 #ifdef USE_RUNAWAY_TAKEOFF
 201                 // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
 202                 // by a runaway pidSum detection auto-disarm.
 203                 // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
 204                 // before they're able to rearm
 205                 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 206 #endif
 207                 unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 208             }
 209         }
 210         return;
 211     } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE && !IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) { // disable stick arming if STICK COMMAND DISABLE SW is active
 212         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 213             doNotRepeat = true;
 214             if (!ARMING_FLAG(ARMED)) {
 215                 // Arm via YAW
 216                 tryArm();
 217                 if (isTryingToArm() ||
 218                     ((getArmingDisableFlags() == ARMING_DISABLED_CALIBRATING) && armingConfig()->gyro_cal_on_first_arm)) {
 219                     doNotRepeat = false;
 220                 }
 221             } else {
 222                 resetArmingDisabled();
 223             }
 224         }
 225         return;
 226     } else {
 227         resetTryingToArm();
 228     }
 229
 230     if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
 231         return;
 232     }
 233     doNotRepeat = true;
 234
 235     #ifdef USE_USB_CDC_HID
 236     // If this target is used as a joystick, we should leave here.
 237     if (cdcDeviceIsMayBeActive() || IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) {
 238         return;
 239     }
 240     #endif
 241
 242     // actions during not armed
 243
 244     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 245         // GYRO calibration
 246         gyroStartCalibration(false);
 247
 248 #ifdef USE_GPS
 249         if (featureIsEnabled(FEATURE_GPS)) {
 250             GPS_reset_home_position();
 251         }
 252 #endif
 253
 254 #ifdef USE_BARO
 255         if (sensors(SENSOR_BARO)) {
 256             baroSetGroundLevel();
 257         }
 258 #endif
 259
 260         return;
 261     }
 262
 263     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 264         // Inflight ACC Calibration
 265         handleInflightCalibrationStickPosition();
 266         return;
 267     }
 268
 269     // Change PID profile
 270     switch (rcSticks) {
 271     case THR_LO + YAW_LO + PIT_CE + ROL_LO:
 272         // ROLL left -> PID profile 1
 273         changePidProfile(0);
 274         return;
 275     case THR_LO + YAW_LO + PIT_HI + ROL_CE:
 276         // PITCH up -> PID profile 2
 277         changePidProfile(1);
 278         return;
 279     case THR_LO + YAW_LO + PIT_CE + ROL_HI:
 280         // ROLL right -> PID profile 3
 281         changePidProfile(2);
 282         return;
 283     }
 284
 285     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 286         saveConfigAndNotify();
 287     }
 288
 289 #ifdef USE_ACC
 290     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 291         // Calibrating Acc
 292         accStartCalibration();
 293         return;
 294     }
 295 #endif
 296
 297 #if defined(USE_MAG)
 298     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 299         // Calibrating Mag
 300         compassStartCalibration();
 301
 302         return;
 303     }
 304 #endif
 305
 306     if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE)) {
 307         // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
 308         rollAndPitchTrims_t accelerometerTrimsDelta;
 309         memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
 310
 311         if (pendingApplyRollAndPitchTrimDeltaSave && ((rcSticks & THR_MASK) != THR_HI)) {
 312             saveConfigAndNotify();
 313             pendingApplyRollAndPitchTrimDeltaSave = false;
 314             return;
 315         }
 316
 317         bool shouldApplyRollAndPitchTrimDelta = false;
 318         switch (rcSticks) {
 319         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 320             accelerometerTrimsDelta.values.pitch = 1;
 321             shouldApplyRollAndPitchTrimDelta = true;
 322             break;
 323         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 324             accelerometerTrimsDelta.values.pitch = -1;
 325             shouldApplyRollAndPitchTrimDelta = true;
 326             break;
 327         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 328             accelerometerTrimsDelta.values.roll = 1;
 329             shouldApplyRollAndPitchTrimDelta = true;
 330             break;
 331         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 332             accelerometerTrimsDelta.values.roll = -1;
 333             shouldApplyRollAndPitchTrimDelta = true;
 334             break;
 335         }
 336         if (shouldApplyRollAndPitchTrimDelta) {
 337 #if defined(USE_ACC)
 338             applyAccelerometerTrimsDelta(&accelerometerTrimsDelta);
 339 #endif
 340             pendingApplyRollAndPitchTrimDeltaSave = true;
 341
 342             beeperConfirmationBeeps(1);
 343
 344             repeatAfter(STICK_AUTOREPEAT_MS);
 345
 346             return;
 347         }
 348     } else {
 349         // in ACRO mode, so use sticks to change RATE profile
 350         switch (rcSticks) {
 351         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 352             changeControlRateProfile(0);
 353             return;
 354         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 355             changeControlRateProfile(1);
 356             return;
 357         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 358             changeControlRateProfile(2);
 359             return;
 360         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 361             changeControlRateProfile(3);
 362             return;
 363         }
 364     }
 365
 366 #ifdef USE_DASHBOARD
 367     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 368         dashboardDisablePageCycling();
 369     }
 370
 371     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 372         dashboardEnablePageCycling();
 373     }
 374 #endif
 375
 376 #ifdef USE_VTX_CONTROL
 377     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_HI) {
 378         vtxIncrementBand();
 379     }
 380     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_LO) {
 381         vtxDecrementBand();
 382     }
 383     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_HI) {
 384         vtxIncrementChannel();
 385     }
 386     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_LO) {
 387         vtxDecrementChannel();
 388     }
 389 #endif
 390
 391 #ifdef USE_CAMERA_CONTROL
 392     if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
 393         cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
 394         repeatAfter(3 * STICK_DELAY_MS);
 395     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
 396         cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
 397         repeatAfter(3 * STICK_DELAY_MS);
 398     } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
 399         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
 400         repeatAfter(3 * STICK_DELAY_MS);
 401     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
 402         cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
 403         repeatAfter(3 * STICK_DELAY_MS);
 404     } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
 405         cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
 406         repeatAfter(3 * STICK_DELAY_MS);
 407     } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
 408         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
 409     }
 410 #endif
 411 }
 412
 413 void rcControlsInit(void)
 414 {
 415     analyzeModeActivationConditions();
 416     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM) && systemConfig()->enableStickArming;
 417 }