forked from Blendman974/faceclaw
87 lines
4.4 KiB
TypeScript
87 lines
4.4 KiB
TypeScript
import { headUpAngleSetting } from "../ui/dashboard-settings";
|
|
import type { ImuReading } from "../native/imu-types";
|
|
declare const com: any;
|
|
|
|
/** Experimental inclination estimate inspired by https://github.com/jimrandomh/faceclaw/pull/64; not a calibrated sensor-axis pitch. */
|
|
export function estimateHeadInclination(samples: ImuReading[]): number | null {
|
|
if (samples.length < 3) return null;
|
|
const mean = samples.reduce((v, s) => [v[0]! + s.x, v[1]! + s.y, v[2]! + s.z], [0, 0, 0]).map(v => v / samples.length);
|
|
const magnitude = Math.hypot(...mean);
|
|
if (!Number.isFinite(magnitude) || magnitude < 1e-6) return null;
|
|
// Reject motion instead of persisting a passing acceleration as an angle.
|
|
for (const s of samples) {
|
|
const length = Math.hypot(s.x, s.y, s.z);
|
|
const dot = (s.x * mean[0]! + s.y * mean[1]! + s.z * mean[2]!) / (length * magnitude);
|
|
if (!Number.isFinite(dot) || Math.acos(Math.max(-1, Math.min(1, dot))) * 180 / Math.PI > 5) return null;
|
|
}
|
|
return Math.acos(Math.min(1, Math.max(...mean.map(Math.abs)) / magnitude)) * 180 / Math.PI;
|
|
}
|
|
|
|
export type HeadAngleDevice = {
|
|
beginHeadAngleMeasurement(): boolean;
|
|
endHeadAngleMeasurement(): void;
|
|
getHeadUpCalibration(): number;
|
|
subscribe(listener: (reading: ImuReading) => void): () => void;
|
|
};
|
|
|
|
let measuring = false;
|
|
export async function setAngleFromCurrentHead(device: HeadAngleDevice | null = androidDevice()): Promise<string> {
|
|
if (measuring) return "Measurement already running";
|
|
if (!device) return "Connect the glasses first";
|
|
measuring = true;
|
|
let release = () => {};
|
|
let lease = false;
|
|
try {
|
|
if (!device.beginHeadAngleMeasurement()) return "Glasses unavailable or measurement busy";
|
|
lease = true;
|
|
const samples: ImuReading[] = [];
|
|
await new Promise<void>((resolve, reject) => {
|
|
let source: number | null = null;
|
|
let sampleTimer: ReturnType<typeof setTimeout> | null = null;
|
|
const timeout = setTimeout(resolve, 4200);
|
|
const finish = () => { clearTimeout(timeout); if (sampleTimer !== null) clearTimeout(sampleTimer); resolve(); };
|
|
try {
|
|
const unsubscribe = device.subscribe(reading => {
|
|
if (![0, 1, 3].includes(reading.source) || ![reading.x, reading.y, reading.z].every(Number.isFinite)) return;
|
|
if (source === null) { source = reading.source; sampleTimer = setTimeout(finish, 1200); }
|
|
if (source === reading.source && samples.length < 200) samples.push(reading);
|
|
});
|
|
release = () => { clearTimeout(timeout); if (sampleTimer !== null) clearTimeout(sampleTimer); unsubscribe(); };
|
|
} catch (error) { clearTimeout(timeout); if (sampleTimer !== null) clearTimeout(sampleTimer); reject(error); }
|
|
});
|
|
const degrees = estimateHeadInclination(samples);
|
|
if (degrees === null) return "No stable sensor data; keep still and retry";
|
|
const calibration = device.getHeadUpCalibration();
|
|
if (!Number.isFinite(calibration)) return "Glasses calibration not received; retry";
|
|
// Preserve the existing raw-angle preference and firmware calibration, rather
|
|
// than changing the meaning of saved angles for existing installations.
|
|
const offset = Math.round((degrees - calibration) / 5) * 5;
|
|
if (offset < 0 || offset > 60) return "Measured angle is outside the supported range";
|
|
headUpAngleSetting.set(String(offset));
|
|
return `Saved estimated head-up angle: ${offset}°`;
|
|
} catch { return "Measurement failed; try again"; }
|
|
finally {
|
|
try { release(); } catch { /* Disconnected communicator. */ }
|
|
try { if (lease) device.endHeadAngleMeasurement(); } catch { /* Disconnected communicator. */ }
|
|
measuring = false;
|
|
}
|
|
}
|
|
|
|
function androidDevice(): HeadAngleDevice | null {
|
|
if (!global.isAndroid) return null;
|
|
let active: any;
|
|
try { active = com.faceclaw.app.FaceclawBleCommunicator.getActive(); } catch { return null; }
|
|
if (!active) return null;
|
|
return {
|
|
beginHeadAngleMeasurement: () => Boolean(active.beginHeadAngleMeasurement()),
|
|
endHeadAngleMeasurement: () => active.endHeadAngleMeasurement(),
|
|
getHeadUpCalibration: () => Number(active.getHeadUpCalibration()),
|
|
subscribe(listener) {
|
|
const proxy = new com.faceclaw.app.FaceclawImuListener({ onImuData: (x: number, y: number, z: number, source: number) =>
|
|
listener({ x: Number(x), y: Number(y), z: Number(z), source: Number(source) }) });
|
|
active.addImuListener(proxy);
|
|
return () => active.removeImuListener(proxy);
|
|
},
|
|
};
|
|
}
|