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