From 08a89ccdbb0ae3c43ea86416bd1b767928c0d808 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 1 Mar 2018 14:42:13 +0100 Subject: [PATCH] FlightTaskAutoLine: if previous type was idle, set thrust to NAN again to ensure thrust is not used. Also, if previous was not a loiter or position type, then reset setpoints to current state. This is needed because during line following, the previous setpoints are requires and cannot be NAN. --- .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 68ffb0914a..91f58c00f9 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -66,13 +66,29 @@ bool FlightTaskAutoLine::activate() bool FlightTaskAutoLine::update() { + bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position; + bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position; + + /* 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state */ + if (follow_line && !follow_line_prev) { + _reset(); + } + + /* The only time a thrust setpoint is sent out is during + * idle. Hence, reset thrust setpoint to NAN in case the + * vehicle exits idle. + */ + if (_type_previous == WaypointType::idle) { + _thrust_setpoint = Vector3f(NAN, NAN, NAN); + } + if (_type == WaypointType::idle) { _generateIdleSetpoints(); } else if (_type == WaypointType::land) { _generateLandSetpoints(); - } else if (_type == WaypointType::loiter || _type == WaypointType::position) { + } else if (follow_line) { _generateSetpoints(); } else if (_type == WaypointType::takeoff) { @@ -88,6 +104,9 @@ bool FlightTaskAutoLine::update() * same as during home... */ _yaw_setpoint = _yaw_wp; + /* Update previous type */ + _type_previous = _type; + return true; }