test for computeMaxSpeedFromDistance and experimental chatgpt fix

src/lib/mathlib/CMakeLists.txt

@@ -50,3 +50,4 @@ px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp)
 px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp)
 px4_add_unit_gtest(SRC math/WelfordMeanVectorTest.cpp)
 px4_add_unit_gtest(SRC math/MaxDistanceToCircleTest.cpp)
+px4_add_unit_gtest(SRC math/MaxSpeedFromDistanceTest.cpp)

src/lib/mathlib/math/MaxSpeedFromDistanceTest.cpp

@@ -0,0 +1,28 @@
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include "Functions.hpp"
+
+#include "TrajMath.hpp"
+
+using namespace math;
+using matrix::Vector2f;
+
+TEST(MaxSpeedFromDistance, MaxSpeedFromDistance)
+{
+	// When max acceleration is higher we should be able to fly faster
+	const float acc_hor_small = 1.f;
+	const float acc_hor_large = 10.f;
+
+	const float jerk_max = 8.f;
+	const float stop_distance = 10.f;
+	const float final_speed = 0.f;
+
+	const float max_vel_smaller = trajectory::computeMaxSpeedFromDistance(jerk_max, acc_hor_small, stop_distance, final_speed);
+	const float max_vel_larger = trajectory::computeMaxSpeedFromDistance(jerk_max, acc_hor_large, stop_distance, final_speed);
+
+	printf("max_vel_larger: %f\n", (double)max_vel_larger);
+	printf("max_vel_smaller: %f\n", (double)max_vel_smaller);
+
+
+	EXPECT_TRUE(max_vel_larger < max_vel_smaller);
+}

src/lib/mathlib/math/TrajMath.hpp

@@ -58,17 +58,46 @@ namespace trajectory
  *
  * @return maximum speed
  */
-inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance,
-		const float final_speed)
+inline float computeMaxSpeedFromDistance(float jerk,
+                                         float accel,
+                                         float x,
+                                         float v_f)
 {
-	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));
+    auto sqr = [](float w){ return w*w; };
 
-	// don't slow down more than the end speed, even if the conservative accel ramp time requests it
-	return fmaxf(max_speed, final_speed);
+    // 1) linear term b = 2 * a^2 / j
+    float b = 2.0f * accel*accel / jerk;
+
+    // 2) constant term c = -2*a*x - v_f^2 - (a^4)/(3*j^2)
+    float  jerk2 = jerk*jerk;
+    float  a4    = accel*accel*accel*accel;
+    float  c     = -2.0f * accel * x
+                   - sqr(v_f)
+                   - (a4 / (3.0f * jerk2));
+
+    // 3) solve quadratic
+    float disc = b*b - 4.0f * c;
+    if (disc <= 0.0f) {
+        // no real solution → can’t brake from anything > v_f
+        return v_f;
+    }
+
+    float v_i = 0.5f * ( -b + sqrtf(disc) );
+
+    // 4) never suggest below your target end‐speed
+    return fmaxf(v_i, v_f);
 }
+// inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance,
+// 		const float final_speed)
+// {
+// 	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));
+
+// 	// don't slow down more than the end speed, even if the conservative accel ramp time requests it
+// 	return fmaxf(max_speed, final_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