AP_Math: prevent FPE in SITL when limitting accel, vectors

Co-authored-by: Leonard Hall <leonardthall@gmail.com>

the  cross-product code can produce something slightly negative.  Stop passing that into safe_sqrt; it isn't *that* safe on SITL!

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;