Add extended TiltModel

2026-05-11 16:29:40 +02:00
parent e035e4584b
commit 2b1dbb6737
17 changed files with 545 additions and 28 deletions
@@ -0,0 +1,123 @@
+#pragma once
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "HalGPIO.h"
+
+class HalTiltSensor;
+extern HalTiltSensor halTiltSensor;
+
+class HalTiltSensor {
+  bool _available = false;
+  uint8_t _i2cAddr = 0;
+  bool _tiltForwardEvent = false;
+  bool _tiltBackEvent = false;
+  bool _inTilt = false;
+  bool _hadActivity = false;
+  bool _filterInitialized = false;
+  float _filteredAxis = 0.0f;
+  unsigned long _filterStartMs = 0;
+  unsigned long _lastTiltMs = 0;
+  unsigned long _lastKalmanMicros = 0;
+  mutable unsigned long _lastPollMs = 0;
+
+  static constexpr float TILT_THRESHOLD_G = 0.45f;     // ~27° tilt to trigger
+  static constexpr float NEUTRAL_THRESHOLD_G = 0.25f;  // Must return below this before next trigger
+  static constexpr float TILT_THRESHOLD_DEG = 27.0f;   // Angle threshold for fused tilt detection
+  static constexpr float NEUTRAL_THRESHOLD_DEG = 15.0f;
+  static constexpr unsigned long COOLDOWN_MS = 600;       // Minimum ms between triggers
+  static constexpr unsigned long POLL_INTERVAL_MS = 50;   // 20 Hz polling
+  static constexpr float FILTER_ALPHA = 0.25f;            // Smoothing factor for axis stabilization
+  static constexpr unsigned long FILTER_WARMUP_MS = 300;  // Stabilization warmup after first sample
+
+  static constexpr uint8_t TILT_I2C_ADDR = 0x6A;
+  static constexpr uint8_t TILT_I2C_ADDR_ALT = 0x6B;
+  static constexpr uint8_t TILT_WHO_AM_I_VALUE = 0x05;
+  static constexpr uint8_t REG_WHO_AM_I = 0x00;
+  static constexpr uint8_t REG_CTRL1 = 0x02;
+  static constexpr uint8_t REG_CTRL2 = 0x03;
+  static constexpr uint8_t REG_CTRL3 = 0x04;
+  static constexpr uint8_t REG_CTRL7 = 0x08;
+  static constexpr uint8_t REG_AX_L = 0x35;
+  static constexpr uint8_t REG_GYRO_X_L = 0x3B;
+  static constexpr uint8_t REG_GYRO_Y_L = 0x3D;
+  static constexpr uint8_t REG_GYRO_Z_L = 0x3F;
+
+  static constexpr uint8_t CTRL2_2G_125HZ = 0x06;
+  static constexpr uint8_t CTRL3_512DPS_125HZ = 0x45;
+  static constexpr uint8_t CTRL7_ACCEL_GYRO_EN = 0x03;
+
+  struct KalmanFilter {
+    KalmanFilter() {
+      Q_angle = 0.001f;
+      Q_bias = 0.003f;
+      R_measure = 0.03f;
+      angle = 0.0f;
+      bias = 0.0f;
+      P[0][0] = 0.0f;
+      P[0][1] = 0.0f;
+      P[1][0] = 0.0f;
+      P[1][1] = 0.0f;
+    }
+
+    float update(float newAngle, float newRate, float dt) {
+      float rate = newRate - bias;
+      angle += dt * rate;
+
+      P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
+      P[0][1] -= dt * P[1][1];
+      P[1][0] -= dt * P[1][1];
+      P[1][1] += Q_bias * dt;
+
+      float S = P[0][0] + R_measure;
+      float K0 = P[0][0] / S;
+      float K1 = P[1][0] / S;
+
+      float y = newAngle - angle;
+      angle += K0 * y;
+      bias += K1 * y;
+
+      float P00_temp = P[0][0];
+      float P01_temp = P[0][1];
+
+      P[0][0] -= K0 * P00_temp;
+      P[0][1] -= K0 * P01_temp;
+      P[1][0] -= K1 * P00_temp;
+      P[1][1] -= K1 * P01_temp;
+
+      return angle;
+    }
+
+    void setAngle(float a) { angle = a; }
+
+   private:
+    float Q_angle;
+    float Q_bias;
+    float R_measure;
+    float angle;
+    float bias;
+    float P[2][2];
+  };
+
+  KalmanFilter _kalmanRoll;
+  KalmanFilter _kalmanPitch;
+
+  bool writeReg(uint8_t reg, uint8_t val) const;
+  bool readReg(uint8_t reg, uint8_t* val) const;
+  bool readReg16LE(uint8_t reg, int16_t* val) const;
+  bool readAccel(float& ax, float& ay, float& az) const;
+  bool readAccelGyro(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) const;
+
+ public:
+  void begin();
+  void deepSleep();
+  void update(bool enabled, uint8_t mode, uint8_t orientation);
+
+  bool wasTiltedForward();
+  bool wasTiltedBack();
+  bool hadActivity();
+
+  bool isAvailable() const { return _available; }
+  void clearPendingEvents();
+};