From eaf001c52fdbf55693aa3e22dd5b1d30d53876c5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:13:43 +0100 Subject: [PATCH] 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 aba1ecce5b..7cae5dda8d 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 9a6b684cf1..b672ba4d54 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -488,11 +488,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 1f34e20fbf..2b5b6e8879 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 d3250e4265..cfee04ff72 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