CalibrationTutorial store and compare accel

Author: sctanf

gui/src/components/onboarding/pages/CalibrationTutorial.tsx

@@ -23,7 +23,9 @@ export enum CalibrationStatus {
 }
 
 export const IMU_CALIBRATION_TIME = 4;
-const ACCEL_TOLERANCE = 0.2; // m/s^2
+export const IMU_SETTLE_TIME = 1;
+const ACCEL_TOLERANCE = 0.5; // m/s^2
+const ACCEL_HYSTERESIS = 0.1; // m/s^2
 
 export function CalibrationTutorialPage() {
   const { l10n } = useLocalization();
@@ -32,16 +34,17 @@ export function CalibrationTutorialPage() {
     CalibrationStatus.WAITING
   );
   const [skipButton, setSkipButton] = useState(false);
+  const [settled, setSettled] = useState(false);
   const { timer, isCounting, startCountdown, abortCountdown } = useCountdown({
-    duration: IMU_CALIBRATION_TIME,
-    onCountdownEnd: () => setCalibrationStatus(CalibrationStatus.SUCCESS),
+    duration: settled ? IMU_CALIBRATION_TIME : IMU_SETTLE_TIME,
+    onCountdownEnd: () => settled ? setCalibrationStatus(CalibrationStatus.SUCCESS) : setSettled(true),
   });
   useTimeout(() => setSkipButton(true), 10000);
   const connectedIMUTrackers = useAtomValue(connectedIMUTrackersAtom);
   const restCalibrationTrackers =
     useRestCalibrationTrackers(connectedIMUTrackers);
   const [rested, setRested] = useState(false);
-  const lastValueMap = useRef(new Map<number, Vector3[]>());
+  const lastValueMap = useRef(new Map<number, Vector3>());
   useEffect(() => {
     const accelLength = restCalibrationTrackers.every((x) => {
       if (
@@ -58,17 +61,32 @@ export function CalibrationTutorialPage() {
       lastValueMap.current.set(trackerId, lastValue);
 
       const vec3 = Vector3FromVec3fT(x.tracker.linearAcceleration);
-      if (lastValues.length > 5) {
-        lastValues.shift();
-        const avg = averageVector(lastValues).lengthSq();
-        lastValues.push(vec3);
-        return vec3.lengthSq() <= avg + ACCEL_TOLERANCE ** 2;
+
+      if (vec3.lengthSq() > ACCEL_TOLERANCE ** 2) {
+        return false;
+      }
+
+      const delta = new Vector3();
+      delta.subVectors(lastValue, vec3);
+
+      if (delta.lengthSq() > ACCEL_HYSTERESIS ** 2) {
+        lastValue.copy(vec3);
+        return false;
       }
-      lastValues.push(vec3);
-      return false;
+
+      return true;
     });
 
-    setRested(accelLength || restCalibrationTrackers.length === 0);
+    if (accelLength && !settled && !isCounting) {
+      abortCountdown();
+      startCountdown();
+    } else if (!accelLength && !settled && isCounting) {
+      abortCountdown();
+    } else if (!accelLength && settled) {
+      setSettled(false);
+    }
+
+    setRested(settled || restCalibrationTrackers.length === 0);
   }, [restCalibrationTrackers]);
 
   useEffect(() => {
@@ -148,7 +166,7 @@ export function CalibrationTutorialPage() {
                 </div>
                 <ProgressBar
                   progress={
-                    isCounting
+                    isCounting && settled
                       ? (IMU_CALIBRATION_TIME - timer) / IMU_CALIBRATION_TIME
                       : calibrationStatus === CalibrationStatus.SUCCESS ||
                           calibrationStatus === CalibrationStatus.ERROR