Multicopter auto - Yaw at waypoint before move

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

@@ -104,10 +104,11 @@ class FlightTaskAuto : public FlightTask
 	uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
 
 	State _current_state{State::none};
-	float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
-	int _mission_gear = landing_gear_s::GEAR_KEEP;
+	float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
+	int _mission_gear{landing_gear_s::GEAR_KEEP};
 
-	float _yaw_sp_prev = NAN;
+	float _yaw_sp_prev{NAN};
+	bool _yaw_sp_aligned{false};
 
 	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
 
@@ -123,7 +124,7 @@ class FlightTaskAuto : public FlightTask
 
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
-	bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
+	bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
 	uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
 
 	matrix::Vector3f
@@ -135,11 +136,10 @@ class FlightTaskAuto : public FlightTask
 	matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
 
 	map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
-	float _reference_altitude = NAN;  /**< Altitude relative to ground. */
-	hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update occured. */
+	float _reference_altitude{NAN};  /**< Altitude relative to ground. */
+	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 
-	WeatherVane *_ext_yaw_handler =
-		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
 
 	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */

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

@@ -65,5 +65,6 @@ class FlightTaskAutoLine : public FlightTaskAutoMapper
 
 private:
 	void _setSpeedAtTarget(); /**< Sets desiered speed at target */
-	float _speed_at_target = 0.0f;
+	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);