151 lines
3.8 KiB
C++
151 lines
3.8 KiB
C++
/**
|
|
* Claude's Eyes - IMU Module Implementation
|
|
*
|
|
* QMI8658 6-axis IMU using SensorLib
|
|
*/
|
|
|
|
#include "imu.h"
|
|
#include "config.h"
|
|
#include <Wire.h>
|
|
#include <SensorQMI8658.hpp>
|
|
|
|
// Global instance
|
|
IMUModule IMU;
|
|
|
|
// Sensor instance
|
|
static SensorQMI8658 qmi;
|
|
|
|
IMUModule::IMUModule()
|
|
: _initialized(false)
|
|
, _lastError(nullptr)
|
|
, _lastUpdateTime(0)
|
|
{
|
|
memset(&_data, 0, sizeof(_data));
|
|
}
|
|
|
|
bool IMUModule::begin() {
|
|
#if !ENABLE_IMU
|
|
Serial.println("[IMU] Disabled in config");
|
|
return false;
|
|
#endif
|
|
|
|
// Initialize I2C (shared with touch on Waveshare board)
|
|
Wire.begin(IMU_SDA_PIN, IMU_SCL_PIN);
|
|
|
|
// Initialize QMI8658
|
|
if (!qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
|
|
// Try alternate address
|
|
if (!qmi.begin(Wire, QMI8658_H_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
|
|
_lastError = "QMI8658 not found";
|
|
Serial.println("[IMU] QMI8658 not found on I2C");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
Serial.printf("[IMU] QMI8658 found, Chip ID: 0x%02X\n", qmi.getChipID());
|
|
|
|
// Configure accelerometer
|
|
// Range: ±8g, ODR: 896.8Hz
|
|
qmi.configAccelerometer(
|
|
SensorQMI8658::ACC_RANGE_8G,
|
|
SensorQMI8658::ACC_ODR_896_8Hz,
|
|
SensorQMI8658::LPF_MODE_0
|
|
);
|
|
|
|
// Configure gyroscope
|
|
// Range: ±512dps, ODR: 896.8Hz
|
|
qmi.configGyroscope(
|
|
SensorQMI8658::GYR_RANGE_512DPS,
|
|
SensorQMI8658::GYR_ODR_896_8Hz,
|
|
SensorQMI8658::LPF_MODE_0
|
|
);
|
|
|
|
// Enable sensors
|
|
qmi.enableAccelerometer();
|
|
qmi.enableGyroscope();
|
|
|
|
_initialized = true;
|
|
Serial.println("[IMU] Initialized successfully");
|
|
|
|
// Take initial reading
|
|
update();
|
|
|
|
return true;
|
|
}
|
|
|
|
void IMUModule::update() {
|
|
if (!_initialized) return;
|
|
|
|
unsigned long now = millis();
|
|
if (now - _lastUpdateTime < UPDATE_INTERVAL) {
|
|
return;
|
|
}
|
|
|
|
float dt = (now - _lastUpdateTime) / 1000.0f;
|
|
_lastUpdateTime = now;
|
|
|
|
// Read sensor data
|
|
if (qmi.getDataReady()) {
|
|
float ax, ay, az;
|
|
float gx, gy, gz;
|
|
|
|
if (qmi.getAccelerometer(ax, ay, az)) {
|
|
_data.accel_x = ax;
|
|
_data.accel_y = ay;
|
|
_data.accel_z = az;
|
|
}
|
|
|
|
if (qmi.getGyroscope(gx, gy, gz)) {
|
|
_data.gyro_x = gx;
|
|
_data.gyro_y = gy;
|
|
_data.gyro_z = gz;
|
|
}
|
|
|
|
_data.temperature = qmi.getTemperature_C();
|
|
|
|
// Compute orientation angles
|
|
computeAngles();
|
|
|
|
// Integrate yaw from gyroscope
|
|
if (dt > 0 && dt < 1.0f) { // Sanity check
|
|
_data.yaw += _data.gyro_z * dt;
|
|
|
|
// Keep yaw in -180 to 180 range
|
|
while (_data.yaw > 180.0f) _data.yaw -= 360.0f;
|
|
while (_data.yaw < -180.0f) _data.yaw += 360.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
void IMUModule::computeAngles() {
|
|
// Calculate pitch and roll from accelerometer
|
|
// Assumes sensor is mounted level when robot is flat
|
|
|
|
// Pitch: rotation around X axis
|
|
// tan(pitch) = -ax / sqrt(ay² + az²)
|
|
_data.pitch = atan2(-_data.accel_x,
|
|
sqrt(_data.accel_y * _data.accel_y +
|
|
_data.accel_z * _data.accel_z)) * 180.0f / PI;
|
|
|
|
// Roll: rotation around Y axis
|
|
// tan(roll) = ay / az
|
|
_data.roll = atan2(_data.accel_y, _data.accel_z) * 180.0f / PI;
|
|
}
|
|
|
|
bool IMUModule::isTilted() {
|
|
// Check if tilted more than 30 degrees in any direction
|
|
const float TILT_THRESHOLD = 30.0f;
|
|
return (abs(_data.pitch) > TILT_THRESHOLD ||
|
|
abs(_data.roll) > TILT_THRESHOLD);
|
|
}
|
|
|
|
bool IMUModule::isUpsideDown() {
|
|
// Z-axis should be positive (pointing up) when right-side up
|
|
// If Z is negative, robot is flipped
|
|
return _data.accel_z < -5.0f; // Threshold accounts for noise
|
|
}
|
|
|
|
void IMUModule::resetYaw() {
|
|
_data.yaw = 0;
|
|
}
|