Add extended TiltModel

2026-05-11 16:29:40 +02:00
parent e035e4584b
commit 2b1dbb6737
17 changed files with 545 additions and 28 deletions
@@ -305,6 +305,11 @@ STR_DIR_DOWN: "Down"
 STR_OK_BUTTON: "OK"
 STR_SLEEP_COVER_FILTER: "Sleep Screen Cover Filter"
 STR_FILTER_CONTRAST: "Contrast"
+STR_TILT_PAGE_TURN: "Tilt Page Turn"
+STR_TILT_STABILIZATION: "Tilt Mode"
+STR_TILT_MODE_RAW: "Raw Acceleration"
+STR_TILT_MODE_SMOOTH: "Smoothed Acceleration"
+STR_TILT_MODE_KALMAN: "Gyro/Accel Fusion"
 STR_CUSTOMISE_STATUS_BAR: "Customise Status Bar"
 STR_CHAPTER_PAGE_COUNT: "Chapter Page Count"
 STR_BOOK_PROGRESS_PERCENTAGE: "Book Progress Percentage"
@@ -251,6 +251,11 @@ STR_DIR_DOWN: "Bas"
 STR_OK_BUTTON: "OK"
 STR_SLEEP_COVER_FILTER: "Filtre écran de veille"
 STR_FILTER_CONTRAST: "Contraste"
+STR_TILT_PAGE_TURN: "Changement de page par inclinaison"
+STR_TILT_STABILIZATION: "Mode inclinaison"
+STR_TILT_MODE_RAW: "Acceleration brute"
+STR_TILT_MODE_SMOOTH: "Acceleration lisse"
+STR_TILT_MODE_KALMAN: "Fusion gyroscope/accelerometre"
 STR_CUSTOMISE_STATUS_BAR: "Personnaliser la barre d'état"
 STR_CHAPTER_PAGE_COUNT: "Nombre de pages du chapitre"
 STR_BOOK_PROGRESS_PERCENTAGE: "Pourcentage de progression"
@@ -242,6 +242,11 @@ STR_DIR_DOWN: "Runter"
 STR_OK_BUTTON: "OK"
 STR_SLEEP_COVER_FILTER: "Standby-Coverfilter"
 STR_FILTER_CONTRAST: "Kontrast"
+STR_TILT_PAGE_TURN: "Seitenblaettern durch Neigen"
+STR_TILT_STABILIZATION: "Neigungsmodus"
+STR_TILT_MODE_RAW: "Rohbeschleunigung"
+STR_TILT_MODE_SMOOTH: "Geglaettete Beschleunigung"
+STR_TILT_MODE_KALMAN: "Gyro-/Beschleunigungsfusion"
 STR_CUSTOMISE_STATUS_BAR: "Statusleiste anpassen"
 STR_CHAPTER_PAGE_COUNT: "Kapitel-Seitenanzahl"
 STR_BOOK_PROGRESS_PERCENTAGE: "Buchfortschritt in %"
@@ -251,6 +251,11 @@ STR_DIR_DOWN: "Giù"
 STR_OK_BUTTON: "OK"
 STR_SLEEP_COVER_FILTER: "Filtro copertina"
 STR_FILTER_CONTRAST: "Contrasto"
+STR_TILT_PAGE_TURN: "Cambio pagina con inclinazione"
+STR_TILT_STABILIZATION: "Modalita inclinazione"
+STR_TILT_MODE_RAW: "Accelerazione grezza"
+STR_TILT_MODE_SMOOTH: "Accelerazione smussata"
+STR_TILT_MODE_KALMAN: "Fusione giroscopio/accelerometro"
 STR_CUSTOMISE_STATUS_BAR: "Personalizza barra di stato"
 STR_CHAPTER_PAGE_COUNT: "Conteggio pagine capitolo"
 STR_BOOK_PROGRESS_PERCENTAGE: "Percentuale avanzamento lettura"
@@ -434,6 +434,10 @@ STR_CRASH_DESCRIPTION: "Szczegółowy raport zapisany do crash_report.txt. Prosi
 STR_CRASH_REASON: "Powód awarii:"
 STR_CRASH_NO_REASON: "(Nie odnotowano powodu)"
 STR_TILT_PAGE_TURN: "Kartkowanie przechyłem"
+STR_TILT_STABILIZATION: "Tryb przechyłu"
+STR_TILT_MODE_RAW: "Surowe przyspieszenie"
+STR_TILT_MODE_SMOOTH: "Wygładzone przyspieszenie"
+STR_TILT_MODE_KALMAN: "Fuzja żyroskopu/akcelerometru"
 STR_RENDER_BENCHMARK: "Benchmark renderowania"
 STR_WEATHER: "Pogoda"
 STR_WEATHER_LOCATION: "Lokalizacja"
@@ -0,0 +1,267 @@
+#include "HalTiltSensor.h"
+
+#include <Logging.h>
+
+HalTiltSensor halTiltSensor;
+
+bool HalTiltSensor::writeReg(uint8_t reg, uint8_t val) const {
+  Wire.beginTransmission(_i2cAddr);
+  Wire.write(reg);
+  Wire.write(val);
+  return Wire.endTransmission() == 0;
+}
+
+bool HalTiltSensor::readReg(uint8_t reg, uint8_t* val) const {
+  Wire.beginTransmission(_i2cAddr);
+  Wire.write(reg);
+  if (Wire.endTransmission(false) != 0) {
+    return false;
+  }
+  Wire.requestFrom(_i2cAddr, static_cast<uint8_t>(1));
+  if (Wire.available() < 1) {
+    return false;
+  }
+  *val = Wire.read();
+  return true;
+}
+
+bool HalTiltSensor::readReg16LE(uint8_t reg, int16_t* val) const {
+  Wire.beginTransmission(_i2cAddr);
+  Wire.write(reg);
+  if (Wire.endTransmission(false) != 0) {
+    return false;
+  }
+  if (Wire.requestFrom(_i2cAddr, static_cast<uint8_t>(2), static_cast<uint8_t>(true)) < 2) {
+    return false;
+  }
+  const uint8_t lo = Wire.read();
+  const uint8_t hi = Wire.read();
+  *val = static_cast<int16_t>((static_cast<uint16_t>(hi) << 8) | lo);
+  return true;
+}
+
+bool HalTiltSensor::readAccel(float& ax, float& ay, float& az) const {
+  int16_t rawAx = 0;
+  int16_t rawAy = 0;
+  int16_t rawAz = 0;
+  if (!readReg16LE(REG_AX_L, &rawAx) || !readReg16LE(REG_AX_L + 2, &rawAy) || !readReg16LE(REG_AX_L + 4, &rawAz)) {
+    return false;
+  }
+
+  constexpr float SCALE = 1.0f / 16384.0f;
+  ax = rawAx * SCALE;
+  ay = rawAy * SCALE;
+  az = rawAz * SCALE;
+  return true;
+}
+
+bool HalTiltSensor::readAccelGyro(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) const {
+  int16_t rawAx = 0;
+  int16_t rawAy = 0;
+  int16_t rawAz = 0;
+  int16_t rawGx = 0;
+  int16_t rawGy = 0;
+  int16_t rawGz = 0;
+  if (!readReg16LE(REG_AX_L, &rawAx) || !readReg16LE(REG_AX_L + 2, &rawAy) || !readReg16LE(REG_AX_L + 4, &rawAz) ||
+      !readReg16LE(REG_GYRO_X_L, &rawGx) || !readReg16LE(REG_GYRO_Y_L, &rawGy) || !readReg16LE(REG_GYRO_Z_L, &rawGz)) {
+    return false;
+  }
+
+  constexpr float ACC_SCALE = 1.0f / 16384.0f;
+  constexpr float GYRO_SCALE = 512.0f / 32768.0f;
+  ax = rawAx * ACC_SCALE;
+  ay = rawAy * ACC_SCALE;
+  az = rawAz * ACC_SCALE;
+  gx = rawGx * GYRO_SCALE;
+  gy = rawGy * GYRO_SCALE;
+  gz = rawGz * GYRO_SCALE;
+  return true;
+}
+
+void HalTiltSensor::begin() {
+  if (!gpio.deviceIsX3()) {
+    _available = false;
+    return;
+  }
+
+  uint8_t whoami = 0;
+  _i2cAddr = TILT_I2C_ADDR;
+  if (!readReg(REG_WHO_AM_I, &whoami) || whoami != TILT_WHO_AM_I_VALUE) {
+    _i2cAddr = TILT_I2C_ADDR_ALT;
+    if (!readReg(REG_WHO_AM_I, &whoami) || whoami != QMI8658_WHO_AM_I_VALUE) {
+      LOG_INF("TILT", "QMI8658 IMU not found");
+      _available = false;
+      return;
+    }
+  }
+
+  LOG_INF("TILT", "QMI8658 IMU found at 0x%02X", _i2cAddr);
+
+  if (!writeReg(REG_CTRL1, 0x40) || !writeReg(REG_CTRL2, CTRL2_2G_125HZ) || !writeReg(REG_CTRL3, CTRL3_512DPS_125HZ) ||
+      !writeReg(REG_CTRL7, CTRL7_ACCEL_GYRO_EN)) {
+    LOG_INF("TILT", "QMI8658 register configuration failed");
+    _available = false;
+    return;
+  }
+
+  _available = true;
+  _lastPollMs = millis();
+  _lastTiltMs = millis();
+  _lastKalmanMicros = 0;
+  _filterInitialized = false;
+  _filterStartMs = millis();
+  LOG_INF("TILT", "QMI8658 accelerometer initialized (±2g, 125 Hz) and gyro enabled");
+}
+
+void HalTiltSensor::deepSleep() {
+  if (!_available) {
+    return;
+  }
+  clearPendingEvents();
+  _inTilt = false;
+  _filterInitialized = false;
+  _lastKalmanMicros = 0;
+}
+
+void HalTiltSensor::clearPendingEvents() {
+  _tiltForwardEvent = false;
+  _tiltBackEvent = false;
+  _hadActivity = false;
+}
+
+void HalTiltSensor::update(bool enabled, uint8_t mode, uint8_t orientation) {
+  if (!enabled || !_available) {
+    return;
+  }
+
+  const unsigned long now = millis();
+  if ((now - _lastPollMs) < POLL_INTERVAL_MS) {
+    return;
+  }
+  _lastPollMs = now;
+
+  bool isAngleMode = mode == 2;
+  float tiltValue = 0.0f;
+  if (isAngleMode) {
+    float ax, ay, az, gx, gy, gz;
+    if (!readAccelGyro(ax, ay, az, gx, gy, gz)) {
+      return;
+    }
+
+    const float accRoll = atan2(ay, sqrt(ax * ax + az * az)) * (180.0f / PI);
+    const float accPitch = atan2(-ax, sqrt(ay * ay + az * az)) * (180.0f / PI);
+
+    const unsigned long nowMicros = micros();
+    const unsigned long deltaMicros = nowMicros - _lastKalmanMicros;
+    float dt = 0.008f;
+    if (_lastKalmanMicros != 0 && deltaMicros <= 150000u) {
+      dt = static_cast<float>(deltaMicros) * 1e-6f;
+    } else {
+      _kalmanRoll.setAngle(accRoll);
+      _kalmanPitch.setAngle(accPitch);
+    }
+    _lastKalmanMicros = nowMicros;
+
+    const float stableRoll = _kalmanRoll.update(accRoll, gx, dt);
+    const float stablePitch = _kalmanPitch.update(accPitch, gy, dt);
+
+    switch (orientation) {
+      case 0:  // PORTRAIT
+        tiltValue = stablePitch;
+        break;
+      case 2:  // INVERTED
+        tiltValue = -stablePitch;
+        break;
+      case 1:  // LANDSCAPE_CW
+        tiltValue = stableRoll;
+        break;
+      case 3:  // LANDSCAPE_CCW
+        tiltValue = -stableRoll;
+        break;
+      default:
+        tiltValue = stablePitch;
+        break;
+    }
+  } else {
+    float ax, ay, az;
+    if (!readAccel(ax, ay, az)) {
+      return;
+    }
+
+    float tiltAxis;
+    switch (orientation) {
+      case 0:  // PORTRAIT
+        tiltAxis = ay;
+        break;
+      case 2:  // INVERTED
+        tiltAxis = -ay;
+        break;
+      case 1:  // LANDSCAPE_CW
+        tiltAxis = ax;
+        break;
+      case 3:  // LANDSCAPE_CCW
+        tiltAxis = -ax;
+        break;
+      default:
+        tiltAxis = ay;
+        break;
+    }
+
+    if (mode == 1) {
+      if (!_filterInitialized) {
+        _filteredAxis = tiltAxis;
+        _filterInitialized = true;
+        _filterStartMs = now;
+      } else {
+        _filteredAxis = _filteredAxis * (1.0f - FILTER_ALPHA) + tiltAxis * FILTER_ALPHA;
+      }
+      if ((now - _filterStartMs) < FILTER_WARMUP_MS) {
+        return;
+      }
+      tiltAxis = _filteredAxis;
+    }
+    tiltValue = tiltAxis;
+  }
+
+  if (_inTilt) {
+    const float neutralThreshold = isAngleMode ? NEUTRAL_THRESHOLD_DEG : NEUTRAL_THRESHOLD_G;
+    if (fabsf(tiltValue) < neutralThreshold) {
+      _inTilt = false;
+    }
+    return;
+  }
+
+  if ((now - _lastTiltMs) < COOLDOWN_MS) {
+    return;
+  }
+
+  if (tiltValue > (isAngleMode ? TILT_THRESHOLD_DEG : TILT_THRESHOLD_G)) {
+    _tiltForwardEvent = true;
+    _inTilt = true;
+    _hadActivity = true;
+    _lastTiltMs = now;
+  } else if (tiltValue < (isAngleMode ? -TILT_THRESHOLD_DEG : -TILT_THRESHOLD_G)) {
+    _tiltBackEvent = true;
+    _inTilt = true;
+    _hadActivity = true;
+    _lastTiltMs = now;
+  }
+}
+
+bool HalTiltSensor::wasTiltedForward() {
+  const bool val = _tiltForwardEvent;
+  _tiltForwardEvent = false;
+  return val;
+}
+
+bool HalTiltSensor::wasTiltedBack() {
+  const bool val = _tiltBackEvent;
+  _tiltBackEvent = false;
+  return val;
+}
+
+bool HalTiltSensor::hadActivity() {
+  const bool val = _hadActivity;
+  _hadActivity = false;
+  return val;
+}
@@ -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();
+};