BMI160: Fix double rest detection (SlimeVR#286)

Fix double rest detection with vqf
nekomona · Jan 16, 2024 · e18184f · e18184f
1 parent 6544ae9
commit e18184f
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 14 deletions.
diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h
@@ -47,7 +47,7 @@ namespace SlimeVR
         class SensorFusion
         {
         public:
-            SensorFusion(float gyrTs, float accTs=-1.0, float magTs=-1.0)
+            SensorFusion(sensor_real_t gyrTs, sensor_real_t accTs=-1.0, sensor_real_t magTs=-1.0)
                 : gyrTs(gyrTs), 
                   accTs( (accTs<0) ? gyrTs : accTs ), 
                   magTs( (magTs<0) ? gyrTs : magTs )

diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp
@@ -4,27 +4,25 @@ namespace SlimeVR
 {
     namespace Sensors
     {
+        #if !SENSOR_FUSION_WITH_RESTDETECT
         void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat)
         {
-            #if !SENSOR_WITH_REST_DETECT
-                if (deltat < 0) deltat = accTs;
-                restDetection.updateAcc(deltat, Axyz);
-            #endif
+            if (deltat < 0) deltat = accTs;
+            restDetection.updateAcc(deltat, Axyz);
             SensorFusion::updateAcc(Axyz, deltat);
         }
 
         void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat)
         {
-            #if !SENSOR_WITH_REST_DETECT
-                if (deltat < 0) deltat = accTs;
-                restDetection.updateGyr(deltat, Gxyz);
-            #endif
+            if (deltat < 0) deltat = gyrTs;
+            restDetection.updateGyr(deltat, Gxyz);
             SensorFusion::updateGyro(Gxyz, deltat);
         }
+        #endif
 
         bool SensorFusionRestDetect::getRestDetected()
         {
-            #if !SENSOR_WITH_REST_DETECT
+            #if !SENSOR_FUSION_WITH_RESTDETECT
                 return restDetection.getRestDetected();
             #elif SENSOR_USE_VQF
                 return vqf.getRestDetected();

diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h
@@ -15,7 +15,7 @@ namespace SlimeVR
 {
     namespace Sensors
     {
-        #if !SENSOR_WITH_REST_DETECT
+        #if !SENSOR_FUSION_WITH_RESTDETECT
         struct SensorRestDetectionParams: RestDetectionParams {
             SensorRestDetectionParams() : RestDetectionParams() {
                 restMinTimeMicros = 2.0f * 1e6;
@@ -30,20 +30,20 @@ namespace SlimeVR
         public:
             SensorFusionRestDetect(float gyrTs, float accTs=-1.0, float magTs=-1.0)
                 : SensorFusion(gyrTs, accTs, magTs)
-            #if !SENSOR_WITH_REST_DETECT
+            #if !SENSOR_FUSION_WITH_RESTDETECT
                 , restDetection(restDetectionParams, gyrTs,
                                 (accTs<0) ? gyrTs : accTs)
             #endif
             {}
 
             bool getRestDetected();
 
-            #if !SENSOR_WITH_REST_DETECT
+            #if !SENSOR_FUSION_WITH_RESTDETECT
                 void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat);
                 void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat);
             #endif
         protected:
-            #if !SENSOR_WITH_REST_DETECT
+            #if !SENSOR_FUSION_WITH_RESTDETECT
                 SensorRestDetectionParams restDetectionParams {};
                 RestDetection restDetection;
             #endif