AP_Math: create and use limit_length_squared

this can avoid problems where floating point precision means that post-square-rooting the lengths are considered equal.  Subtracting the squares after making such a check can lead to attempting to take the square root of a negative number

libraries/AP_Math/control.cpp

@@ -384,10 +384,10 @@ bool limit_accel_xy(const Vector2f& vel, Vector2f& accel, float accel_max)
             float accel_dir = vel_input_unit * accel;
             // cross track acceleration
             Vector2f accel_cross = accel - (vel_input_unit * accel_dir);
-            if (accel_cross.limit_length(accel_max)) {
+            if (accel_cross.limit_length_squared(accel_max)) {
                 accel_dir = 0.0;
             } else {
-                float accel_max_dir = safe_sqrt(MAX(0, sq(accel_max) - accel_cross.length_squared()));
+                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);
             }
             accel = accel_cross + vel_input_unit * accel_dir;

libraries/AP_Math/vector2.cpp

@@ -45,6 +45,29 @@ bool Vector2<T>::limit_length(T max_length)
     return false;
 }
 
+// limit the square of the length of the vector to the square of max_length
+template <typename T>
+bool Vector2<T>::limit_length_squared(T max_length)
+{
+    const T len_squared = length_squared();
+    const T max_length_squared = sq(max_length);
+    if ((len_squared <= max_length_squared)) {
+        // no limitting required
+        return false;
+    }
+    const T len = sqrtf(len_squared);
+    if (!is_positive(len)) {
+        // can't scale zero.  floating point vagaries means this isn't
+        // covered in the above comparison of the squared vector
+        // lengths
+        return false;
+    }
+    x *= (max_length / len);
+    y *= (max_length / len);
+
+    return true;
+}
+
 // dot product
 template <typename T>
 T Vector2<T>::operator *(const Vector2<T> &v) const

libraries/AP_Math/vector2.h

@@ -151,6 +151,9 @@ struct Vector2
     // limit vector to a given length. returns true if vector was limited
     bool limit_length(T max_length);
 
+    // limit the square of the length of the vector to the square of max_length
+    bool limit_length_squared(T max_length);
+
     // normalizes this vector
     void normalize();