From 536a894850aaf432b553b808e89c5da03992065a Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 29 Nov 2018 19:58:21 -0700 Subject: [PATCH] AP_L1_Control: Sanatize loiter radius to prevent bad input from propegating --- libraries/AP_L1_Control/AP_L1_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 3fd843d85a..81c969ff15 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -336,7 +336,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius // scale loiter radius with square of EAS2TAS to allow us to stay // stable at high altitude - radius = loiter_radius(radius); + radius = loiter_radius(fabsf(radius)); // Calculate guidance gains used by PD loop (used during circle tracking) float omega = (6.2832f / _L1_period);