Calibration tutorial fix settling (#1521)

Co-authored-by: Eiren Rain <Eirenliel@users.noreply.github.com>

Author: sctanf

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

@@ -9,7 +9,7 @@ import { useCountdown } from '@/hooks/countdown';
 import classNames from 'classnames';
 import { TaybolIcon } from '@/components/commons/icon/TaybolIcon';
 import { useRestCalibrationTrackers } from '@/hooks/imu-logic';
-import { averageVector, Vector3FromVec3fT } from '@/maths/vector3';
+import { Vector3FromVec3fT } from '@/maths/vector3';
 import { Vector3 } from 'three';
 import { useTimeout } from '@/hooks/timeout';
 import { useAtomValue } from 'jotai';
@@ -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,43 +34,61 @@ 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 (
+        x.device?.id?.id === undefined ||
         x.tracker.trackerId?.trackerNum === undefined ||
-        x.tracker.trackerId.deviceId?.id === undefined ||
         !x.tracker.linearAcceleration
       )
         return false;
 
-      const trackerId =
-        x.tracker.trackerId.trackerNum + (x.tracker.trackerId.trackerNum << 8);
-      const lastValues = lastValueMap.current.get(trackerId) ?? [];
-      lastValueMap.current.set(trackerId, lastValues);
+      const trackerId = x.tracker.trackerId.trackerNum + (x.device.id.id << 8);
+      const lastValue = lastValueMap.current.get(trackerId) ?? new Vector3();
+      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);
-  }, [restCalibrationTrackers]);
+    if (accelLength && !settled && !isCounting) {
+      abortCountdown();
+      startCountdown();
+    } else if (!accelLength && !settled && isCounting) {
+      abortCountdown();
+    } else if (!accelLength && settled) {
+      setSettled(false);
+    }
+
+    setRested(settled || restCalibrationTrackers.length === 0);
+  }, [restCalibrationTrackers, settled, isCounting]);
 
   useEffect(() => {
     if (calibrationStatus === CalibrationStatus.CALIBRATING && !rested) {
@@ -145,7 +165,7 @@ export function CalibrationTutorialPage() {
                 </div>
                 <ProgressBar
                   progress={
-                    isCounting
+                    isCounting && settled
                       ? (IMU_CALIBRATION_TIME - timer) / IMU_CALIBRATION_TIME
                       : calibrationStatus === CalibrationStatus.SUCCESS ||
                           calibrationStatus === CalibrationStatus.ERROR