From 034639d27ab3a71e99fce88bc66cadb4a91804c6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 1 Jan 2025 21:51:53 +1100 Subject: [PATCH] AP_Math: prevent FPE in SITL when limitting accel vectors the cross-product code can produce something slightly negative. Stop passing that into safe_sqrt; it isn't *that* safe on SITL! Co-authored-by: Leonard Hall --- libraries/AP_Math/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index c75e42497072e..326b3f5d40ce3 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -387,7 +387,7 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max) if (accel_cross.limit_length(accel_max)) { accel_dir = 0.0; } else { - float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); + float accel_max_dir = safe_sqrt(MAX(0, sq(accel_max) - accel_cross.length_squared())); accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); } accel = accel_cross + vel_input_unit * accel_dir;