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     static bool pendingApplyRollAndPitchTrimDeltaSave = false;
 148
 149     // checking sticks positions
 150     uint8_t stTmp = 0;
 151     for (int i = 0; i < 4; i++) {
 152         stTmp >>= 2;
 153         if (rcData[i] > rxConfig()->mincheck) {
 154             stTmp |= 0x80;  // check for MIN
 155         }
 156         if (rcData[i] < rxConfig()->maxcheck) {
 157             stTmp |= 0x40;  // check for MAX
 158         }
 159     }
 160     if (stTmp == rcSticks) {
 161         if (rcDelayMs <= INT16_MAX - (getTaskDeltaTimeUs(TASK_SELF) / 1000)) {
 162             rcDelayMs += getTaskDeltaTimeUs(TASK_SELF) / 1000;
 163         }
 164     } else {
 165         rcDelayMs = 0;
 166         doNotRepeat = false;
 167     }
 168     rcSticks = stTmp;
 169
 170     // perform actions
 171     if (!isUsingSticksToArm) {
 172         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 173             rcDisarmTicks = 0;
 174             // Arming via ARM BOX
 175             tryArm();
 176         } else {
 177             resetTryingToArm();
 178             // Disarming via ARM BOX
 179             resetArmingDisabled();
 180             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
 181                 rcDisarmTicks++;
 182                 if (rcDisarmTicks > 3) {
 183                     disarm(DISARM_REASON_SWITCH);
 184                 }
 185             }
 186         }
 187     } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 188         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 189             doNotRepeat = true;
 190             // Disarm on throttle down + yaw
 191             resetTryingToArm();
 192             if (ARMING_FLAG(ARMED))
 193                 disarm(DISARM_REASON_STICKS);
 194             else {
 195                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
 196                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
 197
 198 #ifdef USE_RUNAWAY_TAKEOFF
 199                 // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
 200                 // by a runaway pidSum detection auto-disarm.
 201                 // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
 202                 // before they're able to rearm
 203                 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 204 #endif
 205                 unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 206             }
 207         }
 208         return;
 209     } 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
 210         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 211             doNotRepeat = true;
 212             if (!ARMING_FLAG(ARMED)) {
 213                 // Arm via YAW
 214                 tryArm();
 215                 if (isTryingToArm() ||
 216                     ((getArmingDisableFlags() == ARMING_DISABLED_CALIBRATING) && armingConfig()->gyro_cal_on_first_arm)) {
 217                     doNotRepeat = false;
 218                 }
 219             } else {
 220                 resetArmingDisabled();
 221             }
 222         }
 223         return;
 224     } else {
 225         resetTryingToArm();
 226     }
 227
 228     if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
 229         return;
 230     }
 231     doNotRepeat = true;
 232
 233     #ifdef USE_USB_CDC_HID
 234     // If this target is used as a joystick, we should leave here.
 235     if (cdcDeviceIsMayBeActive() || IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) {
 236         return;
 237     }
 238     #endif
 239
 240     // actions during not armed
 241
 242     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 243         // GYRO calibration
 244         gyroStartCalibration(false);
 245
 246 #ifdef USE_GPS
 247         if (featureIsEnabled(FEATURE_GPS)) {
 248             GPS_reset_home_position();
 249         }
 250 #endif
 251
 252 #ifdef USE_BARO
 253         if (sensors(SENSOR_BARO)) {
 254             baroSetGroundLevel();
 255         }
 256 #endif
 257
 258         return;
 259     }
 260
 261     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 262         // Inflight ACC Calibration
 263         handleInflightCalibrationStickPosition();
 264         return;
 265     }
 266
 267     // Change PID profile
 268     switch (rcSticks) {
 269     case THR_LO + YAW_LO + PIT_CE + ROL_LO:
 270         // ROLL left -> PID profile 1
 271         changePidProfile(0);
 272         return;
 273     case THR_LO + YAW_LO + PIT_HI + ROL_CE:
 274         // PITCH up -> PID profile 2
 275         changePidProfile(1);
 276         return;
 277     case THR_LO + YAW_LO + PIT_CE + ROL_HI:
 278         // ROLL right -> PID profile 3
 279         changePidProfile(2);
 280         return;
 281     }
 282
 283     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 284         saveConfigAndNotify();
 285     }
 286
 287 #ifdef USE_ACC
 288     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 289         // Calibrating Acc
 290         accStartCalibration();
 291         return;
 292     }
 293 #endif
 294
 295 #if defined(USE_MAG)
 296     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 297         // Calibrating Mag
 298         compassStartCalibration();
 299
 300         return;
 301     }
 302 #endif
 303
 304
 305     if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
 306         // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
 307         rollAndPitchTrims_t accelerometerTrimsDelta;
 308         memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
 309
 310         if (pendingApplyRollAndPitchTrimDeltaSave && ((rcSticks & THR_MASK) != THR_HI)) {
 311             saveConfigAndNotify();
 312             pendingApplyRollAndPitchTrimDeltaSave = false;
 313             return;
 314         }
 315
 316         bool shouldApplyRollAndPitchTrimDelta = false;
 317         switch (rcSticks) {
 318         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 319             accelerometerTrimsDelta.values.pitch = 2;
 320             shouldApplyRollAndPitchTrimDelta = true;
 321             break;
 322         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 323             accelerometerTrimsDelta.values.pitch = -2;
 324             shouldApplyRollAndPitchTrimDelta = true;
 325             break;
 326         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 327             accelerometerTrimsDelta.values.roll = 2;
 328             shouldApplyRollAndPitchTrimDelta = true;
 329             break;
 330         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 331             accelerometerTrimsDelta.values.roll = -2;
 332             shouldApplyRollAndPitchTrimDelta = true;
 333             break;
 334         }
 335         if (shouldApplyRollAndPitchTrimDelta) {
 336 #if defined(USE_ACC)
 337             applyAccelerometerTrimsDelta(&accelerometerTrimsDelta);
 338 #endif
 339             pendingApplyRollAndPitchTrimDeltaSave = true;
 340
 341             beeperConfirmationBeeps(1);
 342
 343             repeatAfter(STICK_AUTOREPEAT_MS);
 344
 345             return;
 346         }
 347     } else {
 348         // in ACRO mode, so use sticks to change RATE profile
 349         switch (rcSticks) {
 350         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 351             changeControlRateProfile(0);
 352             return;
 353         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 354             changeControlRateProfile(1);
 355             return;
 356         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 357             changeControlRateProfile(2);
 358             return;
 359         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 360             changeControlRateProfile(3);
 361             return;
 362         }
 363     }
 364
 365 #ifdef USE_DASHBOARD
 366     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 367         dashboardDisablePageCycling();
 368     }
 369
 370     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 371         dashboardEnablePageCycling();
 372     }
 373 #endif
 374
 375 #ifdef USE_VTX_CONTROL
 376     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_HI) {
 377         vtxIncrementBand();
 378     }
 379     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_LO) {
 380         vtxDecrementBand();
 381     }
 382     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_HI) {
 383         vtxIncrementChannel();
 384     }
 385     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_LO) {
 386         vtxDecrementChannel();
 387     }
 388 #endif
 389
 390 #ifdef USE_CAMERA_CONTROL
 391     if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
 392         cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
 393         repeatAfter(3 * STICK_DELAY_MS);
 394     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
 395         cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
 396         repeatAfter(3 * STICK_DELAY_MS);
 397     } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
 398         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
 399         repeatAfter(3 * STICK_DELAY_MS);
 400     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
 401         cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
 402         repeatAfter(3 * STICK_DELAY_MS);
 403     } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
 404         cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
 405         repeatAfter(3 * STICK_DELAY_MS);
 406     } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
 407         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
 408     }
 409 #endif
 410 }
 411
 412 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 413     return MIN(ABS(rcData[axis] - midrc), 500);
 414 }
 415
 416 void rcControlsInit(void)
 417 {
 418     analyzeModeActivationConditions();
 419     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM) && systemConfig()->enableStickArming;
 420 }