Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Add generic Delay filter

Oleg Kalachev186cf88d84ed
diff --git a/flix/imu.ino b/flix/imu.ino
index 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 stationary
static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
gyroBias = gyroCalibrationFilter.update(gyro);
diff --git a/flix/util.h b/flix/util.h
index 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 string
token2 = 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;
+ }
+};