src/main/fc/rc_controls.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include <math.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "blackbox/blackbox.h"
  29
  30 #include "cms/cms.h"
  31
  32 #include "common/axis.h"
  33 #include "common/maths.h"
  34 #include "common/utils.h"
  35
  36 #include "config/feature.h"
  37 #include "config/parameter_group.h"
  38 #include "config/parameter_group_ids.h"
  39
  40 #include "drivers/time.h"
  41
  42 #include "fc/cli.h"
  43 #include "fc/config.h"
  44 #include "fc/controlrate_profile.h"
  45 #include "fc/fc_core.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/rc_curves.h"
  48 #include "fc/rc_modes.h"
  49 #include "fc/runtime_config.h"
  50 #include "fc/settings.h"
  51
  52 #include "flight/pid.h"
  53 #include "flight/failsafe.h"
  54 #include "flight/mixer.h"
  55
  56 #include "io/gps.h"
  57 #include "io/beeper.h"
  58
  59 #include "navigation/navigation.h"
  60
  61 #include "rx/rx.h"
  62
  63 #include "sensors/barometer.h"
  64 #include "sensors/battery.h"
  65 #include "sensors/sensors.h"
  66 #include "sensors/gyro.h"
  67 #include "sensors/acceleration.h"
  68
  69 #define AIRMODE_DEADBAND 25
  70 #define MIN_RC_TICK_INTERVAL_MS             20
  71 #define DEFAULT_RC_SWITCH_DISARM_DELAY_MS   250     // Wait at least 250ms before disarming via switch
  72 #define DEFAULT_PREARM_TIMEOUT              10000   // Prearm is invalidated after 10 seconds
  73
  74 stickPositions_e rcStickPositions;
  75
  76 FASTRAM int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  77
  78 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 4);
  79
  80 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
  81     .deadband = SETTING_DEADBAND_DEFAULT,
  82     .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT,
  83     .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT,
  84     .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT,
  85     .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT,
  86     .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT,
  87     .airmodeThrottleThreshold = SETTING_AIRMODE_THROTTLE_THRESHOLD_DEFAULT,
  88 );
  89
  90 PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 3);
  91
  92 PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
  93     .fixed_wing_auto_arm = SETTING_FIXED_WING_AUTO_ARM_DEFAULT,
  94     .disarm_always = SETTING_DISARM_ALWAYS_DEFAULT,
  95     .switchDisarmDelayMs = SETTING_SWITCH_DISARM_DELAY_DEFAULT,
  96     .prearmTimeoutMs = SETTING_PREARM_TIMEOUT_DEFAULT,
  97 );
  98
  99 bool areSticksInApModePosition(uint16_t ap_mode)
 100 {
 101     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 102 }
 103
 104 bool areSticksDeflected(void)
 105 {
 106     return (ABS(rcCommand[ROLL]) > CONTROL_DEADBAND) || (ABS(rcCommand[PITCH]) > CONTROL_DEADBAND) || (ABS(rcCommand[YAW]) > CONTROL_DEADBAND);
 107 }
 108
 109 bool isRollPitchStickDeflected(uint8_t deadband)
 110 {
 111     return (ABS(rcCommand[ROLL]) > deadband) || (ABS(rcCommand[PITCH]) > deadband);
 112 }
 113
 114 throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
 115 {
 116     int value = rxGetChannelValue(THROTTLE);    // THROTTLE_STATUS_TYPE_RC
 117     if (type == THROTTLE_STATUS_TYPE_COMMAND) {
 118         value = rcCommand[THROTTLE];
 119     }
 120
 121     bool midThrottle = value > (reversibleMotorsConfig()->deadband_low) && value < (reversibleMotorsConfig()->deadband_high);
 122     if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
 123         return THROTTLE_LOW;
 124     }
 125
 126     return THROTTLE_HIGH;
 127 }
 128
 129 bool throttleStickIsLow(void)
 130 {
 131     return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 132 }
 133
 134 int16_t throttleStickMixedValue(void)
 135 {
 136     int16_t throttleValue;
 137
 138     throttleValue = constrain(rxGetChannelValue(THROTTLE), rxConfig()->mincheck, PWM_RANGE_MAX);
 139     throttleValue = (uint16_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);  // [MINCHECK;2000] -> [0;1000]
 140     return rcLookupThrottle(throttleValue);
 141 }
 142
 143 rollPitchStatus_e calculateRollPitchCenterStatus(void)
 144 {
 145     if (((rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))
 146             && ((rxGetChannelValue(ROLL) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(ROLL) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))))
 147         return CENTERED;
 148
 149     return NOT_CENTERED;
 150 }
 151
 152 stickPositions_e getRcStickPositions(void)
 153 {
 154     return rcStickPositions;
 155 }
 156
 157 bool checkStickPosition(stickPositions_e stickPos)
 158 {
 159     const uint8_t mask[4] = { ROL_LO | ROL_HI, PIT_LO | PIT_HI, YAW_LO | YAW_HI, THR_LO | THR_HI };
 160     for (int i = 0; i < 4; i++) {
 161         if (((stickPos & mask[i]) != 0) && ((stickPos & mask[i]) != (rcStickPositions & mask[i]))) {
 162             return false;
 163         }
 164     }
 165
 166     return true;
 167 }
 168
 169 static void updateRcStickPositions(void)
 170 {
 171     stickPositions_e tmp = 0;
 172
 173     tmp |= ((rxGetChannelValue(ROLL) > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2);
 174     tmp |= ((rxGetChannelValue(ROLL) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2);
 175
 176     tmp |= ((rxGetChannelValue(PITCH) > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2);
 177     tmp |= ((rxGetChannelValue(PITCH) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2);
 178
 179     tmp |= ((rxGetChannelValue(YAW) > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2);
 180     tmp |= ((rxGetChannelValue(YAW) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2);
 181
 182     tmp |= ((rxGetChannelValue(THROTTLE) > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2);
 183     tmp |= ((rxGetChannelValue(THROTTLE) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2);
 184
 185     rcStickPositions = tmp;
 186 }
 187
 188 void processRcStickPositions(bool isThrottleLow)
 189 {
 190     static timeMs_t lastTickTimeMs = 0;
 191     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
 192     static uint32_t rcSticks;           // this hold sticks position for command combos
 193     static timeMs_t rcDisarmTimeMs;     // this is an extra guard for disarming through switch to prevent that one frame can disarm it
 194     const timeMs_t currentTimeMs = millis();
 195
 196     updateRcStickPositions();
 197
 198     uint32_t stTmp = getRcStickPositions();
 199     if (stTmp == rcSticks) {
 200         if (rcDelayCommand < 250) {
 201             if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) {
 202                 lastTickTimeMs = currentTimeMs;
 203                 rcDelayCommand++;
 204             }
 205         }
 206     } else {
 207         rcDelayCommand = 0;
 208     }
 209
 210     rcSticks = stTmp;
 211
 212     // perform actions
 213     bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
 214
 215     if (STATE(AIRPLANE) && ifMotorstopFeatureEnabled() && armingConfig()->fixed_wing_auto_arm) {
 216         // Auto arm on throttle when using fixedwing and motorstop
 217         if (!isThrottleLow) {
 218             tryArm();
 219             return;
 220         }
 221     }
 222     else {
 223         if (armingSwitchIsActive) {
 224             rcDisarmTimeMs = currentTimeMs;
 225             tryArm();
 226         } else {
 227             emergencyArmingUpdate(armingSwitchIsActive, false);
 228             // Disarming via ARM BOX
 229             // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
 230             // and can't afford to risk disarming in the air
 231             if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
 232                 const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
 233                 if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
 234                     if (armingConfig()->disarm_always || isThrottleLow) {
 235                         disarm(DISARM_SWITCH);
 236                     }
 237                 }
 238             }
 239             else {
 240                 rcDisarmTimeMs = currentTimeMs;
 241             }
 242         }
 243     }
 244
 245     if (rcDelayCommand != 20) {
 246         return;
 247     }
 248
 249     /* Disable stick commands when armed, in CLI mode or CMS is active */
 250     bool disableStickCommands = ARMING_FLAG(ARMED) || cliMode;
 251 #ifdef USE_CMS
 252     disableStickCommands = disableStickCommands || cmsInMenu;
 253 #endif
 254     if (disableStickCommands) {
 255         return;
 256     }
 257
 258     /* REMAINING SECTION HANDLES STICK COMANDS ONLY */
 259
 260     // GYRO calibration
 261     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 262         gyroStartCalibration();
 263         return;
 264     }
 265
 266
 267 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
 268     // Save waypoint list
 269     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
 270         const bool success = saveNonVolatileWaypointList();
 271         beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
 272     }
 273
 274     // Load waypoint list
 275     if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
 276         const bool success = loadNonVolatileWaypointList(false);
 277         beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
 278     }
 279 #ifdef USE_MULTI_MISSION
 280     // Increment multi mission index up
 281     if (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) {
 282         selectMultiMissionIndex(1);
 283         rcDelayCommand = 0;
 284         return;
 285     }
 286
 287     // Decrement multi mission index down
 288     if (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) {
 289         selectMultiMissionIndex(-1);
 290         rcDelayCommand = 0;
 291         return;
 292     }
 293 #endif
 294     if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_HI) {
 295         resetWaypointList();
 296         beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading
 297     }
 298 #endif
 299
 300     // Multiple configuration profiles
 301     if (feature(FEATURE_TX_PROF_SEL)) {
 302
 303         uint8_t i = 0;
 304
 305         if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 306             i = 1;
 307         else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 308             i = 2;
 309         else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 310             i = 3;
 311
 312         if (i) {
 313             setConfigProfileAndWriteEEPROM(i - 1);
 314             return;
 315         }
 316
 317         i = 0;
 318
 319         // Multiple battery configuration profiles
 320         if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 321             i = 1;
 322         else if (rcSticks == THR_HI + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 323             i = 2;
 324         else if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 325             i = 3;
 326
 327         if (i) {
 328             setConfigBatteryProfileAndWriteEEPROM(i - 1);
 329             batteryDisableProfileAutoswitch();
 330             activateBatteryProfile();
 331             return;
 332         }
 333
 334     }
 335
 336     // Save config
 337     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
 338         saveConfigAndNotify();
 339     }
 340
 341     // Calibrating Acc
 342     if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
 343         accStartCalibration();
 344         return;
 345     }
 346
 347     // Calibrating Mag
 348     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
 349         ENABLE_STATE(CALIBRATE_MAG);
 350         return;
 351     }
 352
 353     // Accelerometer Trim
 354     if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
 355         applyAndSaveBoardAlignmentDelta(0, -2);
 356         rcDelayCommand = 10;
 357         return;
 358     } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
 359         applyAndSaveBoardAlignmentDelta(0, 2);
 360         rcDelayCommand = 10;
 361         return;
 362     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
 363         applyAndSaveBoardAlignmentDelta(-2, 0);
 364         rcDelayCommand = 10;
 365         return;
 366     } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
 367         applyAndSaveBoardAlignmentDelta(2, 0);
 368         rcDelayCommand = 10;
 369         return;
 370     }
 371 }
 372
 373 int32_t getRcStickDeflection(int32_t axis) {
 374     return MIN(ABS(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE), 500);
 375 }