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

AutoSmoothVel - Handle NAN in velocity and position properly

Browse Source
This commit is contained in:
bresch
2019-01-17 17:55:37 +01:00
committed by Lorenz Meier
parent ef54bff4ed
commit a2e9d9ffce
1 changed files with 5 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+5 -9
View File
@@ -155,12 +155,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
MPC_XY_TRAJ_P.get(); // Along-track setpoint + cross-track P controller
}
} else if (!PX4_ISFINITE(_velocity_setpoint(0)) &&
!PX4_ISFINITE(_velocity_setpoint(1))) {
// No position nor velocity setpoints available, set the velocity targer to zero
_velocity_setpoint(0) = 0.f;
_velocity_setpoint(1) = 0.f;
}
if (PX4_ISFINITE(_position_setpoint(2))) {
@@ -175,9 +169,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
_velocity_setpoint(2) = vel_sp_z;
}
} else if (!PX4_ISFINITE(_velocity_setpoint(2))) {
// No position nor velocity setpoints available, set the velocity targer to zero
_velocity_setpoint(2) = 0.f;
}
}
@@ -204,6 +195,11 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
void FlightTaskAutoLineSmoothVel::_generateTrajectory()
{
if (!PX4_ISFINITE(_velocity_setpoint(0)) || !PX4_ISFINITE(_velocity_setpoint(1))
|| !PX4_ISFINITE(_velocity_setpoint(2))) {
return;
}
/* Slow down the trajectory by decreasing the integration time based on the position error.
* This is only performed when the drone is behind the trajectory
*/