Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Add generic Delay filter
Oleg Kalachev186cf88d84ed
diff --git a/flix/imu.ino b/flix/imu.inoindex ceefa6b..45ba006 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -49,9 +49,8 @@ void rotateIMU(Vector& data) {}void calibrateGyroOnce() {- static float landedTime = 0;- landedTime = landed ? landedTime + dt : 0;- if (landedTime < 2) return; // calibrate only if definitely stationary+ static Delay landedDelay(2);+ if (!landedDelay.update(landed)) return; // calibrate only if definitely stationarystatic LowPassFilter<Vector> gyroCalibrationFilter(0.001);gyroBias = gyroCalibrationFilter.update(gyro);diff --git a/flix/util.h b/flix/util.hindex 90925d5..3946d77 100644--- a/flix/util.h+++ b/flix/util.h@@ -10,6 +10,7 @@#include <soc/rtc_cntl_reg.h>const float ONE_G = 9.80665;+extern double t;float mapf(long x, long in_min, long in_max, float out_min, float out_max) {return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;@@ -52,3 +53,23 @@ void splitString(String& str, String& token0, String& token1, String& token2) {token1 = strtok(NULL, " "); // String(NULL) creates empty stringtoken2 = strtok(NULL, "");}++// Delay filter for boolean signals - ensures the signal is on for at least 'delay' seconds+class Delay {+public:+ float delay;+ float start = NAN;++ Delay(float delay) : delay(delay) {}++ bool update(bool on) {+ if (!on) {+ start = NAN;+ return false;+ }+ if (isnan(start)) {+ start = t;+ }+ return t - start >= delay;+ }+};