64 lines
2.9 KiB
C++
64 lines
2.9 KiB
C++
#include "imu.h"
|
|
#include "Wire.h"
|
|
#include <math.h>
|
|
|
|
LSM6DS3 imu(I2C_MODE, 0x6A);
|
|
|
|
// ─── 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;
|
|
for (int i=0; i<BIAS_SAMPLES; i++) {
|
|
sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
|
|
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;
|
|
// Seed gravity estimate from current accel so projection is correct immediately
|
|
gravX = imu.readFloatAccelX(); gravY = imu.readFloatAccelY(); gravZ = imu.readFloatAccelZ();
|
|
|
|
#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); }
|