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,8 +49,19 @@ void FlightTaskAutoLine::_generateSetpoints()
 		_generateHeadingAlongTrack();
 	}
 
-	_generateAltitudeSetpoints();
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
-	_generateXYsetpoints();
+		// 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,61 +145,67 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	_checkEkfResetCounters();
 	_want_takeoff = false;
 
-	if (PX4_ISFINITE(_position_setpoint(0)) &&
+	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
-	    PX4_ISFINITE(_position_setpoint(1))) {
+		// Wait for the yaw setpoint to be aligned
-		// Use position setpoints to generate velocity setpoints
+		_velocity_setpoint.setAll(0.f);
-
+
-		// Get various path specific vectors. */
+	} else {
-		Vector2f pos_traj;
+		if (PX4_ISFINITE(_position_setpoint(0)) &&
-		pos_traj(0) = _trajectory[0].getCurrentPosition();
+		    PX4_ISFINITE(_position_setpoint(1))) {
-		pos_traj(1) = _trajectory[1].getCurrentPosition();
+			// Use position setpoints to generate velocity setpoints
-		Vector2f pos_sp_xy(_position_setpoint);
+
-		Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
+			// Get various path specific vectors. */
-		Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
+			Vector2f pos_traj;
-		Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
+			pos_traj(0) = _trajectory[0].getCurrentPosition();
-		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+			pos_traj(1) = _trajectory[1].getCurrentPosition();
-		Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
+			Vector2f pos_sp_xy(_position_setpoint);
-
+			Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
-		// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
+			Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
-		// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
+			Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
-		// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
+			Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
-		// To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter
+			Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
-		float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get();
+
-		float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length();
+			// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
-		float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
+			// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
-		float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
+			// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
-		speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+			// To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter
-
+			float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get();
-		Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
+			float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length();
-
+			float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
-		for (int i = 0; i < 2; i++) {
+			float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
-			// If available, constrain the velocity using _velocity_setpoint(.)
+			speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
-			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+
-				_velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i));
+			Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
-
+
-			} else {
+			for (int i = 0; i < 2; i++) {
-				_velocity_setpoint(i) = vel_sp_xy(i);
+				// If available, constrain the velocity using _velocity_setpoint(.)
+				if (PX4_ISFINITE(_velocity_setpoint(i))) {
+					_velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i));
+
+				} else {
+					_velocity_setpoint(i) = vel_sp_xy(i);
+				}
+
+				_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
+							 _param_mpc_xy_traj_p.get();  // Along-track setpoint + cross-track P controller
 			}
 
-			_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
-						 _param_mpc_xy_traj_p.get();  // Along-track setpoint + cross-track P controller
 		}
 
-	}
+		if (PX4_ISFINITE(_position_setpoint(2))) {
+			const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
+					       _param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
 
-	if (PX4_ISFINITE(_position_setpoint(2))) {
+			// If available, constrain the velocity using _velocity_setpoint(.)
-		const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
+			if (PX4_ISFINITE(_velocity_setpoint(2))) {
-				       _param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
+				_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
 
-		// If available, constrain the velocity using _velocity_setpoint(.)
+			} else {
-		if (PX4_ISFINITE(_velocity_setpoint(2))) {
+				_velocity_setpoint(2) = vel_sp_z;
-			_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
+			}
 
-		} else {
+			_want_takeoff = _velocity_setpoint(2) < -0.3f;
-			_velocity_setpoint(2) = vel_sp_z;
 		}
-
-		_want_takeoff = _velocity_setpoint(2) < -0.3f;
 	}
 }
 

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