From a0a81de20be21266e59f6fe73319e8c07a892472 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 2 Jan 2024 19:47:43 +0000 Subject: [PATCH 01/49] Copter: notify when fence breach has cleared output fence breach type when switching mode without a fence action do not go into manual recovery only recover if there is a fence action implement auto-takeoff fence options output user-friendly fence names auto-enable fences on auto-takeoff use fence enable_configured() simplify message printing --- ArduCopter/fence.cpp | 5 +++-- ArduCopter/mode.cpp | 10 ++++++---- ArduCopter/mode_auto.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 3 +++ 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 3bfe085c2b869..aba1ecce5bb84 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -24,7 +24,7 @@ void Copter::fence_check() if (new_breaches) { if (!copter.ap.land_complete) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); } // if the user wants some kind of response and motors are armed @@ -81,7 +81,8 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index ce191b0c1ef08..69cda906803ec 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -371,10 +371,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #endif #if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); + if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { + // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover + // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) + // but it should be harmless to disable the fence temporarily in these situations as well + fence.manual_recovery_start(); + } #endif #if AP_CAMERA_ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b5e9a62eec325..db1f384eba866 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -783,10 +783,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { //disable - copter.fence.enable(false); + copter.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence - copter.fence.enable(true); + copter.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } #endif //AP_FENCE_ENABLED diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a771c0fdaa0ef..f22501e47fb70 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -685,6 +685,9 @@ void ModeGuided::takeoff_run() auto_takeoff.run(); if (auto_takeoff.complete && !takeoff_complete) { takeoff_complete = true; +#if AP_FENCE_ENABLED + copter.fence.auto_enable_fence_after_takeoff(); +#endif #if AP_LANDINGGEAR_ENABLED // optionally retract landing gear copter.landinggear.retract_after_takeoff(); From 98173440345e2673690f207eca975bef6fc67b0b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Jan 2024 18:02:54 +0000 Subject: [PATCH 02/49] GCS_MAVLink: use bitmask based enablement for fences --- libraries/GCS_MAVLink/GCS_Fence.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index d6dad1a76990c..b3bfb78a37194 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -16,20 +16,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_int return MAV_RESULT_UNSUPPORTED; } - switch ((uint16_t)packet.param1) { - case 0: // disable fence - fence->enable(false); + uint8_t fences = AC_FENCE_ALL_FENCES; + if (uint8_t(packet.param2)) { + fences = uint8_t(packet.param2); + } + + switch (AC_Fence::MavlinkFenceActions(packet.param1)) { + case AC_Fence::MavlinkFenceActions::DISABLE_FENCE: + fence->enable(false, fences); return MAV_RESULT_ACCEPTED; - case 1: // enable fence - if (!fence->present()) - { + case AC_Fence::MavlinkFenceActions::ENABLE_FENCE: + if (!(fence->present() & fences)) { return MAV_RESULT_FAILED; } - - fence->enable(true); + + fence->enable(true, fences); return MAV_RESULT_ACCEPTED; - case 2: // disable fence floor only - fence->disable_floor(); + case AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE: + fence->enable(false, AC_FENCE_TYPE_ALT_MIN); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; @@ -82,7 +86,7 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type = FENCE_BREACH_BOUNDARY; } - // report on Avoidance liminting + // report on Avoidance limiting uint8_t breach_mitigation = FENCE_MITIGATE_UNKNOWN; #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) const AC_Avoid* avoid = AC_Avoid::get_singleton(); From 4ce89c8489865bae87d0d6744b6cd89b818fd748 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Jan 2024 18:03:19 +0000 Subject: [PATCH 03/49] AP_Mission: add comment about new fence API --- libraries/AP_Mission/AP_Mission.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ff42107ff6418..c11b574e8338f 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1233,7 +1233,8 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - cmd.p1 = packet.param1; // action 0=disable, 1=enable + cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor + // packet.param2; // bitmask see FENCE_TYPE enum break; case MAV_CMD_DO_AUX_FUNCTION: @@ -1746,7 +1747,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - packet.param1 = cmd.p1; // action 0=disable, 1=enable + packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=disable floor, 3=enable except floor break; case MAV_CMD_DO_PARACHUTE: // MAV ID: 208 From 5f51be6dac7242789589df16779811b994c291ba Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 15 Jan 2024 19:35:03 +0000 Subject: [PATCH 04/49] AC_Avoidance: take into account minimum altitude fence when calculating climb rate --- libraries/AC_Avoidance/AC_Avoid.cpp | 40 +++++++++++++++++++++++------ 1 file changed, 32 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 8adc6d7c6dc4a..2756e4b8fddd8 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -379,29 +379,53 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c return; } - // do not adjust climb_rate if level or descending - if (climb_rate_cms <= 0.0f) { + // do not adjust climb_rate if level + if (is_zero(climb_rate_cms)) { return; } + const AP_AHRS &_ahrs = AP::ahrs(); // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + // if descending, adjust for minimum altitude as necessary + if (climb_rate_cms < 0.0f) { +#if AP_FENCE_ENABLED + // calculate distance below fence + AC_Fence *fence = AP::fence(); + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence + && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { + // calculate distance from vehicle to safe altitude + float veh_alt; + _ahrs.get_relative_position_D_home(veh_alt); + // fence.get_safe_alt_min() is UP, veh_alt is DOWN: + const float alt_min = -(fence->get_safe_alt_min() + veh_alt); + + if (alt_min <= 0.0f) { + climb_rate_cms = MAX(climb_rate_cms, 0.0f); + // also calculate backup speed that will get us back to safe altitude + backup_speed = get_max_speed(kP, accel_cmss_limited, -alt_min*100.0f, dt); + } + } +#endif + return; + } + bool limit_alt = false; float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - const AP_AHRS &_ahrs = AP::ahrs(); - #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) { // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); - // _fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = fence->get_safe_alt_max() + veh_alt; - limit_alt = true; + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + alt_diff = fence->get_safe_alt_max() + veh_alt; + limit_alt = true; + } } #endif From 694c6a0a9e58af073d45390d213cc812f72cf90c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 19 Jan 2024 12:30:46 +0000 Subject: [PATCH 05/49] AP_Arming: do not enable minimim altitude fence on arming call appropriate fence method for auto-enablement --- libraries/AP_Arming/AP_Arming.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index fa8cc8aab0e62..83e7cac17447f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1797,11 +1797,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) if (armed) { auto *fence = AP::fence(); if (fence != nullptr) { - // If a fence is set to auto-enable, turn on the fence - if (fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fence: auto-enabled"); - } + fence->auto_enable_fence_on_arming(); } } #endif @@ -1845,7 +1841,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) AC_Fence *fence = AP::fence(); if (fence != nullptr) { if(fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(false); + fence->enable_configured(false); } } #endif From fe40fbb6752675e52d474a5b008002c57390b825 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 19 Jan 2024 20:43:45 +0000 Subject: [PATCH 06/49] Plane: output user-friendly fence messages --- ArduPlane/fence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 33062460df6b0..bef88306ebc58 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -50,7 +50,7 @@ void Plane::fence_check() } if (new_breaches) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); // if the user wants some kind of response and motors are armed const uint8_t fence_act = fence.get_action(); From 427e81e22968f058b9318c0364ea31d53d703441 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 27 Jan 2024 16:36:35 +0000 Subject: [PATCH 07/49] AP_Common: fix initialization of ExpandingString so that it can be used on the stack zero out passed in buffers for ExpandingString --- libraries/AP_Common/ExpandingString.cpp | 7 +++++++ libraries/AP_Common/ExpandingString.h | 2 ++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_Common/ExpandingString.cpp b/libraries/AP_Common/ExpandingString.cpp index 9be2b3a91abf1..a3e7a8b401958 100644 --- a/libraries/AP_Common/ExpandingString.cpp +++ b/libraries/AP_Common/ExpandingString.cpp @@ -25,6 +25,13 @@ extern const AP_HAL::HAL& hal; #define EXPAND_INCREMENT 512 +ExpandingString::ExpandingString(char* s, uint32_t total_len) : buf(0) +{ + set_buffer(s, total_len, 0); + memset(buf, 0, buflen); +} + + /* expand the string buffer */ diff --git a/libraries/AP_Common/ExpandingString.h b/libraries/AP_Common/ExpandingString.h index 2561332f3e89d..d92882e1b3af6 100644 --- a/libraries/AP_Common/ExpandingString.h +++ b/libraries/AP_Common/ExpandingString.h @@ -24,6 +24,8 @@ class ExpandingString { public: + ExpandingString() : buf(0), buflen(0), used(0), allocation_failed(false), external_buffer(false) {} + ExpandingString(char* s, uint32_t total_len); const char *get_string(void) const { return buf; From a540f1dc87043213b4a9a22bb8752fd5e396a8e5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:44:25 +0000 Subject: [PATCH 08/49] AP_Frsky_Telem: use fence enable_configured() --- libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp index 623ad0184f748..afc1f8b7f1a63 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp @@ -153,10 +153,10 @@ MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavl switch ((uint16_t)mav_command_long.param1) { case 0: - fence->enable(false); + fence->enable_configured(false); return MAV_RESULT_ACCEPTED; case 1: - fence->enable(true); + fence->enable_configured(true); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; From ae95da60ae6aae0fbcf2ba2efe2fe69fe0916417 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:44:48 +0000 Subject: [PATCH 09/49] RC_Channel: use fence enable_configured() --- libraries/RC_Channel/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index a2de561a77923..8d27aeb426eb1 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1139,7 +1139,7 @@ void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag) return; } - fence->enable(ch_flag == AuxSwitchPos::HIGH); + fence->enable_configured(ch_flag == AuxSwitchPos::HIGH); } #endif From a433d742003259ed3419844445eaffb15f4ca66e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:46:01 +0000 Subject: [PATCH 10/49] Plane: use fence enable_configured() avoid fence breach clearing spam --- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/fence.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4766cea5c1fba..fc87bb4ad63b2 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -147,10 +147,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { // disable fence - plane.fence.enable(false); + plane.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled"); } else if (cmd.p1 == 1) { // enable fence - plane.fence.enable(true); + plane.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled"); } else if (cmd.p1 == 2) { // disable fence floor only plane.fence.disable_floor(); diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index bef88306ebc58..9615082244736 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -115,7 +115,8 @@ void Plane::fence_check() } LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } From 642ebab881b7b02ca9aaddd7622615767c15bf5e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 17:09:19 +0000 Subject: [PATCH 11/49] Rover: use fence enable_configured() --- Rover/mode_auto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index fa1ea8ece212c..f18ab0a8f9db9 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -596,10 +596,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { //disable - rover.fence.enable(false); + rover.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence - rover.fence.enable(true); + rover.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } #endif From ead7383602c27674d37947c353f469c0f4af7e16 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 16 Feb 2024 14:54:56 -0600 Subject: [PATCH 12/49] Plane: prevent Mode Takeoff calling fence autoenable more than once --- ArduPlane/Plane.h | 3 +++ ArduPlane/mode_takeoff.cpp | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1af208397ff3..443f2d56a1fb4 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -250,6 +250,9 @@ class Plane : public AP_Vehicle { // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached bool long_failsafe_pending; + + //flag that we have already called autoenable fences once in MODE TAKEOFF + bool have_autoenabled_fences; // GCS selection GCS_Plane _gcs; // avoid using this; use gcs() diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 37d0203fcf413..0854e512450a9 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -63,6 +63,7 @@ ModeTakeoff::ModeTakeoff() : bool ModeTakeoff::_enter() { takeoff_mode_setup = false; + plane.have_autoenabled_fences = false; return true; } @@ -154,7 +155,10 @@ void ModeTakeoff::update() } else { if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering #if AP_FENCE_ENABLED - plane.fence.auto_enable_fence_after_takeoff(); + if (!plane.have_autoenabled_fences) { + plane.fence.auto_enable_fence_after_takeoff(); + plane.have_autoenabled_fences = true; + } #endif plane.calc_nav_roll(); plane.calc_nav_pitch(); From 9b5890a1ddf1972a8df597a407913170a6142f6c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 17 Feb 2024 19:55:22 +0000 Subject: [PATCH 13/49] AP_Logger: add events for all fence types --- libraries/AP_Logger/AP_Logger.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index d4c605bcced53..e8a91d62da63d 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -79,8 +79,15 @@ enum class LogEvent : uint8_t { STANDBY_ENABLE = 74, STANDBY_DISABLE = 75, - FENCE_FLOOR_ENABLE = 80, - FENCE_FLOOR_DISABLE = 81, + // Fence events + FENCE_ALT_MAX_ENABLE = 76, + FENCE_ALT_MAX_DISABLE = 77, + FENCE_CIRCLE_ENABLE = 78, + FENCE_CIRCLE_DISABLE = 79, + FENCE_ALT_MIN_ENABLE = 80, + FENCE_ALT_MIN_DISABLE = 81, + FENCE_POLYGON_ENABLE = 82, + FENCE_POLYGON_DISABLE = 83, // if the EKF's source input set is changed (e.g. via a switch or // a script), we log an event: From e00be2856a35cc00b59b105b5c42e90b46d4dc52 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 4 Dec 2023 16:58:40 +0000 Subject: [PATCH 14/49] AC_Fence: add ability to auto-enable fence floor while doing fence checks control copter floor fence with autoenable autoenable floor fence with a margin check for manual recovery only after having checked the fences make auto-disabling for minimum altitude fence an option output message when fence floor auto-enabled re-use fence floor auto-enable/disable from plane auto-disable on landing do not update enable parameter when controlling through mavlink make sure get_enabled_fences() actually returns enabled fences. make current fences enabled internal state rather than persistent implement auto options correctly and on copter add fence names utility use ExpandingString for constructing fence names correctly check whether fences are enabled or not and disable min alt for landing in all auto modes add enable_configured() for use by mavlink and rc add events for all fence types make sure that auto fences are no longer candidates after manual updates add fence debug make sure rc switch is the ultimate authority on fences reset auto mask when enabling or disabling fencing ensure auto-enable on arming works as intended simplify printing fence notices reset autofences when auot-enablement is changed --- libraries/AC_Fence/AC_Fence.cpp | 333 +++++++++++++++++++++++--------- libraries/AC_Fence/AC_Fence.h | 61 ++++-- 2 files changed, 287 insertions(+), 107 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 04a238196f58f..1ba6cc27bfac5 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -40,22 +40,24 @@ extern const AP_HAL::HAL& hal; #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out #endif +//#define AC_FENCE_DEBUG const AP_Param::GroupInfo AC_Fence::var_info[] = { + // @Param: ENABLE // @DisplayName: Fence enable/disable - // @Description: Allows you to enable (1) or disable (0) the fence functionality + // @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted. // @Values: 0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0), // @Param: TYPE // @DisplayName: Fence Type - // @Description: Enabled fence types held as bitmask + // @Description: Configured fence types held as bitmask. Max altitide, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached. // @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons // @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude // @User: Standard - AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_DEFAULT), + AP_GROUPINFO("TYPE", 1, AC_Fence, _configured_fences, AC_FENCE_TYPE_DEFAULT), // @Param: ACTION // @DisplayName: Fence Action @@ -126,18 +128,18 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("RET_ALT", 9, AC_Fence, _ret_altitude, 0.0f, AP_PARAM_FRAME_PLANE), - // @Param{Plane}: AUTOENABLE + // @Param{Plane, Copter}: AUTOENABLE // @DisplayName: Fence Auto-Enable - // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. - // @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed + // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. + // @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed // @Range: 0 3 // @Increment: 1 // @User: Standard - AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), // @Param{Plane}: OPTIONS // @DisplayName: Fence options - // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. + // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), @@ -155,49 +157,156 @@ AC_Fence::AC_Fence() #endif _singleton = this; AP_Param::setup_object_defaults(this, var_info); + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } +} + +// get a user-friendly list of fences +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) +{ + if (!fences) { + return; + } + static const char* FENCE_NAMES[] = { + "Max Alt", + "Circle", + "Polygon", + "Min Alt", + }; + uint8_t i = 0; + uint8_t nfences = 0; + while (fences !=0) { + if (fences & 0x1) { + if (nfences > 0) { + if (!(fences & ~1U)) { + msg.printf(" and "); + } else { + msg.printf(", "); + } + } + msg.printf("%s", FENCE_NAMES[i]); + nfences++; + } + fences >>= 1; + i++; + } + msg.printf(" fence"); + if (nfences>1) { + msg.printf("s"); + } +} + +// print a message about the passed in fences +void AC_Fence::print_fence_message(const char* message, uint8_t fences) const +{ + if (!fences) { + return; + } + + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(fences, e); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s %s", e.get_writeable_string(), message); +} + +// should be called @10Hz to handle loading from eeprom +void AC_Fence::update() +{ + _poly_loader.update(); + // if someone changes the parameter we want to enable or disable everything + if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { + // reset the auto mask since we just reconfigured all of fencing + _auto_enable_mask = AC_FENCE_ALL_FENCES; + _last_enabled = _enabled; + _last_auto_enabled = _auto_enabled; + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } else { + _enabled_fences = 0; + } + } +#ifdef AC_FENCE_DEBUG + static uint32_t last_msg_count = 0; + if (get_enabled_fences() && last_msg_count++ % 10 == 0) { + print_fence_message("fences active", get_enabled_fences()); + } +#endif } /// enable the Fence code generally; a master switch for all fences -void AC_Fence::enable(bool value) +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) { + uint8_t fences = _configured_fences.get() & fence_types; + uint8_t enabled_fences = _enabled_fences; + if (value) { + enabled_fences |= fences; + } else { + enabled_fences &= ~fences; + } + + // fences that were manually changed are no longer eligible for auto-enablement or disablement + if (update_auto_mask) { + // if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure + // it doesn't get re-enabled or disabled + if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = !value; + } + _auto_enable_mask &= ~fences; + } + + uint8_t fences_to_change = _enabled_fences ^ enabled_fences; + + if (!fences_to_change) { + return 0; + } #if HAL_LOGGING_ENABLED - if (_enabled && !value) { - AP::logger().Write_Event(LogEvent::FENCE_DISABLE); - } else if (!_enabled && value) { - AP::logger().Write_Event(LogEvent::FENCE_ENABLE); + AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); + if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MAX_ENABLE : LogEvent::FENCE_ALT_MAX_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_CIRCLE) { + AP::logger().Write_Event(value ? LogEvent::FENCE_CIRCLE_ENABLE : LogEvent::FENCE_CIRCLE_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_ALT_MIN) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MIN_ENABLE : LogEvent::FENCE_ALT_MIN_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_POLYGON) { + AP::logger().Write_Event(value ? LogEvent::FENCE_POLYGON_ENABLE : LogEvent::FENCE_POLYGON_DISABLE); } #endif - _enabled.set(value); + + _enabled_fences = enabled_fences; + if (!value) { - clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); - disable_floor(); - } else { - enable_floor(); + clear_breach(fences_to_change); } + + return fences_to_change; } /// enable/disable fence floor only void AC_Fence::enable_floor() { -#if HAL_LOGGING_ENABLED - if (!_floor_enabled) { - // Floor is currently disabled, enable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE); - } -#endif - _floor_enabled = true; + enable(true, AC_FENCE_TYPE_ALT_MIN); } void AC_Fence::disable_floor() { -#if HAL_LOGGING_ENABLED - if (_floor_enabled) { - // Floor is currently enabled, disable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE); + enable(false, AC_FENCE_TYPE_ALT_MIN); +} + +/* + called on arming +*/ +void AC_Fence::auto_enable_fence_on_arming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; } -#endif - _floor_enabled = false; - clear_breach(AC_FENCE_TYPE_ALT_MIN); + + const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); + print_fence_message("auto-enabled", fences); } /* @@ -205,15 +314,18 @@ void AC_Fence::disable_floor() */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - if (_enabled) { - return; - } switch(auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)"); + case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: { + // auto-enable fences that aren't currently auto-enabled + if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = false; + } + const uint8_t fences = enable(true, _auto_enable_mask, false); + print_fence_message("auto-enabled", fences); break; + } default: // fence does not auto-enable in other takeoff conditions break; @@ -226,13 +338,20 @@ void AC_Fence::auto_enable_fence_after_takeoff(void) void AC_Fence::auto_disable_fence_for_landing(void) { switch (auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: - enable(false); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)"); + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: { + // disable only those fences which are allowed to be disabled + if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = true; + } + const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false); + print_fence_message("auto-disabled", fences); break; + } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: + case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: disable_floor(); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); + _floor_disabled_for_landing = true; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); break; default: // fence does not auto-disable in other landing conditions @@ -240,36 +359,26 @@ void AC_Fence::auto_disable_fence_for_landing(void) } } -bool AC_Fence::present() const +uint8_t AC_Fence::present() const { - const auto enabled_fences = _enabled_fences.get(); - // A fence is present if any of the conditions are true. - // * tin can (circle) is enabled - // * min or max alt is enabled - // * polygon fences are enabled and any fence has been uploaded - if (enabled_fences & AC_FENCE_TYPE_CIRCLE || - enabled_fences & AC_FENCE_TYPE_ALT_MIN || - enabled_fences & AC_FENCE_TYPE_ALT_MAX || - ((enabled_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.total_fence_count() > 0)) { - return true; + uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX; + if (_poly_loader.total_fence_count() > 0) { + mask |= AC_FENCE_TYPE_POLYGON; } - return false; + return _configured_fences.get() & mask; } /// get_enabled_fences - returns bitmask of enabled fences uint8_t AC_Fence::get_enabled_fences() const { - if (!_enabled && !_auto_enabled) { - return 0; - } - return _enabled_fences; + return _enabled_fences & present(); } // additional checks for the polygon fence: bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const { - if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) { // not enabled; all good return true; } @@ -324,20 +433,20 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const fail_msg = nullptr; // if fences are enabled but none selected fail pre-arm check - if (enabled() && !present()) { + if (_enabled && !present()) { fail_msg = "Fences enabled, but none selected"; return false; } // if not enabled or not fence set-up always return true - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled) || !_configured_fences) { return true; } // if we have horizontal limits enabled, check we can get a // relative position from the AHRS - if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || - (_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if ((_configured_fences & AC_FENCE_TYPE_CIRCLE) || + (_configured_fences & AC_FENCE_TYPE_POLYGON)) { Vector2f position; if (!AP::ahrs().get_relative_position_NE_home(position)) { fail_msg = "Fence requires position"; @@ -389,13 +498,14 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const bool AC_Fence::check_fence_alt_max() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (_curr_alt >= _alt_max) { @@ -437,13 +547,14 @@ bool AC_Fence::check_fence_alt_max() bool AC_Fence::check_fence_alt_min() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are under the altitude fence if (_curr_alt <= _alt_min) { @@ -479,13 +590,37 @@ bool AC_Fence::check_fence_alt_min() return false; } + +/// auto enable fence floor +bool AC_Fence::auto_enable_fence_floor() +{ + // altitude fence check + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) { + // not enabled + return false; + } + + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up + + // check if we are over the altitude fence + if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { + enable(true, AC_FENCE_TYPE_ALT_MIN); + gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); + return true; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones bool AC_Fence::check_fence_polygon() { const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = ((_enabled_fences & AC_FENCE_TYPE_POLYGON) && + const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.breached()); if (breached) { if (!was_breached) { @@ -506,7 +641,7 @@ bool AC_Fence::check_fence_polygon() /// fence is breached. bool AC_Fence::check_fence_circle() { - if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) { // not enabled; no breach return false; } @@ -554,34 +689,29 @@ uint8_t AC_Fence::check() uint8_t ret = 0; // clear any breach from a non-enabled fence - clear_breach(~_enabled_fences); + clear_breach(~_configured_fences); // return immediately if disabled - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; } - // check if pilot is attempting to recover manually - if (_manual_recovery_start_ms != 0) { - // we ignore any fence breaches during the manual recovery period which is about 10 seconds - if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { - return 0; - } - // recovery period has passed so reset manual recovery time - // and continue with fence breach checks - _manual_recovery_start_ms = 0; - } - // maximum altitude fence check if (check_fence_alt_max()) { ret |= AC_FENCE_TYPE_ALT_MAX; } - // minimum altitude fence check - if (_floor_enabled && check_fence_alt_min()) { + // minimum altitude fence check, do this before auto-disabling (e.g. because falling) + // so that any action can be taken + if (floor_enabled() && check_fence_alt_min()) { ret |= AC_FENCE_TYPE_ALT_MIN; } + // auto enable floor unless auto enable has been set (which means other behaviour is required) + if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) { + auto_enable_fence_floor(); + } + // circle fence check if (check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; @@ -592,6 +722,18 @@ uint8_t AC_Fence::check() ret |= AC_FENCE_TYPE_POLYGON; } + // check if pilot is attempting to recover manually + // this is done last so that _breached_fences is correct + if (_manual_recovery_start_ms != 0) { + // we ignore any fence breaches during the manual recovery period which is about 10 seconds + if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { + return 0; + } + // recovery period has passed so reset manual recovery time + // and continue with fence breach checks + _manual_recovery_start_ms = 0; + } + // return any new breaches that have occurred return ret; } @@ -695,6 +837,8 @@ void AC_Fence::manual_recovery_start() // record time pilot began manual recovery _manual_recovery_start_ms = AP_HAL::millis(); + + gcs().send_text(MAV_SEVERITY_INFO, "Manual recovery started"); } // methods for mavlink SYS_STATUS message (send_sys_status) @@ -712,7 +856,7 @@ bool AC_Fence::sys_status_enabled() const return false; } // Fence is only enabled when the flag is enabled - return _enabled; + return enabled(); } bool AC_Fence::sys_status_failed() const @@ -742,14 +886,17 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { AP_GROUPEND }; AC_Fence::AC_Fence() {}; -void AC_Fence::enable(bool value) {}; +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { return 0; } -void AC_Fence::disable_floor() {}; +void AC_Fence::enable_floor() {} +void AC_Fence::disable_floor() {} +void AC_Fence::update() {} -void AC_Fence::auto_enable_fence_after_takeoff() {}; -void AC_Fence::auto_disable_fence_for_landing() {}; +void AC_Fence::auto_enable_fence_after_takeoff() {} +void AC_Fence::auto_disable_fence_for_landing() {} +void AC_Fence::auto_enable_fence_on_arming() {} -bool AC_Fence::present() const { return false; } +uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } @@ -758,6 +905,8 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } uint8_t AC_Fence::check() { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { } +void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {} void AC_Fence::manual_recovery_start() {} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index e8eb88943e133..23a57312ef072 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -15,6 +16,8 @@ #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence #define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL +#define AC_FENCE_ARMING_FENCES (AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON) +#define AC_FENCE_ALL_FENCES (AC_FENCE_ARMING_FENCES | AC_FENCE_TYPE_ALT_MIN) // valid actions should a fence be breached #define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action @@ -34,12 +37,19 @@ class AC_Fence public: friend class AC_PolyFence_loader; - enum class AutoEnable + enum class AutoEnable : uint8_t { ALWAYS_DISABLED = 0, - ALWAYS_ENABLED = 1, - ENABLE_DISABLE_FLOOR_ONLY = 2, - ONLY_WHEN_ARMED = 3 + ENABLE_ON_AUTO_TAKEOFF = 1, // enable on auto takeoff + ENABLE_DISABLE_FLOOR_ONLY = 2, // enable on takeoff but disable floor on landing + ONLY_WHEN_ARMED = 3 // enable on arming + }; + + enum class MavlinkFenceActions : uint16_t + { + DISABLE_FENCE = 0, + ENABLE_FENCE = 1, + DISABLE_ALT_MIN_FENCE = 2 }; AC_Fence(); @@ -55,7 +65,12 @@ class AC_Fence static AC_Fence *get_singleton() { return _singleton; } /// enable - allows fence to be enabled/disabled. - void enable(bool value); + /// returns a bitmask of fences that were changed + uint8_t enable(bool value, uint8_t fence_types, bool update_auto_mask = true); + + /// enable_configured - allows configured fences to be enabled/disabled. + /// returns a bitmask of fences that were changed + uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } /// auto_enabled - automaticaly enable/disable fence depending of flight status AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } @@ -72,19 +87,23 @@ class AC_Fence /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing. void auto_disable_fence_for_landing(); - /// enabled - returns true if fence is enabled - bool enabled() const { return _enabled; } + /// auto_enable_fences_on_arming - auto enables all applicable fences on arming + void auto_enable_fence_on_arming(); + + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. + bool auto_enable_fence_floor(); - /// present - returns true if fence is present - bool present() const; + /// enabled - returns whether fencing is enabled or not + bool enabled() const { return _enabled_fences; } + + /// present - returns true if any of the provided types is present + uint8_t present() const; /// get_enabled_fences - returns bitmask of enabled fences uint8_t get_enabled_fences() const; // should be called @10Hz to handle loading from eeprom - void update() { - _poly_loader.update(); - } + void update(); /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool pre_arm_check(const char* &fail_msg) const; @@ -133,6 +152,12 @@ class AC_Fence /// get_return_rally - returns whether returning to fence return point or rally point float get_return_altitude() const { return _ret_altitude; } + /// get a user-friendly list of fences + static void get_fence_names(uint8_t fences, ExpandingString& msg); + + // print a message about the fences to the GCS + void print_fence_message(const char* msg, uint8_t fences) const; + /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds /// should be called whenever the pilot changes the flight mode /// has no effect if no breaches have occurred @@ -190,11 +215,16 @@ class AC_Fence bool pre_arm_check_polygon(const char* &fail_msg) const; bool pre_arm_check_circle(const char* &fail_msg) const; bool pre_arm_check_alt(const char* &fail_msg) const; + // fence floor is enabled + bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } // parameters - AP_Int8 _enabled; // fence enable/disable control + uint8_t _enabled_fences; // fences that are currently enabled/disabled + bool _last_enabled; // value of enabled last time we checked + AP_Int8 _enabled; // overall feature control AP_Int8 _auto_enabled; // top level flag for auto enabling fence - AP_Int8 _enabled_fences; // bit mask holding which fences are enabled + uint8_t _last_auto_enabled; // value of auto_enabled last time we checked + AP_Int8 _configured_fences; // bit mask holding which fences are enabled AP_Int8 _action; // recovery action specified by user AP_Float _alt_max; // altitude upper limit in meters AP_Float _alt_min; // altitude lower limit in meters @@ -216,7 +246,8 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - bool _floor_enabled; // fence floor is enabled + bool _floor_disabled_for_landing; // fence floor is disabled for landing + uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) float _curr_alt; From 1aebd671a5ea5f0be5df4e2ab86df6d27a56f6ac Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 11 Mar 2024 12:40:31 +0000 Subject: [PATCH 15/49] AC_Fence: ensure fence enablement on arming is inverted on disarming correct detection of polyfence --- libraries/AC_Fence/AC_Fence.cpp | 28 +++++++++++++++++++++++----- libraries/AC_Fence/AC_Fence.h | 3 +++ 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 1ba6cc27bfac5..b88befe0403c1 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -229,7 +229,8 @@ void AC_Fence::update() #ifdef AC_FENCE_DEBUG static uint32_t last_msg_count = 0; if (get_enabled_fences() && last_msg_count++ % 10 == 0) { - print_fence_message("fences active", get_enabled_fences()); + print_fence_message("active", get_enabled_fences()); + print_fence_message("breached", get_breaches()); } #endif } @@ -309,6 +310,19 @@ void AC_Fence::auto_enable_fence_on_arming(void) print_fence_message("auto-enabled", fences); } +/* + called on disarming +*/ +void AC_Fence::auto_disable_fence_on_disarming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; + } + + const uint8_t fences = enable(false, _auto_enable_mask, false); + print_fence_message("auto-disabled", fences); +} + /* called when an auto-takeoff is complete */ @@ -349,7 +363,7 @@ void AC_Fence::auto_disable_fence_for_landing(void) } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: - disable_floor(); + enable(false, AC_FENCE_TYPE_ALT_MIN, false); _floor_disabled_for_landing = true; GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); break; @@ -619,10 +633,13 @@ bool AC_Fence::auto_enable_fence_floor() // inclusions zones bool AC_Fence::check_fence_polygon() { + if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { + // not enabled; no breach + return false; + } + const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) && - _poly_loader.breached()); - if (breached) { + if (_poly_loader.breached()) { if (!was_breached) { record_breach(AC_FENCE_TYPE_POLYGON); return true; @@ -895,6 +912,7 @@ void AC_Fence::update() {} void AC_Fence::auto_enable_fence_after_takeoff() {} void AC_Fence::auto_disable_fence_for_landing() {} void AC_Fence::auto_enable_fence_on_arming() {} +void AC_Fence::auto_disable_fence_on_disarming() {} uint8_t AC_Fence::present() const { return 0; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 23a57312ef072..ed933f450feee 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -90,6 +90,9 @@ class AC_Fence /// auto_enable_fences_on_arming - auto enables all applicable fences on arming void auto_enable_fence_on_arming(); + /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming + void auto_disable_fence_on_disarming(); + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); From cda9de2078af22db50c648959105012a778e9d30 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 11 Mar 2024 12:40:57 +0000 Subject: [PATCH 16/49] AP_Arming: ensure fence enablement on arming is inverted on disarming --- libraries/AP_Arming/AP_Arming.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 83e7cac17447f..2be3a1011fe6e 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1840,9 +1840,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) #if AP_FENCE_ENABLED AC_Fence *fence = AP::fence(); if (fence != nullptr) { - if(fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable_configured(false); - } + fence->auto_disable_fence_on_disarming(); } #endif #if defined(HAL_ARM_GPIO_PIN) From 411728866a417a5c2a5b78914da69f10b32bd9b4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 19 May 2024 17:39:50 +1000 Subject: [PATCH 17/49] AC_Fence: clear breach of disabled fence skip breach checks if no fences correct initialisation of _num_fences in the case of no fences in eeprom --- libraries/AC_Fence/AC_Fence.cpp | 1 + libraries/AC_Fence/AC_PolyFence_loader.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index b88befe0403c1..36fb7fbb32694 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -635,6 +635,7 @@ bool AC_Fence::check_fence_polygon() { if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { // not enabled; no breach + clear_breach(AC_FENCE_TYPE_POLYGON); return false; } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 509e039a843bb..b97334476e053 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -227,7 +227,7 @@ bool AC_PolyFence_loader::breached() const // returns true if location is outside the boundary bool AC_PolyFence_loader::breached(const Location& loc) const { - if (!loaded()) { + if (!loaded() || total_fence_count() == 0) { return false; } @@ -493,13 +493,15 @@ bool AC_PolyFence_loader::index_eeprom() if (!count_eeprom_fences()) { return false; } + + void_index(); + if (_eeprom_fence_count == 0) { + _num_fences = 0; _load_attempted = false; return true; } - void_index(); - Debug("Fence: Allocating %u bytes for index", (unsigned)(_eeprom_fence_count*sizeof(FenceIndex))); _index = NEW_NOTHROW FenceIndex[_eeprom_fence_count]; From 7b579305818abc4c1510c4fd6f5b780c4e52aec2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 22 Dec 2023 09:36:34 +0000 Subject: [PATCH 18/49] autotest: add test for auto-disabling min alt fence breaches on disarming clean-up fence manipulation functions and add test for auto-enablement on copter update tests to have some FENCE_ENABLE tests add avoidance minimum and maximum altitude fence add fence switch test while flying add FenceAutoEnableDisableSwitch for auto mode 2 add more scenarios for plane fence auto-enable validate fence rc switch behaviour check fence autoenable by taking off in guided mode more FENCE_AUTOENABLE tests add FenceEnableDisableAux and FenceMinAltAutoEnableAbort --- .../autotest/Generic_Missions/CMAC-fence.txt | 6 + Tools/autotest/arducopter.py | 140 +++++++- Tools/autotest/arduplane.py | 331 ++++++++++++++++++ Tools/autotest/rover.py | 6 + Tools/autotest/vehicle_test_suite.py | 23 +- 5 files changed, 492 insertions(+), 14 deletions(-) create mode 100644 Tools/autotest/Generic_Missions/CMAC-fence.txt diff --git a/Tools/autotest/Generic_Missions/CMAC-fence.txt b/Tools/autotest/Generic_Missions/CMAC-fence.txt new file mode 100644 index 0000000000000..a6169e4e3cbcf --- /dev/null +++ b/Tools/autotest/Generic_Missions/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 21efa3ef5fac1..42b3ef5b9e510 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1605,6 +1605,7 @@ def MaxAltFence(self): "FENCE_ENABLE": 1, "AVOID_ENABLE": 0, "FENCE_TYPE": 1, + "FENCE_ENABLE" : 1, }) self.change_alt(10) @@ -1692,6 +1693,7 @@ def MinAltFence(self): # enable fence, disable avoidance self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 8, "FENCE_ALT_MIN": 20, }) @@ -1737,6 +1739,7 @@ def FenceFloorEnabledLanding(self): self.progress("Test Landing while fence floor enabled") self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, "FENCE_ALT_MIN": 10, "FENCE_ALT_MAX": 20, @@ -1757,16 +1760,100 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + self.wait_landed_and_disarmed() self.assert_fence_enabled() - # Assert fence is not healthy + # Assert fence is not healthy since it was enabled manually self.assert_sensor_state(fence_bit, healthy=False) # Disable the fence using mavlink command to ensure cleaned up SITL state self.do_fence_disable() self.assert_fence_disabled() + def FenceFloorAutoDisableLanding(self): + """Ensures we can initiate and complete an RTL while the fence is enabled""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.progress("Test Landing while fence floor enabled") + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 1, + }) + + self.change_mode("GUIDED") + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + def FenceFloorAutoEnableOnArming(self): + """Ensures we can auto-enable fences on arming and still takeoff and land""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 3, + }) + + self.change_mode("GUIDED") + # Check fence is not enabled + self.assert_fence_disabled() + + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + # Disable the fence using mavlink command to ensure cleaned up SITL state + self.assert_fence_disabled() + def GPSGlitchLoiter(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" @@ -7617,6 +7704,54 @@ def AC_Avoidance_Fence(self): self.set_parameter("FENCE_ENABLE", 1) self.check_avoidance_corners() + def AvoidanceAltFence(self): + '''Test fence avoidance at minimum and maximum altitude''' + ex = None + try: + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 9, # min and max alt fence + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 30, + }) + + self.change_mode('LOITER') + self.wait_ekf_happy() + + tstart = self.get_sim_time() + self.takeoff(15, mode='LOITER') + self.progress("Increasing throttle, vehicle should stay below 30m") + self.set_rc(3, 1920) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt > 30: + raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),)) + + self.progress("Decreasing, vehicle should stay above 10m") + self.set_rc(3, 1080) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt < 10: + raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),)) + + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.land_and_disarm() + self.disarm_vehicle(force=True) + if ex is not None: + raise ex + def ModeFollow(self): '''Fly follow mode''' foll_ofs_x = 30 # metres @@ -10495,6 +10630,7 @@ def tests1c(self): self.AC_Avoidance_Proximity_AVOID_ALT_MIN, self.AC_Avoidance_Fence, self.AC_Avoidance_Beacon, + self.AvoidanceAltFence, self.BaroWindCorrection, self.SetpointGlobalPos, self.ThrowDoubleDrop, @@ -10514,6 +10650,8 @@ def tests1d(self): self.MaxAltFenceAvoid, self.MinAltFence, self.FenceFloorEnabledLanding, + self.FenceFloorAutoDisableLanding, + self.FenceFloorAutoEnableOnArming, self.AutoTuneSwitch, self.GPSGlitchLoiter, self.GPSGlitchLoiter2, diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e57294be1121f..49d13661894a2 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3698,6 +3698,332 @@ def FenceAltCeilFloor(self): self.fly_home_land_and_disarm(timeout=150) + def FenceMinAltAutoEnable(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # check we can takeoff again + for i in [1, 2]: + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + if i == 1: + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + self.fly_home_land_and_disarm() + self.change_mode("FBWA") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + + def FenceMinAltAutoEnableAbort(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + self.load_generic_mission("flaps.txt") + self.change_mode("AUTO") + self.wait_distance_to_waypoint(8, 100, 10000000) + self.set_current_waypoint(8) + # abort the landing + self.wait_altitude(10, 20, timeout=200, relative=True) + self.change_mode("CRUISE") + self.set_rc(2, 1200) + # self.set_rc(3, 1600) # raise throttle + self.wait_altitude(30, 40, timeout=200, relative=True) + + self.change_mode("AUTO") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.fly_home_land_and_disarm(timeout=150) + self.wait_disarmed() + + def FenceAutoEnableDisableSwitch(self): + '''Tests autoenablement of regular fences and manual disablement''' + self.set_parameters({ + "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 50, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 2, + "FENCE_OPTIONS" : 1, + "FENCE_ENABLE" : 1, + "FENCE_RADIUS" : 300, + "FENCE_RET_ALT" : 0, + "FENCE_RET_RALLY" : 0, + "FENCE_TOTAL" : 0, + "TKOFF_ALT" : 75, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + + self.wait_ready_to_arm() + cruise_alt = 75 + self.takeoff(cruise_alt, mode='TAKEOFF') + + self.progress("Fly above ceiling and check there is no breach") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + cruise_alt + 80) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + + self.progress("Fly below floor and check for no breach") + self.change_altitude(self.homeloc.alt + 25) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Fly above floor and check fence is not re-enabled") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + 75) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence Ceiling re-enabled") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + self.fly_home_land_and_disarm(timeout=250) + + def FenceEnableDisableSwitch(self): + '''Tests enablement and disablement of fences on a switch''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "FENCE_TYPE": 4, # Set fence type to polyfence + "FENCE_ACTION": 6, # Set action to GUIDED + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : 0, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("FBWA") + + self.load_fence("CMAC-fence.txt") + + self.set_rc_from_map({ + 3: 1500, + 7: 2000, + }) # Turn fence on with aux function + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for GUIDED") + tstart = self.get_sim_time() + mode = "GUIDED" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + + self.set_rc_from_map({ + 7: 1000, + }) # Turn fence off with aux function + + # wait to no longer be in breach + self.delay_sim_time(5) + self.assert_fence_disabled() + + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + + def FenceEnableDisableAux(self): + '''Tests enablement and disablement of fences via aux command''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + enable = 0 + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to polyfence + AltMin + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : enable, + }) + + if not enable: + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.load_fence("CMAC-fence.txt") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + self.assert_fence_sys_status(True, True, False) + + # wait until we get home + self.wait_distance_to_home(50, 100, timeout=200) + # now check we are now not in breach + self.assert_fence_sys_status(True, True, True) + # Turn fence off with aux function + self.run_auxfunc( + 11, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + # switch back to cruise + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + # re-enable the fences + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + # wait to no longer be in breach + self.wait_distance_to_home(50, 100, timeout=200) + self.assert_fence_sys_status(True, True, True) + + # fly home and land with fences still enabled + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + def FenceBreachedChangeMode(self): '''Tests manual mode change after fence breach, as set with FENCE_OPTIONS''' """ Attempts to change mode while a fence is breached. @@ -5447,6 +5773,11 @@ def tests(self): self.FenceRTLRally, self.FenceRetRally, self.FenceAltCeilFloor, + self.FenceMinAltAutoEnable, + self.FenceMinAltAutoEnableAbort, + self.FenceAutoEnableDisableSwitch, + self.FenceEnableDisableSwitch, + self.FenceEnableDisableAux, self.FenceBreachedChangeMode, self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 8e62d685be7a7..22c38c5c50b52 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -4061,6 +4061,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, target_system=target_system, target_component=target_component) + self.set_parameters({ + "FENCE_TYPE": 2, # circle only + }) self.delay_sim_time(5) # let breaches clear # FIXME: should we allow this? self.progress("Ensure we can arm with no poly in place") @@ -4068,6 +4071,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.wait_ready_to_arm() self.arm_vehicle() self.disarm_vehicle() + self.set_parameters({ + "FENCE_TYPE": 6, # polyfence + circle + }) self.test_poly_fence_noarms_exclusion_circle(target_system=target_system, target_component=target_component) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index ddb506b58518a..16512b61357d0 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2080,6 +2080,8 @@ def roundtrip_fence_using_fencepoint_protocol(self, loc_list, target_system=1, t def load_fence(self, filename): filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) self.progress("Loading fence from (%s)" % str(filepath)) locs = [] for line in open(filepath, 'rb'): @@ -6999,22 +7001,17 @@ def do_set_relay_mavproxy(self, relay_num, on_off): self.mavproxy.expect("Loaded module relay") self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off)) - def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if value: - p1 = 1 - else: - p1 = 0 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1=p1, # param1 - want_result=want_result, - ) - def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(True, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, want_result=want_result) def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(False, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, want_result=want_result) + + def do_fence_disable_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, p2=8, want_result=want_result) + + def do_fence_enable_except_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, p2=7, want_result=want_result) ################################################# # WAIT UTILITIES From bb8f98f6ec16d15d9c38cef3a3f157e2d60198e9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:20:13 +0100 Subject: [PATCH 19/49] AC_Fence: add reset_fence_floor_enable() and use it in plane when landing is aborted --- libraries/AC_Fence/AC_Fence.cpp | 30 ++++++++++++++++++++++++++++-- libraries/AC_Fence/AC_Fence.h | 3 +++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 36fb7fbb32694..e38789c876fd9 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -609,7 +609,9 @@ bool AC_Fence::check_fence_alt_min() bool AC_Fence::auto_enable_fence_floor() { // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) { + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) + || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) + || (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) { // not enabled return false; } @@ -620,7 +622,7 @@ bool AC_Fence::auto_enable_fence_floor() // check if we are over the altitude fence if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { - enable(true, AC_FENCE_TYPE_ALT_MIN); + enable(true, AC_FENCE_TYPE_ALT_MIN, false); gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); return true; } @@ -628,6 +630,30 @@ bool AC_Fence::auto_enable_fence_floor() return false; } +/// reset fence floor auto-enablement +bool AC_Fence::reset_fence_floor_enable() +{ + // altitude fence check + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) { + // not enabled + return false; + } + + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up + + // check if we are under the altitude fence + if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) { + enable(false, AC_FENCE_TYPE_ALT_MIN, false); + _floor_disabled_for_landing = false; + gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)"); + return true; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index ed933f450feee..c346474e27d2a 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -96,6 +96,9 @@ class AC_Fence /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); + /// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude. + bool reset_fence_floor_enable(); + /// enabled - returns whether fencing is enabled or not bool enabled() const { return _enabled_fences; } From 5c9deab159e52467ab1909bd31386b322d493724 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:21:07 +0100 Subject: [PATCH 20/49] Plane: use reset_fence_floor_enable() --- ArduPlane/mode_auto.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 3153d7de31e06..e6381b62df25e 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -54,6 +54,10 @@ void ModeAuto::_exit() } #endif if (restart) { +#if AP_FENCE_ENABLED + // no longer landing to re-enable the possiblity of the fence floor + plane.fence.reset_fence_floor_enable(); +#endif plane.landing.restart_landing_sequence(); } } From cbd8c602ac4a6e537706d3a6a8ae3f8986a5a30e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:21:30 +0100 Subject: [PATCH 21/49] autotest: test aborted landing with fence correctly --- Tools/autotest/arduplane.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 49d13661894a2..24821c55eac4e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3745,7 +3745,7 @@ def FenceMinAltAutoEnable(self): def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' self.set_parameters({ - "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_TYPE": 8, # Set fence type to min alt "FENCE_ACTION": 1, # Set action to RTL "FENCE_ALT_MIN": 25, "FENCE_ALT_MAX": 100, @@ -3760,11 +3760,11 @@ def FenceMinAltAutoEnableAbort(self): self.wait_ready_to_arm() self.arm_vehicle() - # max alt fence should now be enabled - self.assert_fence_enabled() self.takeoff(alt=50, mode='TAKEOFF') self.change_mode("FBWA") + # min alt fence should now be enabled + self.assert_fence_enabled() self.set_rc(3, 1100) # lower throttle self.progress("Waiting for RTL") @@ -3788,6 +3788,8 @@ def FenceMinAltAutoEnableAbort(self): self.set_rc(2, 1200) # self.set_rc(3, 1600) # raise throttle self.wait_altitude(30, 40, timeout=200, relative=True) + # min alt fence should now be re-enabled + self.assert_fence_enabled() self.change_mode("AUTO") self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) From d19fd9cdb96ff11af283a76d79ef2a65d32354fc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:12:38 +0100 Subject: [PATCH 22/49] AC_Fence: disable fences for landing by suppressing in the fence check rather than using a state machine simplify takeoff auto-enablement --- libraries/AC_Fence/AC_Fence.cpp | 111 +++++++++++--------------------- libraries/AC_Fence/AC_Fence.h | 14 ++-- 2 files changed, 43 insertions(+), 82 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index e38789c876fd9..e28ad0b6e2396 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -248,11 +248,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) // fences that were manually changed are no longer eligible for auto-enablement or disablement if (update_auto_mask) { - // if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure - // it doesn't get re-enabled or disabled - if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = !value; - } _auto_enable_mask &= ~fences; } @@ -328,49 +323,30 @@ void AC_Fence::auto_disable_fence_on_disarming(void) */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - switch(auto_enabled()) { - case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: - case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: { - // auto-enable fences that aren't currently auto-enabled - if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = false; - } - const uint8_t fences = enable(true, _auto_enable_mask, false); - print_fence_message("auto-enabled", fences); - break; - } - default: - // fence does not auto-enable in other takeoff conditions - break; + if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) { + return; } + + const uint8_t fences = enable(true, _auto_enable_mask, false); + print_fence_message("auto-enabled", fences); } -/* - called when performing an auto landing - */ -void AC_Fence::auto_disable_fence_for_landing(void) +// return fences that should be auto-disabled when requested +uint8_t AC_Fence::get_auto_disable_fences(void) const { + uint8_t auto_disable = 0; switch (auto_enabled()) { - case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: { - // disable only those fences which are allowed to be disabled - if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = true; - } - const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false); - print_fence_message("auto-disabled", fences); + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: + auto_disable = _auto_enable_mask; break; - } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: - enable(false, AC_FENCE_TYPE_ALT_MIN, false); - _floor_disabled_for_landing = true; - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); + auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; break; default: - // fence does not auto-disable in other landing conditions break; } + return auto_disable; } uint8_t AC_Fence::present() const @@ -609,9 +585,11 @@ bool AC_Fence::check_fence_alt_min() bool AC_Fence::auto_enable_fence_floor() { // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) - || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) - || (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) { + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured + || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled + || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled + || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED + || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) { // not enabled return false; } @@ -621,7 +599,7 @@ bool AC_Fence::auto_enable_fence_floor() _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence - if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { + if (!floor_enabled() && _curr_alt >= _alt_min + _margin) { enable(true, AC_FENCE_TYPE_ALT_MIN, false); gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); return true; @@ -630,30 +608,6 @@ bool AC_Fence::auto_enable_fence_floor() return false; } -/// reset fence floor auto-enablement -bool AC_Fence::reset_fence_floor_enable() -{ - // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) { - // not enabled - return false; - } - - float alt; - AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up - - // check if we are under the altitude fence - if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) { - enable(false, AC_FENCE_TYPE_ALT_MIN, false); - _floor_disabled_for_landing = false; - gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)"); - return true; - } - - return false; -} - // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones @@ -728,41 +682,53 @@ bool AC_Fence::check_fence_circle() /// check - returns bitmask of fence types breached (if any) -uint8_t AC_Fence::check() +uint8_t AC_Fence::check(bool disable_auto_fences) { uint8_t ret = 0; + uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0; + uint8_t fences_to_disable = disabled_fences & _enabled_fences; // clear any breach from a non-enabled fence clear_breach(~_configured_fences); + // clear any breach from disabled fences + clear_breach(fences_to_disable); + + // report on any fences that were auto-disabled + if (fences_to_disable) { + print_fence_message("auto-disabled", fences_to_disable); + } // return immediately if disabled if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; } + // disable the (temporarily) disabled fences + enable(false, disabled_fences, false); + // maximum altitude fence check - if (check_fence_alt_max()) { + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) { ret |= AC_FENCE_TYPE_ALT_MAX; } // minimum altitude fence check, do this before auto-disabling (e.g. because falling) // so that any action can be taken - if (floor_enabled() && check_fence_alt_min()) { + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) { ret |= AC_FENCE_TYPE_ALT_MIN; } - // auto enable floor unless auto enable has been set (which means other behaviour is required) - if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) { + // auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required) + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) { auto_enable_fence_floor(); } // circle fence check - if (check_fence_circle()) { + if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; } // polygon fence check - if (check_fence_polygon()) { + if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) { ret |= AC_FENCE_TYPE_POLYGON; } @@ -937,7 +903,6 @@ void AC_Fence::disable_floor() {} void AC_Fence::update() {} void AC_Fence::auto_enable_fence_after_takeoff() {} -void AC_Fence::auto_disable_fence_for_landing() {} void AC_Fence::auto_enable_fence_on_arming() {} void AC_Fence::auto_disable_fence_on_disarming() {} @@ -947,7 +912,7 @@ uint8_t AC_Fence::get_enabled_fences() const { return 0; } bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } -uint8_t AC_Fence::check() { return 0; } +uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index c346474e27d2a..aa5c4c254d3c2 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -73,7 +73,7 @@ class AC_Fence uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } /// auto_enabled - automaticaly enable/disable fence depending of flight status - AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } + AutoEnable auto_enabled() const { return static_cast(_auto_enabled.get()); } /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value void enable_floor(); @@ -84,21 +84,17 @@ class AC_Fence /// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met void auto_enable_fence_after_takeoff(); - /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing. - void auto_disable_fence_for_landing(); - /// auto_enable_fences_on_arming - auto enables all applicable fences on arming void auto_enable_fence_on_arming(); /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming void auto_disable_fence_on_disarming(); + uint8_t get_auto_disable_fences(void) const; + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); - /// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude. - bool reset_fence_floor_enable(); - /// enabled - returns whether fencing is enabled or not bool enabled() const { return _enabled_fences; } @@ -119,7 +115,8 @@ class AC_Fence /// /// check - returns the fence type that has been breached (if any) - uint8_t check(); + /// disabled_fences can be used to disable fences for certain conditions (e.g. landing) + uint8_t check(bool disable_auto_fence = false); // returns true if the destination is within fence (used to reject waypoints outside the fence) bool check_destination_within_fence(const class Location& loc); @@ -252,7 +249,6 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - bool _floor_disabled_for_landing; // fence floor is disabled for landing uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) float _curr_alt; From ef8f182737dcc374808677bbcdfc0238a5bc5755 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:13:32 +0100 Subject: [PATCH 23/49] Plane: disable fences for landing by suppressing in the fence check rather than using a state machine --- ArduPlane/commands_logic.cpp | 4 ---- ArduPlane/fence.cpp | 10 +++++++++- ArduPlane/mode_auto.cpp | 4 ---- ArduPlane/mode_qland.cpp | 3 --- ArduPlane/quadplane.cpp | 3 --- 5 files changed, 9 insertions(+), 15 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fc87bb4ad63b2..7c57daf81530b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -434,10 +434,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // if we were in an abort we need to explicitly move out of the abort state, as it's sticky set_flight_stage(AP_FixedWing::FlightStage::LAND); } - -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 9615082244736..0217cbebe4703 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -9,8 +9,16 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + uint16_t mission_id = plane.mission.get_current_nav_cmd().id; + bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND +#if HAL_QUADPLANE_ENABLED + || control_mode->mode_number() == Mode::Number::QLAND + || control_mode->mode_number() == Mode::Number::QRTL +#endif + || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_in_landing); if (!fence.enabled()) { // Switch back to the chosen control mode if still in diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index e6381b62df25e..3153d7de31e06 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -54,10 +54,6 @@ void ModeAuto::_exit() } #endif if (restart) { -#if AP_FENCE_ENABLED - // no longer landing to re-enable the possiblity of the fence floor - plane.fence.reset_fence_floor_enable(); -#endif plane.landing.restart_landing_sequence(); } } diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 4205960f871c3..f3a08aabde762 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -16,9 +16,6 @@ bool ModeQLand::_enter() #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif return true; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 05a7948457730..c0c69937fa9e4 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3554,9 +3554,6 @@ bool QuadPlane::verify_vtol_land(void) poscontrol.pilot_correction_done = false; pos_control->set_lean_angle_max_cd(0); poscontrol.xy_correction.zero(); -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif From 807b1d0d42277d7e06ae5976b89164f83af01c5b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:13:43 +0100 Subject: [PATCH 24/49] Copter: disable fences for landing by suppressing in the fence check rather than using a state machine --- ArduCopter/fence.cpp | 6 +++++- ArduCopter/mode_auto.cpp | 5 ----- ArduCopter/mode_land.cpp | 5 ----- ArduCopter/mode_rtl.cpp | 10 ---------- 4 files changed, 5 insertions(+), 21 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index aba1ecce5bb84..7cae5dda8dfa8 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,8 +10,12 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND + || flightmode->mode_number() == Mode::Number::RTL + || flightmode->mode_number() == Mode::Number::SMART_RTL; + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_in_landing); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index db1f384eba866..47d00ee24628e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -480,11 +480,6 @@ void ModeAuto::land_start() copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 1f34e20fbf338..2b5b6e8879260 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -41,11 +41,6 @@ bool ModeLand::init(bool ignore_checks) copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - #if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index d3250e4265642..cfee04ff72024 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -255,11 +255,6 @@ void ModeRTL::descent_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } // rtl_descent_run - implements the final descent to the RTL_ALT @@ -347,11 +342,6 @@ void ModeRTL::land_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } bool ModeRTL::is_landing() const From 24ec1113eb743851efbcca6aaadaa839362d18bd Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:18:59 +0100 Subject: [PATCH 25/49] AP_Mission: generic fence handling in missions --- libraries/AP_Mission/AP_Mission.cpp | 5 ++++ libraries/AP_Mission/AP_Mission.h | 1 + libraries/AP_Mission/AP_Mission_Commands.cpp | 27 ++++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index c11b574e8338f..438b816176df6 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -16,6 +16,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_Mission::var_info[] = { @@ -447,6 +448,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return start_command_camera(cmd); +#endif +#if AP_FENCE_ENABLED + case MAV_CMD_DO_FENCE_ENABLE: + return start_command_fence(cmd); #endif case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index e76c3a638f548..69f8fe2b46b97 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -936,6 +936,7 @@ class AP_Mission bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd); bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd); bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd); + bool start_command_fence(const AP_Mission::Mission_Command& cmd); /* handle format conversion of storage format to allow us to update diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index d44b62e674686..f296c0c268c80 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #if AP_RC_CHANNEL_ENABLED bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) @@ -347,4 +348,30 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss return false; } +bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd) +{ +#if AP_FENCE_ENABLED + AC_Fence* fence = AP::fence(); + + if (fence == nullptr) { + return false; + } + + if (cmd.p1 == 0) { // disable fence + uint8_t fences = fence->enable_configured(false); + fence->print_fence_message("disabled", fences); + return true; + } else if (cmd.p1 == 1) { // enable fence + uint8_t fences = fence->enable_configured(true); + fence->print_fence_message("enabled", fences); + return true; + } else if (cmd.p1 == 2) { // disable fence floor only + fence->disable_floor(); + fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN); + return true; + } +#endif // AP_FENCE_ENABLED + return false; +} + #endif // AP_MISSION_ENABLED From e94dcf229a9b23eb6b51ace56154184b60694d18 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:26 +0100 Subject: [PATCH 26/49] Plane: use generic fence handling in missions --- ArduPlane/commands_logic.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7c57daf81530b..57969f17ce8a4 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -144,21 +144,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_LAND_START: break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { // disable fence - plane.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled"); - } else if (cmd.p1 == 1) { // enable fence - plane.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled"); - } else if (cmd.p1 == 2) { // disable fence floor only - plane.fence.disable_floor(); - gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled"); - } -#endif - break; - case MAV_CMD_DO_AUTOTUNE_ENABLE: autotune_enable(cmd.p1); break; From 4c06ac5f231d113c9f5438807afa59679a0e545e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:36 +0100 Subject: [PATCH 27/49] Copter: use generic fence handling in missions --- ArduCopter/mode_auto.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 47d00ee24628e..e9cbe4ebc48ef 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -774,18 +774,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) // point the camera to a specified angle do_mount_control(cmd); break; - - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - copter.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - copter.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif //AP_FENCE_ENABLED - break; #if AC_NAV_GUIDED == ENABLED case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits From d64f5ea74a2660c02bb21f7df9ce254ce1f69367 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:45 +0100 Subject: [PATCH 28/49] Rover: use generic fence handling in missions --- Rover/mode_auto.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index f18ab0a8f9db9..6e97b0a1d24a3 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -593,18 +593,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_set_reverse(cmd); break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - rover.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - rover.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif - break; - case MAV_CMD_DO_GUIDED_LIMITS: do_guided_limits(cmd); break; From 11441b3e380b559ef6e90bb980e9b272d7a29a93 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 12:02:21 +0100 Subject: [PATCH 29/49] AC_Fence: always disable Min Alt fence on landing allow precise pre-arm messages --- libraries/AC_Fence/AC_Fence.cpp | 48 ++++++++++++++++----------------- libraries/AC_Fence/AC_Fence.h | 8 +++--- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index e28ad0b6e2396..df03fd493b9ba 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -341,10 +341,9 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const break; case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: + default: // when auto disable is not set we still need to disable the altmin fence on landing auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; break; - default: - break; } return auto_disable; } @@ -366,7 +365,7 @@ uint8_t AC_Fence::get_enabled_fences() const } // additional checks for the polygon fence: -bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const { if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) { // not enabled; all good @@ -374,12 +373,12 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const } if (! _poly_loader.loaded()) { - fail_msg = "Fences invalid"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence(s) invalid"); return false; } if (!_poly_loader.check_inclusion_circle_margin(_margin)) { - fail_msg = "Margin is less than inclusion circle radius"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence margin is less than inclusion circle radius"); return false; } @@ -387,14 +386,14 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const } // additional checks for the circle fence: -bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const { if (_circle_radius < 0) { - fail_msg = "Invalid FENCE_RADIUS value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid Circle FENCE_RADIUS value"); return false; } if (_circle_radius < _margin) { - fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS"; + hal.util->snprintf(failure_msg, failure_msg_len, "Circle FENCE_MARGIN is less than FENCE_RADIUS"); return false; } @@ -402,15 +401,15 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const } // additional checks for the alt fence: -bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const { if (_alt_max < 0.0f) { - fail_msg = "Invalid FENCE_ALT_MAX value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MAX value"); return false; } if (_alt_min < -100.0f) { - fail_msg = "Invalid FENCE_ALT_MIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MIN value"); return false; } return true; @@ -418,13 +417,11 @@ bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully -bool AC_Fence::pre_arm_check(const char* &fail_msg) const +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { - fail_msg = nullptr; - // if fences are enabled but none selected fail pre-arm check if (_enabled && !present()) { - fail_msg = "Fences enabled, but none selected"; + hal.util->snprintf(failure_msg, failure_msg_len, "Fences enabled, but none selected"); return false; } @@ -439,42 +436,45 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const (_configured_fences & AC_FENCE_TYPE_POLYGON)) { Vector2f position; if (!AP::ahrs().get_relative_position_NE_home(position)) { - fail_msg = "Fence requires position"; + hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position"); return false; } } - if (!pre_arm_check_polygon(fail_msg)) { + if (!pre_arm_check_polygon(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_circle(fail_msg)) { + if (!pre_arm_check_circle(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_alt(fail_msg)) { + if (!pre_arm_check_alt(failure_msg, failure_msg_len)) { return false; } // check no limits are currently breached if (_breached_fences) { - fail_msg = "vehicle outside fence"; + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(_breached_fences, e); + hal.util->snprintf(failure_msg, failure_msg_len, "vehicle outside %s", e.get_writeable_string()); return false; } // validate FENCE_MARGIN parameter range if (_margin < 0.0f) { - fail_msg = "Invalid FENCE_MARGIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN value"); return false; } if (_alt_max < _alt_min) { - fail_msg = "FENCE_ALT_MAX < FENCE_ALT_MIN"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_ALT_MAX < FENCE_ALT_MIN"); return false; } if (_alt_max - _alt_min <= 2.0f * _margin) { - fail_msg = "FENCE_MARGIN too big"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_MARGIN too big"); return false; } @@ -910,7 +910,7 @@ uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } -bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; } uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index aa5c4c254d3c2..3165d7b73d4d3 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -108,7 +108,7 @@ class AC_Fence void update(); /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully - bool pre_arm_check(const char* &fail_msg) const; + bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; /// /// methods to check we are within the boundaries and recover @@ -215,9 +215,9 @@ class AC_Fence void clear_breach(uint8_t fence_type); // additional checks for the different fence types: - bool pre_arm_check_polygon(const char* &fail_msg) const; - bool pre_arm_check_circle(const char* &fail_msg) const; - bool pre_arm_check_alt(const char* &fail_msg) const; + bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const; // fence floor is enabled bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } From a42d7ae6ad060e24ee545a6c4b7ea8bcc36d96b7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 16:52:43 +0100 Subject: [PATCH 30/49] AP_Arming: allow precise wording of fence pre-arm messages --- libraries/AP_Arming/AP_Arming.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 2be3a1011fe6e..41ec47659d650 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1280,16 +1280,12 @@ bool AP_Arming::fence_checks(bool display_failure) } // check fence is ready - const char *fail_msg = nullptr; - if (fence->pre_arm_check(fail_msg)) { + char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + if (fence->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { return true; } - if (fail_msg == nullptr) { - check_failed(display_failure, "Check fence"); - } else { - check_failed(display_failure, "%s", fail_msg); - } + check_failed(display_failure, "%s", fail_msg); #if AP_SDCARD_STORAGE_ENABLED if (fence->failed_sdcard_storage() || StorageManager::storage_failed()) { From 6cbbd88571e2edd040ac18da4421c832699a7732 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 16:53:07 +0100 Subject: [PATCH 31/49] Copter: don't breach auto-fences when landed --- ArduCopter/fence.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 7cae5dda8dfa8..54b363ec16e98 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -12,7 +12,8 @@ void Copter::fence_check() bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND || flightmode->mode_number() == Mode::Number::RTL - || flightmode->mode_number() == Mode::Number::SMART_RTL; + || flightmode->mode_number() == Mode::Number::SMART_RTL + || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); From 0963060139c3b31ad16b9c8d8947fb095e435ddc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 17:12:10 +0100 Subject: [PATCH 32/49] autotest: fix fence autotests add Plane.FenceMinAltEnableAutoland test that vehicle can be landed manually after descending below fence floor --- Tools/autotest/arducopter.py | 28 ++++++++++++-------- Tools/autotest/arduplane.py | 50 +++++++++++++++++++++++++++++++++--- Tools/autotest/rover.py | 10 ++++---- 3 files changed, 70 insertions(+), 18 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 42b3ef5b9e510..7e59f78f9a076 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1454,7 +1454,7 @@ def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_beha self.wait_altitude(10, 100, relative=True) self.set_rc(3, 1500) self.set_rc(2, 1400) - self.wait_distance_to_home(12, 20) + self.wait_distance_to_home(12, 20, timeout=30) tstart = self.get_sim_time() push_time = 70 # push against barrier for 60 seconds failed_max = False @@ -1513,7 +1513,7 @@ def HorizontalFence(self, timeout=180): self.load_fence("fence-in-middle-of-nowhere.txt") self.delay_sim_time(5) # let fence check run so it loads-from-eeprom - self.assert_prearm_failure("vehicle outside fence") + self.assert_prearm_failure("vehicle outside Polygon fence") self.progress("Failed to arm outside fence (good!)") self.clear_fence() self.delay_sim_time(5) # let fence breach clear @@ -1523,7 +1523,7 @@ def HorizontalFence(self, timeout=180): self.start_subtest("ensure we can't arm with bad radius") self.context_push() self.set_parameter("FENCE_RADIUS", -1) - self.assert_prearm_failure("Invalid FENCE_RADIUS value") + self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value") self.context_pop() self.progress("Failed to arm with bad radius") self.drain_mav() @@ -1741,14 +1741,14 @@ def FenceFloorEnabledLanding(self): "AVOID_ENABLE": 0, "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, - "FENCE_ALT_MIN": 10, - "FENCE_ALT_MAX": 20, + "FENCE_ALT_MIN": 20, + "FENCE_ALT_MAX": 30, }) self.change_mode("GUIDED") self.wait_ready_to_arm() self.arm_vehicle() - self.user_takeoff(alt_min=15) + self.user_takeoff(alt_min=25) # Check fence is enabled self.do_fence_enable() @@ -1760,8 +1760,19 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + # center throttle + self.set_rc(3, 1500) - self.wait_landed_and_disarmed() + # wait until we are below the fence floor and re-enter loiter + self.wait_altitude(5, 15, relative=True) + self.change_mode('LOITER') + # wait for manual recovery to expire + self.delay_sim_time(15) + + # lower throttle and try and land + self.set_rc(3, 1300) + self.wait_altitude(0, 2, relative=True) + self.wait_disarmed() self.assert_fence_enabled() # Assert fence is not healthy since it was enabled manually @@ -1799,8 +1810,6 @@ def FenceFloorAutoDisableLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) - # Assert fence is not healthy now that we are in RTL - self.assert_sensor_state(fence_bit, healthy=False) self.wait_landed_and_disarmed(0) # the breach should have cleared since we auto-disable the @@ -11937,7 +11946,6 @@ def tests(self): def disabled_tests(self): return { "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702", - "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525", "AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191", "GroundEffectCompensation_takeOffExpected": "Flapping", "GroundEffectCompensation_touchDownExpected": "Flapping", diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 24821c55eac4e..6e4122eaf3bd8 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1601,7 +1601,7 @@ def FenceStatic(self): self.delay_sim_time(2) # Allow breach to propagate self.assert_fence_enabled() - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Min Alt fence") self.do_fence_disable() self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) @@ -1620,7 +1620,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -1640,7 +1640,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -3742,6 +3742,49 @@ def FenceMinAltAutoEnable(self): self.set_current_waypoint(0, check_afterwards=False) self.set_rc(3, 1000) # lower throttle + def FenceMinAltEnableAutoland(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 20, + "FENCE_AUTOENABLE": 0, + "FENCE_ENABLE" : 1, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # switch to FBWA + self.change_mode("FBWA") + self.set_rc(3, 1500) # raise throttle + self.wait_altitude(25, 35, timeout=50, relative=True) + self.set_rc(3, 1000) # lower throttle + # Now check we can land + self.fly_home_land_and_disarm() + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' self.set_parameters({ @@ -5776,6 +5819,7 @@ def tests(self): self.FenceRetRally, self.FenceAltCeilFloor, self.FenceMinAltAutoEnable, + self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, self.FenceEnableDisableSwitch, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 22c38c5c50b52..941ae2f76695b 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6881,10 +6881,10 @@ def MissionPolyEnabledPreArm(self): ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.progress("Ensure we can arm when a polyfence fence is cleared when we've previously been in breach") self.clear_fence() @@ -6900,7 +6900,7 @@ def MissionPolyEnabledPreArm(self): ]), ]) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.clear_fence() self.wait_ready_to_arm() @@ -6914,11 +6914,11 @@ def MissionPolyEnabledPreArm(self): self.offset_location_ne(here, 50, 20), # tl, ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.set_parameter('FENCE_TYPE', 2) self.wait_ready_to_arm() self.set_parameter('FENCE_TYPE', 6) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) def OpticalFlow(self): '''lightly test OpticalFlow''' From bb7384e503c43461c1800ce555f1a6df581696a3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Jun 2024 20:46:22 +0100 Subject: [PATCH 33/49] AC_Avoidance: correctly set back away speed for minimum alt fences --- libraries/AC_Avoidance/AC_Avoid.cpp | 96 +++++++++++++++++------------ libraries/AC_Avoidance/AC_Avoid.h | 9 +-- 2 files changed, 56 insertions(+), 49 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 2756e4b8fddd8..6169ad161d1fe 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -369,6 +369,19 @@ void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, } } +// adjust vertical climb rate so vehicle does not break the vertical fence +void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { + float backup_speed = 0.0f; + adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); + if (!is_zero(backup_speed)) { + if (is_negative(backup_speed)) { + climb_rate_cms = MIN(climb_rate_cms, backup_speed); + } else { + climb_rate_cms = MAX(climb_rate_cms, backup_speed); + } + } +} + // adjust vertical climb rate so vehicle does not break the vertical fence void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt) { @@ -388,32 +401,10 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); - // if descending, adjust for minimum altitude as necessary - if (climb_rate_cms < 0.0f) { -#if AP_FENCE_ENABLED - // calculate distance below fence - AC_Fence *fence = AP::fence(); - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence - && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { - // calculate distance from vehicle to safe altitude - float veh_alt; - _ahrs.get_relative_position_D_home(veh_alt); - // fence.get_safe_alt_min() is UP, veh_alt is DOWN: - const float alt_min = -(fence->get_safe_alt_min() + veh_alt); - - if (alt_min <= 0.0f) { - climb_rate_cms = MAX(climb_rate_cms, 0.0f); - // also calculate backup speed that will get us back to safe altitude - backup_speed = get_max_speed(kP, accel_cmss_limited, -alt_min*100.0f, dt); - } - } -#endif - return; - } - - bool limit_alt = false; - float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - + bool limit_min_alt = false; + bool limit_max_alt = false; + float max_alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) + float min_alt_diff = 0.0f; #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); @@ -421,10 +412,15 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + min_alt_diff = -(fence->get_safe_alt_min() + veh_alt); + limit_min_alt = true; + } if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { // fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = fence->get_safe_alt_max() + veh_alt; - limit_alt = true; + max_alt_diff = fence->get_safe_alt_max() + veh_alt; + limit_max_alt = true; } } #endif @@ -437,9 +433,9 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c _ahrs.get_relative_position_D_origin(curr_alt)) { // alt_limit is UP, curr_alt is DOWN: const float ctrl_alt_diff = alt_limit + curr_alt; - if (!limit_alt || ctrl_alt_diff < alt_diff) { - alt_diff = ctrl_alt_diff; - limit_alt = true; + if (!limit_max_alt || ctrl_alt_diff < max_alt_diff) { + max_alt_diff = ctrl_alt_diff; + limit_max_alt = true; } } @@ -449,34 +445,52 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c AP_Proximity *proximity = AP::proximity(); if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff)) { proximity_alt_diff -= _margin; - if (!limit_alt || proximity_alt_diff < alt_diff) { - alt_diff = proximity_alt_diff; - limit_alt = true; + if (!limit_max_alt || proximity_alt_diff < max_alt_diff) { + max_alt_diff = proximity_alt_diff; + limit_max_alt = true; } } #endif // limit climb rate - if (limit_alt) { + if (limit_max_alt || limit_min_alt) { + const float max_back_spd_cms = _backup_speed_z_max * 100.0; // do not allow climbing if we've breached the safe altitude - if (alt_diff <= 0.0f) { + if (max_alt_diff <= 0.0f && limit_max_alt) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude - const float max_back_spd_cms = _backup_speed_z_max * 100.0; if (is_positive(max_back_spd_cms)) { - backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt)); + backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -max_alt_diff*100.0f, dt)); // Constrain to max backup speed backup_speed = MAX(backup_speed, -max_back_spd_cms); } return; + // do not allow descending if we've breached the safe altitude + } else if (min_alt_diff <= 0.0f && limit_min_alt) { + climb_rate_cms = MAX(climb_rate_cms, 0.0f); + // also calculate backup speed that will get us back to safe altitude + if (is_positive(max_back_spd_cms)) { + backup_speed = get_max_speed(kP, accel_cmss_limited, -min_alt_diff*100.0f, dt); + + // Constrain to max backup speed + backup_speed = MIN(backup_speed, max_back_spd_cms); + } + return; } // limit climb rate - const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt); - climb_rate_cms = MIN(max_speed, climb_rate_cms); + if (limit_max_alt) { + const float max_alt_max_speed = get_max_speed(kP, accel_cmss_limited, max_alt_diff*100.0f, dt); + climb_rate_cms = MIN(max_alt_max_speed, climb_rate_cms); + } + + if (limit_min_alt) { + const float max_alt_min_speed = get_max_speed(kP, accel_cmss_limited, min_alt_diff*100.0f, dt); + climb_rate_cms = MAX(-max_alt_min_speed, climb_rate_cms); + } } -# endif +#endif } // adjust roll-pitch to push vehicle away from objects diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 2321d74989951..aae55725ec1a1 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -68,14 +68,7 @@ class AC_Avoid { // adjust vertical climb rate so vehicle does not break the vertical fence void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt); - void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { - float backup_speed = 0.0f; - adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); - if (!is_zero(backup_speed)) { - climb_rate_cms = MIN(climb_rate_cms, backup_speed); - } - } - + void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt); // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in centi-degrees From b12c39b1e27be2277a67102b6a11baa0e293432c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Jun 2024 20:46:49 +0100 Subject: [PATCH 34/49] autotest: add test for minimum altitude avoidance fence --- Tools/autotest/arducopter.py | 54 ++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 7e59f78f9a076..045361546a3d2 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1730,6 +1730,59 @@ def MinAltFence(self): self.zero_throttle() + # MinAltFenceAvoid - fly down and make sure fence action does not trigger + # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance + def MinAltFenceAvoid(self): + '''Test Min Alt Fence Avoidance''' + self.takeoff(30, mode="LOITER") + """Hold loiter position.""" + + # enable fence, only min altitude + # No action, rely on avoidance to prevent the breach + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 8, + "FENCE_ALT_MIN": 20, + "FENCE_ACTION": 0, + }) + + # Try and fly past the fence + self.set_rc(3, 1120) + + # Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts + try: + self.wait_altitude(10, 15, timeout=90, relative=True) + raise NotAchievedException("Avoid should prevent reaching altitude") + except AutoTestTimeoutException: + pass + except Exception as e: + raise e + + # Check ascent is not too fast, allow 10% above the configured backup speed + max_ascent_rate = self.get_parameter("AVOID_BACKUP_SPD") * 1.1 + + def get_climb_rate(mav, m): + m_type = m.get_type() + if m_type != 'VFR_HUD': + return + if m.climb > max_ascent_rate: + raise NotAchievedException("Ascending too fast want %f got %f" % (max_ascent_rate, m.climb)) + + self.context_push() + self.install_message_hook_context(get_climb_rate) + + # Reduce fence alt, this will result in a fence breach, but there is no action. + # Avoid should then backup the vehicle to be over the new fence alt. + self.set_parameters({ + "FENCE_ALT_MIN": 30, + }) + self.wait_altitude(30, 40, timeout=90, relative=True) + + self.context_pop() + + self.set_rc(3, 1500) + self.do_RTL() + def FenceFloorEnabledLanding(self): """Ensures we can initiate and complete an RTL while the fence is enabled. @@ -10658,6 +10711,7 @@ def tests1d(self): self.MaxAltFence, self.MaxAltFenceAvoid, self.MinAltFence, + self.MinAltFenceAvoid, self.FenceFloorEnabledLanding, self.FenceFloorAutoDisableLanding, self.FenceFloorAutoEnableOnArming, From d338e3e1ca50cacfa3f9ce3b79e4b99b84315fbd Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:06:56 +0100 Subject: [PATCH 35/49] Copter: only disable fences when in landing phase --- ArduCopter/fence.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 54b363ec16e98..242dbef0cbad0 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,10 +10,7 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); - bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND - || flightmode->mode_number() == Mode::Number::RTL - || flightmode->mode_number() == Mode::Number::SMART_RTL - || ap.land_complete || !motors->armed(); + bool is_in_landing = flightmode->is_landing() || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); From df0476504dbb089f44a0e573cc57ef8dca1d6600 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:07:13 +0100 Subject: [PATCH 36/49] Plane: only disable fences when in landing phase --- ArduPlane/fence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 0217cbebe4703..5bc3ab93ffa0b 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -13,7 +13,7 @@ void Plane::fence_check() bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND #if HAL_QUADPLANE_ENABLED || control_mode->mode_number() == Mode::Number::QLAND - || control_mode->mode_number() == Mode::Number::QRTL + || quadplane.in_vtol_land_descent() #endif || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); From 0f98294813af8aa2c5ae93fe895dc76bf071883c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:11:46 +0100 Subject: [PATCH 37/49] Copter: don't print fence cleared messages when sitting on the ground --- ArduCopter/fence.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 242dbef0cbad0..5ad1ccb1844c6 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -84,7 +84,9 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches && fence.get_breaches() == 0) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); + if (!copter.ap.land_complete) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); + } // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } From 4c6ef824cc5fad0b904865b0ef5371cda91edd34 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 26 Jun 2024 10:09:08 +0100 Subject: [PATCH 38/49] AC_Fence: support FENCE_OPTIONS on copter --- libraries/AC_Fence/AC_Fence.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index df03fd493b9ba..808f926fa80cc 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -36,8 +36,10 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 100.0 // after fence is broken we recreate the fence 100m further out +#define AC_FENCE_OPTIONS_DEFAULT OPTIONS::DISABLE_MODE_CHANGE #else #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out +#define AC_FENCE_OPTIONS_DEFAULT 0 #endif //#define AC_FENCE_DEBUG @@ -137,12 +139,12 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), - // @Param{Plane}: OPTIONS + // @Param{Plane, Copter}: OPTIONS // @DisplayName: Fence options // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER), AP_GROUPEND }; From 61f462bb7c10aeb92ccdc0717c524cdd42f292e2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 26 Jun 2024 10:09:26 +0100 Subject: [PATCH 39/49] Copter: support FENCE_OPTIONS on copter --- ArduCopter/mode.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 69cda906803ec..97bac5da02ffd 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -347,6 +347,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return false; } +#if AP_FENCE_ENABLED + // may not be allowed to change mode if recovering from fence breach + if (!ignore_checks && + fence.enabled() && + fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && + fence.get_breaches() && + !flightmode->is_landing() && + get_control_mode_reason() == ModeReason::FENCE_BREACHED && + !ap.land_complete) { + mode_change_failed(new_flightmode, "in fence recovery"); + return false; + } +#endif + if (!new_flightmode->init(ignore_checks)) { mode_change_failed(new_flightmode, "init failed"); return false; From 43566e0f20a7a36b9ebabea63b36b665771fcbbb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 16 Jul 2024 18:41:09 +0200 Subject: [PATCH 40/49] Plane: reject guided destinations outside the fence --- ArduPlane/GCS_Mavlink.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 855b9a62413a1..5dcfcdfa72e6c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -841,6 +841,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } +#if AP_FENCE_ENABLED + // reject destination if outside the fence + if (!plane.fence.check_destination_within_fence(requested_position)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + return MAV_RESULT_DENIED; + } +#endif + // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { From f676d414ca4038592556eabb853a3dbba60eca92 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 18 Jul 2024 08:00:11 +1000 Subject: [PATCH 41/49] AC_Fence: fixed FENCE_AUTOENABLE=2 needs to auto-enable on takeoff complete --- libraries/AC_Fence/AC_Fence.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 808f926fa80cc..7cfa677c5097a 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -325,7 +325,8 @@ void AC_Fence::auto_disable_fence_on_disarming(void) */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) { + if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && + auto_enabled() != AutoEnable::ENABLE_DISABLE_FLOOR_ONLY) { return; } From fdae0dd81a08ea389f7af03242a5aafc2365f172 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Jul 2024 07:33:48 +1000 Subject: [PATCH 42/49] Plane: reset previous mode fence breach when disarmed when we are disarmed or we are not in fence breach and we are not flying then reset the previous mode fence reason state so that a new flight will get the correct fence breach action behaviour --- ArduPlane/fence.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 5bc3ab93ffa0b..930a348440eec 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -39,10 +39,23 @@ void Plane::fence_check() return; } + const bool armed = arming.is_armed(); + + /* + if we are either disarmed or we are currently not in breach and + we are not flying then clear the state associated with the + previous mode breach handling. This allows the fence state + machine to reset at the end of a fence breach action such as an + RTL and autoland + */ + if (!armed || (new_breaches == 0 && !plane.is_flying())) { + plane.previous_mode_reason = ModeReason::UNKNOWN; + } + // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached // that's not ever going to be true if we don't call check on AP_Fence while disarmed - if (!arming.is_armed()) { + if (!armed) { return; } From d0b7aff74afb8fbb2974bde65bd03c95d49f0647 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 18 Jul 2024 19:00:19 +0200 Subject: [PATCH 43/49] autotest: test for circle exclusion fence using AUTOENABLE=2 --- Tools/autotest/arduplane.py | 45 +++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6e4122eaf3bd8..6f90634470327 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3898,6 +3898,50 @@ def FenceAutoEnableDisableSwitch(self): self.change_altitude(self.homeloc.alt + cruise_alt) self.fly_home_land_and_disarm(timeout=250) + def FenceCircleExclusionAutoEnable(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 2, # Set fence type to circle + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_AUTOENABLE": 2, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + fence_loc = self.home_position_as_mav_location() + self.location_offset_ne(fence_loc, 300, 0) + + self.upload_fences_from_locations([( + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, { + "radius" : 100, + "loc" : fence_loc + } + )]) + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # Now check we can land + self.fly_home_land_and_disarm() + def FenceEnableDisableSwitch(self): '''Tests enablement and disablement of fences on a switch''' fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE @@ -5822,6 +5866,7 @@ def tests(self): self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, + Test(self.FenceCircleExclusionAutoEnable, speedup=20), self.FenceEnableDisableSwitch, self.FenceEnableDisableAux, self.FenceBreachedChangeMode, From 398fe33ebc55a63d5f6bba788bd98a20eaf40bba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Jul 2024 08:49:28 +1000 Subject: [PATCH 44/49] Plane: fixed re-enable of fence for FENCE_AUTOENABLE=1 the is an adjustment to the previous fix which only worked when we had at least one fence element enabled when we were not flying or disarmed --- ArduPlane/fence.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 930a348440eec..db8bafc496b53 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -20,6 +20,21 @@ void Plane::fence_check() // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); + const bool armed = arming.is_armed(); + + /* + if we are either disarmed or we are currently not in breach and + we are not flying then clear the state associated with the + previous mode breach handling. This allows the fence state + machine to reset at the end of a fence breach action such as an + RTL and autoland + */ + if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) { + if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) { + plane.previous_mode_reason = ModeReason::UNKNOWN; + } + } + if (!fence.enabled()) { // Switch back to the chosen control mode if still in // GUIDED to the return point @@ -39,19 +54,6 @@ void Plane::fence_check() return; } - const bool armed = arming.is_armed(); - - /* - if we are either disarmed or we are currently not in breach and - we are not flying then clear the state associated with the - previous mode breach handling. This allows the fence state - machine to reset at the end of a fence breach action such as an - RTL and autoland - */ - if (!armed || (new_breaches == 0 && !plane.is_flying())) { - plane.previous_mode_reason = ModeReason::UNKNOWN; - } - // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached // that's not ever going to be true if we don't call check on AP_Fence while disarmed From 89e722003ddcac490b2f7f7b38b5b684473004d6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 13:29:21 +0200 Subject: [PATCH 45/49] AP_Mission: address minor review comments --- libraries/AP_Mission/AP_Mission.cpp | 1 - libraries/AP_Mission/AP_Mission_Commands.cpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 438b816176df6..3e138e7cacb40 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1239,7 +1239,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor - // packet.param2; // bitmask see FENCE_TYPE enum break; case MAV_CMD_DO_AUX_FUNCTION: diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index f296c0c268c80..bc0374232050d 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -357,15 +357,15 @@ bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd) return false; } - if (cmd.p1 == 0) { // disable fence + if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_FENCE)) { // disable fence uint8_t fences = fence->enable_configured(false); fence->print_fence_message("disabled", fences); return true; - } else if (cmd.p1 == 1) { // enable fence + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::ENABLE_FENCE)) { // enable fence uint8_t fences = fence->enable_configured(true); fence->print_fence_message("enabled", fences); return true; - } else if (cmd.p1 == 2) { // disable fence floor only + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE)) { // disable fence floor only fence->disable_floor(); fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN); return true; From 768f7806bdd2dd29140307a2d637dc0f302cea22 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 13:29:04 +0200 Subject: [PATCH 46/49] AC_Fence: address minor review comments --- ArduCopter/fence.cpp | 2 +- libraries/AC_Fence/AC_Fence.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 5ad1ccb1844c6..103ccc556ae72 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -84,7 +84,7 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches && fence.get_breaches() == 0) { - if (!copter.ap.land_complete) { + if (!copter.ap.land_complete) { GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); } // record clearing of breach diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 7cfa677c5097a..743877cc1e7f8 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -144,7 +144,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER), + AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), AP_GROUPEND }; @@ -237,7 +237,9 @@ void AC_Fence::update() #endif } -/// enable the Fence code generally; a master switch for all fences +// enable or disable configured fences present in fence_types +// also updates the bitmask of auto enabled fences if update_auto_mask is true +// returns a bitmask of fences that were changed uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) { uint8_t fences = _configured_fences.get() & fence_types; From 96d9985c9ae5cd9687f0b7e9f560a047e62d3354 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 16:54:12 +0200 Subject: [PATCH 47/49] Copter: address minor review comments --- ArduCopter/fence.cpp | 4 ++-- ArduCopter/mode.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 103ccc556ae72..2685f89431be6 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,10 +10,10 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); - bool is_in_landing = flightmode->is_landing() || ap.land_complete || !motors->armed(); + bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(is_in_landing); + const uint8_t new_breaches = fence.check(is_landing_or_landed); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 97bac5da02ffd..89bfbc3235f3a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -354,6 +354,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && fence.get_breaches() && !flightmode->is_landing() && + motors->armed() && get_control_mode_reason() == ModeReason::FENCE_BREACHED && !ap.land_complete) { mode_change_failed(new_flightmode, "in fence recovery"); From 8d7db723ab948b1e75fdde8c6607e7b9bdea5a5c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 16:54:23 +0200 Subject: [PATCH 48/49] Plane: address minor review comments --- ArduPlane/fence.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index db8bafc496b53..1890e709d25b0 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -8,9 +8,11 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + const bool armed = arming.is_armed(); uint16_t mission_id = plane.mission.get_current_nav_cmd().id; - bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND + bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND + || !armed #if HAL_QUADPLANE_ENABLED || control_mode->mode_number() == Mode::Number::QLAND || quadplane.in_vtol_land_descent() @@ -18,9 +20,7 @@ void Plane::fence_check() || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(is_in_landing); - - const bool armed = arming.is_armed(); + const uint8_t new_breaches = fence.check(landing_or_landed); /* if we are either disarmed or we are currently not in breach and From 166cb48c1fcac038b1c3fe0d0db6d1dc68dd2dab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jul 2024 09:59:50 +1000 Subject: [PATCH 49/49] Copter: fully honour FENCE_OPTION to disable mode changes when the user has chosen to disallow mode change during fence breach it should be fully implemented, without a landing exception. as requested by Pete, and discussed on dev call --- ArduCopter/mode.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 89bfbc3235f3a..19de3725dbbeb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -353,7 +353,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) fence.enabled() && fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && fence.get_breaches() && - !flightmode->is_landing() && motors->armed() && get_control_mode_reason() == ModeReason::FENCE_BREACHED && !ap.land_complete) {