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

VelocitySmoothing - Re-enable time stretch, integrate dt to get local time.

Browse Source 
Also split a few functions into smaller ones for readability, fix
formatting, use geters to get the current state of the trajectory
instead of return arguments.
This commit is contained in:
bresch
2019-09-08 15:42:18 +02:00
committed by Matthias Grob
parent c7696488fe
commit 8cc2a7018e
7 changed files with 160 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+6 -2
View File
@@ -144,8 +144,12 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_smoothing[i].updateTraj(_time_stamp_current, _acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_smoothing[i].updateTraj(_deltatime);
_jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
_acceleration_setpoint(i) = _smoothing[i].getCurrentAcceleration();
_vel_sp_smooth(i) = _smoothing[i].getCurrentVelocity();
pos_sp_smooth(i) = _smoothing[i].getCurrentPosition();
}
/* Get yaw setpont, un-smoothed position setpoints.*/
@@ -203,7 +207,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_time_stamp_current, _velocity_setpoint(i));
_smoothing[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only