Let firmware handle roll correction

This commit is contained in:
2026-03-01 21:53:50 +01:00
parent 11baa814c9
commit 9e481096be

View File

@@ -464,6 +464,7 @@ function onDisconnected() {
document.getElementById('badgeCharging').classList.remove('show');
document.getElementById('badgeFull').classList.remove('show');
imuSubscribed = false; vizPaused = true; vizUpdateIndicator(); streamDiagReset();
document.getElementById('orientLabel').textContent = '— not streaming —';
document.getElementById('hashMismatchBanner').style.display = 'none';
clearTelemetry();
if (!userDisconnected && document.getElementById('autoReconnect').checked && savedDevice) {
@@ -538,47 +539,10 @@ function streamDiagPkt() {
}
}
// ── 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]];
}
// Roll compensation is done entirely in firmware (calibrateGyroBias computes
// rollSin/rollCos from boot-pose accel and applies the rotation before moveX/moveY).
// The web visualiser just uses moveX/moveY directly — no re-rotation needed here.
function resetOrient() {} // kept for call-site compatibility
function vizUpdateIndicator() {
const el = document.getElementById('vizLive');
@@ -599,9 +563,9 @@ async function vizSetPaused(paused) {
vizPaused = paused;
if (!paused && chars.imuStream && !imuSubscribed) {
try {
resetOrient();
await chars.imuStream.startNotifications();
imuSubscribed = true;
document.getElementById('orientLabel').textContent = 'roll correction active (firmware)';
log('IMU stream subscribed','ok');
} catch(e) { log(`IMU stream start failed: ${e.message}`,'err'); vizPaused = true; }
} else if (paused && imuSubscribed) {
@@ -609,6 +573,7 @@ async function vizSetPaused(paused) {
await chars.imuStream.stopNotifications();
imuSubscribed = false;
streamDiagReset();
document.getElementById('orientLabel').textContent = '— not streaming —';
} catch(e) { log(`IMU stream stop failed: ${e.message}`,'err'); }
}
vizUpdateIndicator();
@@ -643,29 +608,14 @@ function parseImuStream(dv) {
const single = !!(flags & 0x02);
const dbl = !!(flags & 0x04);
// 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);
// Axis bars: show raw gyro (firmware convention: Z→screen-X, Y→screen-Y)
updateAxisBar('gy', -gyroZ, 30000);
updateAxisBar('gz', -gyroY, 30000);
if (!idle) {
// 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));
// moveX/moveY are already roll-corrected by firmware — use them directly
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));
}
trail.push({x:cursorX, y:cursorY, t:Date.now(), idle});
if (trail.length > TRAIL_LEN) trail.shift();