diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp
index 001c399b965a..874b046f9dc4 100644
--- a/src/lib/pure_pursuit/PurePursuit.cpp
+++ b/src/lib/pure_pursuit/PurePursuit.cpp
@@ -47,14 +47,11 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2
 	// Pure pursuit
 	const Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned;
 	const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned;
-	const float bearing_to_curr_wp = atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0));
 
-	if (curr_pos_to_curr_wp.norm() < lookahead_distance) { // Target current waypoint if closer to it than lookahead
-		return bearing_to_curr_wp;
-	}
-
-	if (prev_wp_to_curr_wp.norm() < FLT_EPSILON) { // Target current waypoint if waypoints overlap
-		return bearing_to_curr_wp;
+	if (curr_pos_to_curr_wp.norm() < lookahead_distance
+	    || prev_wp_to_curr_wp.norm() <
+	    FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap
+		return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0));
 	}
 
 	const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;