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

Copter: support Auto RTL for failsafe events

This commit is contained in:
Iampete1 2021-07-25 00:25:40 +01:00 committed by Randy Mackay
parent b63cc809ce
commit 6561669213
3 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -594,7 +594,8 @@ private:
Failsafe_Action_RTL = 2, Failsafe_Action_RTL = 2,
Failsafe_Action_SmartRTL = 3, Failsafe_Action_SmartRTL = 3,
Failsafe_Action_SmartRTL_Land = 4, Failsafe_Action_SmartRTL_Land = 4,
Failsafe_Action_Terminate = 5 Failsafe_Action_Terminate = 5,
Failsafe_Action_Auto_DO_LAND_START = 6,
}; };
enum class FailsafeOption { enum class FailsafeOption {
@ -731,6 +732,7 @@ private:
void set_mode_RTL_or_land_with_pause(ModeReason reason); void set_mode_RTL_or_land_with_pause(ModeReason reason);
void set_mode_SmartRTL_or_RTL(ModeReason reason); void set_mode_SmartRTL_or_RTL(ModeReason reason);
void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
bool should_disarm_on_failsafe(); bool should_disarm_on_failsafe();
void do_failsafe_action(Failsafe_Action action, ModeReason reason); void do_failsafe_action(Failsafe_Action action, ModeReason reason);

2
ArduCopter/defines.h
View File

@ -152,6 +152,7 @@ enum LoggingParameters {
#define FS_THR_ENABLED_ALWAYS_LAND 3 #define FS_THR_ENABLED_ALWAYS_LAND 3
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5
#define FS_THR_ENABLED_AUTO_RTL_OR_RTL 6
// GCS failsafe definitions (FS_GCS_ENABLE parameter) // GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0 #define FS_GCS_DISABLED 0
@ -160,6 +161,7 @@ enum LoggingParameters {
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
#define FS_GCS_ENABLED_ALWAYS_LAND 5 #define FS_GCS_ENABLED_ALWAYS_LAND 5
#define FS_GCS_ENABLED_AUTO_RTL_OR_RTL 6
// EKF failsafe definitions (FS_EKF_ACTION parameter) // EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe

22
ArduCopter/events.cpp
View File

@ -33,6 +33,9 @@ void Copter::failsafe_radio_on_event()
case FS_THR_ENABLED_ALWAYS_LAND: case FS_THR_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land; desired_action = Failsafe_Action_Land;
break; break;
case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START;
break;
default: default:
desired_action = Failsafe_Action_Land; desired_action = Failsafe_Action_Land;
} }
@ -170,6 +173,9 @@ void Copter::failsafe_gcs_on_event(void)
case FS_GCS_ENABLED_ALWAYS_LAND: case FS_GCS_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land; desired_action = Failsafe_Action_Land;
break; break;
case FS_GCS_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START;
break;
default: // if an invalid parameter value is set, the fallback is RTL default: // if an invalid parameter value is set, the fallback is RTL
desired_action = Failsafe_Action_RTL; desired_action = Failsafe_Action_RTL;
} }
@ -336,6 +342,18 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason)
} }
} }
// Sets mode to Auto and jumps to DO_LAND_START, as set with AUTO_RTL param
// This can come from failsafe or RC option
void Copter::set_mode_auto_do_land_start_or_RTL(ModeReason reason)
{
if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(reason)) {
return;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
}
bool Copter::should_disarm_on_failsafe() { bool Copter::should_disarm_on_failsafe() {
if (ap.in_arming_delay) { if (ap.in_arming_delay) {
return true; return true;
@ -383,6 +401,10 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
#endif #endif
} }
break; break;
case Failsafe_Action_Auto_DO_LAND_START:
set_mode_auto_do_land_start_or_RTL(reason);
AP_Notify::events.failsafe_mode_change = 1;
break;
} }
#if GRIPPER_ENABLED == ENABLED #if GRIPPER_ENABLED == ENABLED