Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+9
View File
@@ -254,6 +254,15 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
_updateTrajConstraints(); _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) { for (int i = 0; i < 3; ++i) {
_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i)); _trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
} }