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.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(void)
 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             const bool switchFailsafe = (failsafeIsActive() && (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)));
 181             if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || switchFailsafe)) {
 182                 rcDisarmTicks++;
 183                 if (rcDisarmTicks > 3) {
 184                     disarm(DISARM_REASON_SWITCH);
 185                 }
 186             }
 187         }
 188     } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 189         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 190             doNotRepeat = true;
 191             // Disarm on throttle down + yaw
 192             resetTryingToArm();
 193             if (ARMING_FLAG(ARMED))
 194                 disarm(DISARM_REASON_STICKS);
 195             else {
 196                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
 197                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
 198
 199 #ifdef USE_RUNAWAY_TAKEOFF
 200                 // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
 201                 // by a runaway pidSum detection auto-disarm.
 202                 // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
 203                 // before they're able to rearm
 204                 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 205 #endif
 206                 unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 207             }
 208         }
 209         return;
 210     } 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
 211         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 212             doNotRepeat = true;
 213             if (!ARMING_FLAG(ARMED)) {
 214                 // Arm via YAW
 215                 tryArm();
 216                 if (isTryingToArm() ||
 217                     ((getArmingDisableFlags() == ARMING_DISABLED_CALIBRATING) && armingConfig()->gyro_cal_on_first_arm)) {
 218                     doNotRepeat = false;
 219                 }
 220             } else {
 221                 resetArmingDisabled();
 222             }
 223         }
 224         return;
 225     } else {
 226         resetTryingToArm();
 227     }
 228
 229     if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
 230         return;
 231     }
 232     doNotRepeat = true;
 233
 234     #ifdef USE_USB_CDC_HID
 235     // If this target is used as a joystick, we should leave here.
 236     if (cdcDeviceIsMayBeActive() || IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) {
 237         return;
 238     }
 239     #endif
 240
 241     // actions during not armed
 242
 243     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 244         // GYRO calibration
 245         gyroStartCalibration(false);
 246
 247 #ifdef USE_GPS
 248         if (featureIsEnabled(FEATURE_GPS)) {
 249             GPS_reset_home_position();
 250         }
 251 #endif
 252
 253 #ifdef USE_BARO
 254         if (sensors(SENSOR_BARO)) {
 255             baroSetGroundLevel();
 256         }
 257 #endif
 258
 259         return;
 260     }
 261
 262     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 263         // Inflight ACC Calibration
 264         handleInflightCalibrationStickPosition();
 265         return;
 266     }
 267
 268     // Change PID profile
 269     switch (rcSticks) {
 270     case THR_LO + YAW_LO + PIT_CE + ROL_LO:
 271         // ROLL left -> PID profile 1
 272         changePidProfile(0);
 273         return;
 274     case THR_LO + YAW_LO + PIT_HI + ROL_CE:
 275         // PITCH up -> PID profile 2
 276         changePidProfile(1);
 277         return;
 278     case THR_LO + YAW_LO + PIT_CE + ROL_HI:
 279         // ROLL right -> PID profile 3
 280         changePidProfile(2);
 281         return;
 282     }
 283
 284     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 285         saveConfigAndNotify();
 286     }
 287
 288 #ifdef USE_ACC
 289     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 290         // Calibrating Acc
 291         accStartCalibration();
 292         return;
 293     }
 294 #endif
 295
 296 #if defined(USE_MAG)
 297     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 298         // Calibrating Mag
 299         compassStartCalibration();
 300
 301         return;
 302     }
 303 #endif
 304
 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 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 414     return MIN(abs((int32_t)rcData[axis] - midrc), 500);
 415 }
 416
 417 void rcControlsInit(void)
 418 {
 419     analyzeModeActivationConditions();
 420     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM) && systemConfig()->enableStickArming;
 421 }