Fix off-track tracking in AutoLineSmoothVel (#13321)

* Use position instead of last setpoint

This calculates the target velocities better taking into account disturbances along
the flight route. Previously entry angles and more were calculated assuming the flight path
originates directly from the direction of the previous waypoint. This corrects this assumption
to instead make the direction come from the vehicle location.

* Allow to specify a final speed given a braking distance.

This is to allow planning to not stop at a waypoint, but instead
to reach the waypoint while maintaining a certain velocity

* Updated src/lib/matrix

* Account for speed at target when determining constraints

* Separate constraints into x/y components

* Use setpoint position, not vehicle position

* Fix whitespace, add documentation

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -439,7 +439,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 						const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
 						const float vel_max_posctrl = xy_p * stop_distance;
 
-						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
+						const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
 						const float projection = bin_direction.dot(setpoint_dir);
 						float vel_max_bin = vel_max;
 

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

@@ -173,12 +173,12 @@ float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint
 	return math::constrain(val, min, max);
 }
 
-float FlightTaskAutoLineSmoothVel::_constrainAbsPrioritizeMin(float val, float min, float max)
+float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
 {
-	return math::sign(val) * math::max(math::min(fabsf(val), fabsf(max)), fabsf(min));
+	return math::sign(val) * math::min(fabsf(val), fabsf(max));
 }
 
-float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const
+float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const
 {
 	// Compute the maximum allowed speed at the waypoint assuming that we want to
 	// connect the two lines (prev-current and current-next)
@@ -190,17 +190,22 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const
 	// 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 = Vector2f(&(_target - _next_wp)(0)).length();
-	const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius;
+	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);
-		const float alpha = acosf(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() *
-					  Vector2f(&(_target - _next_wp)(0)).unit_or_zero());
+		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.
@@ -214,14 +219,12 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const
 	return speed_at_target;
 }
 
-float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) const
+float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const
 {
-	float max_speed = math::trajectory::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(),
+	float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(),
 			  _param_mpc_acc_hor.get(),
-			  braking_distance);
-	// 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
-	max_speed = math::min(max_speed, braking_distance * _param_mpc_xy_traj_p.get());
+			  braking_distance,
+			  final_speed);
 
 	return max_speed;
 }
@@ -248,34 +251,24 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			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);
+			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());
 
-			// Unconstrained desired velocity vector
-			Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
+			const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
 
-			Vector2f vel_max_xy;
-			vel_max_xy(0) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)));
-			vel_max_xy(1) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)));
+			// 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;
 
-			const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
-			Vector2f vel_min_xy;
+			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)));
 
-			if (has_reached_altitude) {
-				// Compute the minimum speed in NE frame. This is used
-				// to force the drone to pass the waypoint with a desired speed
-				Vector2f u_prev_to_target_xy((_target - _prev_wp).unit_or_zero());
-				vel_min_xy = u_prev_to_target_xy * _getSpeedAtTarget();
 
-			} else {
-				// The drone has to change altitude, stop at the waypoint
-				vel_min_xy.setAll(0.f);
-			}
+			const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
 
 			// Constrain the norm of each component using min and max values
-			Vector2f vel_sp_constrained_xy;
-			vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
-			vel_sp_constrained_xy(1) = _constrainAbsPrioritizeMin(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
+			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)));
 
 			for (int i = 0; i < 2; i++) {
 				// If available, constrain the velocity using _velocity_setpoint(.)
@@ -286,7 +279,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 					_velocity_setpoint(i) = vel_sp_constrained_xy(i);
 				}
 			}
-
 		}
 
 		if (PX4_ISFINITE(_position_setpoint(2))) {

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

@@ -69,19 +69,11 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 
 	static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
 
-	/**
-	 * Constrain the abs value below max but above min
-	 * Min can be larger than max and has priority over it
-	 * The whole computation is done on the absolute values but the returned
-	 * value has the sign of val
-	 * @param val the value to constrain and boost
-	 * @param min the minimum value that the function should return
-	 * @param max the value by which val is constrained before the boost is applied
-	 */
-	static float _constrainAbsPrioritizeMin(float val, float min, float max);
-
-	float _getSpeedAtTarget() const;
-	float _getMaxSpeedFromDistance(float braking_distance) const;
+	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;
 
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _updateTrajConstraints();

src/lib/mathlib/math/TrajMath.hpp

@@ -45,23 +45,26 @@ namespace math
 namespace trajectory
 {
 
-/* Compute the maximum possible speed on the track given the remaining distance,
- * the maximum acceleration and the maximum jerk.
+/* Compute the maximum possible speed on the track given the desired speed,
+ * 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*accel*(x - vel*2*accel/jerk)
+ * Equation to solve: vel_final^2 = vel_initial^2 - 2*accel*(x - vel_initial*2*accel/jerk)
  *
  * @param jerk maximum jerk
  * @param accel maximum acceleration
- * @param braking_distance distance to the desired stopping point
+ * @param braking_distance distance to the desired point
+ * @param final_speed the still-remaining speed of the vehicle when it reaches the braking_distance
  *
  * @return maximum speed
  */
-inline float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance)
+inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance,
+		const float final_speed)
 {
-	float b =  4.0f * accel * accel / jerk;
-	float c = - 2.0f * accel * braking_distance;
-	float max_speed = 0.5f * (-b + sqrtf(b * b - 4.0f * c));
+	auto sqr = [](float f) {return f * f;};
+	float b =  4.0f * sqr(accel) / jerk;
+	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;
 }