Fix rest detection timescale

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
SlimeVR · Dec 1, 2023 · 9ae0021 · 9ae0021
1 parent 14f2752
commit 9ae0021
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp
@@ -173,9 +173,9 @@ namespace SlimeVR
 
         void SensorFusion::calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3])
         {
-            accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY;
-            accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
-            accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
+            accout[0] = (accin[0] - gravVec[0]) * CONST_EARTH_GRAVITY;
+            accout[1] = (accin[1] - gravVec[1]) * CONST_EARTH_GRAVITY;
+            accout[2] = (accin[2] - gravVec[2]) * CONST_EARTH_GRAVITY;
         }
     }
 }
diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp
@@ -8,14 +8,14 @@ namespace SlimeVR
         void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat)
         {
             if (deltat < 0) deltat = accTs;
-            restDetection.updateAcc(deltat, Axyz);
+            restDetection.updateAcc(deltat * 1e6, Axyz);
             SensorFusion::updateAcc(Axyz, deltat);
         }
 
         void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat)
         {
             if (deltat < 0) deltat = gyrTs;
-            restDetection.updateGyr(deltat, Gxyz);
+            restDetection.updateGyr(deltat * 1e6, Gxyz);
             SensorFusion::updateGyro(Gxyz, deltat);
         }
         #endif

diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp
@@ -530,7 +530,7 @@ 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, (float)dtMicros * 1.0e-6);
 
     optimistic_yield(100);
 }
@@ -548,7 +548,7 @@ 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, (float)dtMicros * 1.0e-6);
 
     optimistic_yield(100);
 }