From 993d35aaeafaa08ea2b22d55aaf59d44965d347e Mon Sep 17 00:00:00 2001 From: wigwagwent <66268000+wigwagwent@users.noreply.github.com> Date: Fri, 8 Mar 2024 05:30:57 -0700 Subject: [PATCH] Fix rest detection timescale (#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 --- src/motionprocessing/RestDetection.h | 28 +++++++++++++------------- src/sensors/SensorFusionRestDetect.cpp | 2 +- src/sensors/SensorFusionRestDetect.h | 2 +- src/sensors/bmi160sensor.cpp | 18 ++++++++--------- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h index c0215baff..81dac51e6 100644 --- a/src/motionprocessing/RestDetection.h +++ b/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; diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index 2e6630d37..1221810a4 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/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 diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index e38d351c0..025e8de5d 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/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 } diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index a94036ff8..9ffabd567 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/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 @@ -825,12 +825,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; @@ -853,9 +853,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; }