Browse Source

AP_Motors: prevent negative thrust

this prevents a sqrt of a negative number
mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
a2ed8fb313
  1. 1
      libraries/AP_Motors/AP_MotorsMulticopter.cpp

1
libraries/AP_Motors/AP_MotorsMulticopter.cpp

@ -328,6 +328,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const @@ -328,6 +328,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0, 1);
return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
(get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max());
}

Loading…
Cancel
Save