From 730ac9cb203116a5b8b7622a592a285b5f00dbfc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 21 Jul 2021 14:23:47 +0930 Subject: [PATCH] AC_PosControl: Add MAX(bla,0) because safe_sqrt isn't safe --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 509fb3cf63..94d2528674 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1187,7 +1187,9 @@ float AC_PosControl::crosstrack_error() const // crosstrack is the horizontal distance to the closest point to the current track const Vector2f vel_unit = _vel_desired.xy().normalized(); const float dot_error = pos_error * vel_unit; - return safe_sqrt(pos_error.length_squared() - sq(dot_error)); + + // todo: remove MAX of zero when safe_sqrt fixed + return safe_sqrt(MAX(pos_error.length_squared() - sq(dot_error), 0.0)); } }