From a24b614dec0f59d5c11f39f93532c4f7640375de Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 2 Aug 2018 09:52:55 +0200
Subject: [PATCH] FlightTaskAutoLine: generate heading along track if yaw
 setpoint is not valid

---
 src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp | 16 ++++++++++++++++
 src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp |  1 +
 2 files changed, 17 insertions(+)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
index 022f61ef9d..178df74117 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
@@ -44,10 +44,26 @@ static constexpr float SIGMA_NORM		=	0.001f;
 
 void FlightTaskAutoLine::_generateSetpoints()
 {
+	if (!PX4_ISFINITE(_yaw_setpoint)) {
+		// no valid heading -> set heading along track
+		_generateHeadingAlongTrack();
+	}
+
 	_generateAltitudeSetpoints();
 	_generateXYsetpoints();
 }
 
+void FlightTaskAutoLine::_generateHeadingAlongTrack()
+{
+	Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
+
+	if (!_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest)) {
+		// heading could not be computed. best we can do is to set heading
+		// to current yaw
+		_yaw_setpoint = _yaw;
+	}
+}
+
 void FlightTaskAutoLine::_generateXYsetpoints()
 {
 	Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
index 10a57e3ad6..33cbecd006 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
@@ -59,6 +59,7 @@ protected:
 
 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 
+	void _generateHeadingAlongTrack(); /**< Generates heading along track. */
 	void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
 	void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
 };