AP_Math: prevent FPE in SITL when limitting accel, vectors

libraries/AP_Math/AP_Math.cpp

@@ -67,14 +67,17 @@ template float safe_asin<short>(const short v);
 template float safe_asin<float>(const float v);
 template float safe_asin<double>(const double v);
 
+// sqrt which takes any type and returns 0 if the input is NaN or less than zero
 template <typename T>
 float safe_sqrt(const T v)
 {
-    float ret = sqrtf(static_cast<float>(v));
-    if (isnan(ret)) {
-        return 0;
+    // cast before checking so we sqrtf the same value we check
+    const float val = static_cast<float>(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<int>(const int v);

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);
             }

libraries/AP_Math/tests/test_control.cpp

@@ -5,6 +5,9 @@
 #include <AP_Math/vector3.h>
 #include <AP_Math/control.h>
 
+#include <fenv.h>
+#include <setjmp.h>
+
 TEST(Control, test_control)
 {
     postype_t pos_start = 17;
@@ -465,6 +468,58 @@ TEST(Control, test_control)
     EXPECT_FLOAT_EQ(velxy.y, 0.0);
 }
 
+// catch floating point exceptions
+sigjmp_buf avert_your_eyes_children;
+static void _tc_sig_fpe(int signum)
+{
+    siglongjmp(avert_your_eyes_children, 1);
+}
+
+TEST(Control, test_limit_accel)
+{
+    // reproduction of FPE (https://github.com/ArduPilot/ardupilot/issues/28969)
+    // FPE will only be raised in SITL HAL, so compiling for linux HAL
+    // isn't useful.
+    const Vector2f vel{
+        0.984285712, 0.176583186
+    };
+    Vector2f accel{99.9008408, -557.304077};
+    const float accel_max = 566.187256;
+
+    struct sigaction old_sa_fpe = {};
+
+    struct sigaction sa_fpe = {};
+    sigemptyset(&sa_fpe.sa_mask);
+    sa_fpe.sa_handler = _tc_sig_fpe;
+    if (sigaction(SIGFPE, &sa_fpe, &old_sa_fpe) == -1) {
+        abort();
+    }
+    const int excepts = FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
+    fexcept_t old_except_flags;
+    if (fegetexceptflag(&old_except_flags, excepts) == -1) {
+        abort();
+    }
+
+    feenableexcept(excepts);
+
+    bool signal_caught = false;
+    if (sigsetjmp(avert_your_eyes_children, 1)) {
+        // we come through here if an FPE is triggered (via a goto in
+        // our custom signal handler, _tc_sig_fpe)
+        signal_caught = true;
+    } else {
+        // we come through here normally
+        EXPECT_TRUE(limit_accel_xy(vel, accel, accel_max));
+    }
+
+    EXPECT_FALSE(signal_caught);
+
+    // now restore the original fpe handling
+    if (fesetexceptflag(&old_except_flags, excepts) == -1) {
+        abort();
+    }
+    sigaction(SIGFPE, &old_sa_fpe, nullptr);
+}
 
 AP_GTEST_MAIN()
 int hal = 0;