FlightTaskAutoLine: generate heading along track if yaw setpoint is not valid

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));

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. */
 };