IMU visualization

2026-03-01 21:35:36 +01:00
parent 9786d83ab0
commit a3b5425d0f
7 changed files with 333 additions and 28 deletions
--- a/source/ble_config.cpp
+++ b/source/ble_config.cpp
@@ -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 ─────────────────────────────────────────────────────────
--- a/source/imu.cpp
+++ b/source/imu.cpp
@@ -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);
--- a/source/imu.h
+++ b/source/imu.h
@@ -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();
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -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

--- a/web/app.js
+++ b/web/app.js
@@ -16,16 +16,40 @@ let device=null, server=null, chars={}, userDisconnected=false;
 let currentChargeStatus=0, currentBattPct=null;

 // ── GATT write queue (prevents "operation already in progress") ───────────────
+// Serialises all GATT writes. Features:
+//   • Per-operation 3s timeout — hangs don't block the queue forever
+//   • Max depth of 2 pending ops — drops excess writes when device goes silent
+//   • gattQueueReset() flushes on disconnect so a reconnect starts clean
+const GATT_TIMEOUT_MS = 3000;
+const GATT_MAX_DEPTH  = 2;
 let _gattQueue = Promise.resolve();
-function gattWrite(char, value) {
-  const p = _gattQueue.then(() => char.writeValueWithResponse(value));
-  _gattQueue = p.catch(() => {});
+let _gattDepth = 0;
+
+function _withTimeout(promise, ms) {
+  return new Promise((resolve, reject) => {
+    const t = setTimeout(() => reject(new Error(`GATT timeout (${ms}ms)`)), ms);
+    promise.then(v => { clearTimeout(t); resolve(v); },
+                 e => { clearTimeout(t); reject(e); });
+  });
+}
+
+function _enqueue(fn) {
+  if (_gattDepth >= GATT_MAX_DEPTH) {
+    return Promise.reject(new Error('GATT queue full — device unreachable?'));
+  }
+  _gattDepth++;
+  const p = _gattQueue.then(() => _withTimeout(fn(), GATT_TIMEOUT_MS));
+  _gattQueue = p.catch(() => {}).finally(() => { _gattDepth = Math.max(0, _gattDepth - 1); });
  return p;
 }
-function gattCmd(char, value) {
-  const p = _gattQueue.then(() => char.writeValueWithoutResponse(value));
-  _gattQueue = p.catch(() => {});
-  return p;
+
+function gattWrite(char, value) { return _enqueue(() => char.writeValueWithResponse(value)); }
+function gattCmd (char, value)  { return _enqueue(() => char.writeValueWithoutResponse(value)); }
+
+function gattQueueReset() {
+  // Drain the chain so a reconnect starts with a fresh resolved promise
+  _gattQueue = Promise.resolve();
+  _gattDepth = 0;
 }

 // ── Logging ──────────────────────────────────────────────────────────────────
@@ -388,12 +412,13 @@ function setStatus(state) {
 function onDisconnected() {
  log('Device disconnected','warn');
  const savedDevice = device;
+  gattQueueReset();
  chars={}; device=null; server=null;
  setStatus('disconnected');
  document.getElementById('battBar').style.display='none';
  document.getElementById('badgeCharging').classList.remove('show');
  document.getElementById('badgeFull').classList.remove('show');
-  imuSubscribed = false; vizPaused = true; vizUpdateIndicator();
+  imuSubscribed = false; vizPaused = true; vizUpdateIndicator(); streamDiagReset();
  clearTelemetry();
  if (!userDisconnected && document.getElementById('autoReconnect').checked && savedDevice) {
    log('Auto-reconnecting…','info');
@@ -426,6 +451,89 @@ let cursorX = canvas.width/2, cursorY = canvas.height/2, trail = [];
 let vizPaused = true;
 let imuSubscribed = false;

+// ── Stream diagnostics ────────────────────────────────────────────────────────
+let streamPktCount  = 0;      // packets received this second
+let streamPktTotal  = 0;      // lifetime packet count
+let streamLastPktT  = 0;      // timestamp of last packet (for gap detection)
+let streamLastRateT = 0;      // timestamp of last rate log
+let streamFreezeTimer = null; // fires if no packet for >1s while subscribed
+
+function streamDiagReset() {
+  streamPktCount = streamPktTotal = streamLastPktT = streamLastRateT = 0;
+  if (streamFreezeTimer) { clearTimeout(streamFreezeTimer); streamFreezeTimer = null; }
+}
+
+function streamDiagPkt() {
+  const now = Date.now();
+
+  // Gap detection — warn if >300ms since last packet while streaming
+  if (streamLastPktT) {
+    const gap = now - streamLastPktT;
+    if (gap > 300) log(`[STREAM] gap ${gap}ms (pkt #${streamPktTotal})`, 'warn');
+  }
+  streamLastPktT = now;
+  streamPktCount++;
+  streamPktTotal++;
+
+  // Reset freeze watchdog — 1.5s without a packet = freeze
+  if (streamFreezeTimer) clearTimeout(streamFreezeTimer);
+  streamFreezeTimer = setTimeout(() => {
+    log(`[STREAM] FROZEN — no packet for 1.5s (total rx: ${streamPktTotal})`, 'err');
+    streamFreezeTimer = null;
+  }, 1500);
+
+  // Log rate every 5s
+  if (streamLastRateT === 0) streamLastRateT = now;
+  if (now - streamLastRateT >= 5000) {
+    const rate = (streamPktCount / ((now - streamLastRateT) / 1000)).toFixed(1);
+    log(`[STREAM] ${rate} pkt/s · total ${streamPktTotal}`, 'info');
+    streamPktCount = 0;
+    streamLastRateT = now;
+  }
+}
+
+// ── Orientation (boot-pose tilt compensation) ─────────────────────────────────
+// We average the first N accel frames to determine which way gravity points,
+// then build a 2×2 rotation matrix so that "up on screen" stays world-up
+// regardless of how the device is rotated flat on the table.
+const ORIENT_SAMPLES = 30;
+let orientSamples = 0;
+let orientAccum   = [0, 0, 0];
+// 2×2 basis: [row0=[gY→screenX coeff, gZ→screenX coeff],
+//             row1=[gY→screenY coeff, gZ→screenY coeff]]
+// Identity by default (no rotation assumed until samples collected)
+let orientBasis = [[1, 0], [0, 1]];
+
+function buildOrientBasis() {
+  // Average accel vector = gravity direction in device frame
+  const gx = orientAccum[0] / ORIENT_SAMPLES;
+  const gy = orientAccum[1] / ORIENT_SAMPLES;
+  // gz unused — device assumed roughly flat (|gz| ≈ 1g, gx/gy ≈ 0 when flat)
+  // The horizontal tilt (yaw rotation of the device on the table) is the angle
+  // between device-X axis and world horizontal, derived from gx/gy.
+  // Firmware uses gyroZ for screen-X and gyroY for screen-Y.
+  // A device rotated θ clockwise on the table needs gyros counter-rotated by θ.
+  const norm = Math.sqrt(gx*gx + gy*gy) || 1;
+  const sinT =  gx / norm;   // sin of table-yaw angle
+  const cosT = -gy / norm;   // cos of table-yaw angle (negative Y = gravity down)
+  // Rotation matrix for gyro [gyroY, gyroZ] → [screenX, screenY]:
+  // Normally: screenX = -gyroZ, screenY = -gyroY (firmware convention reflected)
+  // With tilt θ: apply 2D rotation by θ to that vector.
+  orientBasis = [
+    [-sinT,  -cosT],  // screenX row
+    [-cosT,   sinT],  // screenY row
+  ];
+  const deg = (Math.atan2(gx, -gy) * 180 / Math.PI).toFixed(1);
+  log(`Orient locked — device yaw ≈ ${deg}° from nominal`, 'info');
+  document.getElementById('orientLabel').textContent = `yaw offset ${deg}° · complementary filter active`;
+}
+
+function resetOrient() {
+  orientSamples = 0;
+  orientAccum   = [0, 0, 0];
+  orientBasis   = [[1, 0], [0, 1]];
+}
+
 function vizUpdateIndicator() {
  const el = document.getElementById('vizLive');
  if (!imuSubscribed || vizPaused) {
@@ -445,6 +553,7 @@ async function vizSetPaused(paused) {
  vizPaused = paused;
  if (!paused && chars.imuStream && !imuSubscribed) {
    try {
+      resetOrient();
      await chars.imuStream.startNotifications();
      imuSubscribed = true;
      log('IMU stream subscribed','ok');
@@ -453,6 +562,7 @@ async function vizSetPaused(paused) {
    try {
      await chars.imuStream.stopNotifications();
      imuSubscribed = false;
+      streamDiagReset();
    } catch(e) { log(`IMU stream stop failed: ${e.message}`,'err'); }
  }
  vizUpdateIndicator();
@@ -472,31 +582,55 @@ function parseImuStream(dv) {
    return;
  }

-  let gyroY, gyroZ, moveX, moveY, flags;
+  let gyroY, gyroZ, accelX, accelY, accelZ, moveX, moveY, flags;
  try {
-    gyroY = view.getInt16(0, true);
-    gyroZ = view.getInt16(2, true);
-    moveX = view.getInt8(10);
-    moveY = view.getInt8(11);
-    flags = view.getUint8(12);
+    gyroY  = view.getInt16(0, true);
+    gyroZ  = view.getInt16(2, true);
+    accelX = view.getInt16(4, true);
+    accelY = view.getInt16(6, true);
+    accelZ = view.getInt16(8, true);
+    moveX  = view.getInt8(10);
+    moveY  = view.getInt8(11);
+    flags  = view.getUint8(12);
  } catch(e) { log(`parseImuStream: parse error — ${e.message}`,'err'); return; }
  const idle   = !!(flags & 0x01);
  const single = !!(flags & 0x02);
  const dbl    = !!(flags & 0x04);

-  updateAxisBar('gy', gyroY, 30000);
-  updateAxisBar('gz', gyroZ, 30000);
+  // Accumulate boot-pose from first ORIENT_SAMPLES accel frames (device flat on table)
+  if (orientSamples < ORIENT_SAMPLES) {
+    orientAccum[0] += accelX;
+    orientAccum[1] += accelY;
+    orientAccum[2] += accelZ;
+    orientSamples++;
+    if (orientSamples === ORIENT_SAMPLES) buildOrientBasis();
+  }
+
+  // Rotate gyro into world frame using boot-pose basis
+  // gyroY → pitch axis, gyroZ → yaw axis (firmware convention)
+  const wX = gyroY * orientBasis[0][0] + gyroZ * orientBasis[0][1];
+  const wY = gyroY * orientBasis[1][0] + gyroZ * orientBasis[1][1];
+
+  updateAxisBar('gy', wX, 30000);
+  updateAxisBar('gz', wY, 30000);

  if (!idle) {
-    cursorX = Math.max(4, Math.min(canvas.width  - 4, cursorX + moveX * 1.5));
-    cursorY = Math.max(4, Math.min(canvas.height - 4, cursorY + moveY * 1.5));
+    // moveX/moveY already come corrected from firmware; apply same world rotation
+    const wmX = moveX * orientBasis[0][0] + moveY * orientBasis[1][0];
+    const wmY = moveX * orientBasis[0][1] + moveY * orientBasis[1][1];
+    cursorX = Math.max(4, Math.min(canvas.width  - 4, cursorX + wmX * 1.5));
+    cursorY = Math.max(4, Math.min(canvas.height - 4, cursorY + wmY * 1.5));
  }
  trail.push({x:cursorX, y:cursorY, t:Date.now(), idle});
  if (trail.length > TRAIL_LEN) trail.shift();

+  streamDiagPkt();
+
  if (single) flashTap('Left');
  if (dbl)    flashTap('Right');
  drawViz(idle);
+
+  orientFeedIMU(accelX, accelY, accelZ, gyroY, gyroZ);
 }

 function updateAxisBar(axis, val, max) {
@@ -551,6 +685,113 @@ function drawInitState() {
  ctx.fillStyle=cssVar('--canvas-idle-text');ctx.font='10px Share Tech Mono,monospace';
  ctx.textAlign='center';ctx.fillText('connect to activate stream',W/2,H/2+4);ctx.textAlign='left';
 }
+// ── 3D Orientation Viewer ─────────────────────────────────────────────────────
+// Device box: L=115mm (X), W=36mm (Y), H=20mm (Z)
+// Complementary filter mirrors firmware: α=0.96, dt from packet rate (~50ms)
+const ORIENT_ALPHA = 0.96;
+const DEVICE_L = 1.15, DEVICE_W = 0.36, DEVICE_H = 0.20;  // metres (Three.js units)
+
+let orientScene, orientCamera, orientRenderer, orientMesh, orientEdges;
+let orientQ = new THREE.Quaternion();   // current estimated orientation
+let orientLastT = 0;
+
+function initOrientViewer() {
+  const el = document.getElementById('orientCanvas');
+  const W = el.clientWidth || 340, H = 160;
+  el.width = W; el.height = H;
+
+  orientScene    = new THREE.Scene();
+  orientCamera   = new THREE.PerspectiveCamera(40, W / H, 0.01, 10);
+  orientCamera.position.set(0.6, 0.5, 0.9);
+  orientCamera.lookAt(0, 0, 0);
+
+  orientRenderer = new THREE.WebGLRenderer({ canvas: el, antialias: true, alpha: true });
+  orientRenderer.setSize(W, H);
+  orientRenderer.setClearColor(0x000000, 0);
+
+  // Box geometry
+  const geo  = new THREE.BoxGeometry(DEVICE_L, DEVICE_H, DEVICE_W);
+  const mat  = new THREE.MeshPhongMaterial({
+    color: 0x1a2230, emissive: 0x050a10, specular: 0x00e5ff,
+    shininess: 60, transparent: true, opacity: 0.85,
+  });
+  orientMesh = new THREE.Mesh(geo, mat);
+  orientScene.add(orientMesh);
+
+  // Wireframe edges
+  const edgeMat = new THREE.LineBasicMaterial({ color: 0x00e5ff, linewidth: 1 });
+  orientEdges = new THREE.LineSegments(new THREE.EdgesGeometry(geo), edgeMat);
+  orientMesh.add(orientEdges);
+
+  // "Front" face marker — small arrow along +X (length axis)
+  const arrowGeo = new THREE.ConeGeometry(0.02, 0.07, 6);
+  arrowGeo.rotateZ(-Math.PI / 2);
+  arrowGeo.translate(DEVICE_L / 2 + 0.04, 0, 0);
+  const arrowMesh = new THREE.Mesh(arrowGeo,
+    new THREE.MeshBasicMaterial({ color: 0x00e5ff }));
+  orientMesh.add(arrowMesh);
+
+  // Lighting
+  orientScene.add(new THREE.AmbientLight(0xffffff, 0.4));
+  const dlight = new THREE.DirectionalLight(0xffffff, 0.9);
+  dlight.position.set(1, 2, 2);
+  orientScene.add(dlight);
+
+  orientRenderer.render(orientScene, orientCamera);
+}
+
+function orientUpdateColors() {
+  // Re-read CSS variables so it adapts to theme changes
+  const accent = cssVar('--accent').replace('#','');
+  const c = parseInt(accent, 16);
+  if (orientEdges) orientEdges.material.color.setHex(c);
+}
+
+function orientFeedIMU(ax, ay, az, gyY_mDPS, gyZ_mDPS) {
+  if (!orientRenderer) return;
+  const now = Date.now();
+  const dt  = orientLastT ? Math.min((now - orientLastT) / 1000, 0.1) : 0.05;
+  orientLastT = now;
+
+  // Accel in g (packet is in mg)
+  const axG = ax / 1000, ayG = ay / 1000, azG = az / 1000;
+  const aNorm = Math.sqrt(axG*axG + ayG*ayG + azG*azG);
+
+  // Accel-derived quaternion (gravity reference). LSM6DS3 axes:
+  // device flat, face up: az ≈ +1g
+  // Pitch (tilt front up): ay changes; Roll (tilt right side up): ax changes
+  let qAccel = new THREE.Quaternion();
+  if (aNorm > 0.5 && aNorm < 2.0) {
+    // gravity unit vector in device frame
+    const gx = axG / aNorm, gy = ayG / aNorm, gz = azG / aNorm;
+    // Align device -Z (down face) with gravity
+    const up = new THREE.Vector3(0, 1, 0);          // Three.js world up
+    const gVec = new THREE.Vector3(-gx, -gz, gy);   // map device→Three axes
+    gVec.normalize();
+    qAccel.setFromUnitVectors(gVec, up);
+  } else {
+    qAccel.copy(orientQ);
+  }
+
+  // Gyro integration — firmware sends gyroY (pitch) and gyroZ (yaw), mDPS
+  // Map to Three.js axes: gyroZ→world Y, gyroY→world X
+  const gyRad = gyY_mDPS * (Math.PI / 180) / 1000;
+  const gzRad = gyZ_mDPS * (Math.PI / 180) / 1000;
+  const dq = new THREE.Quaternion(
+    gyRad * dt * 0.5,   // x
+    -gzRad * dt * 0.5,  // y
+    0, 1
+  ).normalize();
+  const qGyro = orientQ.clone().multiply(dq);
+
+  // Complementary filter
+  orientQ.copy(qGyro).slerp(qAccel, 1 - ORIENT_ALPHA);
+  orientQ.normalize();
+
+  orientMesh.quaternion.copy(orientQ);
+  orientRenderer.render(orientScene, orientCamera);
+}
+
 // ── Theme ─────────────────────────────────────────────────────────────────────
 const THEMES = ['auto','dark','light'];
 const THEME_LABELS = {auto:'AUTO',dark:'DARK',light:'LIGHT'};
@@ -567,11 +808,13 @@ function applyTheme(t) {
  document.getElementById('themeBtn').querySelector('span').textContent = THEME_LABELS[t];
  localStorage.setItem('theme', t);
  if (!chars.imuStream) drawInitState();
+  orientUpdateColors();
 }
 (function(){
  const saved = localStorage.getItem('theme') ?? 'auto';
  themeIdx = Math.max(0, THEMES.indexOf(saved));
  applyTheme(saved);
+  initOrientViewer();
 })();

 if (!navigator.bluetooth) {
--- a/web/index.html
+++ b/web/index.html
@@ -6,6 +6,7 @@
 <title>IMU Mouse // Config Terminal</title>
 <link href="https://fonts.googleapis.com/css2?family=Share+Tech+Mono&family=Barlow+Condensed:wght@300;400;600;700;900&display=swap" rel="stylesheet">
 <link rel="stylesheet" href="style.css">
+<script src="https://cdn.jsdelivr.net/npm/three@0.128.0/build/three.min.js"></script>
 </head>
 <body class="disconnected">

@@ -171,11 +172,11 @@
      </div>
      <div class="viz-axes">
        <div class="axis-bar-wrap">
-          <div class="axis-bar-label"><span>GY (up/down)</span><span id="gyVal">0</span></div>
+          <div class="axis-bar-label"><span>X (left/right)</span><span id="gyVal">0</span></div>
          <div class="axis-bar-track"><div class="axis-bar-fill" id="gyBar"></div><div class="axis-bar-center"></div></div>
        </div>
        <div class="axis-bar-wrap">
-          <div class="axis-bar-label"><span>GZ (left/right)</span><span id="gzVal">0</span></div>
+          <div class="axis-bar-label"><span>Y (up/down)</span><span id="gzVal">0</span></div>
          <div class="axis-bar-track"><div class="axis-bar-fill" id="gzBar"></div><div class="axis-bar-center"></div></div>
        </div>
      </div>
@@ -184,6 +185,12 @@
      </div>
    </div>

+    <div class="section-label">Device Orientation</div>
+    <div class="card orient-card">
+      <canvas id="orientCanvas"></canvas>
+      <div style="font-size:9px;color:var(--label);text-align:center;margin-top:6px" id="orientLabel">— not streaming —</div>
+    </div>
+
    <div class="section-label">Live Telemetry</div>
    <div class="telem-grid">
      <div class="telem-cell"><div class="telem-val accent" id="telTemp">--</div><div class="telem-lbl">Temperature °C</div></div>
--- a/web/style.css
+++ b/web/style.css
@@ -222,6 +222,8 @@
  .viz-panel { background:var(--panel2); border:1px solid var(--border); padding:16px; }
  .viz-header { display:flex; justify-content:space-between; align-items:center; margin-bottom:12px; }
  .viz-title { font-family:var(--sans); font-size:11px; font-weight:600; letter-spacing:0.25em; text-transform:uppercase; color:var(--label); }
+  .orient-card { padding:12px; display:flex; flex-direction:column; align-items:center; }
+  #orientCanvas { display:block; width:100%; height:160px; }
  .viz-ctrl-btn { background:none; border:1px solid var(--border); color:var(--label); font-size:11px; line-height:1; padding:3px 8px; cursor:pointer; letter-spacing:0.05em; }
  .viz-ctrl-btn:hover { border-color:var(--accent); color:var(--accent); }
  .viz-live { font-size:9px; letter-spacing:0.2em; display:block; }