83 lines
3.7 KiB
C++
83 lines
3.7 KiB
C++
#include "imu.h"
|
|
#include "Wire.h"
|
|
#include <math.h>
|
|
|
|
LSM6DS3 imu(I2C_MODE, 0x6A);
|
|
|
|
float rollSin = 0.0f; // identity: no rotation
|
|
float rollCos = 1.0f;
|
|
|
|
// ─── I2C helpers ──────────────────────────────────────────────────────────────
|
|
void imuWriteReg(uint8_t reg, uint8_t val) {
|
|
// LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5)
|
|
Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.write(val); Wire1.endTransmission();
|
|
}
|
|
|
|
uint8_t imuReadReg(uint8_t reg) {
|
|
Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.endTransmission(false);
|
|
Wire1.requestFrom((uint8_t)0x6A, (uint8_t)1);
|
|
return Wire1.available() ? Wire1.read() : 0;
|
|
}
|
|
|
|
// ─── Temperature ──────────────────────────────────────────────────────────────
|
|
float readIMUTemp() {
|
|
int16_t raw = (int16_t)((imuReadReg(REG_OUT_TEMP_H) << 8) | imuReadReg(REG_OUT_TEMP_L));
|
|
return 25.0f + (float)raw / 256.0f;
|
|
}
|
|
|
|
// ─── Calibration ──────────────────────────────────────────────────────────────
|
|
void calibrateGyroBias() {
|
|
Serial.println("[CAL] Hold still...");
|
|
double sx=0, sy=0, sz=0;
|
|
double sax=0, say=0;
|
|
for (int i=0; i<BIAS_SAMPLES; i++) {
|
|
sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
|
|
sax += imu.readFloatAccelX(); say += imu.readFloatAccelY();
|
|
digitalWrite(LED_GREEN, (i%20 < 10)); delay(5); // green flutter during calibration
|
|
}
|
|
biasGX = (float)(sx/BIAS_SAMPLES);
|
|
biasGY = (float)(sy/BIAS_SAMPLES);
|
|
biasGZ = (float)(sz/BIAS_SAMPLES);
|
|
calTempC = readIMUTemp();
|
|
angleX = angleY = accumX = accumY = 0.0f;
|
|
|
|
// Roll compensation: compute device yaw on the table from gravity's horizontal components.
|
|
// ax/ay are small when flat; their ratio gives the rotation angle θ.
|
|
// Firmware maps: screenX ← -gz, screenY ← -gy.
|
|
// With device rotated θ CW: screenX ← -(gz·cosθ + gy·sinθ), screenY ← -(gy·cosθ - gz·sinθ).
|
|
float ax_avg = (float)(sax / BIAS_SAMPLES);
|
|
float ay_avg = (float)(say / BIAS_SAMPLES);
|
|
float norm = sqrtf(ax_avg*ax_avg + ay_avg*ay_avg);
|
|
if (norm > 0.05f) { // only update if we can see meaningful tilt (>~3°)
|
|
rollSin = ax_avg / norm;
|
|
rollCos = -ay_avg / norm; // negative: gravity pulls in -Y when flat and nominal
|
|
} else {
|
|
rollSin = 0.0f;
|
|
rollCos = 1.0f;
|
|
}
|
|
Serial.print("[CAL] roll="); Serial.print(atan2f(rollSin, rollCos)*(180.f/PI), 1); Serial.println("deg");
|
|
|
|
#ifdef FEATURE_TELEMETRY
|
|
statRecalCount++;
|
|
float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
|
|
statBiasRms = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
|
|
#endif
|
|
|
|
digitalWrite(LED_GREEN, HIGH); // off after calibration
|
|
Serial.print("[CAL] T="); Serial.print(calTempC,1);
|
|
Serial.print("C bias="); Serial.print(biasGX,4);
|
|
Serial.print(","); Serial.print(biasGY,4);
|
|
Serial.print(","); Serial.println(biasGZ,4);
|
|
}
|
|
|
|
// ─── Motion curve ─────────────────────────────────────────────────────────────
|
|
float applyCurve(float v) {
|
|
switch (cfg.curve) {
|
|
case CURVE_SQUARE: return (v >= 0 ? 1.f : -1.f) * v * v;
|
|
case CURVE_SQRT: return (v >= 0 ? 1.f : -1.f) * sqrtf(fabsf(v));
|
|
default: return v;
|
|
}
|
|
}
|
|
|
|
float applyAcceleration(float d) { return d * (1.0f + fabsf(d) * cfg.accelStrength); }
|