src/main/flight/failsafe.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
  21 #include "platform.h"
  22
  23 #include "build/build_config.h"
  24
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28
  29 #include "config/feature.h"
  30 #include "config/parameter_group.h"
  31 #include "config/parameter_group_ids.h"
  32
  33 #include "drivers/time.h"
  34
  35 #include "io/beeper.h"
  36
  37 #include "fc/fc_core.h"
  38 #include "fc/config.h"
  39 #include "fc/rc_controls.h"
  40 #include "fc/rc_modes.h"
  41 #include "fc/runtime_config.h"
  42 #include "fc/controlrate_profile.h"
  43 #include "fc/settings.h"
  44
  45 #include "flight/failsafe.h"
  46 #include "flight/mixer.h"
  47 #include "flight/pid.h"
  48
  49 #include "navigation/navigation.h"
  50 #include "navigation/navigation_private.h"
  51
  52 #include "rx/rx.h"
  53
  54 #include "sensors/battery.h"
  55 #include "sensors/sensors.h"
  56
  57 /*
  58  * Usage:
  59  *
  60  * failsafeInit() and failsafeReset() must be called before the other methods are used.
  61  *
  62  * failsafeInit() and failsafeReset() can be called in any order.
  63  * failsafeInit() should only be called once.
  64  *
  65  * enable() should be called after system initialisation.
  66  */
  67
  68 static failsafeState_t failsafeState;
  69
  70 PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 3);
  71
  72 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
  73     .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT,                                   // 0.5 sec
  74     .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT,                 // 0.5 seconds (plus 200ms explicit delay)
  75     .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT,                           // 20sec
  76     .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT,         // default throttle low delay for "just disarm" on failsafe condition
  77     .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT,                           // default full failsafe procedure
  78     .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT,                   // 20 deg left
  79     .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT,                 // 10 deg dive (yes, positive means dive)
  80     .failsafe_fw_yaw_rate = SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT,                       // 45 deg/s left yaw (left is negative, 8s for full turn)
  81     .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT,
  82     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
  83     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
  84     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
  85 #ifdef USE_GPS_FIX_ESTIMATION
  86     .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
  87 #endif
  88 );
  89
  90 typedef enum {
  91     FAILSAFE_CHANNEL_HOLD,      // Hold last known good value
  92     FAILSAFE_CHANNEL_NEUTRAL,   // RPY = zero, THR = zero
  93 } failsafeChannelBehavior_e;
  94
  95 typedef struct {
  96     bool                        bypassNavigation;
  97     bool                        forceAngleMode;
  98     failsafeChannelBehavior_e   channelBehavior[4];
  99 } failsafeProcedureLogic_t;
 100
 101 static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
 102     [FAILSAFE_PROCEDURE_AUTO_LANDING] = {
 103             .forceAngleMode = true,
 104             .bypassNavigation = false,
 105             .channelBehavior = {
 106                 FAILSAFE_CHANNEL_NEUTRAL,       // ROLL
 107                 FAILSAFE_CHANNEL_NEUTRAL,       // PITCH
 108                 FAILSAFE_CHANNEL_NEUTRAL,       // YAW
 109                 FAILSAFE_CHANNEL_HOLD           // THROTTLE
 110             }
 111     },
 112
 113     [FAILSAFE_PROCEDURE_DROP_IT] = {
 114             .bypassNavigation = true,
 115             .forceAngleMode = true,
 116             .channelBehavior = {
 117                 FAILSAFE_CHANNEL_NEUTRAL,       // ROLL
 118                 FAILSAFE_CHANNEL_NEUTRAL,       // PITCH
 119                 FAILSAFE_CHANNEL_NEUTRAL,       // YAW
 120                 FAILSAFE_CHANNEL_NEUTRAL        // THROTTLE
 121             }
 122     },
 123
 124     [FAILSAFE_PROCEDURE_RTH] = {
 125             .bypassNavigation = false,
 126             .forceAngleMode = true,
 127             .channelBehavior = {
 128                 FAILSAFE_CHANNEL_NEUTRAL,       // ROLL
 129                 FAILSAFE_CHANNEL_NEUTRAL,       // PITCH
 130                 FAILSAFE_CHANNEL_NEUTRAL,       // YAW
 131                 FAILSAFE_CHANNEL_HOLD           // THROTTLE
 132             }
 133     },
 134
 135     [FAILSAFE_PROCEDURE_NONE] = {
 136             .bypassNavigation = false,
 137             .forceAngleMode = false,
 138             .channelBehavior = {
 139                 FAILSAFE_CHANNEL_HOLD,          // ROLL
 140                 FAILSAFE_CHANNEL_HOLD,          // PITCH
 141                 FAILSAFE_CHANNEL_HOLD,          // YAW
 142                 FAILSAFE_CHANNEL_HOLD           // THROTTLE
 143             }
 144     }
 145 };
 146
 147 /*
 148  * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
 149  */
 150 void failsafeReset(void)
 151 {
 152     if (failsafeState.active) {  // can't reset if still active
 153         return;
 154     }
 155
 156     failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
 157     failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
 158     failsafeState.validRxDataReceivedAt = 0;
 159     failsafeState.validRxDataFailedAt = 0;
 160     failsafeState.throttleLowPeriod = 0;
 161     failsafeState.landingShouldBeFinishedAt = 0;
 162     failsafeState.receivingRxDataPeriod = 0;
 163     failsafeState.receivingRxDataPeriodPreset = 0;
 164     failsafeState.phase = FAILSAFE_IDLE;
 165     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 166     failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure;
 167     failsafeState.controlling = false;
 168
 169     failsafeState.lastGoodRcCommand[ROLL] = 0;
 170     failsafeState.lastGoodRcCommand[PITCH] = 0;
 171     failsafeState.lastGoodRcCommand[YAW] = 0;
 172     failsafeState.lastGoodRcCommand[THROTTLE] = 1000;
 173 }
 174
 175 void failsafeInit(void)
 176 {
 177     failsafeState.events = 0;
 178     failsafeState.monitoring = false;
 179     failsafeState.suspended = false;
 180 }
 181
 182 bool failsafeBypassNavigation(void)
 183 {
 184     return failsafeState.active &&
 185            failsafeState.controlling &&
 186            failsafeProcedureLogic[failsafeState.activeProcedure].bypassNavigation;
 187 }
 188
 189 bool failsafeMayRequireNavigationMode(void)
 190 {
 191     return (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH) ||
 192            (failsafeConfig()->failsafe_min_distance_procedure == FAILSAFE_PROCEDURE_RTH);
 193 }
 194
 195 failsafePhase_e failsafePhase(void)
 196 {
 197     return failsafeState.phase;
 198 }
 199
 200 bool failsafeIsMonitoring(void)
 201 {
 202     return failsafeState.monitoring;
 203 }
 204
 205 bool failsafeIsActive(void)
 206 {
 207     return failsafeState.active;
 208 }
 209
 210 bool failsafeShouldApplyControlInput(void)
 211 {
 212     return failsafeState.controlling;
 213 }
 214
 215 bool failsafeRequiresAngleMode(void)
 216 {
 217     return failsafeState.active &&
 218            failsafeState.controlling &&
 219            failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode;
 220 }
 221
 222 void failsafeStartMonitoring(void)
 223 {
 224     failsafeState.monitoring = true;
 225 }
 226
 227 static bool failsafeShouldHaveCausedLandingByNow(void)
 228 {
 229     return failsafeConfig()->failsafe_off_delay && (millis() > failsafeState.landingShouldBeFinishedAt);
 230 }
 231
 232 static void failsafeSetActiveProcedure(failsafeProcedure_e procedure)
 233 {
 234     failsafeState.activeProcedure = procedure;
 235 }
 236
 237 static void failsafeActivate(failsafePhase_e newPhase)
 238 {
 239     failsafeState.active = true;
 240     failsafeState.controlling = true;
 241     failsafeState.phase = newPhase;
 242     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 243     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 244
 245 #ifdef USE_FW_AUTOLAND
 246     posControl.fwLandState.landAborted = false;
 247 #endif
 248
 249     failsafeState.events++;
 250 }
 251
 252 void failsafeUpdateRcCommandValues(void)
 253 {
 254     if (!failsafeState.active) {
 255         for (int idx = 0; idx < 4; idx++) {
 256             failsafeState.lastGoodRcCommand[idx] = rcCommand[idx];
 257         }
 258     }
 259 }
 260
 261 void failsafeApplyControlInput(void)
 262 {
 263     // Apply channel values
 264     for (int idx = 0; idx < 4; idx++) {
 265         switch (failsafeProcedureLogic[failsafeState.activeProcedure].channelBehavior[idx]) {
 266             case FAILSAFE_CHANNEL_HOLD:
 267                 rcCommand[idx] = failsafeState.lastGoodRcCommand[idx];
 268                 break;
 269
 270             case FAILSAFE_CHANNEL_NEUTRAL:
 271                 switch (idx) {
 272                     case ROLL:
 273                     case PITCH:
 274                     case YAW:
 275                         rcCommand[idx] = 0;
 276                         break;
 277
 278                     case THROTTLE:
 279                         rcCommand[idx] = feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIDDLE : getThrottleIdleValue();
 280                         break;
 281                 }
 282                 break;
 283         }
 284     }
 285 }
 286
 287 bool failsafeIsReceivingRxData(void)
 288 {
 289     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
 290 }
 291
 292 void failsafeOnRxSuspend(void)
 293 {
 294     failsafeState.suspended = true;
 295 }
 296
 297 bool failsafeIsSuspended(void)
 298 {
 299     return failsafeState.suspended;
 300 }
 301
 302 void failsafeOnRxResume(void)
 303 {
 304     failsafeState.suspended = false;                                    // restart monitoring
 305     failsafeState.validRxDataReceivedAt = millis();                     // prevent RX link down trigger, restart rx link up
 306     failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;                     // do so while rx link is up
 307 }
 308
 309 void failsafeOnValidDataReceived(void)
 310 {
 311     failsafeState.validRxDataReceivedAt = millis();
 312     if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
 313         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
 314     }
 315 }
 316
 317 void failsafeOnValidDataFailed(void)
 318 {
 319     failsafeState.validRxDataFailedAt = millis();
 320     if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
 321         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 322     }
 323 }
 324
 325 static bool failsafeCheckStickMotion(void)
 326 {
 327     if (failsafeConfig()->failsafe_stick_motion_threshold > 0) {
 328         uint32_t totalRcDelta = 0;
 329
 330         totalRcDelta += ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE);
 331         totalRcDelta += ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE);
 332         totalRcDelta += ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE);
 333
 334         return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold;
 335     }
 336     else {
 337         return true;
 338     }
 339 }
 340
 341 static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 342 {
 343     if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && failsafeConfig()->failsafe_mission_delay) {
 344         if (!failsafeState.wpModeDelayedFailsafeStart) {
 345             failsafeState.wpModeDelayedFailsafeStart = millis();
 346             return FAILSAFE_PROCEDURE_NONE;
 347         } else if ((millis() - failsafeState.wpModeDelayedFailsafeStart < (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay)) ||
 348                    failsafeConfig()->failsafe_mission_delay == -1) {
 349             return FAILSAFE_PROCEDURE_NONE;
 350         }
 351     }
 352
 353     // Inhibit Failsafe if emergency landing triggered manually or if landing is detected
 354     if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {
 355         return FAILSAFE_PROCEDURE_NONE;
 356     }
 357
 358     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
 359     // GPS must also be working, and home position set
 360     if (failsafeConfig()->failsafe_min_distance > 0 &&
 361             ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
 362 #ifdef USE_GPS_FIX_ESTIMATION
 363                 || STATE(GPS_ESTIMATED_FIX)
 364 #endif
 365                 ) && STATE(GPS_FIX_HOME)) {
 366
 367         // get the distance to the original arming point
 368         uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
 369         if (distance < failsafeConfig()->failsafe_min_distance) {
 370             // Use the alternate, minimum distance failsafe procedure instead
 371             return failsafeConfig()->failsafe_min_distance_procedure;
 372         }
 373     }
 374
 375     return failsafeConfig()->failsafe_procedure;
 376 }
 377
 378 #ifdef USE_GPS_FIX_ESTIMATION
 379 bool checkGPSFixFailsafe(void)
 380 {
 381     if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) {
 382         if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) {
 383             failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis();
 384         } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) {
 385             if ( !posControl.flags.forcedRTHActivated ) {
 386                 failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH);
 387                 failsafeActivate(FAILSAFE_RETURN_TO_HOME);
 388                 activateForcedRTH();
 389                 return true;
 390             }
 391         }
 392     } else {
 393         failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0;
 394     }
 395     return false;
 396 }
 397 #endif
 398
 399
 400 void failsafeUpdateState(void)
 401 {
 402     if (!failsafeIsMonitoring() || failsafeIsSuspended()) {
 403         return;
 404     }
 405
 406     const bool receivingRxDataAndNotFailsafeMode = failsafeIsReceivingRxData() && !IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 407     const bool armed = ARMING_FLAG(ARMED);
 408     const bool sticksAreMoving = failsafeCheckStickMotion();
 409     beeperMode_e beeperMode = BEEPER_SILENCE;
 410
 411     // Beep RX lost only if we are not seeing data and we have been armed earlier
 412     if (!receivingRxDataAndNotFailsafeMode && ARMING_FLAG(WAS_EVER_ARMED)) {
 413         beeperMode = BEEPER_RX_LOST;
 414     }
 415
 416     bool reprocessState;
 417
 418     do {
 419         reprocessState = false;
 420
 421         switch (failsafeState.phase) {
 422             case FAILSAFE_IDLE:
 423                 if (armed) {
 424                     // Track throttle command below minimum time
 425                     if (!throttleStickIsLow()) {
 426                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 427                     }
 428
 429 #ifdef USE_GPS_FIX_ESTIMATION
 430                     if ( checkGPSFixFailsafe() ) {
 431                         reprocessState = true;
 432                     } else
 433 #endif
 434                     if (!receivingRxDataAndNotFailsafeMode) {
 435                         if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
 436                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
 437                             // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero
 438                             failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_DROP_IT);
 439                             failsafeActivate(FAILSAFE_LANDED);  // skip auto-landing procedure
 440                             failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
 441                         } else {
 442                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
 443                             failsafeState.wpModeDelayedFailsafeStart = 0;
 444                         }
 445                         reprocessState = true;
 446                     }
 447                 } else {
 448                     // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
 449                     if (!receivingRxDataAndNotFailsafeMode) {
 450                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 451                     } else {
 452                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 453                     }
 454                     // Throttle low period expired (= low long enough for JustDisarm)
 455                     failsafeState.throttleLowPeriod = 0;
 456                 }
 457                 break;
 458
 459             case FAILSAFE_RX_LOSS_DETECTED:
 460                 if (receivingRxDataAndNotFailsafeMode) {
 461                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 462                 } else {
 463                     // Set active failsafe procedure
 464                     failsafeSetActiveProcedure(failsafeChooseFailsafeProcedure());
 465
 466                     switch (failsafeState.activeProcedure) {
 467                         case FAILSAFE_PROCEDURE_AUTO_LANDING:
 468                             // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level).
 469                             failsafeActivate(FAILSAFE_LANDING);
 470                             activateForcedEmergLanding();
 471                             break;
 472
 473                         case FAILSAFE_PROCEDURE_DROP_IT:
 474                             // Drop the craft
 475                             failsafeActivate(FAILSAFE_LANDED);      // skip auto-landing procedure
 476                             failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
 477                             break;
 478
 479                         case FAILSAFE_PROCEDURE_RTH:
 480                             // Proceed to handling & monitoring RTH navigation
 481                             failsafeActivate(FAILSAFE_RETURN_TO_HOME);
 482                             activateForcedRTH();
 483                             break;
 484                         case FAILSAFE_PROCEDURE_NONE:
 485                         default:
 486                             // Do nothing procedure
 487                             failsafeActivate(FAILSAFE_RX_LOSS_IDLE);
 488                             break;
 489                     }
 490                 }
 491                 reprocessState = true;
 492                 break;
 493
 494             /* A very simple do-nothing failsafe procedure. The only thing it will do is monitor the receiver state and switch out of FAILSAFE condition */
 495             case FAILSAFE_RX_LOSS_IDLE:
 496                 if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
 497                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 498                     reprocessState = true;
 499                 } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) {  // trigger new failsafe procedure if changed
 500                     failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
 501                     reprocessState = true;
 502                 }
 503 #ifdef USE_GPS_FIX_ESTIMATION
 504                 else {
 505                     if ( checkGPSFixFailsafe() ) {
 506                         reprocessState = true;
 507                     }
 508                 }
 509 #endif
 510
 511                 break;
 512
 513             case FAILSAFE_RETURN_TO_HOME:
 514                 if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
 515                     abortForcedRTH();
 516                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 517                     reprocessState = true;
 518                 }
 519                 else {
 520                     if (armed) {
 521                         beeperMode = BEEPER_RX_LOST_LANDING;
 522                     }
 523                     bool rthLanded = false;
 524                     switch (getStateOfForcedRTH()) {
 525                         case RTH_IN_PROGRESS:
 526                             break;
 527
 528                         case RTH_HAS_LANDED:
 529                             rthLanded = true;
 530                             break;
 531
 532                         case RTH_IDLE:
 533                         default:
 534                             // This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure
 535                             abortForcedRTH();
 536                             failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING);
 537                             failsafeActivate(FAILSAFE_LANDING);
 538                             reprocessState = true;
 539                             break;
 540                     }
 541                     if (rthLanded || !armed) {
 542                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
 543                         failsafeState.phase = FAILSAFE_LANDED;
 544                         reprocessState = true;
 545                     }
 546                 }
 547                 break;
 548
 549             case FAILSAFE_LANDING:
 550                 if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
 551                     abortForcedEmergLanding();
 552                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 553                     reprocessState = true;
 554                 } else {
 555                     if (armed) {
 556                         beeperMode = BEEPER_RX_LOST_LANDING;
 557                     }
 558                     bool emergLanded = false;
 559                     switch (getStateOfForcedEmergLanding()) {
 560                         case EMERG_LAND_IN_PROGRESS:
 561                             break;
 562
 563                         case EMERG_LAND_HAS_LANDED:
 564                             emergLanded = true;
 565                             break;
 566
 567                         case EMERG_LAND_IDLE:
 568                         default:
 569                             // If emergency landing was somehow aborted during failsafe - fallback to FAILSAFE_PROCEDURE_DROP_IT
 570                             abortForcedEmergLanding();
 571                             failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_DROP_IT);
 572                             failsafeActivate(FAILSAFE_LANDED);
 573                             reprocessState = true;
 574                             break;
 575                     }
 576                     if (emergLanded || failsafeShouldHaveCausedLandingByNow() || !armed) {
 577                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
 578                         failsafeState.phase = FAILSAFE_LANDED;
 579                         reprocessState = true;
 580                     }
 581                 }
 582                 break;
 583
 584             case FAILSAFE_LANDED:
 585                 ENABLE_ARMING_FLAG(ARMING_DISABLED_FAILSAFE_SYSTEM); // To prevent accidently rearming by an intermittent rx link
 586                 disarm(DISARM_FAILSAFE);
 587                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
 588                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
 589                 failsafeState.controlling = false;  // Failsafe no longer in control of the machine - release control to pilot
 590                 reprocessState = true;
 591                 break;
 592
 593             case FAILSAFE_RX_LOSS_MONITORING:
 594                 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
 595                 if (receivingRxDataAndNotFailsafeMode) {
 596                     if (millis() > failsafeState.receivingRxDataPeriod) {
 597                         // rx link is good now, when arming via ARM switch, it must be OFF first
 598                         if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 599                             // XXX: Requirements for removing the ARMING_DISABLED_FAILSAFE_SYSTEM flag
 600                             // are tested by osd.c to show the user how to re-arm. If these
 601                             // requirements change, update osdArmingDisabledReasonMessage().
 602                             DISABLE_ARMING_FLAG(ARMING_DISABLED_FAILSAFE_SYSTEM);
 603                             failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 604                             reprocessState = true;
 605                         }
 606                     }
 607                 } else {
 608                     failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
 609                 }
 610                 break;
 611
 612             case FAILSAFE_RX_LOSS_RECOVERED:
 613                 // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
 614                 // This is to prevent that JustDisarm is activated on the next iteration.
 615                 // Because that would have the effect of shutting down failsafe handling on intermittent connections.
 616                 failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 617                 failsafeState.phase = FAILSAFE_IDLE;
 618                 failsafeState.active = false;
 619                 failsafeState.controlling = false;
 620                 DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 621                 reprocessState = true;
 622                 break;
 623
 624             default:
 625                 break;
 626         }
 627     } while (reprocessState);
 628
 629     if (beeperMode != BEEPER_SILENCE) {
 630         beeper(beeperMode);
 631     }
 632 }