Refactor PID controllers

Use t variable instead of passing dt argument.
Reset PID automatically on large dts.

flix/control.ino

@@ -100,12 +100,7 @@ void interpretControls() {
 }
 
 void controlAttitude() {
-	if (!armed || attitudeTarget.invalid()) { // skip attitude control
+	if (!armed || attitudeTarget.invalid() || thrustTarget < 0.1) return; // skip attitude control
-		rollPID.reset();
-		pitchPID.reset();
-		yawPID.reset();
-		return;
-	}
 
 	const Vector up(0, 0, 1);
 	Vector upActual = Quaternion::rotateVector(up, attitude);
@@ -113,28 +108,23 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
+	ratesTarget.x = rollPID.update(error.x) + ratesExtra.x;
-	ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
+	ratesTarget.y = pitchPID.update(error.y) + ratesExtra.y;
 
 	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
+	ratesTarget.z = yawPID.update(yawError) + ratesExtra.z;
 }
 
 
 void controlRates() {
-	if (!armed || ratesTarget.invalid()) { // skip rates control
+	if (!armed || ratesTarget.invalid() || thrustTarget < 0.1) return; // skip rates control
-		rollRatePID.reset();
-		pitchRatePID.reset();
-		yawRatePID.reset();
-		return;
-	}
 
 	Vector error = ratesTarget - rates;
 
 	// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
-	torqueTarget.x = rollRatePID.update(error.x, dt);
+	torqueTarget.x = rollRatePID.update(error.x);
-	torqueTarget.y = pitchRatePID.update(error.y, dt);
+	torqueTarget.y = pitchRatePID.update(error.y);
-	torqueTarget.z = yawRatePID.update(error.z, dt);
+	torqueTarget.z = yawRatePID.update(error.z);
 }
 
 void controlTorque() {

flix/pid.h

@@ -9,40 +9,43 @@
 
 class PID {
 public:
-	float p = 0;
+	float p, i, d;
-	float i = 0;
+	float windup;
-	float d = 0;
+	float maxdt;
-	float windup = 0;
 
 	float derivative = 0;
 	float integral = 0;
 
 	LowPassFilter<float> lpf; // low pass filter for derivative term
 
-	PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
+	PID(float p, float i, float d, float windup = 0, float dAlpha = 1, float maxdt = 0.1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha), maxdt(maxdt) {}
 
-	float update(float error, float dt) {
+	float update(float error) {
-		integral += error * dt;
+		float dt = t - prevTime;
 
-		if (isfinite(prevError) && dt > 0) {
+		if (dt > 0 && dt < maxdt) {
-			// calculate derivative if both dt and prevError are valid
+			integral += error * dt;
-			derivative = (error - prevError) / dt;
+			derivative = lpf.update((error - prevError) / dt); // compute derivative and apply low-pass filter
-
+		} else {
-			// apply low pass filter to derivative
+			integral = 0;
-			derivative = lpf.update(derivative);
+			derivative = 0;
 		}
 
 		prevError = error;
+		prevTime = t;
 
 		return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
 	}
 
 	void reset() {
 		prevError = NAN;
+		prevTime = NAN;
 		integral = 0;
 		derivative = 0;
+		lpf.reset();
 	}
 
 private:
 	float prevError = NAN;
+	float prevTime = NAN;
 };