AR_AttitudeControl: reset I if speed controller not called recently

This commit is contained in:
Randy Mackay 2018-09-04 14:38:02 +09:00
parent 135e37e1f4
commit 96097586c3

View File

@ -391,6 +391,7 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor
const uint32_t now = AP_HAL::millis();
if (!speed_control_active()) {
_throttle_speed_pid.reset_filter();
_throttle_speed_pid.reset_I();
_desired_speed = speed;
}
_speed_last_ms = now;