Plane: prevent Mode Takeoff calling fence autoenable more than once

This commit is contained in:
Henry Wurzburg 2024-02-16 14:54:56 -06:00 committed by Peter Barker
parent 4fc59ae67d
commit a6300e35d0
2 changed files with 8 additions and 1 deletions

View File

@ -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()

View File

@ -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();