Line data Source code
1 : #include "Copter.h"
2 :
3 : /*
4 : * This event will be called when the failsafe changes
5 : * boolean failsafe reflects the current state
6 : */
7 :
8 60 : bool Copter::failsafe_option(FailsafeOption opt) const
9 : {
10 60 : return (g2.fs_options & (uint32_t)opt);
11 : }
12 :
13 30 : void Copter::failsafe_radio_on_event()
14 : {
15 30 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
16 :
17 : // set desired action based on FS_THR_ENABLE parameter
18 : FailsafeAction desired_action;
19 30 : switch ((FS_THR_Action)g.failsafe_throttle) {
20 0 : case FS_THR_Action::DISABLED:
21 0 : desired_action = FailsafeAction::NONE;
22 0 : break;
23 22 : case FS_THR_Action::ALWAYS_RTL:
24 : case FS_THR_Action::CONTINUE_MISSION:
25 22 : desired_action = FailsafeAction::RTL;
26 22 : break;
27 4 : case FS_THR_Action::ALWAYS_SMARTRTL_OR_RTL:
28 4 : desired_action = FailsafeAction::SMARTRTL;
29 4 : break;
30 3 : case FS_THR_Action::ALWAYS_SMARTRTL_OR_LAND:
31 3 : desired_action = FailsafeAction::SMARTRTL_LAND;
32 3 : break;
33 1 : case FS_THR_Action::ALWAYS_LAND:
34 1 : desired_action = FailsafeAction::LAND;
35 1 : break;
36 0 : case FS_THR_Action::AUTO_RTL_OR_RTL:
37 0 : desired_action = FailsafeAction::AUTO_DO_LAND_START;
38 0 : break;
39 0 : case FS_THR_Action::BRAKE_OR_LAND:
40 0 : desired_action = FailsafeAction::BRAKE_LAND;
41 0 : break;
42 0 : default:
43 0 : desired_action = FailsafeAction::LAND;
44 : }
45 :
46 : // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
47 30 : if (should_disarm_on_failsafe()) {
48 : // should immediately disarm when we're on the ground
49 12 : announce_failsafe("Radio", "Disarming");
50 12 : arming.disarm(AP_Arming::Method::RADIOFAILSAFE);
51 12 : desired_action = FailsafeAction::NONE;
52 :
53 18 : } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
54 : // Allow landing to continue when battery failsafe requires it (not a user option)
55 1 : announce_failsafe("Radio + Battery", "Continuing Landing");
56 1 : desired_action = FailsafeAction::LAND;
57 :
58 17 : } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
59 : // Allow landing to continue when FS_OPTIONS is set to continue landing
60 0 : announce_failsafe("Radio", "Continuing Landing");
61 0 : desired_action = FailsafeAction::LAND;
62 :
63 17 : } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
64 : // Allow mission to continue when FS_OPTIONS is set to continue mission
65 2 : announce_failsafe("Radio", "Continuing Auto");
66 2 : desired_action = FailsafeAction::NONE;
67 :
68 15 : } else if ((flightmode->in_guided_mode()) && failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) {
69 : // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode
70 1 : announce_failsafe("Radio", "Continuing Guided Mode");
71 1 : desired_action = FailsafeAction::NONE;
72 :
73 : } else {
74 14 : announce_failsafe("Radio");
75 : }
76 :
77 : // Call the failsafe action handler
78 30 : do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE);
79 30 : }
80 :
81 : // failsafe_off_event - respond to radio contact being regained
82 24 : void Copter::failsafe_radio_off_event()
83 : {
84 : // no need to do anything except log the error as resolved
85 : // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
86 24 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
87 24 : gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
88 24 : }
89 :
90 63 : void Copter::announce_failsafe(const char *type, const char *action_undertaken)
91 : {
92 63 : if (action_undertaken != nullptr) {
93 21 : gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe - %s", type, action_undertaken);
94 : } else {
95 42 : gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe", type);
96 : }
97 63 : }
98 :
99 20 : void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
100 : {
101 20 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
102 :
103 20 : FailsafeAction desired_action = (FailsafeAction)action;
104 :
105 : // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
106 20 : if (should_disarm_on_failsafe()) {
107 : // should immediately disarm when we're on the ground
108 2 : arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
109 2 : desired_action = FailsafeAction::NONE;
110 2 : announce_failsafe("Battery", "Disarming");
111 :
112 18 : } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != FailsafeAction::NONE) {
113 : // Allow landing to continue when FS_OPTIONS is set to continue when landing
114 0 : desired_action = FailsafeAction::LAND;
115 0 : announce_failsafe("Battery", "Continuing Landing");
116 : } else {
117 18 : announce_failsafe("Battery");
118 : }
119 :
120 : // Battery FS options already use the Failsafe_Options enum. So use them directly.
121 20 : do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);
122 :
123 20 : }
124 :
125 : // failsafe_gcs_check - check for ground station failsafe
126 228158 : void Copter::failsafe_gcs_check()
127 : {
128 : // Bypass GCS failsafe checks if disabled or GCS never connected
129 228158 : if (g.failsafe_gcs == FS_GCS_Action::DISABLED) {
130 225144 : return;
131 : }
132 :
133 3014 : const uint32_t gcs_last_seen_ms = gcs().sysid_mygcs_last_seen_time_ms();
134 3014 : if (gcs_last_seen_ms == 0) {
135 0 : return;
136 : }
137 :
138 : // calc time since last gcs update
139 : // note: this only looks at the heartbeat from the device id set by sysid_mygcs
140 3014 : const uint32_t last_gcs_update_ms = millis() - gcs_last_seen_ms;
141 3014 : const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
142 :
143 : // Determine which event to trigger
144 3014 : if (last_gcs_update_ms < gcs_timeout_ms && failsafe.gcs) {
145 : // Recovery from a GCS failsafe
146 13 : set_failsafe_gcs(false);
147 13 : failsafe_gcs_off_event();
148 :
149 3001 : } else if (last_gcs_update_ms < gcs_timeout_ms && !failsafe.gcs) {
150 : // No problem, do nothing
151 :
152 1473 : } else if (last_gcs_update_ms > gcs_timeout_ms && failsafe.gcs) {
153 : // Already in failsafe, do nothing
154 :
155 13 : } else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) {
156 : // New GCS failsafe event, trigger events
157 13 : set_failsafe_gcs(true);
158 13 : failsafe_gcs_on_event();
159 : }
160 : }
161 :
162 : // failsafe_gcs_on_event - actions to take when GCS contact is lost
163 13 : void Copter::failsafe_gcs_on_event(void)
164 : {
165 13 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
166 13 : RC_Channels::clear_overrides();
167 :
168 : // convert the desired failsafe response to the FailsafeAction enum
169 : FailsafeAction desired_action;
170 13 : switch ((FS_GCS_Action)g.failsafe_gcs) {
171 0 : case FS_GCS_Action::DISABLED:
172 0 : desired_action = FailsafeAction::NONE;
173 0 : break;
174 6 : case FS_GCS_Action::ALWAYS_RTL:
175 : case FS_GCS_Action::CONTINUE_MISSION:
176 6 : desired_action = FailsafeAction::RTL;
177 6 : break;
178 4 : case FS_GCS_Action::ALWAYS_SMARTRTL_OR_RTL:
179 4 : desired_action = FailsafeAction::SMARTRTL;
180 4 : break;
181 1 : case FS_GCS_Action::ALWAYS_SMARTRTL_OR_LAND:
182 1 : desired_action = FailsafeAction::SMARTRTL_LAND;
183 1 : break;
184 1 : case FS_GCS_Action::ALWAYS_LAND:
185 1 : desired_action = FailsafeAction::LAND;
186 1 : break;
187 0 : case FS_GCS_Action::AUTO_RTL_OR_RTL:
188 0 : desired_action = FailsafeAction::AUTO_DO_LAND_START;
189 0 : break;
190 0 : case FS_GCS_Action::BRAKE_OR_LAND:
191 0 : desired_action = FailsafeAction::BRAKE_LAND;
192 0 : break;
193 1 : default: // if an invalid parameter value is set, the fallback is RTL
194 1 : desired_action = FailsafeAction::RTL;
195 : }
196 :
197 : // Conditions to deviate from FS_GCS_ENABLE parameter setting
198 13 : if (!motors->armed()) {
199 0 : desired_action = FailsafeAction::NONE;
200 0 : announce_failsafe("GCS");
201 :
202 13 : } else if (should_disarm_on_failsafe()) {
203 : // should immediately disarm when we're on the ground
204 0 : arming.disarm(AP_Arming::Method::GCSFAILSAFE);
205 0 : desired_action = FailsafeAction::NONE;
206 0 : announce_failsafe("GCS", "Disarming");
207 :
208 13 : } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
209 : // Allow landing to continue when battery failsafe requires it (not a user option)
210 0 : announce_failsafe("GCS + Battery", "Continuing Landing");
211 0 : desired_action = FailsafeAction::LAND;
212 :
213 13 : } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
214 : // Allow landing to continue when FS_OPTIONS is set to continue landing
215 1 : announce_failsafe("GCS", "Continuing Landing");
216 1 : desired_action = FailsafeAction::LAND;
217 :
218 12 : } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
219 : // Allow mission to continue when FS_OPTIONS is set to continue mission
220 1 : announce_failsafe("GCS", "Continuing Auto Mode");
221 1 : desired_action = FailsafeAction::NONE;
222 :
223 11 : } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) {
224 : // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
225 1 : announce_failsafe("GCS", "Continuing Pilot Control");
226 1 : desired_action = FailsafeAction::NONE;
227 : } else {
228 10 : announce_failsafe("GCS");
229 : }
230 :
231 : // Call the failsafe action handler
232 13 : do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE);
233 13 : }
234 :
235 : // failsafe_gcs_off_event - actions to take when GCS contact is restored
236 13 : void Copter::failsafe_gcs_off_event(void)
237 : {
238 13 : gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
239 13 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
240 13 : }
241 :
242 : // executes terrain failsafe if data is missing for longer than a few seconds
243 228158 : void Copter::failsafe_terrain_check()
244 : {
245 : // trigger within <n> milliseconds of failures while in various modes
246 228158 : bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
247 228158 : bool trigger_event = timeout && flightmode->requires_terrain_failsafe();
248 :
249 : // check for clearing of event
250 228158 : if (trigger_event != failsafe.terrain) {
251 1 : if (trigger_event) {
252 0 : failsafe_terrain_on_event();
253 : } else {
254 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED);
255 1 : failsafe.terrain = false;
256 : }
257 : }
258 228158 : }
259 :
260 : // set terrain data status (found or not found)
261 3368978 : void Copter::failsafe_terrain_set_status(bool data_ok)
262 : {
263 3368978 : uint32_t now = millis();
264 :
265 : // record time of first and latest failures (i.e. duration of failures)
266 3368978 : if (!data_ok) {
267 0 : failsafe.terrain_last_failure_ms = now;
268 0 : if (failsafe.terrain_first_failure_ms == 0) {
269 0 : failsafe.terrain_first_failure_ms = now;
270 : }
271 : } else {
272 : // failures cleared after 0.1 seconds of persistent successes
273 3368978 : if (now - failsafe.terrain_last_failure_ms > 100) {
274 3368978 : failsafe.terrain_last_failure_ms = 0;
275 3368978 : failsafe.terrain_first_failure_ms = 0;
276 : }
277 : }
278 3368978 : }
279 :
280 : // terrain failsafe action
281 1 : void Copter::failsafe_terrain_on_event()
282 : {
283 1 : failsafe.terrain = true;
284 1 : switch (wp_nav->get_terrain_source()) {
285 0 : case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
286 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain %s", "data missing");
287 0 : break;
288 0 : case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
289 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain %s", "Rangefinder Unhealthy");
290 0 : break;
291 1 : case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE:
292 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain %s", "Unavailable");
293 1 : break;
294 : }
295 :
296 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
297 :
298 1 : if (should_disarm_on_failsafe()) {
299 0 : arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
300 : #if MODE_RTL_ENABLED
301 1 : } else if (flightmode->mode_number() == Mode::Number::RTL) {
302 0 : mode_rtl.restart_without_terrain();
303 : #endif
304 : } else {
305 1 : set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE);
306 : }
307 1 : }
308 :
309 : // check for gps glitch failsafe
310 759512 : void Copter::gpsglitch_check()
311 : {
312 759512 : const bool gps_glitching = AP::ahrs().has_status(AP_AHRS::Status::GPS_GLITCHING);
313 :
314 : // log start or stop of gps glitch. AP_Notify update is handled from within AP_AHRS
315 759512 : if (ap.gps_glitching != gps_glitching) {
316 22 : ap.gps_glitching = gps_glitching;
317 22 : if (gps_glitching) {
318 13 : LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH);
319 13 : gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch or Compass error");
320 : } else {
321 9 : LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED);
322 9 : gcs().send_text(MAV_SEVERITY_CRITICAL,"Glitch cleared");
323 : }
324 : }
325 759512 : }
326 :
327 : // dead reckoning alert and failsafe
328 228158 : void Copter::failsafe_deadreckon_check()
329 : {
330 : // update dead reckoning state
331 228158 : const char* dr_prefix_str = "Dead Reckoning";
332 :
333 : // get EKF filter status
334 228158 : const bool ekf_dead_reckoning = AP::ahrs().has_status(AP_AHRS::Status::DEAD_RECKONING);
335 :
336 : // alert user to start or stop of dead reckoning
337 228158 : const uint32_t now_ms = AP_HAL::millis();
338 228158 : if (dead_reckoning.active != ekf_dead_reckoning) {
339 2 : dead_reckoning.active = ekf_dead_reckoning;
340 2 : if (dead_reckoning.active) {
341 1 : dead_reckoning.start_ms = now_ms;
342 1 : gcs().send_text(MAV_SEVERITY_CRITICAL,"%s started", dr_prefix_str);
343 : } else {
344 1 : dead_reckoning.start_ms = 0;
345 1 : dead_reckoning.timeout = false;
346 1 : gcs().send_text(MAV_SEVERITY_CRITICAL,"%s stopped", dr_prefix_str);
347 : }
348 : }
349 :
350 : // check for timeout
351 228158 : if (dead_reckoning.active && !dead_reckoning.timeout) {
352 92 : const uint32_t dr_timeout_ms = uint32_t(constrain_float(g2.failsafe_dr_timeout * 1000.0f, 0.0f, UINT32_MAX));
353 92 : if (now_ms - dead_reckoning.start_ms > dr_timeout_ms) {
354 1 : dead_reckoning.timeout = true;
355 1 : gcs().send_text(MAV_SEVERITY_CRITICAL,"%s timeout", dr_prefix_str);
356 : }
357 : }
358 :
359 : // exit immediately if deadreckon failsafe is disabled
360 228158 : if (g2.failsafe_dr_enable <= 0) {
361 833 : failsafe.deadreckon = false;
362 833 : return;
363 : }
364 :
365 : // check for failsafe action
366 227325 : if (failsafe.deadreckon != ekf_dead_reckoning) {
367 2 : failsafe.deadreckon = ekf_dead_reckoning;
368 :
369 : // only take action in modes requiring position estimate
370 2 : if (failsafe.deadreckon && copter.flightmode->requires_position()) {
371 :
372 : // log error
373 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED);
374 :
375 : // immediately disarm while landed
376 1 : if (should_disarm_on_failsafe()) {
377 1 : arming.disarm(AP_Arming::Method::DEADRECKON_FAILSAFE);
378 1 : return;
379 : }
380 :
381 : // take user specified action
382 0 : do_failsafe_action((FailsafeAction)g2.failsafe_dr_enable.get(), ModeReason::DEADRECKON_FAILSAFE);
383 : }
384 : }
385 : }
386 :
387 : // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
388 : // this is always called from a failsafe so we trigger notification to pilot
389 15 : void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
390 : {
391 : #if MODE_RTL_ENABLED
392 : // attempt to switch to RTL, if this fails then switch to Land
393 15 : if (set_mode(Mode::Number::RTL, reason)) {
394 15 : AP_Notify::events.failsafe_mode_change = 1;
395 15 : return;
396 : }
397 : #endif
398 : // set mode to land will trigger mode change notification to pilot
399 0 : set_mode_land_with_pause(reason);
400 : }
401 :
402 : // set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
403 : // this is always called from a failsafe so we trigger notification to pilot
404 5 : void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
405 : {
406 : #if MODE_SMARTRTL_ENABLED
407 : // attempt to switch to SMART_RTL, if this failed then switch to Land
408 5 : if (set_mode(Mode::Number::SMART_RTL, reason)) {
409 4 : AP_Notify::events.failsafe_mode_change = 1;
410 4 : return;
411 : }
412 : #endif
413 1 : gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
414 1 : set_mode_land_with_pause(reason);
415 : }
416 :
417 : // set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
418 : // this is always called from a failsafe so we trigger notification to pilot
419 9 : void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
420 : {
421 : #if MODE_SMARTRTL_ENABLED
422 : // attempt to switch to SmartRTL, if this failed then attempt to RTL
423 : // if that fails, then land
424 9 : if (set_mode(Mode::Number::SMART_RTL, reason)) {
425 8 : AP_Notify::events.failsafe_mode_change = 1;
426 8 : return;
427 : }
428 : #endif
429 1 : gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
430 1 : set_mode_RTL_or_land_with_pause(reason);
431 : }
432 :
433 : // Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param
434 : // This can come from failsafe or RC option
435 0 : void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
436 : {
437 : #if MODE_AUTO_ENABLED
438 0 : if (set_mode(Mode::Number::AUTO_RTL, reason)) {
439 0 : AP_Notify::events.failsafe_mode_change = 1;
440 0 : return;
441 : }
442 : #endif
443 :
444 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode");
445 0 : set_mode_RTL_or_land_with_pause(reason);
446 : }
447 :
448 : // Sets mode to Brake or LAND with 4 second delay before descent starts
449 : // This can come from failsafe or RC option
450 2 : void Copter::set_mode_brake_or_land_with_pause(ModeReason reason)
451 : {
452 : #if MODE_BRAKE_ENABLED
453 2 : if (set_mode(Mode::Number::BRAKE, reason)) {
454 1 : AP_Notify::events.failsafe_mode_change = 1;
455 1 : return;
456 : }
457 : #endif
458 :
459 1 : gcs().send_text(MAV_SEVERITY_WARNING, "Trying Land Mode");
460 1 : set_mode_land_with_pause(reason);
461 : }
462 :
463 115 : bool Copter::should_disarm_on_failsafe() {
464 115 : if (ap.in_arming_delay) {
465 3 : return true;
466 : }
467 :
468 112 : switch (flightmode->mode_number()) {
469 62 : case Mode::Number::STABILIZE:
470 : case Mode::Number::ACRO:
471 : // if throttle is zero OR vehicle is landed disarm motors
472 62 : return ap.throttle_zero || ap.land_complete;
473 6 : case Mode::Number::AUTO:
474 : case Mode::Number::AUTO_RTL:
475 : // if mission has not started AND vehicle is landed, disarm motors
476 6 : return !ap.auto_armed && ap.land_complete;
477 44 : default:
478 : // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
479 : // if landed disarm
480 44 : return ap.land_complete;
481 : }
482 : }
483 :
484 :
485 63 : void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
486 :
487 : // Execute the specified desired_action
488 63 : switch (action) {
489 23 : case FailsafeAction::NONE:
490 23 : return;
491 10 : case FailsafeAction::LAND:
492 10 : set_mode_land_with_pause(reason);
493 10 : break;
494 13 : case FailsafeAction::RTL:
495 13 : set_mode_RTL_or_land_with_pause(reason);
496 13 : break;
497 9 : case FailsafeAction::SMARTRTL:
498 9 : set_mode_SmartRTL_or_RTL(reason);
499 9 : break;
500 5 : case FailsafeAction::SMARTRTL_LAND:
501 5 : set_mode_SmartRTL_or_land_with_pause(reason);
502 5 : break;
503 1 : case FailsafeAction::TERMINATE: {
504 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
505 : g2.afs.gcs_terminate(true, "Failsafe");
506 : #else
507 1 : arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
508 : #endif
509 1 : break;
510 : }
511 0 : case FailsafeAction::AUTO_DO_LAND_START:
512 0 : set_mode_auto_do_land_start_or_RTL(reason);
513 0 : break;
514 2 : case FailsafeAction::BRAKE_LAND:
515 2 : set_mode_brake_or_land_with_pause(reason);
516 2 : break;
517 : }
518 :
519 : #if AP_GRIPPER_ENABLED
520 40 : if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) {
521 0 : gripper.release();
522 : }
523 : #endif
524 : }
525 :
|
