AC_AttitudeControl: properly protect sqrt_controller from nonpositive acceleration limits

This commit is contained in:
Jonathan Challinger 2015-12-07 00:40:07 -08:00 committed by Randy Mackay
parent 0fe0787265
commit b8223771d3

View File

@ -634,7 +634,7 @@ void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool r
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim) float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
{ {
if (is_zero(second_ord_lim) || is_zero(p)) { if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
return error*p; return error*p;
} }