diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
index 692ccab413..f5c9344685 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
@@ -198,7 +198,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_yaw_setpoint = NAN;
 
 	} else {
-		_set_heading_from_mode();
+		if (_sub_triplet_setpoint->get().current.yaw_valid) {
+			_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+
+		} else {
+			_set_heading_from_mode();
+		}
+
 		_yawspeed_setpoint = NAN;
 	}
 
@@ -220,17 +226,12 @@ void FlightTaskAuto::_set_heading_from_mode()
 
 	switch (MPC_YAW_MODE.get()) {
 
-	case 0: { // Heading is set by waypoint.
-			_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
-			break;
-		}
-
-	case 1: { // Heading points towards the current waypoint.
+	case 0: { // Heading points towards the current waypoint.
 			v = Vector2f(&_target(0)) - Vector2f(&_position(0));
 			break;
 		}
 
-	case 2: { // Heading points towards home.
+	case 1: { // Heading points towards home.
 			if (_sub_home_position->get().valid_hpos) {
 				v = Vector2f(_sub_home_position->get().x, _sub_home_position->get().y) - Vector2f(&_position(0));
 			}
@@ -238,24 +239,28 @@ void FlightTaskAuto::_set_heading_from_mode()
 			break;
 		}
 
-	case 3: { // Heading point away from home.
+	case 2: { // Heading point away from home.
 			if (_sub_home_position->get().valid_hpos) {
 				v = Vector2f(&_position(0)) - Vector2f(_sub_home_position->get().x, _sub_home_position->get().y);
 			}
 
 			break;
 		}
+
+	case 3: { // Along trajectory.
+			// The heading depends on the kind of setpoint generation. This needs to be implemented
+			// in the subclasses where the velocity setpoints are generated.
+			v *= NAN;
+		}
 	}
 
 	// We only adjust yaw if vehicle is outside of acceptance radius.
 	// This prevents excessive yawing.
 	if (PX4_ISFINITE(v.length()) && v.length() > NAV_ACC_RAD.get()) {
-		v.normalize();
-		// To find yaw: take dot product of x = (1,0) and v
-		// and multiply by the sign given of cross product of x and v.
-		// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
-		// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
-		_yaw_setpoint = math::sign(v(1)) * wrap_pi(acosf(v(0)));
+		_compute_heading_from_2D_vector(_yaw_setpoint, v);
+
+	} else {
+		_yaw_setpoint = NAN;
 	}
 }
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
index 524cf467dd..ce7df34149 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
@@ -101,8 +101,8 @@ protected:
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
 					(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
 					(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line
-				    (ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE // defines how heading is executed
- 					); 
+					(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE // defines how heading is executed
+				       );
 
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */