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 <leonardthall@gmail.com>
Co-authored-by: Thomas Watson <twatson52@icloud.com>

libraries/AP_Math/AP_Math.cpp

@@ -67,14 +67,18 @@ 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)) {
+    // cast before checking so we sqrtf the same value we check
+    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);
+    } else {
         return 0;
     }
-    return ret;
 }
 
 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);
             }