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

AltitudeSmoothVel - Update to use new implementation of VelocitySmoothing

Browse Source 
Remove jerk reduction (not needed in the new implementation)
This commit is contained in:
bresch
2019-09-17 09:27:00 +02:00
committed by Matthias Grob
parent 862454827e
commit 0b765a1642
1 changed files with 9 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+9 -14
View File
@@ -106,6 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
{
float pos_sp_smooth;
_smoothing.updateTraj(_deltatime);
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_vel_sp_smooth = _smoothing.getCurrentVelocity();
pos_sp_smooth = _smoothing.getCurrentPosition();
/* Get yaw setpont, un-smoothed position setpoints.*/
FlightTaskManualAltitude::_updateSetpoints();
@@ -142,10 +151,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
_position_lock_z_active = false;
}
// During position lock, lower jerk to help the optimizer
// to converge to 0 acceleration and velocity
jerk = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get();
_smoothing.setMaxJerk(jerk);
_smoothing.updateDurations(_velocity_setpoint(2));
@@ -153,16 +158,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
_smoothing.setCurrentPosition(_position(2));
}
float pos_sp_smooth;
// TODO: move before updateDurations
_smoothing.updateTraj(_deltatime);
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_vel_sp_smooth = _smoothing.getCurrentVelocity();
pos_sp_smooth = _smoothing.getCurrentPosition();
_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
if (fabsf(_vel_sp_smooth) < 0.1f &&