src/main/flight/failsafe.h

   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 #pragma once
  19 #include "common/time.h"
  20 #include "config/parameter_group.h"
  21
  22 #define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
  23 #define MILLIS_PER_TENTH_SECOND         100
  24 #define MILLIS_PER_SECOND              1000
  25 #define PERIOD_OF_1_SECONDS               1 * MILLIS_PER_SECOND
  26 #define PERIOD_OF_3_SECONDS               3 * MILLIS_PER_SECOND
  27 #define PERIOD_OF_30_SECONDS             30 * MILLIS_PER_SECOND
  28 #define PERIOD_RXDATA_FAILURE           200    // millis
  29 #define PERIOD_RXDATA_RECOVERY          200    // millis
  30
  31
  32 typedef struct failsafeConfig_s {
  33     uint16_t failsafe_throttle_low_delay;       // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
  34     uint8_t failsafe_delay;                     // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
  35     uint8_t failsafe_recovery_delay;            // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)
  36     uint8_t failsafe_off_delay;                 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
  37     uint8_t failsafe_procedure;                 // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
  38
  39     int16_t failsafe_fw_roll_angle;             // Settings to be applies during "LAND" procedure on a fixed-wing
  40     int16_t failsafe_fw_pitch_angle;
  41     int16_t failsafe_fw_yaw_rate;
  42     uint16_t failsafe_stick_motion_threshold;
  43     uint16_t failsafe_min_distance;             // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
  44     uint8_t failsafe_min_distance_procedure;    // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
  45     int16_t failsafe_mission_delay;             // Time delay before Failsafe triggered when WP mission in progress (s)
  46 #ifdef USE_GPS_FIX_ESTIMATION
  47     int16_t failsafe_gps_fix_estimation_delay;  // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
  48 #endif
  49 } failsafeConfig_t;
  50
  51 PG_DECLARE(failsafeConfig_t, failsafeConfig);
  52
  53 typedef enum {
  54     FAILSAFE_IDLE = 0,
  55     /* Failsafe mode is not active. All other
  56      * phases indicate that the failsafe flight
  57      * mode is active.
  58      */
  59     FAILSAFE_RX_LOSS_DETECTED,
  60     /* In this phase, the connection from the receiver
  61      * has been confirmed as lost and it will either
  62      * transition into FAILSAFE_RX_LOSS_RECOVERED if the
  63      * RX link is recovered immediately or one of the
  64      * recovery phases otherwise (as configured via
  65      * failsafe_procedure) or into FAILSAFE_RX_LOSS_IDLE
  66      * if failsafe_procedure is NONE.
  67      */
  68     FAILSAFE_RX_LOSS_IDLE,
  69     /* This phase will just do nothing else than wait
  70      * until the RX connection is re-established and the
  71      * sticks are moved more than the failsafe_stick_threshold
  72      * settings and then transition to FAILSAFE_RX_LOSS_RECOVERED.
  73      * Note that this phase is only used when
  74      * failsafe_procedure = NONE.
  75      */
  76     FAILSAFE_RETURN_TO_HOME,
  77     /* Failsafe is executing RTH. This phase is the first one
  78      * enabled when failsafe_procedure = RTH if an RTH is
  79      * deemed possible (RTH might not be activated if e.g.
  80      * a HOME position was not recorded or some required
  81      * sensors are not working at the moment). If RTH can't
  82      * be started, this phase will transition to FAILSAFE_LANDING.
  83      */
  84     FAILSAFE_LANDING,
  85     /* Performs NAV Emergency Landing using controlled descent rate if
  86      * altitude sensors available.
  87      * Otherwise Emergency Landing performs a simplified landing procedure.
  88      * This is done by setting throttle and roll/pitch/yaw controls
  89      * to a pre-configured values that will allow aircraft
  90      * to reach ground in somewhat safe "controlled crash" way.
  91      * This is the first recovery phase enabled when
  92      * failsafe_procedure = LAND. Once timeout expires or if a
  93      * "controlled crash" can't be executed, this phase will
  94      * transition to FAILSAFE_LANDED.
  95      */
  96     FAILSAFE_LANDED,
  97     /* Failsafe has either detected that the model has landed and disabled
  98      * the motors or either decided to drop the model because it couldn't
  99      * perform an emergency landing. It will disarm, prevent re-arming
 100      * and transition into FAILSAFE_RX_LOSS_MONITORING immediately. This is
 101      * the first recovery phase enabled when failsafe_procedure = DROP.
 102      */
 103     FAILSAFE_RX_LOSS_MONITORING,
 104     /* This phase will wait until the RX connection is
 105      * working for some time and if and only if switch arming
 106      * is used and the switch is in the unarmed position
 107      * will allow rearming again.
 108      */
 109     FAILSAFE_RX_LOSS_RECOVERED
 110     /* This phase indicates that the RX link has been re-established and
 111      * it will immediately transition out of failsafe mode (phase will
 112      * transition to FAILSAFE_IDLE.)
 113      */
 114 } failsafePhase_e;
 115
 116 typedef enum {
 117     FAILSAFE_RXLINK_DOWN = 0,
 118     FAILSAFE_RXLINK_UP
 119 } failsafeRxLinkState_e;
 120
 121 typedef enum {
 122     FAILSAFE_PROCEDURE_AUTO_LANDING = 0,
 123     FAILSAFE_PROCEDURE_DROP_IT,
 124     FAILSAFE_PROCEDURE_RTH,
 125     FAILSAFE_PROCEDURE_NONE
 126 } failsafeProcedure_e;
 127
 128 typedef enum {
 129     RTH_IDLE = 0,               // RTH is waiting
 130     RTH_IN_PROGRESS,            // RTH is active
 131     RTH_HAS_LANDED              // RTH is active and has landed.
 132 } rthState_e;
 133
 134 typedef enum {
 135     EMERG_LAND_IDLE = 0,        // Emergency landing is waiting
 136     EMERG_LAND_IN_PROGRESS,     // Emergency landing is active
 137     EMERG_LAND_HAS_LANDED       // Emergency landing is active and has landed.
 138 } emergLandState_e;
 139
 140 typedef struct failsafeState_s {
 141     int16_t events;
 142     bool monitoring;                        // Flag that failsafe is monitoring RC link
 143     bool suspended;                         // Failsafe is temporary suspended. This happens when we temporary suspend RX system due to EEPROM write/read
 144     bool active;                            // Failsafe is active (on RC link loss)
 145     bool controlling;                       // Failsafe is driving the sticks instead of pilot
 146     timeMs_t rxDataFailurePeriod;
 147     timeMs_t rxDataRecoveryPeriod;
 148     timeMs_t validRxDataReceivedAt;
 149     timeMs_t validRxDataFailedAt;
 150     timeMs_t throttleLowPeriod;             // throttle stick must have been below 'min_check' for this period
 151     timeMs_t landingShouldBeFinishedAt;
 152     timeMs_t receivingRxDataPeriod;         // period for the required period of valid rxData
 153     timeMs_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
 154     timeMs_t wpModeDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time
 155 #ifdef USE_GPS_FIX_ESTIMATION
 156     timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time on GPS fix estimation
 157 #endif
 158     failsafeProcedure_e activeProcedure;
 159     failsafePhase_e phase;
 160     failsafeRxLinkState_e rxLinkState;
 161     int16_t lastGoodRcCommand[4];
 162 } failsafeState_t;
 163
 164 void failsafeInit(void);
 165 void failsafeReset(void);
 166
 167 void failsafeStartMonitoring(void);
 168 void failsafeUpdateState(void);
 169
 170 failsafePhase_e failsafePhase(void);
 171 bool failsafeIsMonitoring(void);
 172 bool failsafeIsActive(void);
 173 bool failsafeIsReceivingRxData(void);
 174 void failsafeOnRxSuspend(void);
 175 void failsafeOnRxResume(void);
 176 bool failsafeMayRequireNavigationMode(void);
 177 void failsafeApplyControlInput(void);
 178 bool failsafeRequiresAngleMode(void);
 179 bool failsafeShouldApplyControlInput(void);
 180 bool failsafeBypassNavigation(void);
 181 void failsafeUpdateRcCommandValues(void);
 182
 183 void failsafeOnValidDataReceived(void);
 184 void failsafeOnValidDataFailed(void);