Use upstream
This commit is contained in:
jpirnay
+37
-86
@@ -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();
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user