IMU visualization
This commit is contained in:
+261
-18
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user