nikrozman/air-mouse
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects 2 Releases Activity

IMU visualization

Browse Source
This commit is contained in:
Nik Rozman
2026-03-01 21:35:36 +01:00
parent 9786d83ab0
commit a3b5425d0f
7 changed files with 333 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
source/main.cpp
View File

@@ -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