Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Implement low pass filter in separate module

Oleg Kalachev7cabdc5e627f
diff --git a/flix/control.ino b/flix/control.ino
index 12f2b15..56932e5 100644
--- a/flix/control.ino
+++ b/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);
diff --git a/flix/lpf.h b/flix/lpf.h
new file mode 100644
index 0000000..befa9a0
--- /dev/null
+++ b/flix/lpf.h
@@ -0,0 +1,42 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// 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;
+};
diff --git a/flix/pid.h b/flix/pid.h
index 491e11b..b26189f 100644
--- a/flix/pid.h
+++ b/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;