From e939f60a9edf284ca7c4fe92c3056b14ffbb8afe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 23 Jan 2020 13:34:31 +0100 Subject: [PATCH] FlightTaskAuto: accept invalid triplet_setpoint (and interpret as loiter) This will allow navigator to not output any setpoints while disarmed. Otherwise the position controller outputs warnings in the form of: Auto activation failed with error: Activation Failed The risk here is that it could hide problems, where navigator sends invalid triplets when it should not. --- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index fc19d0de66..650a14c2ea 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -137,10 +137,13 @@ bool FlightTaskAuto::_evaluateTriplets() // Check if triplet is valid. There must be at least a valid altitude. if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) { - // Best we can do is to just set all waypoints to current state and return false. + // Best we can do is to just set all waypoints to current state _prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position; - _type = WaypointType::position; - return false; + _type = WaypointType::loiter; + _yaw_setpoint = _yaw; + _yawspeed_setpoint = NAN; + _updateInternalWaypoints(); + return true; } _type = (WaypointType)_sub_triplet_setpoint.get().current.type;