Use the new trajectory constraints to synchronize and limit velocities

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

@@ -36,8 +36,8 @@
  */
 
 #include "FlightTaskAutoLineSmoothVel.hpp"
-#include <mathlib/mathlib.h>
-#include <float.h>
+
+#include "TrajectoryConstraints.hpp"
 
 using namespace matrix;
 
@@ -178,53 +178,55 @@ float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
 	return math::sign(val) * math::min(fabsf(val), fabsf(max));
 }
 
-float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const
+float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
 {
-	// Compute the maximum allowed speed at the waypoint assuming that we want to
-	// connect the two lines (prev-current and current-next)
-	// with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
-	// The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
-	// This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
-	// the real acceptance radius is smaller.
-	// It can be that the next waypoint is the last one or that the drone will have to stop for some other reason
-	// so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint.
-	float speed_at_target = 0.0f;
-
-	const float distance_current_next = (_target - _next_wp).xy().norm();
-	const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius;
-	const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
-
-
-	if (distance_current_next > 0.001f &&
-	    !waypoint_overlap &&
-	    yaw_align_check_pass) {
-		Vector3f pos_traj;
-		pos_traj(0) = _trajectory[0].getCurrentPosition();
-		pos_traj(1) = _trajectory[1].getCurrentPosition();
-		pos_traj(2) = _trajectory[2].getCurrentPosition();
-		// Max speed between current and next
-		const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed);
-		const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot(
-						  Vector2f((_target - _next_wp).xy()).unit_or_zero()));
-		// We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account
-		// that there is a jerk limit (a direct transition from line to circle is not possible)
-		// MPC_XY_TRAJ_P should be between 0 and 1.
-		float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get();
-		float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha,
-					  accel_tmp,
-					  _target_acceptance_radius);
-		speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());
+
+	math::trajectory::VehicleDynamicLimits config;
+	config.z_accept_rad = _param_nav_mc_alt_rad.get();
+	config.xy_accept_rad = _target_acceptance_radius;
+	config.max_acc_xy = _trajectory[0].getMaxAccel();
+	config.max_jerk = _trajectory[0].getMaxJerk();
+	config.max_speed_xy = _mc_cruise_speed;
+	config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
+
+	Vector3f waypoints[3];
+	waypoints[0] = pos_traj;
+
+	// constrain velocity to go to the position setpoint first if our tracking is really bad
+	if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) {
+		waypoints[1] = _position_setpoint;
+		waypoints[2] = _target;
+
+	} else {
+		waypoints[1] = _target;
+		waypoints[2] = _next_wp;
 	}
 
-	return speed_at_target;
+	float max_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
+
+	return max_speed;
 }
 
-float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const
+float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
 {
-	float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(),
-			  _param_mpc_acc_hor.get(),
-			  braking_distance,
-			  final_speed);
+	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
+			  _trajectory[1].getCurrentPosition(),
+			  _trajectory[2].getCurrentPosition());
+	float z_setpoint = _target(2);
+
+	if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) {
+		z_setpoint = _position_setpoint(2);
+	}
+
+	const float distance_start_target = fabs(z_setpoint - pos_traj(2));
+	const float arrival_z_speed = 0.f;
+
+	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
+					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
+					    distance_start_target, arrival_z_speed));
 
 	return max_speed;
 }
@@ -242,33 +244,43 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 		_velocity_setpoint.setAll(0.f);
 
 	} else {
-		if (PX4_ISFINITE(_position_setpoint(0)) &&
-		    PX4_ISFINITE(_position_setpoint(1))) {
-			// Use position setpoints to generate velocity setpoints
+		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));
 
-			// Get various path specific vectors
-			Vector3f pos_traj;
-			pos_traj(0) = _trajectory[0].getCurrentPosition();
-			pos_traj(1) = _trajectory[1].getCurrentPosition();
-			pos_traj(2) = _trajectory[2].getCurrentPosition();
-			Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy();
-			Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+		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 bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
+			const float xy_speed = _getMaxXYSpeed();
+			const float z_speed = _getMaxZSpeed();
 
-			// If the drone has to change altitude, stop at the waypoint, otherwise fly through
-			const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f;
-			const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_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);
+			math::trajectory::clampToZNorm(vel_sp_constrained, z_speed);
 
-			Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)),
-						_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1)));
+			for (int i = 0; i < 3; i++) {
+				// If available, constrain the velocity using _velocity_setpoint(.)
+				if (PX4_ISFINITE(_velocity_setpoint(i))) {
+					_velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained(i), _velocity_setpoint(i));
 
+				} else {
+					_velocity_setpoint(i) = vel_sp_constrained(i);
+				}
+			}
+		}
 
-			const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
+		else if (xy_pos_setpoint_valid) {
+			// Use 2D position setpoint to generate a 2D velocity setpoint
 
-			// Constrain the norm of each component using min and max values
-			Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)),
-						       _constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1)));
+			// 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());
+
+			Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();
 
 			for (int i = 0; i < 2; i++) {
 				// If available, constrain the velocity using _velocity_setpoint(.)
@@ -281,9 +293,11 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			}
 		}
 
-		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
+		else if (z_pos_setpoint_valid) {
+			// Use Z position setpoint to generate a Z velocity setpoint
+
+			const float z_dir = math::sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
+			const float vel_sp_z = z_dir * _getMaxZSpeed();
 
 			// If available, constrain the velocity using _velocity_setpoint(.)
 			if (PX4_ISFINITE(_velocity_setpoint(2))) {
@@ -292,9 +306,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			} else {
 				_velocity_setpoint(2) = vel_sp_z;
 			}
-
-			_want_takeoff = _velocity_setpoint(2) < -0.3f;
 		}
+
+		_want_takeoff = _velocity_setpoint(2) < -0.3f;
 	}
 }
 
@@ -361,7 +375,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 		_trajectory[i].updateDurations(_velocity_setpoint(i));
 	}
 
-	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
+	VelocitySmoothing::timeSynchronization(_trajectory, 3);
 
 	_jerk_setpoint = jerk_sp_smooth;
 	_acceleration_setpoint = accel_sp_smooth;

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

@@ -71,9 +71,8 @@ protected:
 
 	static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */
 
-	/** Give 0 if next is the last target **/
-	float _getSpeedAtTarget(float next_target_speed) const;
-	float _getMaxSpeedFromDistance(float braking_distance, float final_speed) const;
+	float _getMaxXYSpeed() const;
+	float _getMaxZSpeed() const;
 
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _updateTrajConstraints();
@@ -91,7 +90,6 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
 					(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
-					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
-					(ParamFloat<px4::params::MPC_Z_TRAJ_P>) _param_mpc_z_traj_p
+					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p
 				       );
 };

src/lib/mathlib/math/TrajMath.hpp

@@ -66,7 +66,8 @@ inline float computeMaxSpeedFromDistance(const float jerk, const float accel, co
 	float c = - 2.0f * accel * braking_distance - sqr(final_speed);
 	float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c));
 
-	return max_speed;
+	// don't slow down more than the end speed, even if the conservative accel ramp time requests it
+	return max(max_speed, final_speed);
 }
 
 /* Compute the maximum tangential speed in a circle defined by two line segments of length "d"

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -259,16 +259,6 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
 
-/**
- * Proportional gain for vertical trajectory position error
- *
- * @min 0.1
- * @max 1.0
- * @decimal 1
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_Z_TRAJ_P, 0.3f);
-
 /**
  * Cruise speed when angle prev-current/current-next setpoint
  * is 90 degrees. It should be lower than MPC_XY_CRUISE.