Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Implement low pass filter in separate module
Oleg Kalachev7cabdc5e627f
diff --git a/flix/control.ino b/flix/control.inoindex 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 * 4thrustTarget = 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.hnew file mode 100644index 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.hindex 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;