Create TrajMath library and move waypoint speed calculations in it

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

@@ -38,6 +38,7 @@
 #include "FlightTaskAutoLineSmoothVel.hpp"
 #include <mathlib/mathlib.h>
 #include <float.h>
+#include "TrajMath.hpp"
 
 using namespace matrix;
 
@@ -182,12 +183,14 @@ void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
 
 float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 {
-	// Compute the maximum allowed speed at the waypoint assuming that we want to connect the two lines (prev-current and current-next)
+	// 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.
+	// 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 = Vector2f(&(_target - _next_wp)(0)).length();
@@ -200,12 +203,13 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 		// Max speed between current and next
 		const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next);
 		const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() *
-					 Vector2f(&(_target - _next_wp)(0)).unit_or_zero()) / 2.f;
-		const float tan_alpha = tan(alpha);
-		// 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)
+					 Vector2f(&(_target - _next_wp)(0)).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.
-		const float max_speed_in_turn = sqrtf(_param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get() * _target_acceptance_radius
-						      * tan_alpha);
+		const float max_speed_in_turn = trajmath::computeMaxSpeedInWaypoint(alpha,
+						_param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(),
+						_target_acceptance_radius);
 		speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
 	}
 
@@ -214,13 +218,11 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 
 float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance)
 {
-	// 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() * braking_distance;
-	float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
+	float max_speed = trajmath::computeMaxSpeedFromBrakingDistance(_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());
 
 	return max_speed;

src/lib/FlightTasks/tasks/Utility/TrajMath.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file TrajMath.hpp
+ *
+ * collection of functions used in trajectory generators
+ */
+
+#pragma once
+
+namespace trajmath
+{
+
+/* Compute the maximum possible speed 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*accel*(x - vel*2*accel/jerk)
+ *
+ * @param jerk maximum jerk
+ * @param accel maximum acceleration
+ * @param braking_distance distance to the desired stopping point
+ *
+ * @return maximum speed
+ */
+template<typename T>
+const T computeMaxSpeedFromBrakingDistance(T jerk, T accel, T braking_distance)
+{
+	T b = (T) 4 * accel * accel / jerk;
+	T c = - (T) 2 * accel * braking_distance;
+	T max_speed = (T) 0.5 * (-b + sqrtf(b * b - (T) 4 * c));
+
+	return max_speed;
+}
+
+/* Compute the maximum tangential speed in a circle defined by two line segments of length "d"
+ * forming a V shape, opened by an angle "alpha". The circle is tangent to the end of the
+ * two segments as shown below:
+ *      \\
+ *      | \ d
+ *      /  \
+ *  __='___a\
+ *      d
+ *  @param alpha angle between the two line segments
+ *  @param accel maximum lateral acceleration
+ *  @param d length of the two line segments
+ *
+ *  @return maximum tangential speed
+ */
+template<typename T>
+const T computeMaxSpeedInWaypoint(T alpha, T accel, T d)
+{
+	T tan_alpha = tan(alpha / (T) 2);
+	T max_speed_in_turn = sqrtf(accel * d * tan_alpha);
+
+	return max_speed_in_turn;
+}
+} /* namespace trajmath */