src/main/flight/failsafe.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
  24 #include "platform.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29
  30 #include "pg/pg.h"
  31 #include "pg/pg_ids.h"
  32 #include "pg/rx.h"
  33
  34 #include "drivers/time.h"
  35
  36 #include "config/config.h"
  37 #include "fc/core.h"
  38 #include "fc/rc_controls.h"
  39 #include "fc/rc_modes.h"
  40 #include "fc/runtime_config.h"
  41
  42 #include "flight/failsafe.h"
  43
  44 #include "io/beeper.h"
  45
  46 #include "rx/rx.h"
  47
  48 #include "flight/pid.h"
  49
  50 /*
  51  * Usage:
  52  *
  53  * failsafeInit() and failsafeReset() must be called before the other methods are used.
  54  *
  55  * failsafeInit() and failsafeReset() can be called in any order.
  56  * failsafeInit() should only be called once.
  57  *
  58  * enable() should be called after system initialisation.
  59  */
  60
  61 static failsafeState_t failsafeState;
  62
  63 PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
  64
  65 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
  66     .failsafe_throttle = 1000,                       // default throttle off.
  67     .failsafe_throttle_low_delay = 100,              // default throttle low delay for "just disarm" on failsafe condition
  68     .failsafe_delay = 10,                                // 1 sec, can regain control on signal recovery, at idle in drop mode
  69     .failsafe_off_delay = 10,                            // 1 sec in landing phase, if enabled
  70     .failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss
  71     .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
  72     .failsafe_recovery_delay = 10,                       // 1 sec of valid rx data needed to allow recovering from failsafe procedure
  73     .failsafe_stick_threshold = 30                   // 30 percent of stick deflection to exit GPS Rescue procedure
  74 );
  75
  76 const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
  77     "AUTO-LAND",
  78     "DROP",
  79 #ifdef USE_GPS_RESCUE
  80     "GPS-RESCUE",
  81 #endif
  82 };
  83
  84 /*
  85  * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
  86  */
  87 void failsafeReset(void)
  88 {
  89     failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2
  90     failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
  91     if (failsafeState.rxDataRecoveryPeriod < PERIOD_RXDATA_RECOVERY){
  92         // PERIOD_RXDATA_RECOVERY (200ms) is the minimum allowed RxData recovery time
  93         failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY;
  94     }
  95     failsafeState.validRxDataReceivedAt = 0;
  96     failsafeState.validRxDataFailedAt = 0;
  97     failsafeState.throttleLowPeriod = 0;
  98     failsafeState.landingShouldBeFinishedAt = 0;
  99     failsafeState.receivingRxDataPeriod = 0;
 100     failsafeState.receivingRxDataPeriodPreset = 0;
 101     failsafeState.phase = FAILSAFE_IDLE;
 102     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 103 }
 104
 105 void failsafeInit(void)
 106 {
 107     failsafeState.events = 0;
 108     failsafeState.monitoring = false;
 109
 110     return;
 111 }
 112
 113 failsafePhase_e failsafePhase(void)
 114 {
 115     return failsafeState.phase;
 116 }
 117
 118 bool failsafeIsMonitoring(void)
 119 {
 120     return failsafeState.monitoring;
 121 }
 122
 123 bool failsafeIsActive(void)
 124 {
 125     return failsafeState.active;
 126 }
 127
 128 void failsafeStartMonitoring(void)
 129 {
 130     failsafeState.monitoring = true;
 131 }
 132
 133 static bool failsafeShouldHaveCausedLandingByNow(void)
 134 {
 135     return (millis() > failsafeState.landingShouldBeFinishedAt);
 136 }
 137
 138 static void failsafeActivate(void)
 139 {
 140     failsafeState.active = true;
 141
 142     failsafeState.phase = FAILSAFE_LANDING;
 143
 144     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 145
 146     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 147
 148     failsafeState.events++;
 149 }
 150
 151 bool failsafeIsReceivingRxData(void)
 152 {
 153     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
 154 }
 155
 156 void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
 157 {
 158     failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000);    // / 1000 to convert micros to millis
 159 }
 160
 161 void failsafeOnRxResume(void)
 162 {
 163     failsafeState.validRxDataReceivedAt = millis();                     // prevent RX link down trigger, restart rx link up
 164     failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;                     // do so while rx link is up
 165 }
 166
 167 void failsafeOnValidDataReceived(void)
 168 {
 169     failsafeState.validRxDataReceivedAt = millis();
 170     if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) ||
 171         (cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) {
 172         // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
 173         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
 174         // Allow arming now we have an RX link
 175         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
 176     }
 177 }
 178
 179 void failsafeOnValidDataFailed(void)
 180 {
 181     failsafeState.validRxDataFailedAt = millis();
 182     if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
 183         // rxDataFailurePeriod is stage 1 guard time
 184         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 185         // Prevent arming with no RX link
 186         setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
 187     }
 188 }
 189
 190 void failsafeCheckDataFailurePeriod(void)
 191 // forces link down directly from scheduler?
 192 {
 193     if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
 194         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 195         // Prevent arming with no RX link
 196         setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
 197     }
 198 }
 199
 200 uint32_t failsafeFailurePeriodMs(void)
 201 {
 202     return failsafeState.rxDataFailurePeriod;
 203 }
 204
 205 FAST_CODE_NOINLINE void failsafeUpdateState(void)
 206 // triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
 207 {
 208     if (!failsafeIsMonitoring()) {
 209         return;
 210     }
 211
 212     bool receivingRxData = failsafeIsReceivingRxData();
 213     // should be true when FAILSAFE_RXLINK_UP
 214     // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
 215     // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
 216
 217     bool armed = ARMING_FLAG(ARMED);
 218     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 219     beeperMode_e beeperMode = BEEPER_SILENCE;
 220
 221     if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
 222         // Aux switch set to failsafe stage2 emulates loss of signal without waiting
 223         failsafeOnValidDataFailed();
 224         receivingRxData = false;
 225     }
 226
 227     // Beep RX lost only if we are not seeing data and we have been armed earlier
 228     if (!receivingRxData && (armed || ARMING_FLAG(WAS_EVER_ARMED))) {
 229         beeperMode = BEEPER_RX_LOST;
 230     }
 231
 232     bool reprocessState;
 233
 234     do {
 235         reprocessState = false;
 236
 237         switch (failsafeState.phase) {
 238             case FAILSAFE_IDLE:
 239                 if (armed) {
 240                     // Track throttle command below minimum time
 241                     if (THROTTLE_HIGH == calculateThrottleStatus()) {
 242                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 243                     }
 244                     if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
 245                         // Failsafe switch is configured as KILL switch and is switched ON
 246                         failsafeActivate();
 247                         // Skip auto-landing procedure
 248                         failsafeState.phase = FAILSAFE_LANDED;
 249                         // Require 3 seconds of valid rxData
 250                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
 251                         reprocessState = true;
 252                     } else if (!receivingRxData) {
 253                         if (millis() > failsafeState.throttleLowPeriod
 254 #ifdef USE_GPS_RESCUE
 255                             && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
 256 #endif
 257                             ) {
 258                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
 259                             failsafeActivate();
 260                             failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
 261                             failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
 262                         } else {
 263                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
 264                         }
 265                         reprocessState = true;
 266                     }
 267                 } else {
 268                     // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
 269                     if (failsafeSwitchIsOn) {
 270                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 271                     } else {
 272                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 273                     }
 274                     // Throttle low period expired (= low long enough for JustDisarm)
 275                     failsafeState.throttleLowPeriod = 0;
 276                 }
 277                 break;
 278
 279             case FAILSAFE_RX_LOSS_DETECTED:
 280                 if (receivingRxData) {
 281                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 282                 } else {
 283                     switch (failsafeConfig()->failsafe_procedure) {
 284                         case FAILSAFE_PROCEDURE_AUTO_LANDING:
 285                             // Stabilize, and set Throttle to specified level
 286                             failsafeActivate();
 287                             break;
 288
 289                         case FAILSAFE_PROCEDURE_DROP_IT:
 290                             // Drop the craft
 291                             failsafeActivate();
 292                             // skip auto-landing procedure
 293                             failsafeState.phase = FAILSAFE_LANDED;
 294                             break;
 295 #ifdef USE_GPS_RESCUE
 296                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
 297                             failsafeActivate();
 298                             failsafeState.phase = FAILSAFE_GPS_RESCUE;
 299                             break;
 300 #endif
 301                     }
 302                 }
 303                 reprocessState = true;
 304                 break;
 305
 306             case FAILSAFE_LANDING:
 307                 if (receivingRxData) {
 308                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 309                     reprocessState = true;
 310                 } else {
 311                     if (armed) {
 312                         beeperMode = BEEPER_RX_LOST_LANDING;
 313                     }
 314                     if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
 315                         // require 3 seconds of valid rxData
 316                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
 317                         failsafeState.phase = FAILSAFE_LANDED;
 318                         reprocessState = true;
 319                     }
 320                 }
 321                 break;
 322 #ifdef USE_GPS_RESCUE
 323             case FAILSAFE_GPS_RESCUE:
 324                 if (receivingRxData) {
 325                     if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
 326                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 327                         reprocessState = true;
 328                     }
 329                 } else {
 330                     if (armed) {
 331                         ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 332                         beeperMode = BEEPER_RX_LOST_LANDING;
 333                     } else {
 334                         // require 3 seconds of valid rxData
 335                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
 336                         failsafeState.phase = FAILSAFE_LANDED;
 337                         reprocessState = true;
 338                     }
 339                 }
 340                 break;
 341 #endif
 342             case FAILSAFE_LANDED:
 343                 // Prevent accidently rearming by an intermittent rx link
 344                 setArmingDisabled(ARMING_DISABLED_FAILSAFE);
 345                 disarm(DISARM_REASON_FAILSAFE);
 346                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
 347                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
 348                 reprocessState = true;
 349                 break;
 350
 351             case FAILSAFE_RX_LOSS_MONITORING:
 352                 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
 353                 if (receivingRxData) {
 354                     if (millis() > failsafeState.receivingRxDataPeriod) {
 355                         // rx link is good now
 356                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 357                         reprocessState = true;
 358                     }
 359                 } else {
 360                     failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
 361                 }
 362                 break;
 363
 364             case FAILSAFE_RX_LOSS_RECOVERED:
 365                 // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
 366                 // This is to prevent that JustDisarm is activated on the next iteration.
 367                 // Because that would have the effect of shutting down failsafe handling on intermittent connections.
 368                 failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 369                 failsafeState.phase = FAILSAFE_IDLE;
 370                 failsafeState.active = false;
 371                 DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 372                 unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
 373                 reprocessState = true;
 374                 break;
 375
 376             default:
 377                 break;
 378         }
 379     } while (reprocessState);
 380
 381     if (beeperMode != BEEPER_SILENCE) {
 382         beeper(beeperMode);
 383     }
 384 }