diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index 81d6018cd9..a1bb528592 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -237,6 +237,7 @@ void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpo
 
 	_accel = accel_new;
 	_vel = vel_new;
+	_pos = pos_new;
 
 	/* set output variables */
 	accel_setpoint_smooth = _accel;
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index 9fbf777f5b..2df7e0148a 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -94,7 +94,9 @@ public:
 	void setMaxVel(float max_vel) { _max_vel = max_vel; }
 
 	void setCurrentVelocity(const float vel) { _vel = vel; }
+	float getCurrentVelocity() const { return _vel; }
 	void setCurrentPosition(const float pos) { _pos = pos; }
+	float getCurrentPosition() const { return _pos; }
 
 	/**
 	 * Synchronize several trajectories to have the same total time. This is required to generate
@@ -105,10 +107,11 @@ public:
 	 */
 	static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
 
-private:
 	float getTotalTime() const { return _T1 + _T2 + _T3; }
 	float getVelSp() const { return _vel_sp; }
 
+private:
+
 	/**
 	 * Compute T1, T2, T3 depending on the current state and velocity setpoint.
 	 * @param T123 optional parameter. If set, the total trajectory time will be T123, if not,