From b0714e9552480b3e56190e9a20c08a029d97199c 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 limiting accel vectors The cross-product code can produce something slightly negative. Fix safe_sqrt to avoid barfing on that, as was originally intended, and clarify why it's being used. Co-authored-by: Leonard Hall Co-authored-by: Thomas Watson --- libraries/AP_Math/AP_Math.cpp | 11 +++++++---- libraries/AP_Math/control.cpp | 2 ++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index e553ade4d941ab..49079607db1104 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -67,14 +67,17 @@ template float safe_asin(const short v); template float safe_asin(const float v); template float safe_asin(const double v); +// sqrt which takes any type and returns 0 if the input is NaN or less than zero template float safe_sqrt(const T v) { - float ret = sqrtf(static_cast(v)); - if (isnan(ret)) { - return 0; + // cast before checking so we sqrtf the same value we check + float val = static_cast(v); + // use IEEE-754 compliant function which returns false if val is NaN + if (isgreaterequal(val, 0)) { + return sqrtf(val); } - return ret; + return 0; } template float safe_sqrt(const int v); diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index c75e42497072ed..3c84e8a299b16c 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -387,6 +387,8 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max) if (accel_cross.limit_length(accel_max)) { accel_dir = 0.0; } else { + // limit_length can't absolutely guarantee this subtraction + // won't be slightly negative, so safe_sqrt is used float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared()); accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir); }