Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: get_throttle_out_stop only stops once desired speed reaches zero

Browse Source 
this reduces the final jerk to a when transitioning from forward to reverse and in cases where ATC_ACCEL_MAX is small
This commit is contained in:
Randy Mackay 2018-04-21 15:44:09 +09:00
parent 3324b1a6e3
commit b1329d79ea
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -432,8 +432,8 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// could not get speed so assume stopped // could not get speed so assume stopped
_stopped = true; _stopped = true;
} else { } else {
// if vehicle drops below _stop_speed consider it stopped // if desired speed is zero and vehicle drops below _stop_speed consider it stopped
if (fabsf(speed) <= fabsf(_stop_speed)) { if (is_zero(_desired_speed) && fabsf(speed) <= fabsf(_stop_speed)) {
_stopped = true; _stopped = true;
} }
} }
@ -489,14 +489,14 @@ float AR_AttitudeControl::get_desired_speed() const
// get acceleration limited desired speed // get acceleration limited desired speed
float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed) const 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(); 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)) { if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS) || !is_positive(_throttle_accel_max)) {
return desired_speed; return desired_speed;
} }
// calculate dt // calculate dt
float dt = (now - _speed_last_ms) / 1000.0f; const float dt = (now - _speed_last_ms) / 1000.0f;
// acceleration limit desired speed // acceleration limit desired speed
const float speed_change_max = _throttle_accel_max * dt; const float speed_change_max = _throttle_accel_max * dt;