IMU visualization
This commit is contained in:
@@ -45,10 +45,13 @@ void loadConfig() {
|
||||
}
|
||||
|
||||
void saveConfig() {
|
||||
unsigned long t0 = millis();
|
||||
InternalFS.remove(CONFIG_FILENAME);
|
||||
cfgFile.open(CONFIG_FILENAME, FILE_O_WRITE);
|
||||
if (cfgFile) { cfgFile.write((uint8_t*)&cfg, sizeof(cfg)); cfgFile.close(); Serial.println("[CFG] Saved"); }
|
||||
else { Serial.println("[CFG] ERROR: write failed"); }
|
||||
if (cfgFile) { cfgFile.write((uint8_t*)&cfg, sizeof(cfg)); cfgFile.close(); }
|
||||
unsigned long elapsed = millis() - t0;
|
||||
if (elapsed > 5) { Serial.print("[CFG] Saved ("); Serial.print(elapsed); Serial.println("ms — flash block)"); }
|
||||
else { Serial.println("[CFG] Saved"); }
|
||||
}
|
||||
|
||||
// ─── ConfigBlob push ─────────────────────────────────────────────────────────
|
||||
|
||||
@@ -1,8 +1,12 @@
|
||||
#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)
|
||||
@@ -25,8 +29,10 @@ float readIMUTemp() {
|
||||
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);
|
||||
@@ -35,6 +41,22 @@ void calibrateGyroBias() {
|
||||
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);
|
||||
|
||||
@@ -4,6 +4,11 @@
|
||||
|
||||
extern LSM6DS3 imu;
|
||||
|
||||
// Roll-compensation rotation matrix coefficients (computed in calibrateGyroBias).
|
||||
// Rotate device-frame [gz, gy] → world-frame [screenX, screenY].
|
||||
extern float rollSin; // sin of device yaw on table
|
||||
extern float rollCos; // cos of device yaw on table
|
||||
|
||||
void imuWriteReg(uint8_t reg, uint8_t val);
|
||||
uint8_t imuReadReg(uint8_t reg);
|
||||
float readIMUTemp();
|
||||
|
||||
+29
-6
@@ -15,7 +15,7 @@
|
||||
* ── Feature flag index ───────────────────────────────────────────
|
||||
* FEATURE_CONFIG_SERVICE Custom GATT service (ConfigBlob + Command)
|
||||
* FEATURE_TELEMETRY +24-byte notify characteristic, 1 Hz
|
||||
* FEATURE_IMU_STREAM +14-byte notify characteristic, ~100 Hz
|
||||
* FEATURE_IMU_STREAM +14-byte notify characteristic, ~10 Hz
|
||||
* FEATURE_TAP_DETECTION LSM6DS3 hardware tap engine → L/R clicks
|
||||
* FEATURE_TEMP_COMPENSATION Gyro drift correction by temperature delta
|
||||
* FEATURE_AUTO_RECAL Recalibrate after AUTO_RECAL_MS idle
|
||||
@@ -78,7 +78,7 @@ const unsigned long HEARTBEAT_MS = 10000;
|
||||
const int HEARTBEAT_DUR = 30;
|
||||
const unsigned long BOOT_SAFE_MS = 5000;
|
||||
#ifdef FEATURE_IMU_STREAM
|
||||
const unsigned long IMU_STREAM_RATE_MS = 50;
|
||||
const unsigned long IMU_STREAM_RATE_MS = 100;
|
||||
#endif
|
||||
const float BATT_FULL = 4.20f;
|
||||
const float BATT_EMPTY = 3.00f;
|
||||
@@ -110,8 +110,12 @@ float cachedTempC = 25.0f;
|
||||
|
||||
#ifdef FEATURE_IMU_STREAM
|
||||
bool imuStreamEnabled = false;
|
||||
uint32_t streamNotifyFails = 0; // notify() returned false (TX buffer full)
|
||||
uint32_t streamNotifyOk = 0;
|
||||
unsigned long lastStreamDiag = 0;
|
||||
#endif
|
||||
|
||||
uint32_t loopStalls = 0; // loop iterations where dt > 20ms (behind schedule)
|
||||
bool pendingCal = false;
|
||||
bool pendingReset = false;
|
||||
|
||||
@@ -189,7 +193,7 @@ void setup() {
|
||||
Bluefruit.begin(1, 0);
|
||||
Bluefruit.setTxPower(4);
|
||||
Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
|
||||
Bluefruit.Periph.setConnInterval(12, 24); // 15-30ms — less aggressive, prevents stream disconnect
|
||||
Bluefruit.Periph.setConnInterval(16, 32); // 20-40ms — wider interval reduces SoftDevice TX stalls
|
||||
|
||||
Wire1.begin(); // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
|
||||
if (imu.begin() != 0) {
|
||||
@@ -303,6 +307,7 @@ void loop() {
|
||||
float dt = (now - lastTime) / 1000.0f;
|
||||
lastTime = now;
|
||||
if (dt <= 0.0f || dt > 0.5f) return;
|
||||
if (dt > 0.020f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); }
|
||||
|
||||
cachedTempC = readIMUTemp();
|
||||
|
||||
@@ -355,8 +360,11 @@ void loop() {
|
||||
accumX = accumY = 0.0f;
|
||||
flags |= 0x01;
|
||||
} else {
|
||||
float rawX = applyAcceleration(applyCurve(-fGz * cfg.sensitivity * dt));
|
||||
float rawY = applyAcceleration(applyCurve(-fGy * cfg.sensitivity * dt));
|
||||
// World-frame gyro: rotate device axes by boot-pose yaw
|
||||
float wGz = fGz * rollCos + fGy * rollSin;
|
||||
float wGy = fGy * rollCos - fGz * rollSin;
|
||||
float rawX = applyAcceleration(applyCurve(-wGz * cfg.sensitivity * dt));
|
||||
float rawY = applyAcceleration(applyCurve(-wGy * cfg.sensitivity * dt));
|
||||
if (cfg.axisFlip & 0x01) rawX = -rawX;
|
||||
if (cfg.axisFlip & 0x02) rawY = -rawY;
|
||||
accumX += rawX; accumY += rawY;
|
||||
@@ -380,7 +388,22 @@ void loop() {
|
||||
pkt.moveY = moveY;
|
||||
pkt.flags = flags;
|
||||
pkt._pad = 0;
|
||||
cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt));
|
||||
if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) {
|
||||
streamNotifyOk++;
|
||||
} else {
|
||||
streamNotifyFails++;
|
||||
Serial.print("[STREAM] notify fail #"); Serial.print(streamNotifyFails);
|
||||
Serial.print(" ok="); Serial.println(streamNotifyOk);
|
||||
}
|
||||
|
||||
// Periodic stream health report every 10 seconds
|
||||
if (now - lastStreamDiag >= 10000) {
|
||||
lastStreamDiag = now;
|
||||
Serial.print("[STREAM] ok="); Serial.print(streamNotifyOk);
|
||||
Serial.print(" fail="); Serial.print(streamNotifyFails);
|
||||
Serial.print(" rate="); Serial.print((streamNotifyOk * 1000UL) / 10000); Serial.println("pkt/s");
|
||||
streamNotifyOk = 0; streamNotifyFails = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
Reference in New Issue
Block a user