Merge pull request #209 from jpirnay/x3-tilt
refactor: Use upstream X3 tilt mode
This commit is contained in:
@@ -318,10 +318,6 @@ 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"
|
||||
|
||||
@@ -252,10 +252,6 @@ 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"
|
||||
|
||||
@@ -243,10 +243,6 @@ STR_OK_BUTTON: "OK"
|
||||
STR_SLEEP_COVER_FILTER: "Standby-Coverfilter"
|
||||
STR_FILTER_CONTRAST: "Kontrast"
|
||||
STR_TILT_PAGE_TURN: "Seitenblättern durch Neigen"
|
||||
STR_TILT_STABILIZATION: "Neigungsmodus"
|
||||
STR_TILT_MODE_RAW: "Rohbeschleunigung"
|
||||
STR_TILT_MODE_SMOOTH: "Geglättete 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 %"
|
||||
|
||||
@@ -252,10 +252,6 @@ 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"
|
||||
|
||||
@@ -436,10 +436,6 @@ 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"
|
||||
|
||||
+126
-168
@@ -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,146 @@ 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();
|
||||
_sleepMs = 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() - _sleepMs) < 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;
|
||||
_sleepMs = millis();
|
||||
LOG_INF("GYR", "QMI8658 entered sleep mode");
|
||||
return true;
|
||||
} else {
|
||||
LOG_ERR("GYR", "Failed to put QMI8658 to sleep");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void HalTiltSensor::update(CrossPointTiltPageTurn::Value mode, CrossPointOrientation::Value orientation,
|
||||
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 && inReader) && !_isAwake) {
|
||||
_isAwake = wake();
|
||||
return;
|
||||
} else if ((mode == CrossPointTiltPageTurn::TILT_OFF || !inReader) && _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 +217,9 @@ bool HalTiltSensor::hadActivity() {
|
||||
_hadActivity = false;
|
||||
return val;
|
||||
}
|
||||
|
||||
void HalTiltSensor::clearPendingEvents() {
|
||||
_tiltForwardEvent = false;
|
||||
_tiltBackEvent = false;
|
||||
_hadActivity = false;
|
||||
}
|
||||
|
||||
+38
-86
@@ -5,119 +5,71 @@
|
||||
|
||||
#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 _sleepMs = 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_SPI_BE = (1 << 5); // SPI byte-order bit; no-op on I2C (QMI8658C)
|
||||
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_SPI_BE;
|
||||
|
||||
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(CrossPointTiltPageTurn::Value mode, CrossPointOrientation::Value orientation, bool inReader);
|
||||
|
||||
bool wasTiltedForward();
|
||||
bool wasTiltedBack();
|
||||
bool hadActivity();
|
||||
|
||||
bool isAvailable() const { return _available; }
|
||||
void clearPendingEvents();
|
||||
};
|
||||
|
||||
@@ -267,8 +267,6 @@ class CrossPointSettings {
|
||||
uint8_t imageDithering = IMAGE_DITHER_BAYER;
|
||||
// Tilt-based page turning (X3 only — requires QMI8658 IMU)
|
||||
uint8_t tiltPageTurn = 0;
|
||||
// Tilt input processing mode: 0 = raw accel, 1 = smoothed accel, 2 = gyro/accelerometer fusion
|
||||
uint8_t tiltStabilization = 0;
|
||||
// Action when the computed tilt value crosses the positive threshold.
|
||||
uint8_t tiltPositiveAction = TILT_ACT_NEXT_PAGE;
|
||||
// Action when the computed tilt value crosses the negative threshold.
|
||||
|
||||
@@ -230,11 +230,6 @@ inline const std::vector<SettingInfo> list = {
|
||||
StrId::STR_CAT_CONTROLS)
|
||||
.withSubmenu(StrId::STR_TILT_PAGE_TURN)
|
||||
.withDeviceTarget(SettingDeviceTarget::X3),
|
||||
SettingInfo::Enum(StrId::STR_TILT_STABILIZATION, &CrossPointSettings::tiltStabilization,
|
||||
{StrId::STR_TILT_MODE_RAW, StrId::STR_TILT_MODE_SMOOTH, StrId::STR_TILT_MODE_KALMAN},
|
||||
"tiltStabilization", StrId::STR_CAT_CONTROLS)
|
||||
.withSubmenu(StrId::STR_TILT_PAGE_TURN)
|
||||
.withDeviceTarget(SettingDeviceTarget::X3),
|
||||
SettingInfo::Enum(StrId::STR_DIR_RIGHT, &CrossPointSettings::tiltPositiveAction,
|
||||
{StrId::STR_NONE_OPT, StrId::STR_NEXT_PAGE, StrId::STR_PREV_PAGE}, "tiltPositiveAction",
|
||||
StrId::STR_CAT_CONTROLS)
|
||||
|
||||
+3
-1
@@ -335,7 +335,9 @@ void loop() {
|
||||
gpio.update();
|
||||
buttonEventManager.update();
|
||||
HalClock::updatePeriodic();
|
||||
halTiltSensor.update(SETTINGS.tiltPageTurn, SETTINGS.tiltStabilization, SETTINGS.orientation);
|
||||
halTiltSensor.update(static_cast<CrossPointTiltPageTurn::Value>(SETTINGS.tiltPageTurn),
|
||||
static_cast<CrossPointOrientation::Value>(SETTINGS.orientation),
|
||||
activityManager.isReaderActivity());
|
||||
|
||||
renderer.setFadingFix(SETTINGS.fadingFix);
|
||||
renderer.setTextDarkness(SETTINGS.textDarkness);
|
||||
|
||||
Reference in New Issue
Block a user