Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: accel limit stops

Browse Source
This commit is contained in:
Randy Mackay 2018-05-21 20:39:01 +09:00
parent 444e64a3d0
commit 856d592b1d
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -405,6 +405,9 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// if we were stopped in the last 300ms, assume we are still stopped // if we were stopped in the last 300ms, assume we are still stopped
bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300; bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300;
// get deceleration limited speed
float desired_speed_limited = get_desired_speed_accel_limited(0.0f);
// get speed forward // get speed forward
float speed; float speed;
if (!get_forward_speed(speed)) { if (!get_forward_speed(speed)) {
@ -412,7 +415,7 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
_stopped = true; _stopped = true;
} else { } else {
// if desired speed is zero and vehicle drops below _stop_speed consider it stopped // if desired speed is zero and vehicle drops below _stop_speed consider it stopped
if (is_zero(_desired_speed) && fabsf(speed) <= fabsf(_stop_speed)) { if (is_zero(desired_speed_limited) && fabsf(speed) <= fabsf(_stop_speed)) {
_stopped = true; _stopped = true;
} }
} }
@ -429,7 +432,7 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor
// clear stopped system time // clear stopped system time
_stop_last_ms = 0; _stop_last_ms = 0;
// run speed controller to bring vehicle to stop // run speed controller to bring vehicle to stop
return get_throttle_out_speed(0.0f, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle); return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle);
} }
} }