124 lines
3.6 KiB
C++
124 lines
3.6 KiB
C++
#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();
|
|
};
|