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

PositionSmoothing: guard division by zero

Browse Source 
Note that the unit test also passes without
thechange. But the VelocitySmoothing's
local_time would get NAN. This would leads to
wrong trajectory calculations.
This commit is contained in:
Matthias Grob
2023-11-30 17:16:02 +01:00
parent 11cca72ef1
commit 53076b9863
2 changed files with 27 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/PositionSmoothing.cpp
+7 -2
View File
@@ -279,9 +279,14 @@ void PositionSmoothing::_generateTrajectory(
Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
float position_error = drone_to_trajectory_xy.length();
float time_stretch = 1.f - math::constrain(position_error / _max_allowed_horizontal_error, 0.f, 1.f);
float time_stretch = 1.f;
// Only stretch time if there's no division by zero and the drone isn't ahead of the position setpoint
if ((_max_allowed_horizontal_error > FLT_EPSILON)
&& drone_to_trajectory_xy.dot(vel_traj_xy) >= 0) {
time_stretch = 1.f - math::constrain(position_error / _max_allowed_horizontal_error, 0.f, 1.f);
}
// Don't stretch time if the drone is ahead of the position setpoint
if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
time_stretch = 1.f;
}