AP_Math: Control: protect against divide by zero

This commit is contained in:
Leonard Hall 2021-08-02 15:53:30 +09:30 committed by Andrew Tridgell
parent 551e5fe4dc
commit 7e60feab4a
1 changed files with 1 additions and 1 deletions

View File

@ -400,7 +400,7 @@ float stopping_distance(float velocity, float p, float accel_max)
// based on horizontal and vertical limits. // based on horizontal and vertical limits.
float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg) float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg)
{ {
if (is_zero(direction.length_squared())) { if (is_zero(direction.length_squared()) || is_zero(max_xy) || is_zero(max_z_pos) || is_zero(max_z_neg)) {
return 0.0f; return 0.0f;
} }