Velocity Smoothing - Various improvements, cleanup and corner cases fixes

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

@@ -69,15 +69,6 @@ float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, fl
 	return T1_new;
 }
 
-float VelocitySmoothing::recomputeMaxJerk(float accel_prev, float max_jerk, float T1)
-{
-	/* If T1 is smaller than dt, it means that the jerk is too large to reach the
-	 * desired acceleration with a bang-bang signal => recompute the maximum jerk
-	 */
-	float accel_T1 = accel_prev + max_jerk * T1;
-	return (accel_T1 - accel_prev) / T1;
-}
-
 float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
 {
 	float b = 2.f * accel_prev / max_jerk;
@@ -93,14 +84,22 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
 	float T1_plus = (-b + sqrt_delta) * 0.5f;
 	float T1_minus = (-b - sqrt_delta) * 0.5f;
 
-	float T1 = math::max(math::max(T1_plus, T1_minus), 0.f);
+	float T3_plus = accel_prev / max_jerk + T1_plus;
+	float T3_minus = accel_prev / max_jerk + T1_minus;
+
+	float T1 = 0.f;
+
+	if (T1_plus >= 0.f && T3_plus >= 0.f) {
+		T1 = T1_plus;
+
+	} else if ( T1_minus >= 0.f && T3_minus >= 0.f) {
+		T1 = T1_minus;
+	}
 
 	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
 
-	if ((T1 > FLT_EPSILON) &&
-	    (T1 < _dt)) {
-		_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
-		T1 = _dt;
+	if (T1 < _dt) {
+		T1 = 0.f;
 	}
 
 	return math::max(T1, 0.f);
@@ -135,10 +134,8 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
 
 	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
 
-	if ((T1 > FLT_EPSILON) &&
-	    (T1 < _dt)) {
-		_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
-		T1 = _dt;
+	if (T1 < _dt) {
+		T1 = 0.f;
 	}
 
 	return T1;
@@ -151,6 +148,11 @@ float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float v
 	float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3
 		  - max_jerk * T3 * T3 * 0.5f;
 	float T2 = (vel_setpoint - f) / (accel_prev + max_jerk * T1);
+
+	if (T2 < _dt) {
+		T2 = 0.f;
+	}
+
 	return math::max(T2, 0.f);
 }
 
@@ -163,6 +165,12 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3)
 float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
 {
 	float T3 = accel_prev / max_jerk + T1;
+
+	if (T1 < FLT_EPSILON && T3 < _dt && T3 > 0.f) {
+		T3 = _dt;
+		_max_jerk_T1 = accel_prev / T3;
+	}
+
 	return math::max(T3, 0.f);
 }
 
@@ -171,31 +179,15 @@ void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float
 {
 	accel_out = jerk * dt + accel_prev;
 
-	// Paranoid check, should never be outside the saturations
-	if (accel_out > _max_accel) {
-		accel_out = _max_accel;
-
-	} else if (accel_out < -_max_accel) {
-		accel_out = -_max_accel;
-	}
-
 	vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
 
-	// Paranoid check, should never be outside the saturations
-	if (vel_out > _max_vel) {
-		vel_out = _max_vel;
-
-	} else if (vel_out < -_max_vel) {
-		vel_out = -_max_vel;
-	}
-
 	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)
 {
-	_vel_sp = vel_setpoint;
-	_dt = dt;
+	_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
+	_dt = math::max(dt, FLT_EPSILON);
 	updateDurations();
 }
 
@@ -214,18 +206,9 @@ void VelocitySmoothing::updateDurations(float T123)
 		T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
 	}
 
-	/* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */
-	if (T1 < _dt) {
-		T1 = 0.f;
-	}
-
 	// compute decreasing acceleration time
 	T3 = computeT3(T1, _accel, _max_jerk_T1);
 
-	if (T3 < _dt) {
-		T3 = 0.f;
-	}
-
 	// compute constant acceleration time
 	if (PX4_ISFINITE(T123)) {
 		T2 = computeT2(T123, T1, T3);
@@ -234,10 +217,6 @@ void VelocitySmoothing::updateDurations(float T123)
 		T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
 	}
 
-	if (T2 < _dt) {
-		T2 = 0.f;
-	}
-
 	_T1 = T1;
 	_T2 = T2;
 	_T3 = T3;
@@ -257,8 +236,8 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa
 	if (_T1 > FLT_EPSILON) {
 		_jerk = _max_jerk_T1;
 
-		if (_T1 < dt) {
-			// _T1 was supposed to be _dt, however, now, dt is bogger than _dt. We have to reduce the jerk to avoid an acceleration overshoot.
+		if (_T1 < dt && dt > _dt) {
+			// _T1 was supposed to be _dt, however, now, dt is bigger than _dt. We have to reduce the jerk to avoid an acceleration overshoot.
 			_jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new
 		}
 
@@ -268,7 +247,7 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa
 	} else if (_T3 > FLT_EPSILON) {
 		_jerk = -_max_jerk_T1;
 
-		if (_T3 < dt) {
+		if (_T3 < dt && dt > _dt) {
 			// Same as for _T1 < dt above
 			_jerk *= _dt / dt;
 		}

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

@@ -134,7 +134,6 @@ private:
 	 */
 	inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
 	inline float saturateT1ForAccel(float accel_prev, float max_jerk, float T1);
-	inline float recomputeMaxJerk(float accel_prev, float max_jerk, float T1);
 	/**
 	 * Compute constant acceleration time
 	 */