Multicopter rounded turns (#16376)

* AutoLineSmoothVel: Implement l1-style guidance in turns

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

@@ -451,11 +451,11 @@ State FlightTaskAuto::_getCurrentState()
 		// Target is behind.
 		return_state = State::target_behind;
 
-	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) {
 		// Current position is more than cruise speed in front of previous setpoint.
 		return_state = State::previous_infront;
 
-	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
+	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _target_acceptance_radius) {
 		// Vehicle is more than cruise speed off track.
 		return_state = State::offtrack;
 

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

@@ -116,6 +116,7 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 {
+	_updateTurningCheck();
 	_prepareSetpoints();
 	_generateTrajectory();
 
@@ -125,6 +126,26 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 	}
 }
 
+void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
+{
+	const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
+				_trajectory[1].getCurrentVelocity());
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f target_xy(_target);
+	const Vector2f u_vel_traj = vel_traj.unit_or_zero();
+	const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
+	const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();
+
+	// The vehicle is turning if the angle between the velocity vector
+	// and the direction to the target is greater than 10 degrees, the
+	// velocity is large enough and the drone isn't in the acceptance
+	// radius of the last WP.
+	_is_turning = vel_traj.longerThan(0.2f)
+		      && cos_align < 0.98f
+		      && pos_to_target.longerThan(_target_acceptance_radius);
+}
+
 void FlightTaskAutoLineSmoothVel::_generateHeading()
 {
 	// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
@@ -140,7 +161,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
 	Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
 
 	if ((vel_sp_xy.length() > .1f) &&
-	    (traj_to_target.length() > _target_acceptance_radius)) {
+	    (traj_to_target.length() > 2.f)) {
 		// Generate heading from velocity vector, only if it is long enough
 		// and if the drone is far enough from the target
 		_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
@@ -183,13 +204,10 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
 
 	// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
 	// (eg. Obstacle Avoidance)
-	bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
-	bool z_valid = PX4_ISFINITE(_position_setpoint(2));
-	bool z_modified =  z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
 
 	Vector3f waypoints[3] = {pos_traj, _target, _next_wp};
 
-	if (xy_modified || z_modified) {
+	if (isTargetModified()) {
 		waypoints[2] = waypoints[1] = _position_setpoint;
 	}
 
@@ -224,6 +242,64 @@ float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
 	return max_speed;
 }
 
+Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
+{
+	Vector3f pos_crossing_point{};
+
+	if (isTargetModified()) {
+		// Strictly follow the modified setpoint
+		pos_crossing_point = _position_setpoint;
+
+	} else {
+		if (_is_turning) {
+			// Get the crossing point using L1-style guidance
+			pos_crossing_point.xy() = getL1Point();
+			pos_crossing_point(2) = _target(2);
+
+		} else {
+			pos_crossing_point = _target;
+		}
+	}
+
+	return pos_crossing_point;
+}
+
+bool FlightTaskAutoLineSmoothVel::isTargetModified() const
+{
+	const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
+	const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
+	const bool z_modified =  z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
+
+	return xy_modified || z_modified;
+}
+
+Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
+{
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f target_xy(_target);
+	const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
+	const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
+	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest;
+
+	// Compute along-track error using L1 distance and cross-track error
+	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
+
+	const float l1 = math::max(_target_acceptance_radius, 5.f);
+	float alongtrack_error = 0.f;
+
+	// Protect against sqrt of a negative number
+	if (l1 > crosstrack_error) {
+		alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
+	}
+
+	// Position of the point on the line where L1 intersect the line between the two waypoints
+	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
+
+	return l1_point;
+}
+
 void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 {
 	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
@@ -236,74 +312,93 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
 		// Wait for the yaw setpoint to be aligned
 		_velocity_setpoint.setAll(0.f);
+		return;
+	}
 
-	} else {
-		const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
-		const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
-
-		if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
-			// Use 3D position setpoint to generate a 3D velocity setpoint
-			Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-					  _trajectory[1].getCurrentPosition(),
-					  _trajectory[2].getCurrentPosition());
-			Vector3f u_pos_traj_to_dest = (_position_setpoint - pos_traj).unit_or_zero();
-
-			const float xy_speed = _getMaxXYSpeed();
-			const float z_speed = _getMaxZSpeed();
-
-			Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
-			math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
-			math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
-
-			for (int i = 0; i < 3; i++) {
-				// If available, use the existing velocity as a feedforward, otherwise replace it
-				if (PX4_ISFINITE(_velocity_setpoint(i))) {
-					_velocity_setpoint(i) += vel_sp_constrained(i);
-
-				} else {
-					_velocity_setpoint(i) = vel_sp_constrained(i);
-				}
-			}
-		}
+	const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
+	const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
+
+	if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
+		// Use 3D position setpoint to generate a 3D velocity setpoint
+		Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+				  _trajectory[1].getCurrentPosition(),
+				  _trajectory[2].getCurrentPosition());
 
-		else if (xy_pos_setpoint_valid) {
-			// Use 2D position setpoint to generate a 2D velocity setpoint
+		const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());
 
-			// Get various path specific vectors
-			Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
-			Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj;
-			Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+		float xy_speed = _getMaxXYSpeed();
+		const float z_speed = _getMaxZSpeed();
 
-			Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();
+		if (_is_turning) {
+			// Lock speed during turn
+			xy_speed = math::min(_max_speed_prev, xy_speed);
 
-			for (int i = 0; i < 2; i++) {
-				// If available, use the existing velocity as a feedforward, otherwise replace it
-				if (PX4_ISFINITE(_velocity_setpoint(i))) {
-					_velocity_setpoint(i) += vel_sp_constrained_xy(i);
+		} else {
+			_max_speed_prev = xy_speed;
+		}
+
+		Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
+		math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
+		math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
+
+		for (int i = 0; i < 3; i++) {
+			// If available, use the existing velocity as a feedforward, otherwise replace it
+			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+				_velocity_setpoint(i) += vel_sp_constrained(i);
 
-				} else {
-					_velocity_setpoint(i) = vel_sp_constrained_xy(i);
-				}
+			} else {
+				_velocity_setpoint(i) = vel_sp_constrained(i);
 			}
 		}
+	}
 
-		else if (z_pos_setpoint_valid) {
-			// Use Z position setpoint to generate a Z velocity setpoint
+	else if (xy_pos_setpoint_valid) {
+		// Use 2D position setpoint to generate a 2D velocity setpoint
 
-			const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
-			const float vel_sp_z = z_dir * _getMaxZSpeed();
+		// Get various path specific vectors
+		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
+		Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj;
+		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
 
+		float xy_speed = _getMaxXYSpeed();
+
+		if (_is_turning) {
+			// Lock speed during turn
+			xy_speed = math::min(_max_speed_prev, xy_speed);
+
+		} else {
+			_max_speed_prev = xy_speed;
+		}
+
+		Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;
+
+		for (int i = 0; i < 2; i++) {
 			// If available, use the existing velocity as a feedforward, otherwise replace it
-			if (PX4_ISFINITE(_velocity_setpoint(2))) {
-				_velocity_setpoint(2) += vel_sp_z;
+			if (PX4_ISFINITE(_velocity_setpoint(i))) {
+				_velocity_setpoint(i) += vel_sp_constrained_xy(i);
 
 			} else {
-				_velocity_setpoint(2) = vel_sp_z;
+				_velocity_setpoint(i) = vel_sp_constrained_xy(i);
 			}
 		}
+	}
 
-		_want_takeoff = _velocity_setpoint(2) < -0.3f;
+	else if (z_pos_setpoint_valid) {
+		// Use Z position setpoint to generate a Z velocity setpoint
+
+		const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
+		const float vel_sp_z = z_dir * _getMaxZSpeed();
+
+		// If available, use the existing velocity as a feedforward, otherwise replace it
+		if (PX4_ISFINITE(_velocity_setpoint(2))) {
+			_velocity_setpoint(2) += vel_sp_z;
+
+		} else {
+			_velocity_setpoint(2) = vel_sp_z;
+		}
 	}
+
+	_want_takeoff = _velocity_setpoint(2) < -0.3f;
 }
 
 void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -63,6 +63,7 @@ protected:
 
 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 	void _generateHeading();
+	void _updateTurningCheck();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
 
 	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
@@ -72,6 +73,13 @@ protected:
 	float _getMaxXYSpeed() const;
 	float _getMaxZSpeed() const;
 
+	matrix::Vector3f getCrossingPoint() const;
+	bool isTargetModified() const;
+	matrix::Vector2f getL1Point() const;
+
+	float _max_speed_prev{};
+	bool _is_turning{false};
+
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _updateTrajConstraints();
 	void _generateTrajectory();