From b1329d79eab7f6127335619523c849e4f8575145 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 21 Apr 2018 15:44:09 +0900 Subject: [PATCH] AR_AttitudeControl: get_throttle_out_stop only stops once desired speed reaches zero this reduces the final jerk to a when transitioning from forward to reverse and in cases where ATC_ACCEL_MAX is small --- libraries/APM_Control/AR_AttitudeControl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index b81f487457..d201a57277 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -432,8 +432,8 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor // could not get speed so assume stopped _stopped = true; } else { - // if vehicle drops below _stop_speed consider it stopped - if (fabsf(speed) <= fabsf(_stop_speed)) { + // if desired speed is zero and vehicle drops below _stop_speed consider it stopped + if (is_zero(_desired_speed) && fabsf(speed) <= fabsf(_stop_speed)) { _stopped = true; } } @@ -489,14 +489,14 @@ float AR_AttitudeControl::get_desired_speed() const // get acceleration limited desired speed float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed) const { - // return zero if no recent calls to speed controller + // return input value if no recent calls to speed controller const uint32_t now = AP_HAL::millis(); if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS) || !is_positive(_throttle_accel_max)) { return desired_speed; } // calculate dt - float dt = (now - _speed_last_ms) / 1000.0f; + const float dt = (now - _speed_last_ms) / 1000.0f; // acceleration limit desired speed const float speed_change_max = _throttle_accel_max * dt;