FlightTaskAuto: Update prev and next waypoint depending on validity

This fixes the corner case where a NAV_DELAY command changes the
validity of the next WP but not the rest of the triplet. The logic in
FlightTask was ignoring this change because the check was only based on
WP position change.

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) {

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp

@@ -100,8 +100,10 @@ class FlightTaskAuto : public FlightTask
 
 	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. */