Use upstream

lib/hal/HalTiltSensor.cpp

@@ -17,7 +17,7 @@ bool HalTiltSensor::readReg(uint8_t reg, uint8_t* val) const {
   if (Wire.endTransmission(false) != 0) {
     return false;
   }
-  Wire.requestFrom(_i2cAddr, static_cast<uint8_t>(1));
+  Wire.requestFrom(_i2cAddr, (uint8_t)1);
   if (Wire.available() < 1) {
     return false;
   }
@@ -25,56 +25,28 @@ bool HalTiltSensor::readReg(uint8_t reg, uint8_t* val) const {
   return true;
 }
 
-bool HalTiltSensor::readReg16LE(uint8_t reg, int16_t* val) const {
+bool HalTiltSensor::readGyro(float& gx, float& gy, float& gz) const {
   Wire.beginTransmission(_i2cAddr);
-  Wire.write(reg);
+  Wire.write(REG_GX_L);
   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)) {
+  Wire.requestFrom(_i2cAddr, (uint8_t)6);
+  if (Wire.available() < 6) {
     return false;
   }
 
-  constexpr float SCALE = 1.0f / 16384.0f;
-  ax = rawAx * SCALE;
-  ay = rawAy * SCALE;
-  az = rawAz * SCALE;
-  return true;
-}
+  auto readInt16 = [&]() -> int16_t {
+    const uint8_t lo = Wire.read();
+    const uint8_t hi = Wire.read();
+    return static_cast<int16_t>((hi << 8) | lo);
+  };
 
-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;
+  constexpr float SCALE = 1.0f / 64.0f;  // ±512 dps full scale: 32768 / 512 = 64 LSB/dps
+  gx = readInt16() * SCALE;
+  gy = readInt16() * SCALE;
+  gz = readInt16() * SCALE;
   return true;
 }
 
@@ -85,166 +57,143 @@ void HalTiltSensor::begin() {
   }
 
   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 != TILT_WHO_AM_I_VALUE) {
-      LOG_INF("TILT", "QMI8658 IMU not found");
+  _i2cAddr = I2C_ADDR_QMI8658;
+  if (!readReg(QMI8658_WHO_AM_I_REG, &whoami) || whoami != QMI8658_WHO_AM_I_VALUE) {
+    _i2cAddr = I2C_ADDR_QMI8658_ALT;
+    if (!readReg(QMI8658_WHO_AM_I_REG, &whoami) || whoami != QMI8658_WHO_AM_I_VALUE) {
+      LOG_ERR("GYR", "QMI8658 IMU not found");
       _available = false;
       return;
     }
   }
 
-  LOG_INF("TILT", "QMI8658 IMU found at 0x%02X", _i2cAddr);
+  LOG_INF("GYR", "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");
+  if (!writeReg(REG_CTRL7, CTRL7_DISABLE_ALL) || !writeReg(REG_CTRL3, CTRL3_FS_512DPS | CTRL3_ODR_28HZ) ||
+      !writeReg(REG_CTRL1, CTRL1_BASE | CTRL1_SENSOR_DISABLE)) {
+    LOG_ERR("GYR", "QMI8658 register configuration failed");
     _available = false;
     return;
   }
 
   _available = true;
+  _initMs = millis();
   _lastPollMs = millis();
-  _lastTiltMs = millis();
-  _lastKalmanMicros = 0;
-  _filterInitialized = false;
-  _filterStartMs = millis();
-  LOG_INF("TILT", "QMI8658 accelerometer initialized (±2g, 125 Hz) and gyro enabled");
+  LOG_INF("GYR", "QMI8658 gyro initialized and put to sleep");
 }
 
-void HalTiltSensor::deepSleep() {
+bool HalTiltSensor::wake() {
+  if (!_available) {
+    return false;
+  }
+
+  if ((millis() - _initMs) < SLEEP_STABILIZE_MS) {
+    return false;
+  }
+
+  if (writeReg(REG_CTRL1, CTRL1_BASE) && writeReg(REG_CTRL7, CTRL7_GYRO_ENABLE)) {
+    _lastPollMs = millis();
+    _lastTiltMs = millis();
+    _wakeMs = millis();
+    LOG_INF("GYR", "QMI8658 woke up");
+    return true;
+  } else {
+    LOG_ERR("GYR", "Failed to wake QMI8658");
+    return false;
+  }
+}
+
+bool HalTiltSensor::deepSleep() {
+  if (!_available) {
+    return false;
+  }
+
+  if ((millis() - _wakeMs) < SLEEP_STABILIZE_MS) {
+    return false;
+  }
+
+  if (writeReg(REG_CTRL7, CTRL7_DISABLE_ALL) && writeReg(REG_CTRL1, CTRL1_BASE | CTRL1_SENSOR_DISABLE)) {
+    clearPendingEvents();
+    _inTilt = false;
+    LOG_INF("GYR", "QMI8658 entered sleep mode");
+    return true;
+  } else {
+    LOG_ERR("GYR", "Failed to put QMI8658 to sleep");
+    return false;
+  }
+}
+
+void HalTiltSensor::update(const uint8_t mode, const uint8_t orientation, const bool inReader) {
   if (!_available) {
     return;
   }
-  clearPendingEvents();
-  _inTilt = false;
-  _filterInitialized = false;
-  _lastKalmanMicros = 0;
-}
 
-void HalTiltSensor::clearPendingEvents() {
-  _tiltForwardEvent = false;
-  _tiltBackEvent = false;
-  _hadActivity = false;
-}
+  if ((mode != CrossPointTiltPageTurn::TILT_OFF) && !_isAwake) {
+    _isAwake = wake();
+    return;
+  } else if ((mode == CrossPointTiltPageTurn::TILT_OFF) && _isAwake) {
+    _isAwake = !deepSleep();
+    return;
+  }
 
-void HalTiltSensor::update(bool enabled, uint8_t mode, uint8_t orientation) {
-  if (!enabled || !_available) {
+  if ((mode == CrossPointTiltPageTurn::TILT_OFF) || !inReader) {
     return;
   }
 
   const unsigned long now = millis();
+  if ((now - _wakeMs) < WAKE_STABILIZE_MS) {
+    return;
+  }
+
   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;
-    }
+  float gx, gy, gz;
+  if (!readGyro(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;
+  float tiltAxis;
+  switch (orientation) {
+    case CrossPointOrientation::PORTRAIT:
+      tiltAxis = mode == CrossPointTiltPageTurn::TILT_INVERTED ? -gx : gx;
+      break;
+    case CrossPointOrientation::INVERTED:
+      tiltAxis = mode == CrossPointTiltPageTurn::TILT_INVERTED ? gx : -gx;
+      break;
+    case CrossPointOrientation::LANDSCAPE_CW:
+      tiltAxis = mode == CrossPointTiltPageTurn::TILT_INVERTED ? gy : -gy;
+      break;
+    case CrossPointOrientation::LANDSCAPE_CCW:
+      tiltAxis = mode == CrossPointTiltPageTurn::TILT_INVERTED ? -gy : gy;
+      break;
+    default:
+      tiltAxis = gx;
+      break;
   }
 
   if (_inTilt) {
-    const float neutralThreshold = isAngleMode ? NEUTRAL_THRESHOLD_DEG : NEUTRAL_THRESHOLD_G;
-    if (fabsf(tiltValue) < neutralThreshold) {
+    if (fabsf(tiltAxis) < NEUTRAL_RATE_DPS) {
       _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;
+  } else {
+    if ((now - _lastTiltMs) >= COOLDOWN_MS) {
+      if (tiltAxis > RATE_THRESHOLD_DPS) {
+        _tiltForwardEvent = true;
+        _hadActivity = true;
+        _inTilt = true;
+        _lastTiltMs = now;
+        LOG_INF("GYR", "Forward Trigger=(%.1f) dps", tiltAxis);
+      } else if (tiltAxis < -RATE_THRESHOLD_DPS) {
+        _tiltBackEvent = true;
+        _hadActivity = true;
+        _inTilt = true;
+        _lastTiltMs = now;
+        LOG_INF("GYR", "Backward Trigger=(%.1f) dps", tiltAxis);
+      }
+    }
   }
 }
 
@@ -265,3 +214,9 @@ bool HalTiltSensor::hadActivity() {
   _hadActivity = false;
   return val;
 }
+
+void HalTiltSensor::clearPendingEvents() {
+  _tiltForwardEvent = false;
+  _tiltBackEvent = false;
+  _hadActivity = false;
+}

lib/hal/HalTiltSensor.h

@@ -5,119 +5,70 @@
 
 #include "HalGPIO.h"
 
+namespace CrossPointOrientation {
+enum Value : uint8_t { PORTRAIT = 0, LANDSCAPE_CW = 1, INVERTED = 2, LANDSCAPE_CCW = 3 };
+}
+
+namespace CrossPointTiltPageTurn {
+enum Value : uint8_t { TILT_OFF = 0, TILT_NORMAL = 1, TILT_INVERTED = 2 };
+}
+
 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;
+  bool _inTilt = false;
+  bool _isAwake = false;
+  unsigned long _initMs = 0;
   unsigned long _lastTiltMs = 0;
-  unsigned long _lastKalmanMicros = 0;
+  unsigned long _wakeMs = 0;
+
+  static constexpr float RATE_THRESHOLD_DPS = 270.0f;
+  static constexpr float NEUTRAL_RATE_DPS = 50.0f;
+  static constexpr unsigned long COOLDOWN_MS = 600;
+  static constexpr unsigned long POLL_INTERVAL_MS = 50;
+  static constexpr unsigned long WAKE_STABILIZE_MS = 300;
+  static constexpr unsigned long SLEEP_STABILIZE_MS = 15;
+
   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 REG_GX_L = 0x3B;
 
-  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;
+  static constexpr uint8_t CTRL1_BIG_ENDIAN = (1 << 5);
+  static constexpr uint8_t CTRL1_AUTO_INC = (1 << 6);
+  static constexpr uint8_t CTRL1_SENSOR_DISABLE = (1 << 0);
+  static constexpr uint8_t CTRL1_BASE = CTRL1_AUTO_INC | CTRL1_BIG_ENDIAN;
 
-  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;
-    }
+  static constexpr uint8_t CTRL3_FS_512DPS = (0b101 << 4);
+  static constexpr uint8_t CTRL3_ODR_28HZ = 0b1000;
 
-    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;
+  static constexpr uint8_t CTRL7_DISABLE_ALL = 0x00;
+  static constexpr uint8_t CTRL7_GYRO_ENABLE = (1 << 1);
 
   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;
+  bool readGyro(float& gx, float& gy, float& gz) const;
 
  public:
   void begin();
-  void deepSleep();
-  void update(bool enabled, uint8_t mode, uint8_t orientation);
+  bool wake();
+  bool deepSleep();
+
+  bool isAvailable() const { return _available; }
+
+  void update(const uint8_t mode, const uint8_t orientation, const bool inReader);
 
   bool wasTiltedForward();
   bool wasTiltedBack();
   bool hadActivity();
-
-  bool isAvailable() const { return _available; }
   void clearPendingEvents();
 };