mirror of https://github.com/ArduPilot/ardupilot
Copter: support Auto RTL for failsafe events
This commit is contained in:
parent
b63cc809ce
commit
6561669213
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue