dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PositionSmoothing: guard division by zero 

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-21 16:29:53 +01:00
parent 11cca72ef1
commit 53076b9863
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/motion_planning/PositionSmoothing.cpp
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;
}

20
src/lib/motion_planning/PositionSmoothingTest.cpp
View File

@ -2,6 +2,26 @@
#include <motion_planning/PositionSmoothing.hpp>
TEST(PositionSmoothingBasicTest, AllZeroCase)
{
PositionSmoothing position_smoothing;
PositionSmoothing::PositionSmoothingSetpoints out;
position_smoothing.generateSetpoints(
Vector3f(),
{Vector3f(), Vector3f(), Vector3f()},
Vector3f(),
0.f,
false,
out
);
EXPECT_EQ(out.jerk, Vector3f());
EXPECT_EQ(out.acceleration, Vector3f());
EXPECT_EQ(out.velocity, Vector3f());
EXPECT_EQ(out.position, Vector3f());
EXPECT_EQ(out.unsmoothed_velocity, Vector3f());
}
static constexpr float MAX_JERK = 4.f;
static constexpr float MAX_ACCELERATION = 3.f;