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 <string.h>
  24
  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.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/gps.h"
  52 #include "io/vtx_control.h"
  53
  54 #include "pg/pg.h"
  55 #include "pg/pg_ids.h"
  56 #include "pg/rx.h"
  57
  58 #include "rx/rx.h"
  59
  60 #include "scheduler/scheduler.h"
  61
  62 #include "sensors/acceleration.h"
  63 #include "sensors/barometer.h"
  64 #include "sensors/battery.h"
  65 #include "sensors/compass.h"
  66 #include "sensors/gyro.h"
  67
  68 #include "rc_controls.h"
  69
  70 // true if arming is done via the sticks (as opposed to a switch)
  71 static bool isUsingSticksToArm = true;
  72
  73 float rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  74
  75 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
  76
  77 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
  78     .deadband = 0,
  79     .yaw_deadband = 0,
  80     .alt_hold_deadband = 40,
  81     .alt_hold_fast_change = 1,
  82     .yaw_control_reversed = false,
  83 );
  84
  85 PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1);
  86
  87 PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
  88     .gyro_cal_on_first_arm = 0,  // TODO - Cleanup retarded arm support
  89     .auto_disarm_delay = 5
  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 bool areSticksInApModePosition(uint16_t ap_mode)
 109 {
 110     return fabsf(rcCommand[ROLL]) < ap_mode && fabsf(rcCommand[PITCH]) < ap_mode;
 111 }
 112
 113 throttleStatus_e calculateThrottleStatus(void)
 114 {
 115     if (featureIsEnabled(FEATURE_3D)) {
 116         if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
 117             if (rcData[THROTTLE] < rxConfig()->mincheck) {
 118                 return THROTTLE_LOW;
 119             }
 120         } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) {
 121             return THROTTLE_LOW;
 122         }
 123     } else if (rcData[THROTTLE] < rxConfig()->mincheck) {
 124         return THROTTLE_LOW;
 125     }
 126
 127     return THROTTLE_HIGH;
 128 }
 129
 130 #define ARM_DELAY_MS        500
 131 #define STICK_DELAY_MS      50
 132 #define STICK_AUTOREPEAT_MS 250
 133 #define repeatAfter(t) { \
 134     rcDelayMs -= (t); \
 135     doNotRepeat = false; \
 136 }
 137
 138 void processRcStickPositions()
 139 {
 140     // time the sticks are maintained
 141     static int16_t rcDelayMs;
 142     // hold sticks position for command combos
 143     static uint8_t rcSticks;
 144     // an extra guard for disarming through switch to prevent that one frame can disarm it
 145     static uint8_t rcDisarmTicks;
 146     static bool doNotRepeat;
 147
 148     // checking sticks positions
 149     uint8_t stTmp = 0;
 150     for (int i = 0; i < 4; i++) {
 151         stTmp >>= 2;
 152         if (rcData[i] > rxConfig()->mincheck) {
 153             stTmp |= 0x80;  // check for MIN
 154         }
 155         if (rcData[i] < rxConfig()->maxcheck) {
 156             stTmp |= 0x40;  // check for MAX
 157         }
 158     }
 159     if (stTmp == rcSticks) {
 160         if (rcDelayMs <= INT16_MAX - (getTaskDeltaTimeUs(TASK_SELF) / 1000)) {
 161             rcDelayMs += getTaskDeltaTimeUs(TASK_SELF) / 1000;
 162         }
 163     } else {
 164         rcDelayMs = 0;
 165         doNotRepeat = false;
 166     }
 167     rcSticks = stTmp;
 168
 169     // perform actions
 170     if (!isUsingSticksToArm) {
 171         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 172             rcDisarmTicks = 0;
 173             // Arming via ARM BOX
 174             tryArm();
 175         } else {
 176             resetTryingToArm();
 177             // Disarming via ARM BOX
 178             resetArmingDisabled();
 179             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
 180                 rcDisarmTicks++;
 181                 if (rcDisarmTicks > 3) {
 182                     disarm(DISARM_REASON_SWITCH);
 183                 }
 184             }
 185         }
 186     } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 187         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 188             doNotRepeat = true;
 189             // Disarm on throttle down + yaw
 190             resetTryingToArm();
 191             if (ARMING_FLAG(ARMED))
 192                 disarm(DISARM_REASON_STICKS);
 193             else {
 194                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
 195                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
 196
 197 #ifdef USE_RUNAWAY_TAKEOFF
 198                 // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
 199                 // by a runaway pidSum detection auto-disarm.
 200                 // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
 201                 // before they're able to rearm
 202                 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 203 #endif
 204                 unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 205             }
 206         }
 207         return;
 208     } 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
 209         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 210             doNotRepeat = true;
 211             if (!ARMING_FLAG(ARMED)) {
 212                 // Arm via YAW
 213                 tryArm();
 214                 if (isTryingToArm() ||
 215                     ((getArmingDisableFlags() == ARMING_DISABLED_CALIBRATING) && armingConfig()->gyro_cal_on_first_arm)) {
 216                     doNotRepeat = false;
 217                 }
 218             } else {
 219                 resetArmingDisabled();
 220             }
 221         }
 222         return;
 223     } else {
 224         resetTryingToArm();
 225     }
 226
 227     if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
 228         return;
 229     }
 230     doNotRepeat = true;
 231
 232     #ifdef USE_USB_CDC_HID
 233     // If this target is used as a joystick, we should leave here.
 234     if (cdcDeviceIsMayBeActive() || IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) {
 235         return;
 236     }
 237     #endif
 238
 239     // actions during not armed
 240
 241     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 242         // GYRO calibration
 243         gyroStartCalibration(false);
 244
 245 #ifdef USE_GPS
 246         if (featureIsEnabled(FEATURE_GPS)) {
 247             GPS_reset_home_position();
 248         }
 249 #endif
 250
 251 #ifdef USE_BARO
 252         if (sensors(SENSOR_BARO)) {
 253             baroSetGroundLevel();
 254         }
 255 #endif
 256
 257         return;
 258     }
 259
 260     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 261         // Inflight ACC Calibration
 262         handleInflightCalibrationStickPosition();
 263         return;
 264     }
 265
 266     // Change PID profile
 267     switch (rcSticks) {
 268     case THR_LO + YAW_LO + PIT_CE + ROL_LO:
 269         // ROLL left -> PID profile 1
 270         changePidProfile(0);
 271         return;
 272     case THR_LO + YAW_LO + PIT_HI + ROL_CE:
 273         // PITCH up -> PID profile 2
 274         changePidProfile(1);
 275         return;
 276     case THR_LO + YAW_LO + PIT_CE + ROL_HI:
 277         // ROLL right -> PID profile 3
 278         changePidProfile(2);
 279         return;
 280     }
 281
 282     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 283         saveConfigAndNotify();
 284     }
 285
 286 #ifdef USE_ACC
 287     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 288         // Calibrating Acc
 289         accStartCalibration();
 290         return;
 291     }
 292 #endif
 293
 294 #if defined(USE_MAG)
 295     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 296         // Calibrating Mag
 297         compassStartCalibration();
 298
 299         return;
 300     }
 301 #endif
 302
 303
 304     if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
 305         // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
 306         rollAndPitchTrims_t accelerometerTrimsDelta;
 307         memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
 308
 309         bool shouldApplyRollAndPitchTrimDelta = false;
 310         switch (rcSticks) {
 311         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 312             accelerometerTrimsDelta.values.pitch = 2;
 313             shouldApplyRollAndPitchTrimDelta = true;
 314             break;
 315         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 316             accelerometerTrimsDelta.values.pitch = -2;
 317             shouldApplyRollAndPitchTrimDelta = true;
 318             break;
 319         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 320             accelerometerTrimsDelta.values.roll = 2;
 321             shouldApplyRollAndPitchTrimDelta = true;
 322             break;
 323         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 324             accelerometerTrimsDelta.values.roll = -2;
 325             shouldApplyRollAndPitchTrimDelta = true;
 326             break;
 327         }
 328         if (shouldApplyRollAndPitchTrimDelta) {
 329 #if defined(USE_ACC)
 330             applyAccelerometerTrimsDelta(&accelerometerTrimsDelta);
 331 #endif
 332             saveConfigAndNotify();
 333
 334             repeatAfter(STICK_AUTOREPEAT_MS);
 335
 336             return;
 337         }
 338     } else {
 339         // in ACRO mode, so use sticks to change RATE profile
 340         switch (rcSticks) {
 341         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 342             changeControlRateProfile(0);
 343             return;
 344         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 345             changeControlRateProfile(1);
 346             return;
 347         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 348             changeControlRateProfile(2);
 349             return;
 350         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 351             changeControlRateProfile(3);
 352             return;
 353         }
 354     }
 355
 356 #ifdef USE_DASHBOARD
 357     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 358         dashboardDisablePageCycling();
 359     }
 360
 361     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 362         dashboardEnablePageCycling();
 363     }
 364 #endif
 365
 366 #ifdef USE_VTX_CONTROL
 367     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_HI) {
 368         vtxIncrementBand();
 369     }
 370     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_LO) {
 371         vtxDecrementBand();
 372     }
 373     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_HI) {
 374         vtxIncrementChannel();
 375     }
 376     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_LO) {
 377         vtxDecrementChannel();
 378     }
 379 #endif
 380
 381 #ifdef USE_CAMERA_CONTROL
 382     if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
 383         cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
 384         repeatAfter(3 * STICK_DELAY_MS);
 385     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
 386         cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
 387         repeatAfter(3 * STICK_DELAY_MS);
 388     } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
 389         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
 390         repeatAfter(3 * STICK_DELAY_MS);
 391     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
 392         cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
 393         repeatAfter(3 * STICK_DELAY_MS);
 394     } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
 395         cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
 396         repeatAfter(3 * STICK_DELAY_MS);
 397     } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
 398         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
 399     }
 400 #endif
 401 }
 402
 403 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 404     return MIN(ABS(rcData[axis] - midrc), 500);
 405 }
 406
 407 void rcControlsInit(void)
 408 {
 409     analyzeModeActivationConditions();
 410     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM) && systemConfig()->enableStickArming;
 411 }