diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
index 7b3f026d1a..592221631c 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
@@ -195,13 +195,16 @@ bool FlightTaskAuto::_evaluateTriplets()
 	// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed.
 
 	bool triplet_update = true;
+	const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid)
+						|| (_next_was_valid != _sub_triplet_setpoint.get().next.valid);
 
 	if (PX4_ISFINITE(_triplet_target(0))
 	    && PX4_ISFINITE(_triplet_target(1))
 	    && PX4_ISFINITE(_triplet_target(2))
 	    && fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f
 	    && fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f
-	    && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f) {
+	    && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f
+	    && !prev_next_validity_changed) {
 		// Nothing has changed: just keep old waypoints.
 		triplet_update = false;
 
@@ -231,6 +234,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_triplet_prev_wp = _position;
 		}
 
+		_prev_was_valid = _sub_triplet_setpoint.get().previous.valid;
+
 		if (_type == WaypointType::loiter) {
 			_triplet_next_wp = _triplet_target;
 
@@ -242,6 +247,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		} else {
 			_triplet_next_wp = _triplet_target;
 		}
+
+		_next_was_valid = _sub_triplet_setpoint.get().next.valid;
 	}
 
 	if (_ext_yaw_handler != nullptr) {
diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
index 838589c1a3..fca28f6185 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
@@ -100,8 +100,10 @@ protected:
 
 	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
 	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */
+	bool _prev_was_valid{false};
 	matrix::Vector3f _target{}; /**< Target waypoint  (local frame).*/
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
+	bool _next_was_valid{false};
 	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */