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 = 4,                             // 0,4sec
  69     .failsafe_off_delay = 10,                        // 1sec
  70     .failsafe_switch_mode = 0,                       // 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 = 20,                   // 2 sec of valid rx data (plus 200ms) 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 = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
  90     failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
  91     failsafeState.validRxDataReceivedAt = 0;
  92     failsafeState.validRxDataFailedAt = 0;
  93     failsafeState.throttleLowPeriod = 0;
  94     failsafeState.landingShouldBeFinishedAt = 0;
  95     failsafeState.receivingRxDataPeriod = 0;
  96     failsafeState.receivingRxDataPeriodPreset = 0;
  97     failsafeState.phase = FAILSAFE_IDLE;
  98     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
  99 }
 100
 101 void failsafeInit(void)
 102 {
 103     failsafeState.events = 0;
 104     failsafeState.monitoring = false;
 105
 106     return;
 107 }
 108
 109 failsafePhase_e failsafePhase(void)
 110 {
 111     return failsafeState.phase;
 112 }
 113
 114 bool failsafeIsMonitoring(void)
 115 {
 116     return failsafeState.monitoring;
 117 }
 118
 119 bool failsafeIsActive(void)
 120 {
 121     return failsafeState.active;
 122 }
 123
 124 void failsafeStartMonitoring(void)
 125 {
 126     failsafeState.monitoring = true;
 127 }
 128
 129 static bool failsafeShouldHaveCausedLandingByNow(void)
 130 {
 131     return (millis() > failsafeState.landingShouldBeFinishedAt);
 132 }
 133
 134 static void failsafeActivate(void)
 135 {
 136     failsafeState.active = true;
 137
 138     failsafeState.phase = FAILSAFE_LANDING;
 139
 140     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 141     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 142
 143     failsafeState.events++;
 144 }
 145
 146 static void failsafeApplyControlInput(void)
 147 {
 148 #ifdef USE_GPS_RESCUE
 149     if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
 150         ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 151         return;
 152     }
 153 #endif
 154
 155     for (int i = 0; i < 3; i++) {
 156         rcData[i] = rxConfig()->midrc;
 157     }
 158     rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
 159 }
 160
 161 bool failsafeIsReceivingRxData(void)
 162 {
 163     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
 164 }
 165
 166 void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
 167 {
 168     failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000);    // / 1000 to convert micros to millis
 169 }
 170
 171 void failsafeOnRxResume(void)
 172 {
 173     failsafeState.validRxDataReceivedAt = millis();                     // prevent RX link down trigger, restart rx link up
 174     failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;                     // do so while rx link is up
 175 }
 176
 177 void failsafeOnValidDataReceived(void)
 178 {
 179     failsafeState.validRxDataReceivedAt = millis();
 180     if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
 181         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
 182         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
 183     }
 184 }
 185
 186 void failsafeOnValidDataFailed(void)
 187 {
 188     setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
 189     failsafeState.validRxDataFailedAt = millis();
 190     if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
 191         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
 192     }
 193 }
 194
 195 void failsafeUpdateState(void)
 196 {
 197     if (!failsafeIsMonitoring()) {
 198         return;
 199     }
 200
 201     bool receivingRxData = failsafeIsReceivingRxData();
 202     bool armed = ARMING_FLAG(ARMED);
 203     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 204     beeperMode_e beeperMode = BEEPER_SILENCE;
 205
 206     if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) {
 207         receivingRxData = false; // force Stage2
 208     }
 209
 210     // Beep RX lost only if we are not seeing data and we have been armed earlier
 211     if (!receivingRxData && (armed || ARMING_FLAG(WAS_EVER_ARMED))) {
 212         beeperMode = BEEPER_RX_LOST;
 213     }
 214
 215     bool reprocessState;
 216
 217     do {
 218         reprocessState = false;
 219
 220         switch (failsafeState.phase) {
 221             case FAILSAFE_IDLE:
 222                 if (armed) {
 223                     // Track throttle command below minimum time
 224                     if (THROTTLE_HIGH == calculateThrottleStatus()) {
 225                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 226                     }
 227                     // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
 228                     if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) {
 229                         // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
 230                         failsafeActivate();
 231                         failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
 232                         failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS;    // require 1 seconds of valid rxData
 233                         reprocessState = true;
 234                     } else if (!receivingRxData) {
 235                         if (millis() > failsafeState.throttleLowPeriod
 236 #ifdef USE_GPS_RESCUE
 237                             && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
 238 #endif
 239                             ) {
 240                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
 241                             failsafeActivate();
 242                             failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
 243                             failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
 244                         } else {
 245                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
 246                         }
 247                         reprocessState = true;
 248                     }
 249                 } else {
 250                     // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
 251                     if (failsafeSwitchIsOn) {
 252                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
 253                     } else {
 254                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 255                     }
 256                     // Throttle low period expired (= low long enough for JustDisarm)
 257                     failsafeState.throttleLowPeriod = 0;
 258                 }
 259                 break;
 260
 261             case FAILSAFE_RX_LOSS_DETECTED:
 262                 if (receivingRxData) {
 263                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 264                 } else {
 265                     switch (failsafeConfig()->failsafe_procedure) {
 266                         case FAILSAFE_PROCEDURE_AUTO_LANDING:
 267                             // Stabilize, and set Throttle to specified level
 268                             failsafeActivate();
 269                             break;
 270
 271                         case FAILSAFE_PROCEDURE_DROP_IT:
 272                             // Drop the craft
 273                             failsafeActivate();
 274                             failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
 275                             break;
 276 #ifdef USE_GPS_RESCUE
 277                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
 278                             failsafeActivate();
 279                             failsafeState.phase = FAILSAFE_GPS_RESCUE;
 280                             break;
 281 #endif
 282                     }
 283                 }
 284                 reprocessState = true;
 285                 break;
 286
 287             case FAILSAFE_LANDING:
 288                 if (receivingRxData) {
 289                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 290                     reprocessState = true;
 291                 }
 292                 if (armed) {
 293                     failsafeApplyControlInput();
 294                     beeperMode = BEEPER_RX_LOST_LANDING;
 295                 }
 296                 if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
 297                     failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
 298                     failsafeState.phase = FAILSAFE_LANDED;
 299                     reprocessState = true;
 300                 }
 301                 break;
 302 #ifdef USE_GPS_RESCUE
 303             case FAILSAFE_GPS_RESCUE:
 304                 if (receivingRxData) {
 305                     if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
 306                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 307                         reprocessState = true;
 308                     }
 309                 }
 310                 if (armed) {
 311                     failsafeApplyControlInput();
 312                     beeperMode = BEEPER_RX_LOST_LANDING;
 313                 } else {
 314                     failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
 315                     failsafeState.phase = FAILSAFE_LANDED;
 316                     reprocessState = true;
 317                 }
 318                 break;
 319 #endif
 320             case FAILSAFE_LANDED:
 321                 setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
 322                 disarm(DISARM_REASON_FAILSAFE);
 323                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
 324                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
 325                 reprocessState = true;
 326                 break;
 327
 328             case FAILSAFE_RX_LOSS_MONITORING:
 329                 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
 330                 if (receivingRxData) {
 331                     if (millis() > failsafeState.receivingRxDataPeriod) {
 332                         // rx link is good now, when arming via ARM switch, it must be OFF first
 333                         if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
 334                             unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
 335                             failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
 336                             reprocessState = true;
 337                         }
 338                     }
 339                 } else {
 340                     failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
 341                 }
 342                 break;
 343
 344             case FAILSAFE_RX_LOSS_RECOVERED:
 345                 // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
 346                 // This is to prevent that JustDisarm is activated on the next iteration.
 347                 // Because that would have the effect of shutting down failsafe handling on intermittent connections.
 348                 failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
 349                 failsafeState.phase = FAILSAFE_IDLE;
 350                 failsafeState.active = false;
 351                 DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
 352                 reprocessState = true;
 353                 break;
 354
 355             default:
 356                 break;
 357         }
 358     } while (reprocessState);
 359
 360     if (beeperMode != BEEPER_SILENCE) {
 361         beeper(beeperMode);
 362     }
 363 }