Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: tidy failsafe action handling 

Mode changes will not work when mode not compiled in
This commit is contained in:
Peter Barker 2024-06-05 13:48:05 +10:00 committed by Peter Barker
parent c376781c08
commit 136e76e5ea
1 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduCopter/events.cpp
View File

@ -379,41 +379,46 @@ void Copter::failsafe_deadreckon_check()
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason) void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason)
{ {
#if MODE_RTL_ENABLED
// attempt to switch to RTL, if this fails then switch to Land // attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(Mode::Number::RTL, reason)) { if (set_mode(Mode::Number::RTL, reason)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause(reason);
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1; AP_Notify::events.failsafe_mode_change = 1;
return;
} }
#endif
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause(reason);
} }
// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts // set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason) void Copter::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
{ {
#if MODE_SMARTRTL_ENABLED
// attempt to switch to SMART_RTL, if this failed then switch to Land // attempt to switch to SMART_RTL, if this failed then switch to Land
if (!set_mode(Mode::Number::SMART_RTL, reason)) { if (set_mode(Mode::Number::SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
set_mode_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1; AP_Notify::events.failsafe_mode_change = 1;
return;
} }
#endif
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
set_mode_land_with_pause(reason);
} }
// 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 // 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
// this is always called from a failsafe so we trigger notification to pilot // this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason) void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
{ {
#if MODE_SMARTRTL_ENABLED
// attempt to switch to SmartRTL, if this failed then attempt to RTL // attempt to switch to SmartRTL, if this failed then attempt to RTL
// if that fails, then land // if that fails, then land
if (!set_mode(Mode::Number::SMART_RTL, reason)) { if (set_mode(Mode::Number::SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1; AP_Notify::events.failsafe_mode_change = 1;
return;
} }
#endif
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
} }
// Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param // Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param