From 04e2877974e762cb83a57ab3e1fda4f128fa93e9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 2 Jan 2024 19:53:12 +0000 Subject: [PATCH] Copter: check fence at faster rates when going faster to avoid massive fence breaches --- ArduCopter/Copter.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index e765bbca9f5b6..5782538ee22e2 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -149,6 +149,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(rc_loop, 250, 130, 3), SCHED_TASK(throttle_loop, 50, 75, 6), +#if AP_FENCE_ENABLED + SCHED_TASK(fence_check, 25, 100, 7), +#endif SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9), #if AP_OPTICALFLOW_ENABLED SCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12), @@ -622,12 +625,6 @@ void Copter::three_hz_loop() // check for deadreckoning failsafe failsafe_deadreckon_check(); -#if AP_FENCE_ENABLED - // check if we have breached a fence - fence_check(); -#endif // AP_FENCE_ENABLED - - // update ch6 in flight tuning tuning();