diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 23a0ee172f..724ae6e74b 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -1463,7 +1463,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 	/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
 	float wp_distance_save = wp_distance;
 
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) {
+	if (fabsf(wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) >= radians(90.0f)) {
 		wp_distance_save = 0.0f;
 	}
 
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index bd52f62e7b..bb53148a72 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -97,6 +97,7 @@ using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Vector2f;
 using matrix::Vector3f;
+using matrix::wrap_pi;
 
 using uORB::Subscription;