TrajMath: move from FlightTasks/Utility into mathlib library because the function is also used by other libraries

src/lib/mathlib/math/TrajMath.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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 for trajectory generation
+ */
+
+#pragma once
+
+namespace math
+{
+
+namespace trajectory
+{
+
+/* 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
+ */
+inline float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance)
+{
+	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));
+
+	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
+ */
+inline float computeMaxSpeedInWaypoint(const float alpha, const float accel, const float d)
+{
+	float tan_alpha = tanf(alpha / 2.0f);
+	float max_speed_in_turn = sqrtf(accel * d * tan_alpha);
+
+	return max_speed_in_turn;
+}
+} /* namespace traj */
+} /* namespace math */

src/lib/mathlib/mathlib.h

@@ -45,5 +45,6 @@
 #include "math/Functions.hpp"
 #include "math/matrix_alg.h"
 #include "math/SearchMin.hpp"
+#include "math/TrajMath.hpp"
 
 #endif