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/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;
 }