aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: support Auto RTL for failsafe events

Browse Source
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_SmartRTL = 3,
Failsafe_Action_SmartRTL_Land = 4,
Failsafe_Action_Terminate = 5
Failsafe_Action_Terminate = 5,
Failsafe_Action_Auto_DO_LAND_START = 6,
};
enum class FailsafeOption {
@ -731,6 +732,7 @@ private:
void set_mode_RTL_or_land_with_pause(ModeReason reason);
void set_mode_SmartRTL_or_RTL(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();
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_SMARTRTL_OR_RTL 4
#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)
#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_LAND 4
#define FS_GCS_ENABLED_ALWAYS_LAND 5
#define FS_GCS_ENABLED_AUTO_RTL_OR_RTL 6
// EKF failsafe definitions (FS_EKF_ACTION parameter)
#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:
desired_action = Failsafe_Action_Land;
break;
case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START;
break;
default:
desired_action = Failsafe_Action_Land;
}
@ -170,6 +173,9 @@ void Copter::failsafe_gcs_on_event(void)
case FS_GCS_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land;
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
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() {
if (ap.in_arming_delay) {
return true;
@ -383,6 +401,10 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
#endif
}
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