diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp
index 4b3a427a21..8f05f39e2d 100644
--- a/src/lib/motion_planning/PositionSmoothing.cpp
+++ b/src/lib/motion_planning/PositionSmoothing.cpp
@@ -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;
 	}
diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp
index f550a5ee77..9ce32d9fd5 100644
--- a/src/lib/motion_planning/PositionSmoothingTest.cpp
+++ b/src/lib/motion_planning/PositionSmoothingTest.cpp
@@ -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;