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

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -39,8 +39,6 @@
 
 #include <CollisionPrevention/CollisionPrevention.hpp>
 
-#include <FlightTasks/tasks/Utility/TrajMath.hpp>
-
 using namespace matrix;
 using namespace time_literals;
 namespace
@@ -294,7 +292,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 						float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f);
 						float stop_distance =  math::max(0.f, distance - min_dist_to_keep - delay_distance);
 						float vel_max_posctrl = xy_p * stop_distance;
-						float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
+						float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
 						Vector2f  vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
 						float vel_max_bin = vel_max_vec.dot(setpoint_dir);
 

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

@@ -38,7 +38,6 @@
 #include "FlightTaskAutoLineSmoothVel.hpp"
 #include <mathlib/mathlib.h>
 #include <float.h>
-#include "TrajMath.hpp"
 
 using namespace matrix;
 
@@ -212,9 +211,10 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 		// 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 = trajmath::computeMaxSpeedInWaypoint(alpha,
-						_param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(),
-						_target_acceptance_radius);
+		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);
 	}
 
@@ -223,7 +223,7 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 
 float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance)
 {
-	float max_speed = trajmath::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(),
+	float max_speed = math::trajectory::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

src/lib/mathlib/math/TrajMath.hpp

@@ -34,12 +34,15 @@
 /**
  * @file TrajMath.hpp
  *
- * collection of functions used in trajectory generators
+ * collection of functions used for trajectory generation
  */
 
 #pragma once
 
-namespace trajmath
+namespace math
+{
+
+namespace trajectory
 {
 
 /* Compute the maximum possible speed on the track given the remaining distance,
@@ -54,7 +57,7 @@ namespace trajmath
  *
  * @return maximum speed
  */
-float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance)
+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;
@@ -77,11 +80,12 @@ float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, co
  *
  *  @return maximum tangential speed
  */
-float computeMaxSpeedInWaypoint(const float alpha, const float accel, const float d)
+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 trajmath */
+} /* 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