From a6300e35d0b66224f01cf1961f1313875c858d6f Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 16 Feb 2024 14:54:56 -0600 Subject: [PATCH] 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 b1af208397..443f2d56a1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -250,6 +250,9 @@ private: // 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 37d0203fcf..0854e51245 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();