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 @@ class FlightTaskAuto : public FlightTask
 	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();
-	_generateXYsetpoints();
+	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 @@ class FlightTaskAutoLine : public FlightTaskAutoMapper
 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)) &&
-	    PX4_ISFINITE(_position_setpoint(1))) {
-		// Use position setpoints to generate velocity setpoints
-
-		// Get various path specific vectors. */
-		Vector2f pos_traj;
-		pos_traj(0) = _trajectory[0].getCurrentPosition();
-		pos_traj(1) = _trajectory[1].getCurrentPosition();
-		Vector2f pos_sp_xy(_position_setpoint);
-		Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
-		Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
-		Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
-		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
-		Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
-
-		// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
-		// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
-		// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
-		// 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();
-		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));
-		float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
-		speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
-
-		Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
-
-		for (int i = 0; i < 2; 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);
+	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
+
+			// Get various path specific vectors. */
+			Vector2f pos_traj;
+			pos_traj(0) = _trajectory[0].getCurrentPosition();
+			pos_traj(1) = _trajectory[1].getCurrentPosition();
+			Vector2f pos_sp_xy(_position_setpoint);
+			Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
+			Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
+			Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
+			Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+			Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
+
+			// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
+			// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
+			// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
+			// 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();
+			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));
+			float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
+			speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+
+			Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
+
+			for (int i = 0; i < 2; 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))) {
-		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 available, constrain the velocity using _velocity_setpoint(.)
+			if (PX4_ISFINITE(_velocity_setpoint(2))) {
+				_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
 
-		// If available, constrain the velocity using _velocity_setpoint(.)
-		if (PX4_ISFINITE(_velocity_setpoint(2))) {
-			_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
+			} else {
+				_velocity_setpoint(2) = vel_sp_z;
+			}
 
-		} else {
-			_velocity_setpoint(2) = vel_sp_z;
+			_want_takeoff = _velocity_setpoint(2) < -0.3f;
 		}
-
-		_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);