Skip to content

Commit

Permalink
Change rest detection timescale to seconds from microseconds
Browse files Browse the repository at this point in the history
  • Loading branch information
@wigwagwent
wigwagwent committed Feb 10, 2024
1 parent 8bacf9c commit 5599172
Show file tree
Hide file tree
Showing 4 changed files with 26 additions and 26 deletions.
28 changes: 14 additions & 14 deletions src/motionprocessing/RestDetection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,14 +20,14 @@
struct RestDetectionParams {
sensor_real_t biasClip;
sensor_real_t biasSigmaRest;
uint32_t restMinTimeMicros;
uint32_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)
Expand Down Expand Up @@ -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]) +
Expand All @@ -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;
}

Expand All @@ -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(uint32_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;
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand All @@ -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);
Expand Down Expand Up @@ -235,7 +235,7 @@ class RestDetection {
private:
RestDetectionParams params;
bool restDetected;
uint32_t restTimeMicros;
uint32_t restTime;
sensor_real_t gyrLastSquaredDeviation = 0;
sensor_real_t accLastSquaredDeviation = 0;

Expand Down
4 changes: 2 additions & 2 deletions src/sensors/SensorFusionRestDetect.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 * 1e6, Axyz);
restDetection.updateAcc(deltat, 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 * 1e6, Gxyz);
restDetection.updateGyr(Gxyz);
SensorFusion::updateGyro(Gxyz, deltat);
}
#endif
Expand Down
2 changes: 1 addition & 1 deletion src/sensors/SensorFusionRestDetect.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down
18 changes: 9 additions & 9 deletions src/sensors/bmi160sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, (float)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
Expand All @@ -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, (float)dtMicros * 1.0e-6);
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
Expand Down Expand Up @@ -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;
Expand All @@ -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;
}
Expand Down

0 comments on commit 5599172

Please sign in to comment.