Trajectory - Overload integrate function to allow for custom integration period

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

@@ -139,10 +139,10 @@ float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
 	return math::max(T3, 0.f);
 }
 
-void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
+void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev,
 				   float &accel_out, float &vel_out, float &pos_out)
 {
-	accel_out = jerk * _dt + accel_prev;
+	accel_out = jerk * dt + accel_prev;
 
 	if (accel_out > _max_accel) {
 		accel_out = _max_accel;
@@ -151,7 +151,7 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
 		accel_out = -_max_accel;
 	}
 
-	vel_out = _dt * 0.5f * (accel_out + accel_prev) + vel_prev;
+	vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
 
 	if (vel_out > _max_vel) {
 		vel_out = _max_vel;
@@ -160,7 +160,7 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
 		vel_out = -_max_vel;
 	}
 
-	pos_out = _dt / 3.f * (vel_out + accel_prev * _dt * 0.5f + 2.f * vel_prev) + _pos;
+	pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos;
 }
 
 void VelocitySmoothing::updateDurations(float dt, float vel_setpoint)
@@ -216,10 +216,16 @@ void VelocitySmoothing::updateDurations(float T123)
 
 void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
 				  float &pos_setpoint_smooth)
+{
+	integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
+}
+
+void VelocitySmoothing::integrate(float dt,float &accel_setpoint_smooth, float &vel_setpoint_smooth,
+				  float &pos_setpoint_smooth)
 {
 	/* Integrate the trajectory */
 	float accel_new, vel_new, pos_new;
-	integrateT(_jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
+	integrateT(dt, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
 
 	/* Apply correct jerk (min, max or zero) */
 	if (_T1 > 0.f) {

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp

@@ -77,11 +77,14 @@ public:
 
 	/**
 	 * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
+	 * @param dt optional integration period. If not given, the integration period provided during updateDuration call is used.
+	 * 	A dt different from the one given during the computation of T1-T3 can be used to fast-forward or slow-down the trajectory.
 	 * @param acc_setpoint_smooth returned smoothed acceleration setpoint
 	 * @param vel_setpoint_smooth returned smoothed velocity setpoint
 	 * @param pos_setpoint_smooth returned smoothed position setpoint
 	 */
 	void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
+	void integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
 
 	/* Get / Set constraints (constraints can be updated at any time) */
 	float getMaxJerk() const { return _max_jerk; }
@@ -142,7 +145,7 @@ private:
 	/**
 	 * Integrate the jerk, acceleration and velocity to get the new setpoints and states.
 	 */
-	inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
+	inline void integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev,
 			       float &accel_out, float &vel_out, float &pos_out);
 
 	/* Inputs */