diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp
index c75e42497072e..326b3f5d40ce3 100644
--- a/libraries/AP_Math/control.cpp
+++ b/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;