From 1761ef39520a271982a7a01ef6cd2b934d359a1b Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 26 Jun 2020 11:22:02 +0200 Subject: [PATCH] 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 | 9 ++++++++- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp | 2 ++ 2 files changed, 10 insertions(+), 1 deletion(-) 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. */