Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix rest detection timescale #305

Merged
merged 5 commits into from
Mar 8, 2024
Merged

Fix rest detection timescale #305

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8c944b6
Fix rest detection timescale
wigwagwent Dec 1, 2023
805fcd4
Revert calcLinearAcc Change
wigwagwent Feb 10, 2024
8bacf9c
Merge remote-tracking branch 'upstream/main' into SensorFusionInconsi…
wigwagwent Feb 10, 2024
5599172
Change rest detection timescale to seconds from microseconds
wigwagwent Feb 10, 2024
5f93586
Update from comments
Eirenliel Mar 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 14 additions & 14 deletions src/motionprocessing/RestDetection.h
View file
Open in desktop
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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

uint32_t -> sensor_real_t

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]) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

uint32_t -> sensor_real_t

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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

uint32_t -> sensor_real_t

sensor_real_t gyrLastSquaredDeviation = 0;
sensor_real_t accLastSquaredDeviation = 0;

Expand Down
2 changes: 1 addition & 1 deletion src/sensors/SensorFusionRestDetect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/sensors/SensorFusionRestDetect.h
View file
Open in desktop
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
Open in desktop
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, (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
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, 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
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
Loading