diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index 6e78d4e84f..0db669f24c 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -35,8 +35,6 @@
 
 #include <cstdio>
 #include <float.h>
-#include <math.h>
-#include <px4_defines.h>
 
 #include <mathlib/mathlib.h>
 
@@ -210,22 +208,22 @@ void VelocitySmoothing::updateDurations(float T123)
 	_max_jerk_T1 = (_vel_sp - _vel > 0.f) ? _max_jerk : -_max_jerk;
 
 	// compute increasing acceleration time
-	if (PX4_ISFINITE(T123)) {
-		T1 = computeT1(T123, _accel, _vel, _vel_sp, _max_jerk_T1);
+	if (T123 < 0.f) {
+		T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
 
 	} else {
-		T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
+		T1 = computeT1(T123, _accel, _vel, _vel_sp, _max_jerk_T1);
 	}
 
 	// compute decreasing acceleration time
 	T3 = computeT3(T1, _accel, _max_jerk_T1);
 
 	// compute constant acceleration time
-	if (PX4_ISFINITE(T123)) {
-		T2 = computeT2(T123, T1, T3);
+	if (T123 < 0.f) {
+		T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
 
 	} else {
-		T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
+		T2 = computeT2(T123, T1, T3);
 	}
 
 	_T1 = T1;
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index 460698380a..7e744384c1 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -33,8 +33,6 @@
 
 #pragma once
 
-#include <matrix/matrix/math.hpp>
-
 /**
  * @class VelocitySmoothing
  *
@@ -115,6 +113,9 @@ public:
 	static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
 
 	float getTotalTime() const { return _T1 + _T2 + _T3; }
+	float getT1() const { return _T1; }
+	float getT2() const { return _T2; }
+	float getT3() const { return _T3; }
 	float getVelSp() const { return _vel_sp; }
 
 private:
@@ -124,7 +125,7 @@ private:
 	 * @param T123 optional parameter. If set, the total trajectory time will be T123, if not,
 	 * 		the algorithm optimizes for time.
 	 */
-	void updateDurations(float T123 = NAN);
+	void updateDurations(float T123 = -1.f);
 	/**
 	 * Compute increasing acceleration time
 	 */