Fix rest detection timescale (SlimeVR#305)

Sensor fusion expects the time to be in seconds while rest detection expects it to be in microseconds. This makes is so when the update sensor fusion is called the rest detection now gets the time in micros

Co-authored-by: Eiren Rain <[email protected]>

src/motionprocessing/RestDetection.h

@@ -20,14 +20,14 @@
 struct RestDetectionParams {
     sensor_real_t biasClip;
     sensor_real_t biasSigmaRest;
-    uint32_t restMinTimeMicros;
+    sensor_real_t restMinTime;
     sensor_real_t restFilterTau;
     sensor_real_t restThGyr;
     sensor_real_t restThAcc;
     RestDetectionParams():
         biasClip(2.0f),
         biasSigmaRest(0.03f),
-        restMinTimeMicros(1.5 * 1e6),
+        restMinTime(1.5),
         restFilterTau(0.5f),
         restThGyr(2.0f),
         restThAcc(0.5f)
@@ -103,7 +103,7 @@ class RestDetection {
     }
 #endif
 
-    void updateGyr(uint32_t dtMicros, sensor_real_t gyr[3]) {
+    void updateGyr(sensor_real_t gyr[3]) {
 #ifdef REST_DETECTION_DISABLE_LPF
         gyrLastSquaredDeviation =
             square(gyr[0] - lastSample.gyr[0]) +
@@ -114,7 +114,7 @@ class RestDetection {
         if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
                 || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip
                 || fabs(lastSample.gyr[2]) > biasClip) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         }
 
@@ -134,13 +134,13 @@ class RestDetection {
         if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0))
                 || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip
                 || fabs(restLastGyrLp[2]) > biasClip) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         }
 #endif
     }
 
-    void updateAcc(uint32_t dtMicros, sensor_real_t acc[3]) {
+    void updateAcc(sensor_real_t dt, sensor_real_t acc[3]) {
         if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) && acc[2] == sensor_real_t(0.0)) {
             return;
         }
@@ -152,11 +152,11 @@ class RestDetection {
             square(acc[2] - lastSample.acc[2]);
 
         if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
+            restTime += dt;
+            if (restTime >= params.restMinTime) {
                 restDetected = true;
             }
         }
@@ -174,11 +174,11 @@ class RestDetection {
             square(acc[2] - restLastAccLp[2]);
 
         if (accLastSquaredDeviation >= square(params.restThAcc)) {
-            restTimeMicros = 0;
+            restTime = 0;
             restDetected = false;
         } else {
-            restTimeMicros += dtMicros;
-            if (restTimeMicros >= params.restMinTimeMicros) {
+            restTime += dt;
+            if (restTime >= params.restMinTime) {
                 restDetected = true;
             }
         }
@@ -195,7 +195,7 @@ class RestDetection {
 
         gyrLastSquaredDeviation = 0.0;
         accLastSquaredDeviation = 0.0;
-        restTimeMicros = 0.0;
+        restTime = 0.0;
         std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0);
         std::fill(restGyrLpState, restGyrLpState + 3*2, NaN);
         std::fill(restLastAccLp, restLastAccLp + 3, 0.0);
@@ -235,7 +235,7 @@ class RestDetection {
 private:
     RestDetectionParams params;
     bool restDetected;
-    uint32_t restTimeMicros;
+    sensor_real_t restTime;
     sensor_real_t gyrLastSquaredDeviation = 0;
     sensor_real_t accLastSquaredDeviation = 0;
 

src/sensors/SensorFusionRestDetect.cpp

@@ -15,7 +15,7 @@ namespace SlimeVR
         void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat)
         {
             if (deltat < 0) deltat = gyrTs;
-            restDetection.updateGyr(deltat, Gxyz);
+            restDetection.updateGyr(Gxyz);
             SensorFusion::updateGyro(Gxyz, deltat);
         }
         #endif

src/sensors/SensorFusionRestDetect.h

@@ -18,7 +18,7 @@ namespace SlimeVR
         #if !SENSOR_FUSION_WITH_RESTDETECT
         struct SensorRestDetectionParams: RestDetectionParams {
             SensorRestDetectionParams() : RestDetectionParams() {
-                restMinTimeMicros = 2.0f * 1e6;
+                restMinTime = 2.0f;
                 restThGyr = 0.6f; // 400 norm
                 restThAcc = 0.06f; // 100 norm
             }

src/sensors/bmi160sensor.cpp

@@ -530,9 +530,9 @@ void BMI160Sensor::onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int1
     }
     remapGyroAccel(&Gxyz[0], &Gxyz[1], &Gxyz[2]);
 
-    sfusion.updateGyro(Gxyz, (double)dtMicros * 1.0e-6);
+	sfusion.updateGyro(Gxyz, (sensor_real_t)dtMicros * 1.0e-6);
 
-    optimistic_yield(100);
+	optimistic_yield(100);
 }
 void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) {
     #if BMI160_DEBUG
@@ -548,9 +548,9 @@ void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int
     lastAxyz[1] = Axyz[1];
     lastAxyz[2] = Axyz[2];
 
-    sfusion.updateAcc(Axyz, dtMicros);
+	sfusion.updateAcc(Axyz, (sensor_real_t)dtMicros * 1.0e-6);
 
-    optimistic_yield(100);
+	optimistic_yield(100);
 }
 void BMI160Sensor::onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) {
     #if BMI160_DEBUG
@@ -823,12 +823,12 @@ void BMI160Sensor::maybeCalibrateAccel() {
         m_Logger.debug("Calculating accelerometer calibration data...");
     #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT
         RestDetectionParams calibrationRestDetectionParams;
-        calibrationRestDetectionParams.restMinTimeMicros = 3 * 1e6;
+        calibrationRestDetectionParams.restMinTime = 3;
         calibrationRestDetectionParams.restThAcc = 0.25f;
         RestDetection calibrationRestDetection(
             calibrationRestDetectionParams,
-            BMI160_ODR_GYR_MICROS / 1e6f,
-            BMI160_ODR_ACC_MICROS / 1e6f
+            BMI160_ODR_GYR_MICROS * 1.0e-6,
+            BMI160_ODR_ACC_MICROS * 1.0e-6
         );
 
         constexpr uint16_t expectedPositions = 6;
@@ -851,9 +851,9 @@ void BMI160Sensor::maybeCalibrateAccel() {
             scaled[1] = ay * BMI160_ASCALE;
             scaled[2] = az * BMI160_ASCALE;
 
-            calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS, scaled);
+			calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS * 1.0e-6, scaled);
 
-            if (waitForMotion) {
+			if (waitForMotion) {
                 if (!calibrationRestDetection.getRestDetected()) {
                     waitForMotion = false;
                 }