MPC auto - Add MPC_YAW_MODE: towards waypoint (yaw first) mode. This mode ensures that

the vehicle yaws towards the next waypoint before accelerating. This is
required for drones with front vision and aerodynamic multicopters such
as standard vtol planes or highspeed multirotors.

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -118,6 +118,8 @@ void FlightTaskAuto::_limitYawRate()
 {
 	const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
 
+	_yaw_sp_aligned = true;
+
 	if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) {
 		// Limit the rate of change of the yaw setpoint
 		const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
@@ -126,10 +128,16 @@ void FlightTaskAuto::_limitYawRate()
 		_yaw_setpoint = _yaw_sp_prev + dyaw;
 		_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
 		_yaw_sp_prev = _yaw_setpoint;
+
+		// The yaw setpoint is aligned when its rate is not saturated
+		_yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max);
 	}
 
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {
 		_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
+
+		// The yaw setpoint is aligned when its rate is not saturated
+		_yaw_sp_aligned = fabsf(_yawspeed_setpoint) < yawrate_max;
 	}
 }
 
@@ -300,6 +308,7 @@ void FlightTaskAuto::_set_heading_from_mode()
 	switch (_param_mpc_yaw_mode.get()) {
 
 	case 0: // Heading points towards the current waypoint.
+	case 4: // Same as 0 but yaw fisrt and then go
 		v = Vector2f(_target) - Vector2f(_position);
 		break;
 

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -108,6 +108,7 @@ protected:
 	int _mission_gear = landing_gear_s::GEAR_KEEP;
 
 	float _yaw_sp_prev = NAN;
+	bool _yaw_sp_aligned{false};
 
 	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
 

src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp

@@ -49,9 +49,20 @@ void FlightTaskAutoLine::_generateSetpoints()
 		_generateHeadingAlongTrack();
 	}
 
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
+		// Wait for the yaw setpoint to be aligned
+		if (!_position_locked) {
+			_velocity_setpoint.setAll(0.f);
+			_position_setpoint = _position;
+			_position_locked = true;
+		}
+
+	} else {
+		_position_locked = false;
 		_generateAltitudeSetpoints();
 		_generateXYsetpoints();
 	}
+}
 
 void FlightTaskAutoLine::_setSpeedAtTarget()
 {

src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp

@@ -66,4 +66,5 @@ protected:
 private:
 	void _setSpeedAtTarget(); /**< Sets desiered speed at target */
 	float _speed_at_target = 0.0f;
+	bool _position_locked{false};
 };

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -145,6 +145,11 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	_checkEkfResetCounters();
 	_want_takeoff = false;
 
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
+		// Wait for the yaw setpoint to be aligned
+		_velocity_setpoint.setAll(0.f);
+
+	} else {
 		if (PX4_ISFINITE(_position_setpoint(0)) &&
 		    PX4_ISFINITE(_position_setpoint(1))) {
 			// Use position setpoints to generate velocity setpoints
@@ -202,6 +207,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			_want_takeoff = _velocity_setpoint(2) < -0.3f;
 		}
 	}
+}
 
 void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 {

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -762,11 +762,12 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
  * Specifies the heading in Auto.
  *
  * @min 0
- * @max 3
+ * @max 4
  * @value 0 towards waypoint
  * @value 1 towards home
  * @value 2 away from home
  * @value 3 along trajectory
+ * @value 4 towards waypoint (yaw first)
  * @group Mission
  */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);