From 8c3a726609c981335e9d4b029cc96cbaa709e075 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 16 Dec 2024 06:40:46 -0600 Subject: [PATCH] update FS actions metadata with DO_RETURN_PATH_START --- ArduCopter/Parameters.cpp | 6 +++--- ArduPlane/AP_Arming.cpp | 20 ++++++++++++++----- ArduPlane/Parameters.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 4 ++-- 4 files changed, 21 insertions(+), 11 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a90c06b789acd9..44a14b195627f3 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -161,7 +161,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled. - // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START or RTL,7:Brake or Land + // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Brake or Land // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), @@ -225,7 +225,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START or RTL,7:Enabled always Brake or Land + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Enabled always Brake or Land // @User: Standard GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), @@ -1044,7 +1044,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FS_DR_ENABLE // @DisplayName: DeadReckon Failsafe Action // @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates - // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL + // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @User: Standard AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL), diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index fca35f8d088fe4..886e74784d5b62 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -433,16 +433,26 @@ bool AP_Arming_Plane::mission_checks(bool report) { // base checks bool ret = AP_Arming::mission_checks(report); - if (plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { - if (plane.mission.contains_item(MAV_CMD_DO_LAND_START)) { + + switch (RtlAutoland(plane.g.rtl_autoland)) { + case RtlAutoland::RTL_THEN_DO_LAND_START: + case RtlAutoland::RTL_IMMEDIATE_DO_LAND_START: + case RtlAutoland::NO_RTL_GO_AROUND: + if (!plane.mission.contains_item(MAV_CMD_DO_LAND_START)) { ret = false; - check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); + check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START mission item expected"); } - if (plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) { + break; + case RtlAutoland::DO_RETURN_PATH_START: + if (!plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) { ret = false; - check_failed(ARMING_CHECK_MISSION, report, "DO_RETURN_PATH_START set and RTL_AUTOLAND disabled"); + check_failed(ARMING_CHECK_MISSION, report, "DO_RETURN_PATH_START mission item expected"); } + break; + case RtlAutoland::RTL_DISABLE: + break; } + #if HAL_QUADPLANE_ENABLED if (plane.quadplane.available()) { const uint16_t num_commands = plane.mission.num_commands(); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 8dcd2352200863..78d30542f76afa 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -745,7 +745,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: RTL_AUTOLAND // @DisplayName: RTL auto land - // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation). + // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).If this is set to 0 and there is a DO_LAND_START or DO_RETURN_PATH_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around (see wiki for aborting autolandings) without it changing RTL behaviour. // @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item // @User: Standard GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index eac0d2b02b5f51..8865a42d37296c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @DisplayName: Low battery failsafe action // @Description: What action the vehicle should perform if it hits a low battery failsafe // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,6:Loiter to QLand - // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL + // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate // @Values{Tracker}: 0:None @@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @DisplayName: Critical battery failsafe action // @Description: What action the vehicle should perform if it hits a critical battery failsafe // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,5:Parachute,6:Loiter to QLand - // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL + // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate // @Values{Tracker}: 0:None