From 186cf88d84ed5725a12bd92ec494282c853b56a3 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 17 Oct 2025 19:19:27 +0300
Subject: [PATCH] Add generic Delay filter

---
 flix/imu.ino |  5 ++---
 flix/util.h  | 21 +++++++++++++++++++++
 2 files changed, 23 insertions(+), 3 deletions(-)

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;
+	}
+};