fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 08:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

VelocitySmoothing - Improve computation of the direction of the trajectory by predicting the velocity at zero acceleration instead of the current velocity

Browse Source 
This helps when the current velocity is smaller than the target but that
the acceleration is too large such that the velocity will overshoot.
Without this check, the algorithm increases the acceleration which leads
to an even larger overshoot.
This commit is contained in:
bresch 2019-09-02 11:10:01 +02:00 committed by Matthias Grob
parent 052932fa95
commit c7696488fe
1 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
View File

@ -166,8 +166,22 @@ void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint)
_t0 = t_now;
_state_init = _state;
// Compute the velocity at which the trajectory will be
// when the acceleration will be zero
float vel_zero_acc = _state.v;
if (fabsf(_state.a) > FLT_EPSILON) {
float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration
float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration
vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc;
}
/* Depending of the direction, start accelerating positively or negatively */
_direction = math::sign(_vel_sp - _state.v);
_direction = math::sign(_vel_sp - vel_zero_acc);
if (_direction == 0) {
// If by braking immediately the velocity is exactly
// the require one with zero acceleration, then brake
_direction = -math::sign(_vel_sp - _state.v);
}
if (_direction != 0) {
updateDurations();