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 #include "pg/pg.h"
  38 #include "pg/pg_ids.h"
  39 #include "pg/rx.h"
  40
  41 #include "cms/cms.h"
  42
  43 #include "drivers/camera_control.h"
  44
  45 #include "fc/config.h"
  46 #include "fc/fc_core.h"
  47 #include "fc/rc_controls.h"
  48 #include "fc/fc_rc.h"
  49 #include "fc/runtime_config.h"
  50
  51 #include "io/gps.h"
  52 #include "io/beeper.h"
  53 #include "io/motors.h"
  54 #include "io/vtx_control.h"
  55 #include "io/dashboard.h"
  56
  57 #include "sensors/barometer.h"
  58 #include "sensors/battery.h"
  59 #include "sensors/sensors.h"
  60 #include "sensors/gyro.h"
  61 #include "sensors/acceleration.h"
  62
  63 #include "rx/rx.h"
  64 #include "scheduler/scheduler.h"
  65
  66 #include "flight/pid.h"
  67 #include "flight/failsafe.h"
  68
  69 static pidProfile_t *pidProfile;
  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, 0);
  77
  78 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
  79     .deadband = 0,
  80     .yaw_deadband = 0,
  81     .alt_hold_deadband = 40,
  82     .alt_hold_fast_change = 1,
  83     .yaw_control_reversed = false,
  84 );
  85
  86 PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1);
  87
  88 PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
  89     .gyro_cal_on_first_arm = 0,  // TODO - Cleanup retarded arm support
  90     .auto_disarm_delay = 5
  91 );
  92
  93 PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
  94 PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
  95     .deadband3d_low = 1406,
  96     .deadband3d_high = 1514,
  97     .neutral3d = 1460,
  98     .deadband3d_throttle = 50,
  99     .limit3d_low = 1000,
 100     .limit3d_high = 2000,
 101     .switched_mode3d = false
 102 );
 103
 104 bool isUsingSticksForArming(void)
 105 {
 106     return isUsingSticksToArm;
 107 }
 108
 109 bool areSticksInApModePosition(uint16_t ap_mode)
 110 {
 111     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 112 }
 113
 114 throttleStatus_e calculateThrottleStatus(void)
 115 {
 116     if (feature(FEATURE_3D)) {
 117         if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
 118             if (rcData[THROTTLE] < rxConfig()->mincheck) {
 119                 return THROTTLE_LOW;
 120             }
 121         } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) {
 122             return THROTTLE_LOW;
 123         }
 124     } else if (rcData[THROTTLE] < rxConfig()->mincheck) {
 125         return THROTTLE_LOW;
 126     }
 127
 128     return THROTTLE_HIGH;
 129 }
 130
 131 #define ARM_DELAY_MS        500
 132 #define STICK_DELAY_MS      50
 133 #define STICK_AUTOREPEAT_MS 250
 134 #define repeatAfter(t) { \
 135     rcDelayMs -= (t); \
 136     doNotRepeat = false; \
 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 #ifdef USE_CMS
 149     if (cmsInMenu) {
 150         return;
 151     }
 152 #endif
 153
 154     // checking sticks positions
 155     uint8_t stTmp = 0;
 156     for (int i = 0; i < 4; i++) {
 157         stTmp >>= 2;
 158         if (rcData[i] > rxConfig()->mincheck) {
 159             stTmp |= 0x80;  // check for MIN
 160         }
 161         if (rcData[i] < rxConfig()->maxcheck) {
 162             stTmp |= 0x40;  // check for MAX
 163         }
 164     }
 165     if (stTmp == rcSticks) {
 166         if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) {
 167             rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
 168         }
 169     } else {
 170         rcDelayMs = 0;
 171         doNotRepeat = false;
 172     }
 173     rcSticks = stTmp;
 174
 175     // perform actions
 176     if (!isUsingSticksToArm) {
 177         if (IS_RC_MODE_ACTIVE(BOXARM)) {
 178             rcDisarmTicks = 0;
 179             // Arming via ARM BOX
 180             tryArm();
 181         } else {
 182             resetTryingToArm();
 183             // Disarming via ARM BOX
 184             resetArmingDisabled();
 185             if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive()  ) {
 186                 rcDisarmTicks++;
 187                 if (rcDisarmTicks > 3) {
 188                     disarm();
 189                 }
 190             }
 191         }
 192     } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
 193         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 194             doNotRepeat = true;
 195             // Disarm on throttle down + yaw
 196             resetTryingToArm();
 197             if (ARMING_FLAG(ARMED))
 198                 disarm();
 199             else {
 200                 beeper(BEEPER_DISARM_REPEAT);     // sound tone while stick held
 201                 repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
 202
 203 #ifdef USE_RUNAWAY_TAKEOFF
 204                 // Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
 205                 // by a runaway pidSum detection auto-disarm.
 206                 // This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
 207                 // before they're able to rearm
 208                 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 209 #endif
 210             }
 211         }
 212         return;
 213     } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
 214         if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
 215             doNotRepeat = true;
 216             if (!ARMING_FLAG(ARMED)) {
 217                 // Arm via YAW
 218                 tryArm();
 219                 if (isTryingToArm()) {
 220                     doNotRepeat = false;
 221                 }
 222             } else {
 223                 resetArmingDisabled();
 224             }
 225         }
 226         return;
 227     } else {
 228         resetTryingToArm();
 229     }
 230
 231     if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) {
 232         return;
 233     }
 234     doNotRepeat = true;
 235
 236     // actions during not armed
 237
 238     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 239         // GYRO calibration
 240         gyroStartCalibration(false);
 241
 242 #ifdef USE_GPS
 243         if (feature(FEATURE_GPS)) {
 244             GPS_reset_home_position();
 245         }
 246 #endif
 247
 248 #ifdef USE_BARO
 249         if (sensors(SENSOR_BARO))
 250             baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 251 #endif
 252
 253         return;
 254     }
 255
 256     if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 257         // Inflight ACC Calibration
 258         handleInflightCalibrationStickPosition();
 259         return;
 260     }
 261
 262     // Change PID profile
 263     switch (rcSticks) {
 264     case THR_LO + YAW_LO + PIT_CE + ROL_LO:
 265         // ROLL left -> PID profile 1
 266         changePidProfile(0);
 267         return;
 268     case THR_LO + YAW_LO + PIT_HI + ROL_CE:
 269         // PITCH up -> PID profile 2
 270         changePidProfile(1);
 271         return;
 272     case THR_LO + YAW_LO + PIT_CE + ROL_HI:
 273         // ROLL right -> PID profile 3
 274         changePidProfile(2);
 275         return;
 276     }
 277
 278     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 279         saveConfigAndNotify();
 280     }
 281
 282     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 283         // Calibrating Acc
 284         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 285         return;
 286     }
 287
 288
 289     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 290         // Calibrating Mag
 291         ENABLE_STATE(CALIBRATE_MAG);
 292         return;
 293     }
 294
 295
 296     if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
 297         // in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
 298         rollAndPitchTrims_t accelerometerTrimsDelta;
 299         memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
 300
 301         bool shouldApplyRollAndPitchTrimDelta = false;
 302         switch (rcSticks) {
 303         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 304             accelerometerTrimsDelta.values.pitch = 2;
 305             shouldApplyRollAndPitchTrimDelta = true;
 306             break;
 307         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 308             accelerometerTrimsDelta.values.pitch = -2;
 309             shouldApplyRollAndPitchTrimDelta = true;
 310             break;
 311         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 312             accelerometerTrimsDelta.values.roll = 2;
 313             shouldApplyRollAndPitchTrimDelta = true;
 314             break;
 315         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 316             accelerometerTrimsDelta.values.roll = -2;
 317             shouldApplyRollAndPitchTrimDelta = true;
 318             break;
 319         }
 320         if (shouldApplyRollAndPitchTrimDelta) {
 321             applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
 322             repeatAfter(STICK_AUTOREPEAT_MS);
 323             return;
 324         }
 325     } else {
 326         // in ACRO mode, so use sticks to change RATE profile
 327         switch (rcSticks) {
 328         case THR_HI + YAW_CE + PIT_HI + ROL_CE:
 329             changeControlRateProfile(0);
 330             return;
 331         case THR_HI + YAW_CE + PIT_LO + ROL_CE:
 332             changeControlRateProfile(1);
 333             return;
 334         case THR_HI + YAW_CE + PIT_CE + ROL_HI:
 335             changeControlRateProfile(2);
 336             return;
 337         case THR_HI + YAW_CE + PIT_CE + ROL_LO:
 338             changeControlRateProfile(3);
 339             return;
 340         }
 341     }
 342
 343 #ifdef USE_DASHBOARD
 344     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 345         dashboardDisablePageCycling();
 346     }
 347
 348     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 349         dashboardEnablePageCycling();
 350     }
 351 #endif
 352
 353 #ifdef USE_VTX_CONTROL
 354     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_HI) {
 355         vtxIncrementBand();
 356     }
 357     if (rcSticks ==  THR_HI + YAW_LO + PIT_CE + ROL_LO) {
 358         vtxDecrementBand();
 359     }
 360     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_HI) {
 361         vtxIncrementChannel();
 362     }
 363     if (rcSticks ==  THR_HI + YAW_HI + PIT_CE + ROL_LO) {
 364         vtxDecrementChannel();
 365     }
 366 #endif
 367
 368 #ifdef USE_CAMERA_CONTROL
 369     if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
 370         cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
 371         repeatAfter(3 * STICK_DELAY_MS);
 372     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
 373         cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
 374         repeatAfter(3 * STICK_DELAY_MS);
 375     } else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
 376         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
 377         repeatAfter(3 * STICK_DELAY_MS);
 378     } else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
 379         cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
 380         repeatAfter(3 * STICK_DELAY_MS);
 381     } else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
 382         cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
 383         repeatAfter(3 * STICK_DELAY_MS);
 384     } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
 385         cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
 386     }
 387 #endif
 388 }
 389
 390 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 391     return MIN(ABS(rcData[axis] - midrc), 500);
 392 }
 393
 394 void useRcControlsConfig(pidProfile_t *pidProfileToUse)
 395 {
 396     pidProfile = pidProfileToUse;
 397
 398     isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
 399 }