diff --git a/lib/hal/HalTiltSensor.cpp b/lib/hal/HalTiltSensor.cpp index 09214d2c..211fde9e 100644 --- a/lib/hal/HalTiltSensor.cpp +++ b/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(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(2), static_cast(true)) < 2) { - return false; - } - const uint8_t lo = Wire.read(); - const uint8_t hi = Wire.read(); - *val = static_cast((static_cast(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((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(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; +} diff --git a/lib/hal/HalTiltSensor.h b/lib/hal/HalTiltSensor.h index a5cfa45c..d0e69c02 100644 --- a/lib/hal/HalTiltSensor.h +++ b/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(); }; diff --git a/src/main.cpp b/src/main.cpp index 5be9d546..d1be51cb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -335,7 +335,7 @@ void loop() { gpio.update(); buttonEventManager.update(); HalClock::updatePeriodic(); - halTiltSensor.update(SETTINGS.tiltPageTurn, SETTINGS.tiltStabilization, SETTINGS.orientation); + halTiltSensor.update(SETTINGS.tiltPageTurn, SETTINGS.orientation, activityManager.isReaderActivity()); renderer.setFadingFix(SETTINGS.fadingFix); renderer.setTextDarkness(SETTINGS.textDarkness);