diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index 9bd8b488068..1af8065e355 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -768,8 +768,8 @@ bool FlightTaskAuto::isTargetModified() const
 
 bool FlightTaskAuto::_hasPassedCurrentWaypoint() const
 {
-	const Vector2f u_previous_to_current = Vector2f(_triplet_current - _triplet_previous).unit_or_zero();
-	return u_previous_to_current * Vector2f(_triplet_current - _position) < 0.f;
+	const Vector3f u_previous_to_current = (_triplet_current - _triplet_previous).unit_or_zero();
+	return u_previous_to_current * (_triplet_current - _position) < 0.f;
 }
 
 void FlightTaskAuto::_updateTrajConstraints()
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
index 46a54c467fb..db9a0637a53 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
@@ -195,6 +195,6 @@ private:
 	bool _evaluatePositionSetpointTriplet();
 	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
 	bool _evaluateGlobalReference(); /**< Check is global reference is available. */
-	bool _hasPassedCurrentWaypoint() const; /**< True if the vehicle is horizontally past the current waypoint */
+	bool _hasPassedCurrentWaypoint() const; /**< True if the vehicle is past the current waypoint */
 	void _set_heading_from_mode(); /**< @see  MPC_YAW_MODE */
 };