Let firmware handle roll correction
This commit is contained in:
+13
-63
@@ -464,6 +464,7 @@ function onDisconnected() {
|
|||||||
document.getElementById('badgeCharging').classList.remove('show');
|
document.getElementById('badgeCharging').classList.remove('show');
|
||||||
document.getElementById('badgeFull').classList.remove('show');
|
document.getElementById('badgeFull').classList.remove('show');
|
||||||
imuSubscribed = false; vizPaused = true; vizUpdateIndicator(); streamDiagReset();
|
imuSubscribed = false; vizPaused = true; vizUpdateIndicator(); streamDiagReset();
|
||||||
|
document.getElementById('orientLabel').textContent = '— not streaming —';
|
||||||
document.getElementById('hashMismatchBanner').style.display = 'none';
|
document.getElementById('hashMismatchBanner').style.display = 'none';
|
||||||
clearTelemetry();
|
clearTelemetry();
|
||||||
if (!userDisconnected && document.getElementById('autoReconnect').checked && savedDevice) {
|
if (!userDisconnected && document.getElementById('autoReconnect').checked && savedDevice) {
|
||||||
@@ -538,47 +539,10 @@ function streamDiagPkt() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// ── Orientation (boot-pose tilt compensation) ─────────────────────────────────
|
// Roll compensation is done entirely in firmware (calibrateGyroBias computes
|
||||||
// We average the first N accel frames to determine which way gravity points,
|
// rollSin/rollCos from boot-pose accel and applies the rotation before moveX/moveY).
|
||||||
// then build a 2×2 rotation matrix so that "up on screen" stays world-up
|
// The web visualiser just uses moveX/moveY directly — no re-rotation needed here.
|
||||||
// regardless of how the device is rotated flat on the table.
|
function resetOrient() {} // kept for call-site compatibility
|
||||||
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() {
|
function vizUpdateIndicator() {
|
||||||
const el = document.getElementById('vizLive');
|
const el = document.getElementById('vizLive');
|
||||||
@@ -599,9 +563,9 @@ async function vizSetPaused(paused) {
|
|||||||
vizPaused = paused;
|
vizPaused = paused;
|
||||||
if (!paused && chars.imuStream && !imuSubscribed) {
|
if (!paused && chars.imuStream && !imuSubscribed) {
|
||||||
try {
|
try {
|
||||||
resetOrient();
|
|
||||||
await chars.imuStream.startNotifications();
|
await chars.imuStream.startNotifications();
|
||||||
imuSubscribed = true;
|
imuSubscribed = true;
|
||||||
|
document.getElementById('orientLabel').textContent = 'roll correction active (firmware)';
|
||||||
log('IMU stream subscribed','ok');
|
log('IMU stream subscribed','ok');
|
||||||
} catch(e) { log(`IMU stream start failed: ${e.message}`,'err'); vizPaused = true; }
|
} catch(e) { log(`IMU stream start failed: ${e.message}`,'err'); vizPaused = true; }
|
||||||
} else if (paused && imuSubscribed) {
|
} else if (paused && imuSubscribed) {
|
||||||
@@ -609,6 +573,7 @@ async function vizSetPaused(paused) {
|
|||||||
await chars.imuStream.stopNotifications();
|
await chars.imuStream.stopNotifications();
|
||||||
imuSubscribed = false;
|
imuSubscribed = false;
|
||||||
streamDiagReset();
|
streamDiagReset();
|
||||||
|
document.getElementById('orientLabel').textContent = '— not streaming —';
|
||||||
} catch(e) { log(`IMU stream stop failed: ${e.message}`,'err'); }
|
} catch(e) { log(`IMU stream stop failed: ${e.message}`,'err'); }
|
||||||
}
|
}
|
||||||
vizUpdateIndicator();
|
vizUpdateIndicator();
|
||||||
@@ -643,29 +608,14 @@ function parseImuStream(dv) {
|
|||||||
const single = !!(flags & 0x02);
|
const single = !!(flags & 0x02);
|
||||||
const dbl = !!(flags & 0x04);
|
const dbl = !!(flags & 0x04);
|
||||||
|
|
||||||
// Accumulate boot-pose from first ORIENT_SAMPLES accel frames (device flat on table)
|
// Axis bars: show raw gyro (firmware convention: Z→screen-X, Y→screen-Y)
|
||||||
if (orientSamples < ORIENT_SAMPLES) {
|
updateAxisBar('gy', -gyroZ, 30000);
|
||||||
orientAccum[0] += accelX;
|
updateAxisBar('gz', -gyroY, 30000);
|
||||||
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) {
|
if (!idle) {
|
||||||
// moveX/moveY already come corrected from firmware; apply same world rotation
|
// moveX/moveY are already roll-corrected by firmware — use them directly
|
||||||
const wmX = moveX * orientBasis[0][0] + moveY * orientBasis[1][0];
|
cursorX = Math.max(4, Math.min(canvas.width - 4, cursorX + moveX * 1.5));
|
||||||
const wmY = moveX * orientBasis[0][1] + moveY * orientBasis[1][1];
|
cursorY = Math.max(4, Math.min(canvas.height - 4, cursorY + moveY * 1.5));
|
||||||
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});
|
trail.push({x:cursorX, y:cursorY, t:Date.now(), idle});
|
||||||
if (trail.length > TRAIL_LEN) trail.shift();
|
if (trail.length > TRAIL_LEN) trail.shift();
|
||||||
|
|||||||
Reference in New Issue
Block a user