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

ManualSmoothVel - Set jerk to 1.0 during position hold to help the optimizer to converge towards 0 vel and acc

Browse Source
This commit is contained in:
bresch
2019-03-07 12:04:32 +01:00
committed by Beat Küng
parent 06f683bdda
commit 1fefa76e5d
1 changed files with 13 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+13
View File
@@ -141,6 +141,19 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_position_lock_z_active = false; _position_lock_z_active = false;
} }
// During position lock, lower jerk to help the optimizer
// to converge to 0 acceleration and velocity
if (_position_lock_xy_active) {
jerk[0] = 1.f;
jerk[1] = 1.f;
} else {
jerk[0] = _jerk_max.get();
jerk[1] = _jerk_max.get();
}
jerk[2] = _position_lock_z_active ? 1.f : _jerk_max.get();
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]); _smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i)); _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));