Implement low pass filter in separate module

flix/control.ino

@@ -6,6 +6,7 @@
 #include "vector.h"
 #include "quaternion.h"
 #include "pid.h"
+#include "lpf.h"
 
 #define PITCHRATE_P 0.05
 #define PITCHRATE_I 0.2
@@ -31,17 +32,20 @@
 #define YAWRATE_MAX 360 * DEG_TO_RAD
 #define MAX_TILT 30 * DEG_TO_RAD
 
+#define RATES_LFP_ALPHA 0.8
+#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
+
 enum { MANUAL, ACRO, STAB } mode = STAB;
 bool armed = false;
 
-PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM);
-PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM);
+PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
+PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
 PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
 PID rollPID(ROLL_P, ROLL_I, ROLL_D);
 PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
 PID yawPID(YAW_P, 0, 0);
 
-Vector ratesFiltered;
+LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
@@ -206,7 +210,7 @@ void controlRate()
 	// collective thrust is throttle * 4
 	thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
 
-	ratesFiltered = rates * 0.8 + ratesFiltered * 0.2; // cut-off frequency 40 Hz
+	Vector ratesFiltered = ratesFilter.update(rates);
 
 	torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
 	torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);

flix/lpf.h

@@ -0,0 +1,42 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Low pass filter implementation
+
+#pragma once
+
+template <typename T> // Using template to make the filter usable for scalar and vector values
+class LowPassFilter
+{
+public:
+	float alpha; // smoothing constant, 1 means filter disabled
+	T output;
+
+	LowPassFilter(float alpha): alpha(alpha) {};
+
+	T update(const T input)
+	{
+		if (alpha == 1) { // filter disabled
+			return input;
+		}
+
+		if (!initialized) {
+			output = input;
+			initialized = true;
+		}
+		return output = output * (1 - alpha) + input * alpha;
+	}
+
+	void setCutOffFrequency(float cutOffFreq, float dt)
+	{
+		alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
+	}
+
+	void reset()
+	{
+		initialized = false;
+	}
+
+private:
+	bool initialized = false;
+};

flix/pid.h

@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include "lpf.h"
+
 class PID
 {
 public:
@@ -16,15 +18,20 @@ public:
 	float derivative = 0;
 	float integral = 0;
 
-	PID(float p, float i, float d, float windup = 0) : p(p), i(i), d(d), windup(windup) {};
+	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) {};
 
 	float update(float error, float dt)
 	{
 		integral += error * dt;
+
 		if (isfinite(prevError) && dt > 0) {
 			// calculate derivative if both dt and prevError are valid
-			float _derivative = (error - prevError) / dt;
-			derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:
+			derivative = (error - prevError) / dt;
+
+			// apply low pass filter to derivative
+			derivative = lpf.update(derivative);
 		}
 
 		prevError = error;