Browse Source

AP_Math: Control: protect against divide by zero

gps-1.3.1
Leonard Hall 4 years ago committed by Randy Mackay
parent
commit
3a8924c185
  1. 2
      libraries/AP_Math/control.cpp

2
libraries/AP_Math/control.cpp

@ -400,7 +400,7 @@ float stopping_distance(float velocity, float p, float accel_max) @@ -400,7 +400,7 @@ float stopping_distance(float velocity, float p, float accel_max)
// based on horizontal and vertical limits.
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;
}

Loading…
Cancel
Save