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