Let firmware handle roll correction
This commit is contained in:
76
web/app.js
76
web/app.js
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user