diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp
index ffe68840cb..cc52a984de 100644
--- a/src/lib/motion_planning/TrajectoryConstraints.hpp
+++ b/src/lib/motion_planning/TrajectoryConstraints.hpp
@@ -104,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
  *
  * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
  */
-template <size_t N>
+template <int N>
 float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
 {
 	static_assert(N >= 2, "Need at least 2 points to compute speed");
 
 	float max_speed = 0.f;
 
-	for (size_t j = 0; j < N - 1; j++) {
-		size_t i = N - 2 - j;
+	// go backwards through the waypoints
+	for (int i = (N - 2); i >= 0; i--) {
 		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
 				waypoints[i + 1],
 				waypoints[min(i + 2, N - 1)],