dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AutoLineSmoothVel - Reduce jerk if acceleration, velocity and desired velocity are small to help the optimizer to converge towards zero

This commit is contained in:
bresch 2019-05-09 14:09:15 +02:00 committed by Lorenz Meier
parent 3499b6ff89
commit a7cf981c8c
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -254,6 +254,15 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
_updateTrajConstraints();
// If the acceleration and velocities are small and that we want to stop, reduce the amplitude of the jerk signal
// to help the optimizer to converge towards zero
if (Vector2f(_velocity_setpoint).length() < (0.01f * _param_mpc_xy_traj_p.get())
&& Vector2f(accel_sp_smooth).length() < 0.2f
&& Vector2f(vel_sp_smooth).length() < 0.1f) {
_trajectory[0].setMaxJerk(1.f);
_trajectory[1].setMaxJerk(1.f);
}
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
}