From c5706f62832d9ee816c4559495ecde95a031d894 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 15 Mar 2019 05:42:24 +0100 Subject: [PATCH] FlightTaskAuto - Recover position control after local position reset (#11636) * FlightTaskAuto - Explicitly check is _triplet_target is finite to decide if the target has to be updated or not. If the _target is NAN, always try to update it to get a valid setpoint. --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index c669bd7102..5989bd4d37 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -153,8 +153,12 @@ bool FlightTaskAuto::_evaluateTriplets() bool triplet_update = true; - if (!(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)) { + 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) { // Nothing has changed: just keep old waypoints. triplet_update = false;