2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
26 #include "build/debug.h"
28 #include "common/axis.h"
31 #include "pg/pg_ids.h"
33 #include "drivers/time.h"
35 #include "fc/config.h"
36 #include "fc/fc_core.h"
37 #include "fc/rc_controls.h"
38 #include "fc/rc_modes.h"
39 #include "fc/runtime_config.h"
41 #include "flight/failsafe.h"
43 #include "io/beeper.h"
44 #include "io/motors.h"
48 #include "flight/pid.h"
53 * failsafeInit() and failsafeReset() must be called before the other methods are used.
55 * failsafeInit() and failsafeReset() can be called in any order.
56 * failsafeInit() should only be called once.
58 * enable() should be called after system initialisation.
61 static failsafeState_t failsafeState
;
63 PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t
, failsafeConfig
, PG_FAILSAFE_CONFIG
, 2);
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_kill_switch
= 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
75 * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
77 void failsafeReset(void)
79 failsafeState
.rxDataFailurePeriod
= PERIOD_RXDATA_FAILURE
+ failsafeConfig()->failsafe_delay
* MILLIS_PER_TENTH_SECOND
;
80 failsafeState
.validRxDataReceivedAt
= 0;
81 failsafeState
.validRxDataFailedAt
= 0;
82 failsafeState
.throttleLowPeriod
= 0;
83 failsafeState
.landingShouldBeFinishedAt
= 0;
84 failsafeState
.receivingRxDataPeriod
= 0;
85 failsafeState
.receivingRxDataPeriodPreset
= 0;
86 failsafeState
.phase
= FAILSAFE_IDLE
;
87 failsafeState
.rxLinkState
= FAILSAFE_RXLINK_DOWN
;
90 void failsafeInit(void)
92 failsafeState
.events
= 0;
93 failsafeState
.monitoring
= false;
98 failsafePhase_e
failsafePhase(void)
100 return failsafeState
.phase
;
103 bool failsafeIsMonitoring(void)
105 return failsafeState
.monitoring
;
108 bool failsafeIsActive(void)
110 return failsafeState
.active
;
113 void failsafeStartMonitoring(void)
115 failsafeState
.monitoring
= true;
118 static bool failsafeShouldHaveCausedLandingByNow(void)
120 return (millis() > failsafeState
.landingShouldBeFinishedAt
);
123 static void failsafeActivate(void)
125 failsafeState
.active
= true;
126 failsafeState
.phase
= FAILSAFE_LANDING
;
127 ENABLE_FLIGHT_MODE(FAILSAFE_MODE
);
128 failsafeState
.landingShouldBeFinishedAt
= millis() + failsafeConfig()->failsafe_off_delay
* MILLIS_PER_TENTH_SECOND
;
130 failsafeState
.events
++;
133 static void failsafeApplyControlInput(void)
135 for (int i
= 0; i
< 3; i
++) {
136 rcData
[i
] = rxConfig()->midrc
;
138 rcData
[THROTTLE
] = failsafeConfig()->failsafe_throttle
;
141 bool failsafeIsReceivingRxData(void)
143 return (failsafeState
.rxLinkState
== FAILSAFE_RXLINK_UP
);
146 void failsafeOnRxSuspend(uint32_t usSuspendPeriod
)
148 failsafeState
.validRxDataReceivedAt
+= (usSuspendPeriod
/ 1000); // / 1000 to convert micros to millis
151 void failsafeOnRxResume(void)
153 failsafeState
.validRxDataReceivedAt
= millis(); // prevent RX link down trigger, restart rx link up
154 failsafeState
.rxLinkState
= FAILSAFE_RXLINK_UP
; // do so while rx link is up
157 void failsafeOnValidDataReceived(void)
159 failsafeState
.validRxDataReceivedAt
= millis();
160 if ((failsafeState
.validRxDataReceivedAt
- failsafeState
.validRxDataFailedAt
) > PERIOD_RXDATA_RECOVERY
) {
161 failsafeState
.rxLinkState
= FAILSAFE_RXLINK_UP
;
162 unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE
);
166 void failsafeOnValidDataFailed(void)
168 setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE
); // To prevent arming with no RX link
169 failsafeState
.validRxDataFailedAt
= millis();
170 if ((failsafeState
.validRxDataFailedAt
- failsafeState
.validRxDataReceivedAt
) > failsafeState
.rxDataFailurePeriod
) {
171 failsafeState
.rxLinkState
= FAILSAFE_RXLINK_DOWN
;
175 void failsafeUpdateState(void)
177 if (!failsafeIsMonitoring()) {
181 bool receivingRxData
= failsafeIsReceivingRxData();
182 bool armed
= ARMING_FLAG(ARMED
);
183 bool failsafeSwitchIsOn
= IS_RC_MODE_ACTIVE(BOXFAILSAFE
);
184 beeperMode_e beeperMode
= BEEPER_SILENCE
;
186 // Beep RX lost only if we are not seeing data and we have been armed earlier
187 if (!receivingRxData
&& ARMING_FLAG(WAS_EVER_ARMED
)) {
188 beeperMode
= BEEPER_RX_LOST
;
194 reprocessState
= false;
196 switch (failsafeState
.phase
) {
199 // Track throttle command below minimum time
200 if (THROTTLE_HIGH
== calculateThrottleStatus()) {
201 failsafeState
.throttleLowPeriod
= millis() + failsafeConfig()->failsafe_throttle_low_delay
* MILLIS_PER_TENTH_SECOND
;
203 // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
204 if (failsafeSwitchIsOn
&& failsafeConfig()->failsafe_kill_switch
) {
205 // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
207 failsafeState
.phase
= FAILSAFE_LANDED
; // skip auto-landing procedure
208 failsafeState
.receivingRxDataPeriodPreset
= PERIOD_OF_1_SECONDS
; // require 1 seconds of valid rxData
209 reprocessState
= true;
210 } else if (!receivingRxData
) {
211 if (millis() > failsafeState
.throttleLowPeriod
) {
212 // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
214 failsafeState
.phase
= FAILSAFE_LANDED
; // skip auto-landing procedure
215 failsafeState
.receivingRxDataPeriodPreset
= PERIOD_OF_3_SECONDS
; // require 3 seconds of valid rxData
217 failsafeState
.phase
= FAILSAFE_RX_LOSS_DETECTED
;
219 reprocessState
= true;
222 // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
223 if (failsafeSwitchIsOn
) {
224 ENABLE_FLIGHT_MODE(FAILSAFE_MODE
);
226 DISABLE_FLIGHT_MODE(FAILSAFE_MODE
);
228 // Throttle low period expired (= low long enough for JustDisarm)
229 failsafeState
.throttleLowPeriod
= 0;
233 case FAILSAFE_RX_LOSS_DETECTED
:
234 if (receivingRxData
) {
235 failsafeState
.phase
= FAILSAFE_RX_LOSS_RECOVERED
;
237 switch (failsafeConfig()->failsafe_procedure
) {
239 case FAILSAFE_PROCEDURE_AUTO_LANDING
:
240 // Stabilize, and set Throttle to specified level
244 case FAILSAFE_PROCEDURE_DROP_IT
:
247 failsafeState
.phase
= FAILSAFE_LANDED
; // skip auto-landing procedure
248 failsafeState
.receivingRxDataPeriodPreset
= PERIOD_OF_3_SECONDS
; // require 3 seconds of valid rxData
252 reprocessState
= true;
255 case FAILSAFE_LANDING
:
256 if (receivingRxData
) {
257 failsafeState
.phase
= FAILSAFE_RX_LOSS_RECOVERED
;
258 reprocessState
= true;
261 failsafeApplyControlInput();
262 beeperMode
= BEEPER_RX_LOST_LANDING
;
264 if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed
) {
265 failsafeState
.receivingRxDataPeriodPreset
= PERIOD_OF_30_SECONDS
; // require 30 seconds of valid rxData
266 failsafeState
.phase
= FAILSAFE_LANDED
;
267 reprocessState
= true;
271 case FAILSAFE_LANDED
:
272 setArmingDisabled(ARMING_DISABLED_FAILSAFE
); // To prevent accidently rearming by an intermittent rx link
274 failsafeState
.receivingRxDataPeriod
= millis() + failsafeState
.receivingRxDataPeriodPreset
; // set required period of valid rxData
275 failsafeState
.phase
= FAILSAFE_RX_LOSS_MONITORING
;
276 reprocessState
= true;
279 case FAILSAFE_RX_LOSS_MONITORING
:
280 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
281 if (receivingRxData
) {
282 if (millis() > failsafeState
.receivingRxDataPeriod
) {
283 // rx link is good now, when arming via ARM switch, it must be OFF first
284 if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM
))) {
285 unsetArmingDisabled(ARMING_DISABLED_FAILSAFE
);
286 failsafeState
.phase
= FAILSAFE_RX_LOSS_RECOVERED
;
287 reprocessState
= true;
291 failsafeState
.receivingRxDataPeriod
= millis() + failsafeState
.receivingRxDataPeriodPreset
;
295 case FAILSAFE_RX_LOSS_RECOVERED
:
296 // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period.
297 // This is to prevent that JustDisarm is activated on the next iteration.
298 // Because that would have the effect of shutting down failsafe handling on intermittent connections.
299 failsafeState
.throttleLowPeriod
= millis() + failsafeConfig()->failsafe_throttle_low_delay
* MILLIS_PER_TENTH_SECOND
;
300 failsafeState
.phase
= FAILSAFE_IDLE
;
301 failsafeState
.active
= false;
302 DISABLE_FLIGHT_MODE(FAILSAFE_MODE
);
303 reprocessState
= true;
309 } while (reprocessState
);
311 if (beeperMode
!= BEEPER_SILENCE
) {