From 52c98b56cbf063add40bd5f8ccbb6eb36435c02c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sat, 29 Jul 2017 13:50:54 -0700 Subject: [PATCH] L1_Control: Ensure that LIM_BANK passes a sea level sanity check This fixes #6637 where the LIM_BANK can be set such that the vehicle demands a radius at sea level that is much higher then the loiter radius was configured to be. This effectively feels like a fly away and is due to bad parameters usually. If this happens just fall back to the simple EAS2TAS scaling of the radius. --- libraries/AP_L1_Control/AP_L1_Control.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 98ef91e5e2..1b07f29d40 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -158,8 +158,14 @@ float AP_L1_Control::loiter_radius(const float radius) const return radius * eas2tas_sq; } else { float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level; - // select the requested radius, or the required altitude scale, whichever is safer - return MAX(sea_level_radius * eas2tas_sq, radius); + if (sea_level_radius > radius) { + // If we've told the plane that its sea level radius is unachievable fallback to + // straight altitude scaling + return radius * eas2tas_sq; + } else { + // select the requested radius, or the required altitude scale, whichever is safer + return MAX(sea_level_radius * eas2tas_sq, radius); + } } }