github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Merge branch 'pr/375' into glove-dynamic-sfusion

Browse Source
This commit is contained in:
Eiren Rain
2025-02-05 18:11:02 +01:00
parent c6c1cc9acf 6350b9dd2a
commit b811251b19
39 changed files with 3527 additions and 1528 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
lib/vqf/vqf.cpp
View File

@@ -2,7 +2,7 @@
//
// SPDX-License-Identifier: MIT
// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs)
// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
// Removed batch update functions
#include "vqf.h"
@@ -18,41 +18,6 @@
inline vqf_real_t square(vqf_real_t x) { return x*x; }
VQFParams::VQFParams()
: tauAcc(3.0)
, tauMag(9.0)
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
, motionBiasEstEnabled(true)
#endif
, restBiasEstEnabled(true)
, magDistRejectionEnabled(true)
, biasSigmaInit(0.5)
, biasForgettingTime(100.0)
, biasClip(2.0)
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
, biasSigmaMotion(0.1)
, biasVerticalForgettingFactor(0.0001)
#endif
, biasSigmaRest(0.03)
, restMinT(1.5)
, restFilterTau(0.5)
, restThGyr(2.0)
, restThAcc(0.5)
, magCurrentTau(0.05)
, magRefTau(20.0)
, magNormTh(0.1)
, magDipTh(10.0)
, magNewTime(20.0)
, magNewFirstTime(5.0)
, magNewMinGyr(20.0)
, magMinUndisturbedTime(0.5)
, magMaxRejectionTime(60.0)
, magRejectionFactor(2.0)
{
}
VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs)
{
coeffs.gyrTs = gyrTs;
@@ -73,7 +38,7 @@ VQF::VQF(const VQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t
setup();
}
void VQF::updateGyr(const vqf_real_t gyr[3], double gyrTs)
void VQF::updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs)
{
// rest detection
if (params.restBiasEstEnabled || params.magDistRejectionEnabled) {
@@ -537,8 +502,8 @@ void VQF::setTauAcc(vqf_real_t tauAcc)
return;
}
params.tauAcc = tauAcc;
double newB[3];
double newA[3];
vqf_real_t newB[3];
vqf_real_t newA[3];
filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA);
filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState);
@@ -731,15 +696,15 @@ vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts)
}
}
void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[])
void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[], vqf_real_t outA[])
{
assert(tau > 0);
assert(Ts > 0);
// second order Butterworth filter based on https://stackoverflow.com/a/52764064
double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response
double C = tan(M_PI*fc*double(Ts));
double D = C*C + sqrt(2)*C + 1;
double b0 = C*C/D;
vqf_real_t fc = (M_SQRT2 / (2.0*M_PI))/vqf_real_t(tau); // time constant of dampened, non-oscillating part of step response
vqf_real_t C = tan(M_PI*fc*vqf_real_t(Ts));
vqf_real_t D = C*C + sqrt(2)*C + 1;
vqf_real_t b0 = C*C/D;
outB[0] = b0;
outB[1] = 2*b0;
outB[2] = b0;
@@ -748,7 +713,7 @@ void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA
outA[1] = (1-sqrt(2)*C+C*C)/D; // a2
}
void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[])
void VQF::filterInitialState(vqf_real_t x0, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t out[])
{
// initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter
// update equation)
@@ -756,9 +721,9 @@ void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2]
out[1] = x0*(b[2] - a[1]);
}
void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[],
const double a_old[], const double b_new[],
const double a_new[], double state[])
void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const vqf_real_t b_old[],
const vqf_real_t a_old[], const vqf_real_t b_new[],
const vqf_real_t a_new[], vqf_real_t state[])
{
if (isnan(state[0])) {
return;
@@ -769,18 +734,18 @@ void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const do
}
}
vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])
vqf_real_t VQF::filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2])
{
// difference equations based on scipy.signal.lfilter documentation
// assumes that a0 == 1.0
double y = b[0]*x + state[0];
vqf_real_t y = b[0]*x + state[0];
state[0] = b[1]*x - a[0]*y + state[1];
state[1] = b[2]*x - a[1]*y;
return y;
}
void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3],
const double a[2], double state[], vqf_real_t out[])
void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const vqf_real_t b[3],
const vqf_real_t a[2], vqf_real_t state[], vqf_real_t out[])
{
assert(N>=2);
@@ -873,17 +838,17 @@ void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2
bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9])
{
// in = [a b c; d e f; g h i]
double A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
double D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
double G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
double B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
double E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
double H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
double C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
double F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
double I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
vqf_real_t A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h)
vqf_real_t D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h)
vqf_real_t G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e)
vqf_real_t B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g)
vqf_real_t E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g)
vqf_real_t H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d)
vqf_real_t C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g)
vqf_real_t F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g)
vqf_real_t I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d)
double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
vqf_real_t det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C;
if (det >= -EPS && det <= EPS) {
std::fill(out, out+9, 0);
@@ -917,16 +882,7 @@ void VQF::setup()
coeffs.biasP0 = square(params.biasSigmaInit*100.0);
// the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds
coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
#endif
vqf_real_t pRest = square(params.biasSigmaRest*100.0);
coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
updateBiasForgettingTime(params.biasForgettingTime);
filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA);
filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA);
@@ -941,3 +897,16 @@ void VQF::setup()
resetState();
}
void VQF::updateBiasForgettingTime(float biasForgettingTime) {
coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime;
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
vqf_real_t pMotion = square(params.biasSigmaMotion*100.0);
coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion;
coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10));
#endif
vqf_real_t pRest = square(params.biasSigmaRest*100.0);
coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest;
}

1947
lib/vqf/vqf.h
View File

File diff suppressed because it is too large Load Diff

15
src/configuration/Configuration.cpp
View File

@@ -160,6 +160,21 @@ void Configuration::setSensor(size_t sensorID, const SensorConfig& config) {
m_Sensors[sensorID] = config;
}
void Configuration::eraseSensors() {
m_Sensors.clear();
SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
char path[17];
sprintf(path, DIR_CALIBRATIONS "/%s", f.name());
f.close();
LittleFS.remove(path);
});
save();
}
void Configuration::loadSensors() {
SlimeVR::Utils::forEachFile(DIR_CALIBRATIONS, [&](SlimeVR::Utils::File f) {
SensorConfig sensorConfig;

1
src/configuration/Configuration.h
View File

@@ -46,6 +46,7 @@ public:
size_t getSensorCount() const;
SensorConfig getSensor(size_t sensorID) const;
void setSensor(size_t sensorID, const SensorConfig& config);
void eraseSensors();
bool loadTemperatureCalibration(
uint8_t sensorId,

30
src/configuration/SensorConfig.h
View File

@@ -73,6 +73,32 @@ struct SoftFusionSensorConfig {
float G_Sens[3];
uint8_t MotionlessData[60];
// temperature sampling rate (placed at the end to not break existing configs)
float T_Ts;
};
struct NonBlockingSensorConfig {
ImuID ImuType;
uint16_t MotionlessDataLen;
bool sensorTimestepsCalibrated;
float A_Ts;
float G_Ts;
float M_Ts;
float T_Ts;
bool motionlessCalibrated;
uint8_t MotionlessData[60];
uint8_t gyroPointsCalibrated;
float gyroMeasurementTemperature1;
float G_off1[3];
float gyroMeasurementTemperature2;
float G_off2[3];
bool accelCalibrated[3];
float A_off[3];
};
struct MPU6050SensorConfig {
@@ -131,7 +157,8 @@ enum class SensorConfigType {
MPU9250,
ICM20948,
SFUSION,
BNO0XX
BNO0XX,
NONBLOCKING,
};
const char* calibrationConfigTypeToString(SensorConfigType type);
@@ -146,6 +173,7 @@ struct SensorConfig {
MPU9250SensorConfig mpu9250;
ICM20948SensorConfig icm20948;
BNO0XXSensorConfig bno0XX;
NonBlockingSensorConfig nonblocking;
} data;
};

12
src/debug.h
View File

@@ -82,7 +82,7 @@
// Experimental
#define OPTIMIZE_UPDATES true
#define I2C_SPEED 400000
#define I2C_SPEED 800000
#define COMPLIANCE_MODE true
#define USE_ATTENUATION COMPLIANCE_MODE&& ESP8266
@@ -100,4 +100,14 @@
#define FIRMWARE_VERSION "UNKNOWN"
#endif
#ifndef USE_NONBLOCKING_CALIBRATION
#define USE_NONBLOCKING_CALIBRATION true
#endif
#define DEBUG_MEASURE_SENSOR_TIME_TAKEN true
#ifndef DEBUG_MEASURE_SENSOR_TIME_TAKEN
#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false
#endif
#endif // SLIMEVR_DEBUG_H_

55
src/debugging/TimeTaken.cpp Normal file
View File

@@ -0,0 +1,55 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "TimeTaken.h"
namespace SlimeVR::Debugging {
TimeTakenMeasurer::TimeTakenMeasurer(const char* name)
: name{name} {}
void TimeTakenMeasurer::before() { startMicros = micros(); }
void TimeTakenMeasurer::after() {
uint64_t elapsedMicros = micros() - startMicros;
timeTakenMicros += elapsedMicros;
uint64_t sinceLastReportMillis = millis() - lastTimeTakenReportMillis;
if (sinceLastReportMillis < static_cast<uint64_t>(SecondsBetweenReports * 1e3)) {
return;
}
float usedPercentage = static_cast<float>(timeTakenMicros) / 1e3f
/ static_cast<float>(sinceLastReportMillis) * 100;
m_Logger.info(
"%s: %.2f%% of the last period taken (%.2f/%lld millis)",
name,
usedPercentage,
timeTakenMicros / 1e3f,
sinceLastReportMillis
);
timeTakenMicros = 0;
lastTimeTakenReportMillis = millis();
}
} // namespace SlimeVR::Debugging

58
src/debugging/TimeTaken.h Normal file
View File

@@ -0,0 +1,58 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstdint>
#include "logging/Logger.h"
namespace SlimeVR::Debugging {
/*
* Usage:
*
* TimeTakenMeasurer measurer{"Some event"};
*
* ...
*
* measurer.before();
* thing to measure
* measurer.after();
*/
class TimeTakenMeasurer {
public:
explicit TimeTakenMeasurer(const char* name);
void before();
void after();
private:
static constexpr float SecondsBetweenReports = 1.0f;
const char* name;
SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("TimeTaken");
uint64_t lastTimeTakenReportMillis = 0;
uint64_t timeTakenMicros = 0;
uint64_t startMicros = 0;
};
} // namespace SlimeVR::Debugging

13
src/main.cpp
View File

@@ -27,6 +27,7 @@
#include "Wire.h"
#include "batterymonitor.h"
#include "credentials.h"
#include "debugging/TimeTaken.h"
#include "globals.h"
#include "logging/Logger.h"
#include "ota.h"
@@ -42,6 +43,10 @@ SlimeVR::Configuration::Configuration configuration;
SlimeVR::Network::Manager networkManager;
SlimeVR::Network::Connection networkConnection;
#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
SlimeVR::Debugging::TimeTakenMeasurer sensorMeasurer{"Sensors"};
#endif
int sensorToCalibrate = -1;
bool blinking = false;
unsigned long blinkStart = 0;
@@ -124,7 +129,15 @@ void loop() {
SerialCommands::update();
OTA::otaUpdate();
networkManager.update();
#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
sensorMeasurer.before();
#endif
sensorManager.update();
#if DEBUG_MEASURE_SENSOR_TIME_TAKEN
sensorMeasurer.after();
#endif
battery.Loop();
ledManager.update();
I2CSCAN::update();

7
src/sensors/SensorFusion.cpp
View File

@@ -211,5 +211,12 @@ void SensorFusion::calcLinearAcc(
accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY;
accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY;
}
#if SENSOR_USE_VQF
void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) {
vqf.updateBiasForgettingTime(biasForgettingTime);
}
#endif
} // namespace Sensors
} // namespace SlimeVR

33
src/sensors/SensorFusion.h
View File

@@ -38,23 +38,18 @@
namespace SlimeVR {
namespace Sensors {
#if SENSOR_USE_VQF
struct SensorVQFParams : VQFParams {
SensorVQFParams()
: VQFParams() {
#ifndef VQF_NO_MOTION_BIAS_ESTIMATION
motionBiasEstEnabled = true;
#endif
tauAcc = 2.0f;
restMinT = 2.0f;
restThGyr = 0.6f; // 400 norm
restThAcc = 0.06f; // 100 norm
}
constexpr VQFParams DefaultVQFParams = VQFParams{
.tauAcc = 2.0f,
.restMinT = 2.0f,
.restThGyr = 0.6f,
.restThAcc = 0.06f,
};
#endif
class SensorFusion {
public:
SensorFusion(
VQFParams vqfParams,
sensor_real_t gyrTs,
sensor_real_t accTs = -1.0,
sensor_real_t magTs = -1.0
@@ -62,12 +57,13 @@ public:
: gyrTs(gyrTs)
, accTs((accTs < 0) ? gyrTs : accTs)
, magTs((magTs < 0) ? gyrTs : magTs)
, vqfParams(vqfParams)
#if SENSOR_USE_MAHONY
#elif SENSOR_USE_MADGWICK
#elif SENSOR_USE_BASICVQF
, basicvqf(gyrTs, ((accTs < 0) ? gyrTs : accTs), ((magTs < 0) ? gyrTs : magTs))
#elif SENSOR_USE_VQF
, vqf(vqfParams,
, vqf(this->vqfParams,
gyrTs,
((accTs < 0) ? gyrTs : accTs),
((magTs < 0) ? gyrTs : magTs))
@@ -75,6 +71,13 @@ public:
{
}
explicit SensorFusion(
sensor_real_t gyrTs,
sensor_real_t accTs = -1.0,
sensor_real_t magTs = -1.0
)
: SensorFusion(DefaultVQFParams, gyrTs, accTs, magTs) {}
void update6D(
sensor_real_t Axyz[3],
sensor_real_t Gxyz[3],
@@ -106,6 +109,10 @@ public:
sensor_real_t accout[3]
);
#if SENSOR_USE_VQF
void updateBiasForgettingTime(float biasForgettingTime);
#endif
protected:
sensor_real_t gyrTs;
sensor_real_t accTs;
@@ -118,7 +125,7 @@ protected:
#elif SENSOR_USE_BASICVQF
BasicVQF basicvqf;
#elif SENSOR_USE_VQF
SensorVQFParams vqfParams{};
VQFParams vqfParams;
VQF vqf;
#endif

15
src/sensors/SensorFusionRestDetect.h
View File

@@ -33,6 +33,21 @@ public:
{
}
#if SENSOR_USE_VQF
SensorFusionRestDetect(
VQFParams vqfParams,
float gyrTs,
float accTs = -1.0,
float magTs = -1.0
)
: SensorFusion(vqfParams, gyrTs, accTs, magTs)
#if !SENSOR_FUSION_WITH_RESTDETECT
, restDetection(restDetectionParams, gyrTs, (accTs < 0) ? gyrTs : accTs)
#endif
{
}
#endif
bool getRestDetected();
#if !SENSOR_FUSION_WITH_RESTDETECT

44
src/sensors/SensorManager.cpp
View File

@@ -24,15 +24,19 @@
#include "SensorManager.h"
#include <map>
#include <type_traits>
#include "bmi160sensor.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "debug.h"
#include "icm20948sensor.h"
#include "mpu6050sensor.h"
#include "mpu9250sensor.h"
#include "sensorinterface/I2CPCAInterface.h"
#include "sensorinterface/MCP23X17PinInterface.h"
#include "sensors/softfusion/SoftfusionCalibration.h"
#include "sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h"
#include "softfusion/drivers/bmi270.h"
#include "softfusion/drivers/icm42688.h"
#include "softfusion/drivers/icm45605.h"
@@ -49,26 +53,40 @@
#include "driver/i2c.h"
#endif
#if USE_NONBLOCKING_CALIBRATION
#define SFCALIBRATOR SlimeVR::Sensors::NonBlockingCalibration::NonBlockingCalibrator
#else
#define SFCALIBRATOR SlimeVR::Sensor::SoftfusionCalibrator
#endif
namespace SlimeVR {
namespace Sensors {
using SoftFusionLSM6DS3TRC
= SoftFusionSensor<SoftFusion::Drivers::LSM6DS3TRC, SoftFusion::I2CImpl>;
using SoftFusionICM42688
= SoftFusionSensor<SoftFusion::Drivers::ICM42688, SoftFusion::I2CImpl>;
using SoftFusionLSM6DS3TRC = SoftFusionSensor<
SoftFusion::Drivers::LSM6DS3TRC,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionICM42688 = SoftFusionSensor<
SoftFusion::Drivers::ICM42688,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionBMI270
= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl>;
= SoftFusionSensor<SoftFusion::Drivers::BMI270, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSV
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl>;
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSV, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSO
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionLSM6DSR
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
= SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionMPU6050
= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
using SoftFusionICM45686
= SoftFusionSensor<SoftFusion::Drivers::ICM45686, SoftFusion::I2CImpl>;
using SoftFusionICM45605
= SoftFusionSensor<SoftFusion::Drivers::ICM45605, SoftFusion::I2CImpl>;
= SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl, SFCALIBRATOR>;
using SoftFusionICM45686 = SoftFusionSensor<
SoftFusion::Drivers::ICM45686,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
using SoftFusionICM45605 = SoftFusionSensor<
SoftFusion::Drivers::ICM45605,
SoftFusion::I2CImpl,
SFCALIBRATOR>;
void SensorManager::setup() {
std::map<int, DirectPinInterface*> directPinInterfaces;

6
src/sensors/SensorManager.h
View File

@@ -57,7 +57,7 @@ public:
void update();
std::vector<std::unique_ptr<Sensor>>& getSensors() { return m_Sensors; };
std::vector<std::unique_ptr<::Sensor>>& getSensors() { return m_Sensors; };
SensorTypeID getSensorType(size_t id) {
if (id < m_Sensors.size()) {
return m_Sensors[id]->getSensorType();
@@ -68,11 +68,11 @@ public:
private:
SlimeVR::Logging::Logger m_Logger;
std::vector<std::unique_ptr<Sensor>> m_Sensors;
std::vector<std::unique_ptr<::Sensor>> m_Sensors;
Adafruit_MCP23X17 m_MCP;
template <typename ImuType>
std::unique_ptr<Sensor> buildSensor(
std::unique_ptr<::Sensor> buildSensor(
uint8_t sensorID,
std::optional<uint8_t> imuAddress,
float rotation,

122
src/sensors/softfusion/CalibrationBase.h Normal file
View File

@@ -0,0 +1,122 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstddef>
#include <cstdint>
#include <functional>
#include "configuration/SensorConfig.h"
#include "motionprocessing/types.h"
#include "sensors/SensorFusionRestDetect.h"
namespace SlimeVR::Sensor {
template <typename IMU, typename RawSensorT, typename RawVectorT>
class CalibrationBase {
public:
CalibrationBase(
SlimeVR::Sensors::SensorFusionRestDetect& fusion,
IMU& sensor,
uint8_t sensorId,
SlimeVR::Logging::Logger& logger,
float TempTs,
float AScale,
float GScale
)
: fusion{fusion}
, sensor{sensor}
, sensorId{sensorId}
, logger{logger}
, TempTs{TempTs}
, AScale{AScale}
, GScale{GScale} {}
static constexpr bool HasMotionlessCalib
= requires(IMU& i) { typename IMU::MotionlessCalibrationData; };
static constexpr size_t MotionlessCalibDataSize() {
if constexpr (HasMotionlessCalib) {
return sizeof(typename IMU::MotionlessCalibrationData);
} else {
return 0;
}
}
using EatSamplesFn = std::function<void(const uint32_t)>;
using ReturnLastFn
= std::function<std::tuple<RawVectorT, RawVectorT, int16_t>(const uint32_t)>;
virtual void startCalibration(
int calibrationType,
const EatSamplesFn& eatSamplesForSeconds,
const ReturnLastFn& eatSamplesReturnLast
){};
virtual bool calibrationMatches(
const SlimeVR::Configuration::SensorConfig& sensorCalibration
) = 0;
virtual void assignCalibration(const Configuration::SensorConfig& sensorCalibration)
= 0;
virtual void begin() {}
virtual void tick() {}
virtual void scaleAccelSample(sensor_real_t accelSample[3]) = 0;
virtual float getAccelTimestep() = 0;
virtual void scaleGyroSample(sensor_real_t gyroSample[3]) = 0;
virtual float getGyroTimestep() = 0;
virtual float getTempTimestep() = 0;
virtual const uint8_t* getMotionlessCalibrationData() = 0;
virtual void provideAccelSample(const RawSensorT accelSample[3]) {}
virtual void provideGyroSample(const RawSensorT gyroSample[3]) {}
virtual void provideTempSample(float tempSample) {}
virtual float getZROChange() { return IMU::TemperatureZROChange; };
protected:
void recalcFusion() {
fusion = Sensors::SensorFusionRestDetect(
IMU::SensorVQFParams,
getGyroTimestep(),
getAccelTimestep(),
getTempTimestep()
);
}
Sensors::SensorFusionRestDetect& fusion;
IMU& sensor;
uint8_t sensorId;
SlimeVR::Logging::Logger& logger;
float TempTs;
float AScale;
float GScale;
};
} // namespace SlimeVR::Sensor

438
src/sensors/softfusion/SoftfusionCalibration.h Normal file
View File

@@ -0,0 +1,438 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Tailsy13, Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstring>
#include <vector>
#include "GlobalVars.h"
#include "configuration/SensorConfig.h"
#include "logging/Logger.h"
#include "motionprocessing/RestDetection.h"
#include "motionprocessing/types.h"
#include "sensors/SensorFusionRestDetect.h"
#include "sensors/softfusion/CalibrationBase.h"
namespace SlimeVR::Sensor {
template <typename IMU, typename RawSensorT, typename RawVectorT>
class SoftfusionCalibrator : public CalibrationBase<IMU, RawSensorT, RawVectorT> {
public:
static constexpr bool HasUpsideDownCalibration = true;
using Base = CalibrationBase<IMU, RawSensorT, RawVectorT>;
SoftfusionCalibrator(
Sensors::SensorFusionRestDetect& fusion,
IMU& sensor,
uint8_t sensorId,
SlimeVR::Logging::Logger& logger,
float TempTs,
float AScale,
float GScale
)
: Base{fusion, sensor, sensorId, logger, TempTs, AScale, GScale} {
calibration.T_Ts = TempTs;
}
void startCalibration(
int calibrationType,
const Base::EatSamplesFn& eatSamplesForSeconds,
const Base::ReturnLastFn& eatSamplesReturnLast
) final {
if (calibrationType == 0) {
// ALL
calibrateSampleRate(eatSamplesForSeconds);
if constexpr (Base::HasMotionlessCalib) {
typename IMU::MotionlessCalibrationData calibData;
sensor.motionlessCalibration(calibData);
std::memcpy(calibration.MotionlessData, &calibData, sizeof(calibData));
}
// Gryoscope offset calibration can only happen after any motionless
// gyroscope calibration, otherwise we are calculating the offset based
// on an incorrect starting point
calibrateGyroOffset(eatSamplesReturnLast);
calibrateAccel(eatSamplesForSeconds);
} else if (calibrationType == 1) {
calibrateSampleRate(eatSamplesForSeconds);
} else if (calibrationType == 2) {
calibrateGyroOffset(eatSamplesReturnLast);
} else if (calibrationType == 3) {
calibrateAccel(eatSamplesForSeconds);
} else if (calibrationType == 4) {
if constexpr (Base::HasMotionlessCalib) {
typename IMU::MotionlessCalibrationData calibData;
sensor.motionlessCalibration(calibData);
std::memcpy(calibration.MotionlessData, &calibData, sizeof(calibData));
} else {
logger.info("Sensor doesn't provide any custom motionless calibration");
}
}
saveCalibration();
}
bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration
) final {
return sensorCalibration.type
== SlimeVR::Configuration::SensorConfigType::SFUSION
&& (sensorCalibration.data.sfusion.ImuType == IMU::Type)
&& (sensorCalibration.data.sfusion.MotionlessDataLen
== Base::MotionlessCalibDataSize());
}
void assignCalibration(const Configuration::SensorConfig& sensorCalibration) final {
calibration = sensorCalibration.data.sfusion;
Base::recalcFusion();
}
void scaleAccelSample(sensor_real_t accelSample[3]) final {
float tmp[3];
for (uint8_t i = 0; i < 3; i++) {
tmp[i] = (accelSample[i] - calibration.A_B[i]);
}
accelSample[0]
= (calibration.A_Ainv[0][0] * tmp[0] + calibration.A_Ainv[0][1] * tmp[1]
+ calibration.A_Ainv[0][2] * tmp[2])
* AScale;
accelSample[1]
= (calibration.A_Ainv[1][0] * tmp[0] + calibration.A_Ainv[1][1] * tmp[1]
+ calibration.A_Ainv[1][2] * tmp[2])
* AScale;
accelSample[2]
= (calibration.A_Ainv[2][0] * tmp[0] + calibration.A_Ainv[2][1] * tmp[1]
+ calibration.A_Ainv[2][2] * tmp[2])
* AScale;
}
float getAccelTimestep() final { return calibration.A_Ts; }
void scaleGyroSample(sensor_real_t gyroSample[3]) final {
gyroSample[0] = static_cast<sensor_real_t>(
GScale * (gyroSample[0] - calibration.G_off[0])
);
gyroSample[1] = static_cast<sensor_real_t>(
GScale * (gyroSample[1] - calibration.G_off[1])
);
gyroSample[2] = static_cast<sensor_real_t>(
GScale * (gyroSample[2] - calibration.G_off[2])
);
}
float getGyroTimestep() final { return calibration.G_Ts; }
float getTempTimestep() final { return calibration.T_Ts; }
const uint8_t* getMotionlessCalibrationData() final {
return calibration.MotionlessData;
}
private:
static constexpr auto GyroCalibDelaySeconds = 5;
static constexpr auto GyroCalibSeconds = 5;
static constexpr auto SampleRateCalibDelaySeconds = 1;
static constexpr auto SampleRateCalibSeconds = 5;
static constexpr auto AccelCalibDelaySeconds = 3;
static constexpr auto AccelCalibRestSeconds = 3;
void saveCalibration() {
logger.debug("Saving the calibration data");
SlimeVR::Configuration::SensorConfig calibration{};
calibration.type = SlimeVR::Configuration::SensorConfigType::SFUSION;
calibration.data.sfusion = this->calibration;
configuration.setSensor(sensorId, calibration);
configuration.save();
}
void calibrateGyroOffset(const Base::ReturnLastFn& eatSamplesReturnLast) {
// Wait for sensor to calm down before calibration
logger.info(
"Put down the device and wait for baseline gyro reading calibration (%d "
"seconds)",
GyroCalibDelaySeconds
);
ledManager.on();
auto lastSamples = eatSamplesReturnLast(GyroCalibDelaySeconds);
ledManager.off();
calibration.temperature = std::get<2>(lastSamples) / IMU::TemperatureSensitivity
+ IMU::TemperatureBias;
logger.trace("Calibration temperature: %f", calibration.temperature);
ledManager.pattern(100, 100, 3);
ledManager.on();
logger.info("Gyro calibration started...");
int32_t sumXYZ[3] = {0};
const auto targetCalib = millis() + 1000 * GyroCalibSeconds;
uint32_t sampleCount = 0;
while (millis() < targetCalib) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[&sumXYZ,
&sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sumXYZ[0] += xyz[0];
sumXYZ[1] += xyz[1];
sumXYZ[2] += xyz[2];
++sampleCount;
},
[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
);
}
ledManager.off();
calibration.G_off[0]
= static_cast<float>(sumXYZ[0]) / static_cast<float>(sampleCount);
calibration.G_off[1]
= static_cast<float>(sumXYZ[1]) / static_cast<float>(sampleCount);
calibration.G_off[2]
= static_cast<float>(sumXYZ[2]) / static_cast<float>(sampleCount);
logger.info(
"Gyro offset after %d samples: %f %f %f",
sampleCount,
UNPACK_VECTOR_ARRAY(calibration.G_off)
);
}
void calibrateAccel(const Base::EatSamplesFn& eatSamplesForSeconds) {
auto magneto = std::make_unique<MagnetoCalibration>();
logger.info(
"Put the device into 6 unique orientations (all sides), leave it still and "
"do not hold/touch for %d seconds each",
AccelCalibRestSeconds
);
ledManager.on();
eatSamplesForSeconds(AccelCalibDelaySeconds);
ledManager.off();
RestDetectionParams calibrationRestDetectionParams;
calibrationRestDetectionParams.restMinTime = AccelCalibRestSeconds;
calibrationRestDetectionParams.restThAcc = 0.25f;
RestDetection calibrationRestDetection(
calibrationRestDetectionParams,
IMU::GyrTs,
IMU::AccTs
);
constexpr uint16_t expectedPositions = 6;
constexpr uint16_t numSamplesPerPosition = 96;
uint16_t numPositionsRecorded = 0;
uint16_t numCurrentPositionSamples = 0;
bool waitForMotion = true;
std::vector<float> accelCalibrationChunk;
accelCalibrationChunk.resize(numSamplesPerPosition * 3);
ledManager.pattern(100, 100, 6);
ledManager.on();
logger.info("Gathering accelerometer data...");
logger.info(
"Waiting for position %i, you can leave the device as is...",
numPositionsRecorded + 1
);
bool samplesGathered = false;
while (!samplesGathered) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[]
= {static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[0])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[1])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[2])
)};
calibrationRestDetection.updateAcc(IMU::AccTs, scaledData);
if (waitForMotion) {
if (!calibrationRestDetection.getRestDetected()) {
waitForMotion = false;
}
return;
}
if (calibrationRestDetection.getRestDetected()) {
const uint16_t i = numCurrentPositionSamples * 3;
accelCalibrationChunk[i + 0] = xyz[0];
accelCalibrationChunk[i + 1] = xyz[1];
accelCalibrationChunk[i + 2] = xyz[2];
numCurrentPositionSamples++;
if (numCurrentPositionSamples >= numSamplesPerPosition) {
for (int i = 0; i < numSamplesPerPosition; i++) {
magneto->sample(
accelCalibrationChunk[i * 3 + 0],
accelCalibrationChunk[i * 3 + 1],
accelCalibrationChunk[i * 3 + 2]
);
}
numPositionsRecorded++;
numCurrentPositionSamples = 0;
if (numPositionsRecorded < expectedPositions) {
ledManager.pattern(50, 50, 2);
ledManager.on();
logger.info(
"Recorded, waiting for position %i...",
numPositionsRecorded + 1
);
waitForMotion = true;
}
}
} else {
numCurrentPositionSamples = 0;
}
if (numPositionsRecorded >= expectedPositions) {
samplesGathered = true;
}
},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[](const int16_t rawTemp, const sensor_real_t timeDelta) {}
);
}
ledManager.off();
logger.debug("Calculating accelerometer calibration data...");
accelCalibrationChunk.resize(0);
float A_BAinv[4][3];
magneto->current_calibration(A_BAinv);
logger.debug("Finished calculating accelerometer calibration");
logger.debug("Accelerometer calibration matrix:");
logger.debug("{");
for (int i = 0; i < 3; i++) {
calibration.A_B[i] = A_BAinv[0][i];
calibration.A_Ainv[0][i] = A_BAinv[1][i];
calibration.A_Ainv[1][i] = A_BAinv[2][i];
calibration.A_Ainv[2][i] = A_BAinv[3][i];
logger.debug(
" %f, %f, %f, %f",
A_BAinv[0][i],
A_BAinv[1][i],
A_BAinv[2][i],
A_BAinv[3][i]
);
}
logger.debug("}");
}
void calibrateSampleRate(const Base::EatSamplesFn& eatSamplesForSeconds) {
logger.debug(
"Calibrating IMU sample rate in %d second(s)...",
SampleRateCalibDelaySeconds
);
ledManager.on();
eatSamplesForSeconds(SampleRateCalibDelaySeconds);
uint32_t accelSamples = 0;
uint32_t gyroSamples = 0;
uint32_t tempSamples = 0;
const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds;
logger.debug("Counting samples now...");
uint32_t currentTime;
while ((currentTime = millis()) < calibTarget) {
sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
accelSamples++;
},
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
gyroSamples++;
},
[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
tempSamples++;
}
);
yield();
}
const auto millisFromStart = static_cast<float>(
currentTime - (calibTarget - 1000 * SampleRateCalibSeconds)
);
logger.debug(
"Collected %d gyro, %d acc samples during %d ms",
gyroSamples,
accelSamples,
millisFromStart
);
calibration.A_Ts
= millisFromStart / (static_cast<float>(accelSamples) * 1000.0f);
calibration.G_Ts
= millisFromStart / (static_cast<float>(gyroSamples) * 1000.0f);
calibration.T_Ts
= millisFromStart / (static_cast<float>(tempSamples) * 1000.0f);
logger.debug(
"Gyro frequency %fHz, accel frequency: %fHz, temperature frequency: %fHz",
1.0 / calibration.G_Ts,
1.0 / calibration.A_Ts,
1.0 / calibration.T_Ts
);
ledManager.off();
// fusion needs to be recalculated
Base::recalcFusion();
}
SlimeVR::Configuration::SoftFusionSensorConfig calibration = {
// let's create here transparent calibration that doesn't affect input data
.ImuType = {IMU::Type},
.MotionlessDataLen = {Base::MotionlessCalibDataSize()},
.A_B = {0.0, 0.0, 0.0},
.A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
.M_B = {0.0, 0.0, 0.0},
.M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
.G_off = {0.0, 0.0, 0.0},
.temperature = 0.0,
.A_Ts = IMU::AccTs,
.G_Ts = IMU::GyrTs,
.M_Ts = IMU::MagTs,
.G_Sens = {1.0, 1.0, 1.0},
.MotionlessData = {},
.T_Ts = 0,
};
private:
using Base::AScale;
using Base::GScale;
using Base::logger;
using Base::sensor;
using Base::sensorId;
};
} // namespace SlimeVR::Sensor

27
src/sensors/softfusion/TempGradientCalculator.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include "TempGradientCalculator.h"
#include <functional>
TemperatureGradientCalculator::TemperatureGradientCalculator(
const std::function<void(float gradient)>& callback
)
: callback{callback} {}
void TemperatureGradientCalculator::feedSample(float sample, float timeStep) {
tempSum += sample * timeStep;
}
void TemperatureGradientCalculator::tick() {
uint64_t nextCheck
= lastAverageSentMillis + static_cast<uint64_t>(AveragingTimeSeconds * 1e3);
if (millis() < nextCheck) {
return;
}
lastAverageSentMillis = nextCheck;
float average = tempSum / AveragingTimeSeconds;
callback((average - lastTempAverage) / AveragingTimeSeconds);
lastTempAverage = average;
tempSum = 0;
}

46
src/sensors/softfusion/TempGradientCalculator.h Normal file
View File

@@ -0,0 +1,46 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2025 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <Arduino.h>
#include <cstdint>
#include <functional>
class TemperatureGradientCalculator {
public:
explicit TemperatureGradientCalculator(
const std::function<void(float gradient)>& callback
);
void feedSample(float sample, float timeStep);
void tick();
private:
std::function<void(float gradient)> callback;
static constexpr float AveragingTimeSeconds = 5.0f;
float tempSum = 0;
uint64_t lastAverageSentMillis = millis();
float lastTempAverage = 0;
};

29
src/sensors/softfusion/drivers/bmi270.h
View File

@@ -30,6 +30,7 @@
#include <limits>
#include "bmi270fw.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -52,6 +53,16 @@ struct BMI270 {
static constexpr float GyroSensitivity = 32.768f;
static constexpr float AccelSensitivity = 2048.0f;
static constexpr float TemperatureZROChange = 6.667f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.5f,
.biasClip = 1.0f,
.restThGyr = 0.5f,
.restThAcc = 0.196f,
};
struct MotionlessCalibrationData {
bool valid;
uint8_t x, y, z;
@@ -314,7 +325,7 @@ struct BMI270 {
return true;
}
void motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) {
bool motionlessCalibration(MotionlessCalibrationData& gyroSensitivity) {
// perfrom gyroscope motionless sensitivity calibration (CRT)
// need to start from clean state according to spec
restartAndInit();
@@ -340,6 +351,8 @@ struct BMI270 {
i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn);
delay(100);
bool success;
if (status != 0) {
logger.error(
"CRT failed with status 0x%x. Recalibrate again to enable CRT.",
@@ -348,6 +361,8 @@ struct BMI270 {
if (status == 0x03) {
logger.error("Reason: tracker was moved during CRT!");
}
success = false;
} else {
std::array<uint8_t, 3> crt_values;
i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data());
@@ -361,6 +376,8 @@ struct BMI270 {
gyroSensitivity.x = crt_values[0];
gyroSensitivity.y = crt_values[1];
gyroSensitivity.z = crt_values[2];
success = true;
}
// CRT seems to leave some state behind which isn't persisted after
@@ -369,6 +386,8 @@ struct BMI270 {
restartAndInit();
setNormalConfig(gyroSensitivity);
return success;
}
float getDirectTemp() const {
@@ -391,8 +410,12 @@ struct BMI270 {
return to_ret;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
const auto bytes_to_read = std::min(

72
src/sensors/softfusion/drivers/icm42688.h
View File

@@ -27,6 +27,8 @@
#include <array>
#include <cstdint>
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
@@ -42,12 +44,28 @@ struct ICM42688 {
static constexpr float GyrTs = 1.0 / 500.0;
static constexpr float AccTs = 1.0 / 100.0;
static constexpr float TempTs = 1.0 / 500.0;
static constexpr float MagTs = 1.0 / 100;
static constexpr float GyroSensitivity = 32.8f;
static constexpr float AccelSensitivity = 4096.0f;
static constexpr bool Uses32BitSensorData = true;
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 2.07f;
static constexpr float TemperatureZROChange = 20.0f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.5f,
.biasClip = 1.0f,
.restThGyr = 0.5f,
.restThAcc = 0.196f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
@@ -59,7 +77,6 @@ struct ICM42688 {
static constexpr uint8_t reg = 0x75;
static constexpr uint8_t value = 0x47;
};
static constexpr uint8_t TempData = 0x1d;
struct DeviceConfig {
static constexpr uint8_t reg = 0x11;
@@ -78,8 +95,8 @@ struct ICM42688 {
struct FifoConfig1 {
static constexpr uint8_t reg = 0x5f;
static constexpr uint8_t value
= 0b1 | (0b1 << 1)
| (0b0 << 2); // fifo accel en=1, gyro=1, temp=0 todo: fsync, hires
= 0b1 | (0b1 << 1) | (0b1 << 2)
| (0b0 << 4); // fifo accel en=1, gyro=1, temp=1, hires=1
};
struct GyroConfig {
static constexpr uint8_t reg = 0x4f;
@@ -111,15 +128,18 @@ struct ICM42688 {
struct {
int16_t accel[3];
int16_t gyro[3];
uint8_t temp;
uint8_t timestamp[2]; // cannot do uint16_t because it's unaligned
uint16_t temp;
uint16_t timestamp;
uint8_t xlsb;
uint8_t ylsb;
uint8_t zlsb;
} part;
uint8_t raw[15];
uint8_t raw[19];
};
};
#pragma pack(pop)
static constexpr size_t FullFifoEntrySize = 16;
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
bool initialize() {
// perform initialization step
@@ -137,14 +157,12 @@ struct ICM42688 {
return true;
}
float getDirectTemp() const {
const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
float result = ((float)value / 132.48f) + 25.0f;
return result;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTemperatureSample
) {
const auto fifo_bytes = i2c.readReg16(Regs::FifoCount);
std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
@@ -161,13 +179,31 @@ struct ICM42688 {
&read_buffer[i + 0x1],
sizeof(FifoEntryAligned)
); // skip fifo header
processGyroSample(entry.part.gyro, GyrTs);
const int32_t gyroData[3]{
static_cast<int32_t>(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf),
static_cast<int32_t>(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf),
static_cast<int32_t>(entry.part.gyro[2]) << 4 | (entry.part.zlsb & 0xf),
};
processGyroSample(gyroData, GyrTs);
if (entry.part.accel[0] != -32768) {
processAccelSample(entry.part.accel, AccTs);
const int32_t accelData[3]{
static_cast<int32_t>(entry.part.accel[0]) << 4
| (static_cast<int32_t>(entry.part.xlsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[1]) << 4
| (static_cast<int32_t>(entry.part.ylsb) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[2]) << 4
| (static_cast<int32_t>(entry.part.zlsb) & 0xf0 >> 4),
};
processAccelSample(accelData, AccTs);
}
if (entry.part.temp != 0x8000) {
processTemperatureSample(static_cast<int16_t>(entry.part.temp), TempTs);
}
}
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

11
src/sensors/softfusion/drivers/icm45605.h
View File

@@ -24,6 +24,7 @@
#pragma once
#include "icm45base.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -39,6 +40,14 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
static constexpr auto Name = "ICM-45605";
static constexpr auto Type = SensorTypeID::ICM45605;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.3f,
.biasClip = 0.6f,
.restThGyr = 0.3f,
.restThAcc = 0.0098f,
};
ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: ICM45Base<I2CImpl>{i2c, logger} {}
@@ -49,8 +58,6 @@ struct ICM45605 : public ICM45Base<I2CImpl> {
};
};
float getDirectTemp() const { return ICM45Base<I2CImpl>::getDirectTemp(); }
bool initialize() {
ICM45Base<I2CImpl>::softResetIMU();
return ICM45Base<I2CImpl>::initializeBase();

11
src/sensors/softfusion/drivers/icm45686.h
View File

@@ -24,6 +24,7 @@
#pragma once
#include "icm45base.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -39,6 +40,14 @@ struct ICM45686 : public ICM45Base<I2CImpl> {
static constexpr auto Name = "ICM-45686";
static constexpr auto Type = SensorTypeID::ICM45686;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 0.5f,
.biasClip = 1.0f,
.restThGyr = 0.5f,
.restThAcc = 0.196f,
};
ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
: ICM45Base<I2CImpl>{i2c, logger} {}
@@ -59,8 +68,6 @@ struct ICM45686 : public ICM45Base<I2CImpl> {
};
};
float getDirectTemp() const { return ICM45Base<I2CImpl>::getDirectTemp(); }
using ICM45Base<I2CImpl>::i2c;
bool initialize() {

90
src/sensors/softfusion/drivers/icm45base.h
View File

@@ -30,7 +30,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers {
// and gyroscope range at 4000dps
// using high resolution mode
// Uses 32.768kHz clock
// Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz
// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
// Timestamps reading not used, as they're useless (constant predefined increment)
template <typename I2CImpl>
@@ -38,7 +38,7 @@ struct ICM45Base {
static constexpr uint8_t Address = 0x68;
static constexpr float GyrTs = 1.0 / 409.6;
static constexpr float AccTs = 1.0 / 204.8;
static constexpr float AccTs = 1.0 / 102.4;
static constexpr float TempTs = 1.0 / 409.6;
static constexpr float MagTs = 1.0 / 100;
@@ -49,8 +49,13 @@ struct ICM45Base {
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 128.0f;
static constexpr float TemperatureZROChange = 20.0f;
static constexpr bool Uses32BitSensorData = true;
static constexpr int fifoReadSize
= 8; // Can't be more than 12 or it will overflow i2c read size, default 8
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
@@ -74,7 +79,7 @@ struct ICM45Base {
struct AccelConfig {
static constexpr uint8_t reg = 0x1b;
static constexpr uint8_t value
= (0b000 << 4) | 0b1000; // 32g, odr = 204.8Hz
= (0b000 << 4) | 0b1001; // 32g, odr = 102.4Hz
};
struct FifoConfig0 {
@@ -106,20 +111,20 @@ struct ICM45Base {
#pragma pack(push, 1)
struct FifoEntryAligned {
union {
struct {
int16_t accel[3];
int16_t gyro[3];
uint16_t temp;
uint16_t timestamp;
uint8_t lsb[3];
} part;
uint8_t raw[19];
};
uint8_t header;
int16_t accel[3];
int16_t gyro[3];
uint16_t temp;
uint16_t timestamp;
uint8_t lsb[3];
};
struct FifoBuffer {
FifoEntryAligned entry[fifoReadSize];
};
#pragma pack(pop)
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned);
static constexpr size_t FullFifoBufferSize = sizeof(FifoBuffer);
void softResetIMU() {
i2c.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset);
@@ -138,54 +143,49 @@ struct ICM45Base {
return true;
}
float getDirectTemp() const {
const auto value = static_cast<int16_t>(i2c.readReg16(BaseRegs::TempData));
float result = ((float)value / 132.48f) + 25.0f;
return result;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTemperatureSample
) {
const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount);
const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize);
std::array<uint8_t, FullFifoEntrySize * 8> read_buffer; // max 8 readings
FifoBuffer read_buffer;
const auto bytes_to_read = std::min(
static_cast<size_t>(read_buffer.size()),
static_cast<size_t>(FullFifoBufferSize),
static_cast<size_t>(fifo_bytes)
)
/ FullFifoEntrySize * FullFifoEntrySize;
i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data());
i2c.readBytes(BaseRegs::FifoData, bytes_to_read, (uint8_t*)&read_buffer);
uint8_t index = 0;
for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
FifoEntryAligned entry;
memcpy(
entry.raw,
&read_buffer[i + 0x1],
sizeof(FifoEntryAligned)
); // skip fifo header
FifoEntryAligned entry = read_buffer.entry[index++];
const int32_t gyroData[3]{
static_cast<int32_t>(entry.part.gyro[0]) << 4
| (entry.part.lsb[0] & 0xf),
static_cast<int32_t>(entry.part.gyro[1]) << 4
| (entry.part.lsb[1] & 0xf),
static_cast<int32_t>(entry.part.gyro[2]) << 4
| (entry.part.lsb[2] & 0xf),
static_cast<int32_t>(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf),
static_cast<int32_t>(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf),
static_cast<int32_t>(entry.gyro[2]) << 4 | (entry.lsb[2] & 0xf),
};
processGyroSample(gyroData, GyrTs);
if (entry.part.accel[0] != -32768) {
if (entry.accel[0] != -32768) {
const int32_t accelData[3]{
static_cast<int32_t>(entry.part.accel[0]) << 4
| (static_cast<int32_t>(entry.part.lsb[0]) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[1]) << 4
| (static_cast<int32_t>(entry.part.lsb[1]) & 0xf0 >> 4),
static_cast<int32_t>(entry.part.accel[2]) << 4
| (static_cast<int32_t>(entry.part.lsb[2]) & 0xf0 >> 4),
static_cast<int32_t>(entry.accel[0]) << 4
| (static_cast<int32_t>(entry.lsb[0]) & 0xf0 >> 4),
static_cast<int32_t>(entry.accel[1]) << 4
| (static_cast<int32_t>(entry.lsb[1]) & 0xf0 >> 4),
static_cast<int32_t>(entry.accel[2]) << 4
| (static_cast<int32_t>(entry.lsb[2]) & 0xf0 >> 4),
};
processAccelSample(accelData, AccTs);
}
if (entry.temp != 0x8000) {
processTemperatureSample(static_cast<int16_t>(entry.temp), TempTs);
}
}
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
}; // namespace SlimeVR::Sensors::SoftFusion::Drivers

19
src/sensors/softfusion/drivers/lsm6ds-common.h
View File

@@ -38,14 +38,6 @@ struct LSM6DSOutputHandler {
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
template <typename Regs>
float getDirectTemp() const {
const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
float result = ((float)value / 256.0f) + 25.0f;
return result;
}
#pragma pack(push, 1)
struct FifoEntryAligned {
union {
@@ -57,12 +49,14 @@ struct LSM6DSOutputHandler {
static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
template <typename AccelCall, typename GyroCall, typename Regs>
template <typename AccelCall, typename GyroCall, typename TempCall, typename Regs>
void bulkRead(
AccelCall& processAccelSample,
GyroCall& processGyroSample,
TempCall& processTempSample,
float GyrTs,
float AccTs
float AccTs,
float TempTs
) {
constexpr auto FIFO_SAMPLES_MASK = 0x3ff;
constexpr auto FIFO_OVERRUN_LATCHED_MASK = 0x800;
@@ -99,9 +93,12 @@ struct LSM6DSOutputHandler {
case 0x02: // Accel NC
processAccelSample(entry.xyz, AccTs);
break;
case 0x03: // Temperature
processTempSample(entry.xyz[0], TempTs);
break;
}
}
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

40
src/sensors/softfusion/drivers/lsm6ds3trc.h
View File

@@ -27,6 +27,8 @@
#include <array>
#include <cstdint>
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
@@ -44,10 +46,24 @@ struct LSM6DS3TRC {
static constexpr float GyrTs = 1.0 / Freq;
static constexpr float AccTs = 1.0 / Freq;
static constexpr float MagTs = 1.0 / Freq;
static constexpr float TempTs = 1.0 / Freq;
static constexpr float GyroSensitivity = 28.571428571f;
static constexpr float AccelSensitivity = 4098.360655738f;
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 256.0f;
static constexpr float TemperatureZROChange = 2.0f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 3.0f,
.biasClip = 6.0f,
.restThGyr = 3.0f,
.restThAcc = 0.392f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger logger;
LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
@@ -59,7 +75,6 @@ struct LSM6DS3TRC {
static constexpr uint8_t reg = 0x0f;
static constexpr uint8_t value = 0x6a;
};
static constexpr uint8_t OutTemp = 0x20;
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b11 << 2) | (0b0110 << 4); // 8g, 416Hz
@@ -75,6 +90,10 @@ struct LSM6DS3TRC {
static constexpr uint8_t value = (1 << 6) | (1 << 2); // BDU = 1, IF_INC =
// 1
};
struct FifoCtrl2 {
static constexpr uint8_t reg = 0x07;
static constexpr uint8_t value = 0b1000; // temperature in fifo
};
struct FifoCtrl3 {
static constexpr uint8_t reg = 0x08;
static constexpr uint8_t value
@@ -97,20 +116,18 @@ struct LSM6DS3TRC {
i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value);
i2c.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value);
i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value);
i2c.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value);
i2c.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value);
i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value);
return true;
}
float getDirectTemp() const {
const auto value = static_cast<int16_t>(i2c.readReg16(Regs::OutTemp));
float result = ((float)value / 256.0f) + 25.0f;
return result;
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTemperatureSample
) {
const auto read_result = i2c.readReg16(Regs::FifoStatus);
if (read_result & 0x4000) { // overrun!
// disable and re-enable fifo to clear it
@@ -145,8 +162,9 @@ struct LSM6DS3TRC {
reinterpret_cast<const int16_t*>(&read_buffer[i + 3]),
AccTs
);
processTemperatureSample(read_buffer[i + 9], TempTs);
}
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

49
src/sensors/softfusion/drivers/lsm6dso.h
View File

@@ -28,6 +28,7 @@
#include <cstdint>
#include "lsm6ds-common.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -44,14 +45,29 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
static constexpr float GyrFreq = 416;
static constexpr float AccFreq = 104;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 52;
static constexpr float GyrTs = 1.0 / GyrFreq;
static constexpr float AccTs = 1.0 / AccFreq;
static constexpr float MagTs = 1.0 / MagFreq;
static constexpr float TempTs = 1.0 / TempFreq;
static constexpr float GyroSensitivity = 1000 / 35.0f;
static constexpr float AccelSensitivity = 1000 / 0.244f;
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 256.0f;
static constexpr float TemperatureZROChange = 10.0;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 1.0f,
.biasClip = 2.0f,
.restThGyr = 1.0f,
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
@@ -59,7 +75,6 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t reg = 0x0f;
static constexpr uint8_t value = 0x6c;
};
static constexpr uint8_t OutTemp = 0x20;
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
@@ -81,7 +96,8 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;
static constexpr uint8_t value = (0b110); // continuous mode
static constexpr uint8_t value = (0b110110); // continuous mode,
// temperature at 52Hz
};
static constexpr uint8_t FifoStatus = 0x3a;
@@ -103,19 +119,22 @@ struct LSM6DSO : LSM6DSOutputHandler<I2CImpl> {
return true;
}
float getDirectTemp() const {
return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
processAccelSample,
processGyroSample,
GyrTs,
AccTs
);
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

48
src/sensors/softfusion/drivers/lsm6dsr.h
View File

@@ -44,14 +44,29 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
static constexpr float GyrFreq = 416;
static constexpr float AccFreq = 104;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 52;
static constexpr float GyrTs = 1.0 / GyrFreq;
static constexpr float AccTs = 1.0 / AccFreq;
static constexpr float MagTs = 1.0 / MagFreq;
static constexpr float TempTs = 1.0 / TempFreq;
static constexpr float GyroSensitivity = 1000 / 35.0f;
static constexpr float AccelSensitivity = 1000 / 0.244f;
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 256.0f;
static constexpr float TemperatureZROChange = 20.0f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 1.0f,
.biasClip = 2.0f,
.restThGyr = 1.0f,
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
@@ -59,7 +74,6 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t reg = 0x0f;
static constexpr uint8_t value = 0x6b;
};
static constexpr uint8_t OutTemp = 0x20;
struct Ctrl1XL {
static constexpr uint8_t reg = 0x10;
static constexpr uint8_t value = (0b01001100); // XL at 104 Hz, 8g FS
@@ -81,7 +95,8 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;
static constexpr uint8_t value = (0b110); // continuous mode
static constexpr uint8_t value = (0b110110); // continuous mode,
// temperature at 52Hz
};
static constexpr uint8_t FifoStatus = 0x3a;
@@ -103,19 +118,22 @@ struct LSM6DSR : LSM6DSOutputHandler<I2CImpl> {
return true;
}
float getDirectTemp() const {
return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
processAccelSample,
processGyroSample,
GyrTs,
AccTs
);
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

49
src/sensors/softfusion/drivers/lsm6dsv.h
View File

@@ -28,6 +28,7 @@
#include <cstdint>
#include "lsm6ds-common.h"
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
@@ -44,14 +45,29 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
static constexpr float GyrFreq = 480;
static constexpr float AccFreq = 120;
static constexpr float MagFreq = 120;
static constexpr float TempFreq = 60;
static constexpr float GyrTs = 1.0 / GyrFreq;
static constexpr float AccTs = 1.0 / AccFreq;
static constexpr float MagTs = 1.0 / MagFreq;
static constexpr float TempTs = 1.0 / TempFreq;
static constexpr float GyroSensitivity = 1000 / 35.0f;
static constexpr float AccelSensitivity = 1000 / 0.244f;
static constexpr float TemperatureBias = 25.0f;
static constexpr float TemperatureSensitivity = 256.0f;
static constexpr float TemperatureZROChange = 16.667f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 1.0f,
.biasClip = 2.0f,
.restThGyr = 1.0f,
.restThAcc = 0.192f,
};
using LSM6DSOutputHandler<I2CImpl>::i2c;
struct Regs {
@@ -59,7 +75,6 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
static constexpr uint8_t reg = 0x0f;
static constexpr uint8_t value = 0x70;
};
static constexpr uint8_t OutTemp = 0x20;
struct HAODRCFG {
static constexpr uint8_t reg = 0x62;
static constexpr uint8_t value = (0b00); // 1st ODR table
@@ -93,7 +108,8 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
};
struct FifoCtrl4Mode {
static constexpr uint8_t reg = 0x0a;
static constexpr uint8_t value = (0b110); // continuous mode
static constexpr uint8_t value = (0b110110); // continuous mode,
// temperature at 60Hz
};
static constexpr uint8_t FifoStatus = 0x1b;
@@ -118,19 +134,22 @@ struct LSM6DSV : LSM6DSOutputHandler<I2CImpl> {
return true;
}
float getDirectTemp() const {
return LSM6DSOutputHandler<I2CImpl>::template getDirectTemp<Regs>();
}
template <typename AccelCall, typename GyroCall>
void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
LSM6DSOutputHandler<I2CImpl>::template bulkRead<AccelCall, GyroCall, Regs>(
processAccelSample,
processGyroSample,
GyrTs,
AccTs
);
template <typename AccelCall, typename GyroCall, typename TempCall>
void bulkRead(
AccelCall&& processAccelSample,
GyroCall&& processGyroSample,
TempCall&& processTempSample
) {
LSM6DSOutputHandler<I2CImpl>::
template bulkRead<AccelCall, GyroCall, TempCall, Regs>(
processAccelSample,
processGyroSample,
processTempSample,
GyrTs,
AccTs,
TempTs
);
}
};
} // namespace SlimeVR::Sensors::SoftFusion::Drivers
} // namespace SlimeVR::Sensors::SoftFusion::Drivers

12
src/sensors/softfusion/drivers/mpu6050.h
View File

@@ -29,6 +29,8 @@
#include <array>
#include <cstdint>
#include "vqf.h"
namespace SlimeVR::Sensors::SoftFusion::Drivers {
// Driver uses acceleration range at 8g
@@ -65,6 +67,16 @@ struct MPU6050 {
static constexpr float GyroSensitivity = 32.8f;
static constexpr float AccelSensitivity = 4096.0f;
static constexpr float TemperatureZROChange = 1.6f;
static constexpr VQFParams SensorVQFParams{
.motionBiasEstEnabled = true,
.biasSigmaInit = 20.0f,
.biasClip = 40.0f,
.restThGyr = 20.0f,
.restThAcc = 0.784f,
};
I2CImpl i2c;
SlimeVR::Logging::Logger& logger;
MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger)

154
src/sensors/softfusion/nonblockingcalibration/AccelBiasCalibrationStep.h Normal file
View File

@@ -0,0 +1,154 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cmath>
#include <optional>
#include "../../../consts.h"
#include "CalibrationStep.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename SensorRawT>
class AccelBiasCalibrationStep : public CalibrationStep<SensorRawT> {
using CalibrationStep<SensorRawT>::sensorConfig;
using typename CalibrationStep<SensorRawT>::TickResult;
public:
AccelBiasCalibrationStep(
SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig,
float accelScale
)
: CalibrationStep<SensorRawT>{sensorConfig}
, accelScale{accelScale} {}
void start() override final {
CalibrationStep<SensorRawT>::start();
calibrationData = CalibrationData{};
}
TickResult tick() override final {
if (!calibrationData.value().axisDetermined) {
return TickResult::CONTINUE;
}
if (calibrationData.value().largestAxis == -1) {
return TickResult::SKIP;
}
if (calibrationData.value().sampleCount < accelBiasCalibrationSampleCount) {
return TickResult::CONTINUE;
}
float accelAverage = calibrationData.value().accelSum
/ static_cast<float>(calibrationData.value().sampleCount);
float expected = accelAverage > 0 ? CONST_EARTH_GRAVITY : -CONST_EARTH_GRAVITY;
float accelOffset = accelAverage * accelScale - expected;
sensorConfig.A_off[calibrationData.value().largestAxis] = accelOffset;
sensorConfig.accelCalibrated[calibrationData.value().largestAxis] = true;
return TickResult::DONE;
}
void cancel() override final { calibrationData.reset(); }
void processAccelSample(const SensorRawT accelSample[3]) override final {
if (calibrationData.value().axisDetermined) {
calibrationData.value().accelSum
+= accelSample[calibrationData.value().largestAxis];
calibrationData.value().sampleCount++;
return;
}
float absAxes[3]{
std::abs(static_cast<float>(accelSample[0])),
std::abs(static_cast<float>(accelSample[1])),
std::abs(static_cast<float>(accelSample[2])),
};
size_t largestAxis;
if (absAxes[0] > absAxes[1] && absAxes[0] > absAxes[2]) {
largestAxis = 0;
} else if (absAxes[1] > absAxes[2]) {
largestAxis = 1;
} else {
largestAxis = 2;
}
if (sensorConfig.accelCalibrated[largestAxis]) {
calibrationData.value().axisDetermined = true;
calibrationData.value().largestAxis = -1;
return;
}
float smallAxisPercentage1
= absAxes[(largestAxis + 1) % 3] / absAxes[largestAxis];
float smallAxisPercentage2
= absAxes[(largestAxis + 2) % 3] / absAxes[largestAxis];
if (smallAxisPercentage1 > allowableVerticalAxisPercentage
|| smallAxisPercentage2 > allowableVerticalAxisPercentage) {
calibrationData.value().axisDetermined = true;
calibrationData.value().largestAxis = -1;
return;
}
calibrationData.value().axisDetermined = true;
calibrationData.value().largestAxis = largestAxis;
calibrationData.value().currentAxis[0] = accelSample[0];
calibrationData.value().currentAxis[1] = accelSample[1];
calibrationData.value().currentAxis[2] = accelSample[2];
}
bool allAxesCalibrated() {
return sensorConfig.accelCalibrated[0] && sensorConfig.accelCalibrated[1]
&& sensorConfig.accelCalibrated[2];
}
bool anyAxesCalibrated() {
return sensorConfig.accelCalibrated[0] || sensorConfig.accelCalibrated[1]
|| sensorConfig.accelCalibrated[2];
}
private:
static constexpr size_t accelBiasCalibrationSampleCount = 96;
static constexpr float allowableVerticalAxisPercentage = 0.05;
struct CalibrationData {
bool axisDetermined = false;
int16_t currentAxis[3]{0, 0, 0};
int32_t largestAxis = -1;
int32_t accelSum = 0;
size_t sampleCount = 0;
};
std::optional<CalibrationData> calibrationData;
float accelScale;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

66
src/sensors/softfusion/nonblockingcalibration/CalibrationStep.h Normal file
View File

@@ -0,0 +1,66 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename SensorRawT>
class CalibrationStep {
public:
enum class TickResult {
CONTINUE,
SKIP,
DONE,
};
CalibrationStep(SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig)
: sensorConfig{sensorConfig} {}
virtual ~CalibrationStep() = default;
virtual void start() { restDetectionDelayStartMillis = millis(); }
virtual TickResult tick() = 0;
virtual void cancel() = 0;
virtual bool requiresRest() { return true; }
virtual void processAccelSample(const SensorRawT accelSample[3]) {}
virtual void processGyroSample(const SensorRawT accelSample[3]) {}
virtual void processTempSample(float tempSample) {}
bool restDetectionDelayElapsed() {
return (millis() - restDetectionDelayStartMillis)
>= restDetectionDelaySeconds * 1e3;
}
protected:
SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig;
float restDetectionDelaySeconds = 5.0f;
private:
uint32_t restDetectionDelayStartMillis;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

196
src/sensors/softfusion/nonblockingcalibration/GyroBiasCalibrationStep.h Normal file
View File

@@ -0,0 +1,196 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cmath>
#include <optional>
#include "CalibrationStep.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename SensorRawT>
class GyroBiasCalibrationStep : public CalibrationStep<SensorRawT> {
using CalibrationStep<SensorRawT>::sensorConfig;
using typename CalibrationStep<SensorRawT>::TickResult;
public:
GyroBiasCalibrationStep(
SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig
)
: CalibrationStep<SensorRawT>{sensorConfig} {}
void start() override final {
CalibrationStep<SensorRawT>::start();
calibrationData = {millis()};
}
TickResult tick() override final {
if (millis() - calibrationData.value().startMillis
< gyroBiasCalibrationSeconds * 1e3) {
return TickResult::CONTINUE;
}
float gyroOffsetX = calibrationData.value().gyroSums[0]
/ static_cast<float>(calibrationData.value().sampleCount);
float gyroOffsetY = calibrationData.value().gyroSums[1]
/ static_cast<float>(calibrationData.value().sampleCount);
float gyroOffsetZ = calibrationData.value().gyroSums[2]
/ static_cast<float>(calibrationData.value().sampleCount);
if (sensorConfig.gyroPointsCalibrated == 0) {
sensorConfig.G_off1[0] = gyroOffsetX;
sensorConfig.G_off1[1] = gyroOffsetY;
sensorConfig.G_off1[2] = gyroOffsetZ;
sensorConfig.gyroPointsCalibrated = 1;
sensorConfig.gyroMeasurementTemperature1
= calibrationData.value().temperature;
return TickResult::DONE;
}
if (sensorConfig.gyroPointsCalibrated == 1) {
if (calibrationData.value().temperature
> sensorConfig.gyroMeasurementTemperature1) {
sensorConfig.G_off2[0] = gyroOffsetX;
sensorConfig.G_off2[1] = gyroOffsetY;
sensorConfig.G_off2[2] = gyroOffsetZ;
sensorConfig.gyroMeasurementTemperature2
= calibrationData.value().temperature;
} else {
sensorConfig.G_off2[0] = sensorConfig.G_off1[0];
sensorConfig.G_off2[1] = sensorConfig.G_off1[1];
sensorConfig.G_off2[2] = sensorConfig.G_off1[2];
sensorConfig.gyroMeasurementTemperature2
= sensorConfig.gyroMeasurementTemperature1;
sensorConfig.G_off1[0] = gyroOffsetX;
sensorConfig.G_off1[1] = gyroOffsetY;
sensorConfig.G_off1[2] = gyroOffsetZ;
sensorConfig.gyroMeasurementTemperature1
= calibrationData.value().temperature;
}
sensorConfig.gyroPointsCalibrated = 2;
return TickResult::DONE;
}
if (calibrationData.value().temperature
< sensorConfig.gyroMeasurementTemperature1) {
sensorConfig.G_off1[0] = gyroOffsetX;
sensorConfig.G_off1[1] = gyroOffsetY;
sensorConfig.G_off1[2] = gyroOffsetZ;
sensorConfig.gyroMeasurementTemperature1
= calibrationData.value().temperature;
} else {
sensorConfig.G_off2[0] = gyroOffsetX;
sensorConfig.G_off2[1] = gyroOffsetY;
sensorConfig.G_off2[2] = gyroOffsetZ;
sensorConfig.gyroMeasurementTemperature2
= calibrationData.value().temperature;
}
return TickResult::DONE;
}
void cancel() override final { calibrationData.reset(); }
void processGyroSample(const SensorRawT gyroSample[3]) override final {
calibrationData.value().gyroSums[0] += gyroSample[0];
calibrationData.value().gyroSums[1] += gyroSample[1];
calibrationData.value().gyroSums[2] += gyroSample[2];
calibrationData.value().sampleCount++;
}
void processTempSample(float tempSample) override final {
calibrationData.value().temperature = tempSample;
if (sensorConfig.gyroPointsCalibrated == 0) {
return;
}
if (sensorConfig.gyroPointsCalibrated == 1) {
float tempDiff
= std::abs(sensorConfig.gyroMeasurementTemperature1 - tempSample);
if (tempDiff < gyroBiasTemperatureDifference) {
calibrationData.value().gyroSums[0] = 0;
calibrationData.value().gyroSums[1] = 0;
calibrationData.value().gyroSums[2] = 0;
calibrationData.value().sampleCount = 0;
calibrationData.value().startMillis = millis();
}
return;
}
if (tempSample >= sensorConfig.gyroMeasurementTemperature1
&& tempSample <= sensorConfig.gyroMeasurementTemperature2) {
calibrationData.value().gyroSums[0] = 0;
calibrationData.value().gyroSums[1] = 0;
calibrationData.value().gyroSums[2] = 0;
calibrationData.value().sampleCount = 0;
calibrationData.value().startMillis = millis();
}
}
void swapCalibrationIfNecessary() {
if (sensorConfig.gyroPointsCalibrated == 2
&& sensorConfig.gyroMeasurementTemperature1
> sensorConfig.gyroMeasurementTemperature2) {
float tempG_off[3]{
sensorConfig.G_off1[0],
sensorConfig.G_off1[1],
sensorConfig.G_off1[2],
};
float tempGTemperature = sensorConfig.gyroMeasurementTemperature1;
sensorConfig.G_off1[0] = sensorConfig.G_off2[0];
sensorConfig.G_off1[1] = sensorConfig.G_off2[1];
sensorConfig.G_off1[2] = sensorConfig.G_off2[2];
sensorConfig.gyroMeasurementTemperature1
= sensorConfig.gyroMeasurementTemperature2;
sensorConfig.G_off2[0] = tempG_off[0];
sensorConfig.G_off2[1] = tempG_off[1];
sensorConfig.G_off2[2] = tempG_off[2];
sensorConfig.gyroMeasurementTemperature2 = tempGTemperature;
}
}
private:
static constexpr float gyroBiasCalibrationSeconds = 5;
static constexpr float gyroBiasTemperatureDifference = 5;
struct CalibrationData {
uint64_t startMillis = 0;
float temperature = 0;
int32_t gyroSums[3]{0, 0, 0};
size_t sampleCount = 0;
};
std::optional<CalibrationData> calibrationData;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

99
src/sensors/softfusion/nonblockingcalibration/MotionlessCalibrationStep.h Normal file
View File

@@ -0,0 +1,99 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <cstring>
#include <optional>
#include "CalibrationStep.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename IMU, typename SensorRawT>
class MotionlessCalibrationStep : public CalibrationStep<SensorRawT> {
using CalibrationStep<SensorRawT>::sensorConfig;
using typename CalibrationStep<SensorRawT>::TickResult;
public:
MotionlessCalibrationStep(
SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig,
IMU& imu
)
: CalibrationStep<SensorRawT>{sensorConfig}
, imu{imu} {}
void start() override final {
CalibrationStep<SensorRawT>::start();
calibrationData = {millis()};
}
TickResult tick() override final {
if constexpr (HasMotionlessCalib) {
if (millis() - calibrationData.value().startMillis
< motionlessCalibrationDelay * 1e3) {
return TickResult::CONTINUE;
}
typename IMU::MotionlessCalibrationData motionlessCalibrationData;
if (!imu.motionlessCalibration(motionlessCalibrationData)) {
return TickResult::CONTINUE;
}
std::memcpy(
sensorConfig.MotionlessData,
&motionlessCalibrationData,
sizeof(motionlessCalibrationData)
);
sensorConfig.motionlessCalibrated = true;
return TickResult::DONE;
} else {
return TickResult::DONE;
}
}
void cancel() override final { calibrationData.reset(); }
private:
static constexpr float motionlessCalibrationDelay = 5;
static constexpr bool HasMotionlessCalib
= requires(IMU& i) { typename IMU::MotionlessCalibrationData; };
static constexpr size_t MotionlessCalibDataSize() {
if constexpr (HasMotionlessCalib) {
return sizeof(typename IMU::MotionlessCalibrationData);
} else {
return 0;
}
}
struct CalibrationData {
uint64_t startMillis = 0;
};
std::optional<CalibrationData> calibrationData;
IMU& imu;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

432
src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h Normal file
View File

@@ -0,0 +1,432 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <vector3.h>
#include <cstdint>
#include "../../../GlobalVars.h"
#include "../../../configuration/Configuration.h"
#include "../../SensorFusionRestDetect.h"
#include "AccelBiasCalibrationStep.h"
#include "GyroBiasCalibrationStep.h"
#include "MotionlessCalibrationStep.h"
#include "NullCalibrationStep.h"
#include "SampleRateCalibrationStep.h"
#include "configuration/SensorConfig.h"
#include "logging/Logger.h"
#include "sensors/softfusion/CalibrationBase.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename IMU, typename RawSensorT, typename RawVectorT>
class NonBlockingCalibrator
: public Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT> {
public:
static constexpr bool HasUpsideDownCalibration = false;
using Base = Sensor::CalibrationBase<IMU, RawSensorT, RawVectorT>;
NonBlockingCalibrator(
SensorFusionRestDetect& fusion,
IMU& imu,
uint8_t sensorId,
Logging::Logger& logger,
float TempTs,
float AScale,
float GScale
)
: Base(fusion, imu, sensorId, logger, TempTs, AScale, GScale) {
calibration.T_Ts = TempTs;
activeCalibration.T_Ts = TempTs;
}
bool calibrationMatches(const Configuration::SensorConfig& sensorCalibration
) final {
return sensorCalibration.type
== SlimeVR::Configuration::SensorConfigType::NONBLOCKING
&& (sensorCalibration.data.sfusion.ImuType == IMU::Type)
&& (sensorCalibration.data.sfusion.MotionlessDataLen
== Base::MotionlessCalibDataSize());
}
void assignCalibration(const Configuration::SensorConfig& sensorCalibration) final {
calibration = sensorCalibration.data.nonblocking;
activeCalibration = sensorCalibration.data.nonblocking;
calculateZROChange();
}
void begin() final {
startupMillis = millis();
gyroBiasCalibrationStep.swapCalibrationIfNecessary();
computeNextCalibrationStep();
calculateZROChange();
printCalibration();
}
void tick() final {
if (skippedAStep && !lastTickRest && fusion.getRestDetected()) {
computeNextCalibrationStep();
skippedAStep = false;
}
if (millis() - startupMillis < initialStartupDelaySeconds * 1e3) {
return;
}
if (!fusion.getRestDetected() && currentStep->requiresRest()) {
if (isCalibrating) {
currentStep->cancel();
isCalibrating = false;
}
lastTickRest = fusion.getRestDetected();
return;
}
if (!isCalibrating) {
isCalibrating = true;
currentStep->start();
}
if (currentStep->requiresRest() && !currentStep->restDetectionDelayElapsed()) {
lastTickRest = fusion.getRestDetected();
return;
}
auto result = currentStep->tick();
switch (result) {
case CalibrationStep<RawSensorT>::TickResult::DONE:
stepCalibrationForward();
break;
case CalibrationStep<RawSensorT>::TickResult::SKIP:
stepCalibrationForward(false);
break;
case CalibrationStep<RawSensorT>::TickResult::CONTINUE:
break;
}
lastTickRest = fusion.getRestDetected();
}
void scaleAccelSample(sensor_real_t accelSample[3]) final {
accelSample[0] = accelSample[0] * AScale - activeCalibration.A_off[0];
accelSample[1] = accelSample[1] * AScale - activeCalibration.A_off[1];
accelSample[2] = accelSample[2] * AScale - activeCalibration.A_off[2];
}
float getAccelTimestep() final { return activeCalibration.A_Ts; }
void scaleGyroSample(sensor_real_t gyroSample[3]) final {
gyroSample[0] = static_cast<sensor_real_t>(
GScale * (gyroSample[0] - activeCalibration.G_off1[0])
);
gyroSample[1] = static_cast<sensor_real_t>(
GScale * (gyroSample[1] - activeCalibration.G_off1[1])
);
gyroSample[2] = static_cast<sensor_real_t>(
GScale * (gyroSample[2] - activeCalibration.G_off1[2])
);
}
float getGyroTimestep() final { return activeCalibration.G_Ts; }
float getTempTimestep() final { return activeCalibration.T_Ts; }
const uint8_t* getMotionlessCalibrationData() final {
return activeCalibration.MotionlessData;
}
void provideAccelSample(const RawSensorT accelSample[3]) final {
if (isCalibrating) {
currentStep->processAccelSample(accelSample);
}
}
void provideGyroSample(const RawSensorT gyroSample[3]) final {
if (isCalibrating) {
currentStep->processGyroSample(gyroSample);
}
}
void provideTempSample(float tempSample) final {
if (isCalibrating) {
currentStep->processTempSample(tempSample);
}
}
void calculateZROChange() {
if (activeCalibration.gyroPointsCalibrated < 2) {
activeZROChange = IMU::TemperatureZROChange;
}
float diffX
= (activeCalibration.G_off2[0] - activeCalibration.G_off1[0]) * GScale;
float diffY
= (activeCalibration.G_off2[1] - activeCalibration.G_off1[1]) * GScale;
float diffZ
= (activeCalibration.G_off2[2] - activeCalibration.G_off1[2]) * GScale;
float maxDiff
= std::max(std::max(std::abs(diffX), std::abs(diffY)), std::abs(diffZ));
activeZROChange = 0.1f / maxDiff
/ (activeCalibration.gyroMeasurementTemperature2
- activeCalibration.gyroMeasurementTemperature1);
}
float getZROChange() final { return activeZROChange; }
private:
enum class CalibrationStepEnum {
NONE,
SAMPLING_RATE,
MOTIONLESS,
GYRO_BIAS,
ACCEL_BIAS,
};
void computeNextCalibrationStep() {
if (!calibration.sensorTimestepsCalibrated) {
nextCalibrationStep = CalibrationStepEnum::SAMPLING_RATE;
currentStep = &sampleRateCalibrationStep;
} else if (!calibration.motionlessCalibrated && Base::HasMotionlessCalib) {
nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
currentStep = &motionlessCalibrationStep;
} else if (calibration.gyroPointsCalibrated == 0) {
nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
currentStep = &gyroBiasCalibrationStep;
// } else if (!accelBiasCalibrationStep.allAxesCalibrated()) {
// nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS;
// currentStep = &accelBiasCalibrationStep;
} else {
nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
currentStep = &gyroBiasCalibrationStep;
}
}
void stepCalibrationForward(bool save = true) {
currentStep->cancel();
switch (nextCalibrationStep) {
case CalibrationStepEnum::NONE:
return;
case CalibrationStepEnum::SAMPLING_RATE:
nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
currentStep = &motionlessCalibrationStep;
if (save) {
printCalibration(CalibrationPrintFlags::TIMESTEPS);
}
break;
case CalibrationStepEnum::MOTIONLESS:
nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
currentStep = &gyroBiasCalibrationStep;
if (save) {
printCalibration(CalibrationPrintFlags::MOTIONLESS);
}
break;
case CalibrationStepEnum::GYRO_BIAS:
if (calibration.gyroPointsCalibrated == 1) {
// nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS;
// currentStep = &accelBiasCalibrationStep;
nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
currentStep = &gyroBiasCalibrationStep;
}
if (save) {
printCalibration(CalibrationPrintFlags::GYRO_BIAS);
}
break;
case CalibrationStepEnum::ACCEL_BIAS:
nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
currentStep = &gyroBiasCalibrationStep;
if (save) {
printCalibration(CalibrationPrintFlags::ACCEL_BIAS);
}
if (!accelBiasCalibrationStep.allAxesCalibrated()) {
skippedAStep = true;
}
break;
}
isCalibrating = false;
if (save) {
saveCalibration();
}
}
void saveCalibration() {
SlimeVR::Configuration::SensorConfig calibration{};
calibration.type = SlimeVR::Configuration::SensorConfigType::NONBLOCKING;
calibration.data.nonblocking = this->calibration;
configuration.setSensor(sensorId, calibration);
configuration.save();
ledManager.blink(100);
}
enum class CalibrationPrintFlags {
TIMESTEPS = 1,
MOTIONLESS = 2,
GYRO_BIAS = 4,
ACCEL_BIAS = 8,
};
static constexpr CalibrationPrintFlags PrintAllCalibration
= CalibrationPrintFlags::TIMESTEPS | CalibrationPrintFlags::MOTIONLESS
| CalibrationPrintFlags::GYRO_BIAS | CalibrationPrintFlags::ACCEL_BIAS;
void printCalibration(CalibrationPrintFlags toPrint = PrintAllCalibration) {
if (any(toPrint & CalibrationPrintFlags::TIMESTEPS)) {
if (calibration.sensorTimestepsCalibrated) {
logger.info(
"Calibrated timesteps: Accel %f, Gyro %f, Temperature %f",
calibration.A_Ts,
calibration.G_Ts,
calibration.T_Ts
);
} else {
logger.info("Sensor timesteps not calibrated");
}
}
if (Base::HasMotionlessCalib
&& any(toPrint & CalibrationPrintFlags::MOTIONLESS)) {
if (calibration.motionlessCalibrated) {
logger.info("Motionless calibration done");
} else {
logger.info("Motionless calibration not done");
}
}
if (any(toPrint & CalibrationPrintFlags::GYRO_BIAS)) {
if (calibration.gyroPointsCalibrated != 0) {
logger.info(
"Calibrated gyro bias at %fC: %f %f %f",
calibration.gyroMeasurementTemperature1,
calibration.G_off1[0],
calibration.G_off1[1],
calibration.G_off1[2]
);
} else {
logger.info("Gyro bias not calibrated");
}
if (calibration.gyroPointsCalibrated == 2) {
logger.info(
"Calibrated gyro bias at %fC: %f %f %f",
calibration.gyroMeasurementTemperature2,
calibration.G_off2[0],
calibration.G_off2[1],
calibration.G_off2[2]
);
}
}
if (any(toPrint & CalibrationPrintFlags::ACCEL_BIAS)) {
if (accelBiasCalibrationStep.allAxesCalibrated()) {
logger.info(
"Calibrated accel bias: %f %f %f",
calibration.A_off[0],
calibration.A_off[1],
calibration.A_off[2]
);
} else if (accelBiasCalibrationStep.anyAxesCalibrated()) {
logger.info(
"Partially calibrated accel bias: %f %f %f",
calibration.A_off[0],
calibration.A_off[1],
calibration.A_off[2]
);
} else {
logger.info("Accel bias not calibrated");
}
}
}
CalibrationStepEnum nextCalibrationStep = CalibrationStepEnum::MOTIONLESS;
static constexpr float initialStartupDelaySeconds = 5;
uint64_t startupMillis = millis();
SampleRateCalibrationStep<RawSensorT> sampleRateCalibrationStep{calibration};
MotionlessCalibrationStep<IMU, RawSensorT> motionlessCalibrationStep{
calibration,
sensor};
GyroBiasCalibrationStep<RawSensorT> gyroBiasCalibrationStep{calibration};
AccelBiasCalibrationStep<RawSensorT> accelBiasCalibrationStep{
calibration,
static_cast<float>(Base::AScale)};
NullCalibrationStep<RawSensorT> nullCalibrationStep{calibration};
CalibrationStep<RawSensorT>* currentStep = &nullCalibrationStep;
bool isCalibrating = false;
bool skippedAStep = false;
bool lastTickRest = false;
SlimeVR::Configuration::NonBlockingSensorConfig calibration{
// let's create here transparent calibration that doesn't affect input data
.ImuType = {IMU::Type},
.MotionlessDataLen = {Base::MotionlessCalibDataSize()},
.sensorTimestepsCalibrated = false,
.A_Ts = IMU::AccTs,
.G_Ts = IMU::GyrTs,
.M_Ts = IMU::MagTs,
.T_Ts = 0,
.motionlessCalibrated = false,
.MotionlessData = {},
.gyroPointsCalibrated = 0,
.gyroMeasurementTemperature1 = 0,
.G_off1 = {0.0, 0.0, 0.0},
.gyroMeasurementTemperature2 = 0,
.G_off2 = {0.0, 0.0, 0.0},
.accelCalibrated = {false, false, false},
.A_off = {0.0, 0.0, 0.0},
};
float activeZROChange = 0;
Configuration::NonBlockingSensorConfig activeCalibration = calibration;
using Base::AScale;
using Base::fusion;
using Base::GScale;
using Base::logger;
using Base::sensor;
using Base::sensorId;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

46
src/sensors/softfusion/nonblockingcalibration/NullCalibrationStep.h Normal file
View File

@@ -0,0 +1,46 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include "CalibrationStep.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename SensorRawT>
class NullCalibrationStep : public CalibrationStep<SensorRawT> {
using CalibrationStep<SensorRawT>::sensorConfig;
using typename CalibrationStep<SensorRawT>::TickResult;
public:
NullCalibrationStep(SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig)
: CalibrationStep<SensorRawT>{sensorConfig} {}
void start() override final { CalibrationStep<SensorRawT>::start(); }
TickResult tick() override final { return TickResult::CONTINUE; }
void cancel() override final {}
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

95
src/sensors/softfusion/nonblockingcalibration/SampleRateCalibrationStep.h Normal file
View File

@@ -0,0 +1,95 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#pragma once
#include <optional>
#include "CalibrationStep.h"
namespace SlimeVR::Sensors::NonBlockingCalibration {
template <typename SensorRawT>
class SampleRateCalibrationStep : public CalibrationStep<SensorRawT> {
using CalibrationStep<SensorRawT>::sensorConfig;
using typename CalibrationStep<SensorRawT>::TickResult;
public:
SampleRateCalibrationStep(
SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig
)
: CalibrationStep<SensorRawT>{sensorConfig} {}
void start() override final {
CalibrationStep<SensorRawT>::start();
calibrationData = {millis()};
}
TickResult tick() override final {
float elapsedTime = (millis() - calibrationData.value().startMillis) / 1e3f;
if (elapsedTime < samplingRateCalibrationSeconds) {
return TickResult::CONTINUE;
}
float accelTimestep = elapsedTime / calibrationData.value().accelSamples;
float gyroTimestep = elapsedTime / calibrationData.value().gyroSamples;
float tempTimestep = elapsedTime / calibrationData.value().tempSamples;
sensorConfig.A_Ts = accelTimestep;
sensorConfig.G_Ts = gyroTimestep;
sensorConfig.T_Ts = tempTimestep;
sensorConfig.sensorTimestepsCalibrated = true;
return TickResult::DONE;
}
void cancel() override final { calibrationData.reset(); }
bool requiresRest() override final { return false; }
void processAccelSample(const SensorRawT accelSample[3]) override final {
calibrationData.value().accelSamples++;
}
void processGyroSample(const SensorRawT GyroSample[3]) override final {
calibrationData.value().gyroSamples++;
}
void processTempSample(float tempSample) override final {
calibrationData.value().tempSamples++;
}
private:
static constexpr float samplingRateCalibrationSeconds = 5;
struct CalibrationData {
uint64_t startMillis = 0;
uint64_t accelSamples = 0;
uint64_t gyroSamples = 0;
uint64_t tempSamples = 0;
};
std::optional<CalibrationData> calibrationData;
};
} // namespace SlimeVR::Sensors::NonBlockingCalibration

525
src/sensors/softfusion/softfusionsensor.h
View File

@@ -23,46 +23,57 @@
#pragma once
#include <cstdint>
#include <cstdlib>
#include <tuple>
#include "../RestCalibrationDetector.h"
#include "../SensorFusionRestDetect.h"
#include "../sensor.h"
#include "GlobalVars.h"
#include "motionprocessing/types.h"
#include "sensors/softfusion/TempGradientCalculator.h"
namespace SlimeVR::Sensors {
template <template <typename I2CImpl> typename T, typename I2CImpl>
template <
template <typename I2CImpl>
typename T,
typename I2CImpl,
template <typename IMU, typename RawSensorT, typename RawVectorT>
typename Calibrator>
class SoftFusionSensor : public Sensor {
using imu = T<I2CImpl>;
static constexpr sensor_real_t getDefaultTempTs() {
if constexpr (DirectTempReadOnly) {
return DirectTempReadTs;
} else {
return imu::TempTs;
}
}
static constexpr bool Uses32BitSensorData
= requires(imu& i) { i.Uses32BitSensorData; };
static constexpr bool DirectTempReadOnly = requires(imu& i) { i.getDirectTemp(); };
using RawSensorT =
typename std::conditional<Uses32BitSensorData, int32_t, int16_t>::type;
using RawVectorT = std::array<RawSensorT, 3>;
static constexpr auto UpsideDownCalibrationInit = true;
static constexpr auto GyroCalibDelaySeconds = 5;
static constexpr auto GyroCalibSeconds = 5;
static constexpr auto SampleRateCalibDelaySeconds = 1;
static constexpr auto SampleRateCalibSeconds = 5;
static constexpr auto AccelCalibDelaySeconds = 3;
static constexpr auto AccelCalibRestSeconds = 3;
static constexpr double GScale
static constexpr float GScale
= ((32768. / imu::GyroSensitivity) / 32768.) * (PI / 180.0);
static constexpr double AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
static constexpr float AScale = CONST_EARTH_GRAVITY / imu::AccelSensitivity;
static constexpr bool HasMotionlessCalib
= requires(imu& i) { typename imu::MotionlessCalibrationData; };
static constexpr size_t MotionlessCalibDataSize() {
if constexpr (HasMotionlessCalib) {
return sizeof(typename imu::MotionlessCalibrationData);
} else {
return 0;
}
}
using Calib = Calibrator<imu, RawSensorT, RawVectorT>;
static constexpr auto UpsideDownCalibrationInit = Calib::HasUpsideDownCalibration;
static constexpr float DirectTempReadFreq = 15;
static constexpr float DirectTempReadTs = 1.0f / DirectTempReadFreq;
float lastReadTemperature = 0;
uint32_t lastTempPollTime = micros();
bool detected() const {
const auto value = m_sensor.i2c.readReg(imu::Regs::WhoAmI::reg);
@@ -79,25 +90,27 @@ class SoftFusionSensor : public Sensor {
return true;
}
void sendData() {
Sensor::sendData();
sendTempIfNeeded();
}
void sendTempIfNeeded() {
uint32_t now = micros();
constexpr float maxSendRateHz = 2.0f;
constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
uint32_t elapsed = now - m_lastTemperaturePacketSent;
if (elapsed >= sendInterval) {
const float temperature = m_sensor.getDirectTemp();
m_lastTemperaturePacketSent = now - (elapsed - sendInterval);
networkConnection.sendTemperature(sensorId, temperature);
networkConnection.sendTemperature(sensorId, lastReadTemperature);
}
}
void recalcFusion() {
m_fusion = SensorFusionRestDetect(
m_calibration.G_Ts,
m_calibration.A_Ts,
m_calibration.M_Ts
TemperatureGradientCalculator tempGradientCalculator{[&](float gradient) {
m_fusion.updateBiasForgettingTime(
calibrator.getZROChange() / std::fabs(gradient)
);
}
}};
void processAccelSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sensor_real_t accelData[]
@@ -105,40 +118,39 @@ class SoftFusionSensor : public Sensor {
static_cast<sensor_real_t>(xyz[1]),
static_cast<sensor_real_t>(xyz[2])};
float tmp[3];
for (uint8_t i = 0; i < 3; i++) {
tmp[i] = (accelData[i] - m_calibration.A_B[i]);
}
calibrator.scaleAccelSample(accelData);
accelData[0]
= (m_calibration.A_Ainv[0][0] * tmp[0] + m_calibration.A_Ainv[0][1] * tmp[1]
+ m_calibration.A_Ainv[0][2] * tmp[2])
* AScale;
accelData[1]
= (m_calibration.A_Ainv[1][0] * tmp[0] + m_calibration.A_Ainv[1][1] * tmp[1]
+ m_calibration.A_Ainv[1][2] * tmp[2])
* AScale;
accelData[2]
= (m_calibration.A_Ainv[2][0] * tmp[0] + m_calibration.A_Ainv[2][1] * tmp[1]
+ m_calibration.A_Ainv[2][2] * tmp[2])
* AScale;
m_fusion.updateAcc(accelData, calibrator.getAccelTimestep());
m_fusion.updateAcc(accelData, m_calibration.A_Ts);
calibrator.provideAccelSample(xyz);
}
void processGyroSample(const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[] = {
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[0]) - m_calibration.G_off[0])
),
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[1]) - m_calibration.G_off[1])
),
static_cast<sensor_real_t>(
GScale * (static_cast<sensor_real_t>(xyz[2]) - m_calibration.G_off[2])
)
};
m_fusion.updateGyro(scaledData, m_calibration.G_Ts);
sensor_real_t gyroData[]
= {static_cast<sensor_real_t>(xyz[0]),
static_cast<sensor_real_t>(xyz[1]),
static_cast<sensor_real_t>(xyz[2])};
calibrator.scaleGyroSample(gyroData);
m_fusion.updateGyro(gyroData, calibrator.getGyroTimestep());
calibrator.provideGyroSample(xyz);
}
void
processTempSample(const int16_t rawTemperature, const sensor_real_t timeDelta) {
if constexpr (!DirectTempReadOnly) {
const float scaledTemperature = imu::TemperatureBias
+ static_cast<float>(rawTemperature)
* (1.0 / imu::TemperatureSensitivity);
lastReadTemperature = scaledTemperature;
tempGradientCalculator.feedSample(
lastReadTemperature,
calibrator.getTempTimestep()
);
calibrator.provideTempSample(lastReadTemperature);
}
}
void eatSamplesForSeconds(const uint32_t seconds) {
@@ -155,15 +167,18 @@ class SoftFusionSensor : public Sensor {
}
m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[](const int16_t xyz, const sensor_real_t timeDelta) {}
);
}
}
std::pair<RawVectorT, RawVectorT> eatSamplesReturnLast(const uint32_t milliseconds
std::tuple<RawVectorT, RawVectorT, int16_t> eatSamplesReturnLast(
const uint32_t milliseconds
) {
RawVectorT accel = {0};
RawVectorT gyro = {0};
int16_t temp = 0;
const auto targetDelay = millis() + milliseconds;
while (millis() < targetDelay) {
m_sensor.bulkRead(
@@ -176,11 +191,14 @@ class SoftFusionSensor : public Sensor {
gyro[0] = xyz[0];
gyro[1] = xyz[1];
gyro[2] = xyz[2];
},
[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
temp = rawTemp;
}
);
yield();
}
return std::make_pair(accel, gyro);
return std::make_tuple(accel, gyro, temp);
}
public:
@@ -196,9 +214,9 @@ public:
uint8_t
)
: Sensor(imu::Name, imu::Type, id, i2cAddress, rotation, sclPin, sdaPin)
, m_fusion(imu::GyrTs, imu::AccTs, imu::MagTs)
, m_fusion(imu::SensorVQFParams, imu::GyrTs, imu::AccTs, imu::MagTs)
, m_sensor(I2CImpl(i2cAddress), m_Logger) {}
~SoftFusionSensor() {}
~SoftFusionSensor() override = default;
void checkSensorTimeout() {
uint32_t now = millis();
@@ -222,50 +240,73 @@ public:
void motionLoop() override final {
sendTempIfNeeded();
calibrator.tick();
// read fifo updating fusion
uint32_t now = micros();
if constexpr (DirectTempReadOnly) {
uint32_t tempElapsed = now - lastTempPollTime;
if (tempElapsed >= DirectTempReadTs * 1e6) {
lastTempPollTime
= now
- (tempElapsed - static_cast<uint32_t>(DirectTempReadTs * 1e6));
lastReadTemperature = m_sensor.getDirectTemp();
calibrator.provideTempSample(lastReadTemperature);
tempGradientCalculator.feedSample(
lastReadTemperature,
DirectTempReadTs
);
}
}
tempGradientCalculator.tick();
constexpr uint32_t targetPollIntervalMicros = 6000;
uint32_t elapsed = now - m_lastPollTime;
if (elapsed >= targetPollIntervalMicros) {
m_lastPollTime = now - (elapsed - targetPollIntervalMicros);
}
// send new fusion values when time is up
now = micros();
constexpr float maxSendRateHz = 100.0f;
constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
elapsed = now - m_lastRotationPacketSent;
if (elapsed >= sendInterval) {
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
processAccelSample(xyz, timeDelta);
},
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
processGyroSample(xyz, timeDelta);
},
[&](const int16_t rawTemp, const sensor_real_t timeDelta) {
processTempSample(rawTemp, timeDelta);
}
);
optimistic_yield(100);
if (!m_fusion.isUpdated()) {
checkSensorTimeout();
return;
}
hadData = true;
m_lastRotationUpdateMillis = millis();
hadData = true;
m_fusion.clearUpdated();
}
// send new fusion values when time is up
now = micros();
constexpr float maxSendRateHz = 120.0f;
constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
elapsed = now - m_lastRotationPacketSent;
if (elapsed >= sendInterval) {
m_lastRotationPacketSent = now - (elapsed - sendInterval);
m_lastRotationPacketSent = now - (elapsed - sendInterval);
setFusedRotation(m_fusion.getQuaternionQuat());
setAcceleration(m_fusion.getLinearAccVec());
optimistic_yield(100);
}
setFusedRotation(m_fusion.getQuaternionQuat());
setAcceleration(m_fusion.getLinearAccVec());
optimistic_yield(100);
if (calibrationDetector.update(m_fusion)) {
markRestCalibrationComplete();
}
}
void motionSetup() override final {
void motionSetup() final {
if (!detected()) {
m_status = SensorStatus::SENSOR_ERROR;
return;
@@ -276,12 +317,8 @@ public:
// If no compatible calibration data is found, the calibration data will just be
// zero-ed out
if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::SFUSION
&& (sensorCalibration.data.sfusion.ImuType == imu::Type)
&& (sensorCalibration.data.sfusion.MotionlessDataLen
== MotionlessCalibDataSize())) {
m_calibration = sensorCalibration.data.sfusion;
recalcFusion();
if (calibrator.calibrationMatches(sensorCalibration)) {
calibrator.assignCalibration(sensorCalibration);
} else if (sensorCalibration.type == SlimeVR::Configuration::SensorConfigType::NONE) {
m_Logger.warn(
"No calibration data found for sensor %d, ignoring...",
@@ -296,11 +333,17 @@ public:
m_Logger.info("Please recalibrate");
}
calibrator.begin();
bool initResult = false;
if constexpr (HasMotionlessCalib) {
if constexpr (Calib::HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
std::memcpy(&calibData, m_calibration.MotionlessData, sizeof(calibData));
std::memcpy(
&calibData,
calibrator.getMotionlessCalibrationData(),
sizeof(calibData)
);
initResult = m_sensor.initialize(calibData);
} else {
initResult = m_sensor.initialize();
@@ -317,7 +360,7 @@ public:
[[maybe_unused]] auto lastRawSample = eatSamplesReturnLast(1000);
if constexpr (UpsideDownCalibrationInit) {
auto gravity = static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(lastRawSample.first[2])
AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
);
m_Logger.info(
"Gravity read: %.1f (need < -7.5 to start calibration)",
@@ -328,7 +371,7 @@ public:
m_Logger.info("Flip front in 5 seconds to start calibration");
lastRawSample = eatSamplesReturnLast(5000);
gravity = static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(lastRawSample.first[2])
AScale * static_cast<sensor_real_t>(std::get<0>(lastRawSample)[2])
);
if (gravity > 7.5f) {
m_Logger.debug("Starting calibration...");
@@ -342,303 +385,27 @@ public:
}
}
void startCalibration(int calibrationType) override final {
if (calibrationType == 0) {
// ALL
calibrateSampleRate();
if constexpr (HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
m_sensor.motionlessCalibration(calibData);
std::memcpy(
m_calibration.MotionlessData,
&calibData,
sizeof(calibData)
);
}
// Gryoscope offset calibration can only happen after any motionless
// gyroscope calibration, otherwise we are calculating the offset based
// on an incorrect starting point
calibrateGyroOffset();
calibrateAccel();
} else if (calibrationType == 1) {
calibrateSampleRate();
} else if (calibrationType == 2) {
calibrateGyroOffset();
} else if (calibrationType == 3) {
calibrateAccel();
} else if (calibrationType == 4) {
if constexpr (HasMotionlessCalib) {
typename imu::MotionlessCalibrationData calibData;
m_sensor.motionlessCalibration(calibData);
std::memcpy(
m_calibration.MotionlessData,
&calibData,
sizeof(calibData)
);
} else {
m_Logger.info("Sensor doesn't provide any custom motionless calibration"
);
}
}
saveCalibration();
}
void saveCalibration() {
m_Logger.debug("Saving the calibration data");
SlimeVR::Configuration::SensorConfig calibration;
calibration.type = SlimeVR::Configuration::SensorConfigType::SFUSION;
calibration.data.sfusion = m_calibration;
configuration.setSensor(sensorId, calibration);
configuration.save();
}
void calibrateGyroOffset() {
// Wait for sensor to calm down before calibration
m_Logger.info(
"Put down the device and wait for baseline gyro reading calibration (%d "
"seconds)",
GyroCalibDelaySeconds
);
ledManager.on();
eatSamplesForSeconds(GyroCalibDelaySeconds);
ledManager.off();
m_calibration.temperature = m_sensor.getDirectTemp();
m_Logger.trace("Calibration temperature: %f", m_calibration.temperature);
ledManager.pattern(100, 100, 3);
ledManager.on();
m_Logger.info("Gyro calibration started...");
int32_t sumXYZ[3] = {0};
const auto targetCalib = millis() + 1000 * GyroCalibSeconds;
uint32_t sampleCount = 0;
while (millis() < targetCalib) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {},
[&sumXYZ,
&sampleCount](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
sumXYZ[0] += xyz[0];
sumXYZ[1] += xyz[1];
sumXYZ[2] += xyz[2];
++sampleCount;
}
);
}
ledManager.off();
m_calibration.G_off[0] = ((double)sumXYZ[0]) / sampleCount;
m_calibration.G_off[1] = ((double)sumXYZ[1]) / sampleCount;
m_calibration.G_off[2] = ((double)sumXYZ[2]) / sampleCount;
m_Logger.info(
"Gyro offset after %d samples: %f %f %f",
sampleCount,
UNPACK_VECTOR_ARRAY(m_calibration.G_off)
void startCalibration(int calibrationType) final {
calibrator.startCalibration(
calibrationType,
[&](const uint32_t seconds) { eatSamplesForSeconds(seconds); },
[&](const uint32_t millis) { return eatSamplesReturnLast(millis); }
);
}
void calibrateAccel() {
auto magneto = std::make_unique<MagnetoCalibration>();
m_Logger.info(
"Put the device into 6 unique orientations (all sides), leave it still and "
"do not hold/touch for %d seconds each",
AccelCalibRestSeconds
);
ledManager.on();
eatSamplesForSeconds(AccelCalibDelaySeconds);
ledManager.off();
RestDetectionParams calibrationRestDetectionParams;
calibrationRestDetectionParams.restMinTime = AccelCalibRestSeconds;
calibrationRestDetectionParams.restThAcc = 0.25f;
RestDetection calibrationRestDetection(
calibrationRestDetectionParams,
imu::GyrTs,
imu::AccTs
);
constexpr uint16_t expectedPositions = 6;
constexpr uint16_t numSamplesPerPosition = 96;
uint16_t numPositionsRecorded = 0;
uint16_t numCurrentPositionSamples = 0;
bool waitForMotion = true;
auto accelCalibrationChunk
= std::make_unique<float[]>(numSamplesPerPosition * 3);
ledManager.pattern(100, 100, 6);
ledManager.on();
m_Logger.info("Gathering accelerometer data...");
m_Logger.info(
"Waiting for position %i, you can leave the device as is...",
numPositionsRecorded + 1
);
bool samplesGathered = false;
while (!samplesGathered) {
#ifdef ESP8266
ESP.wdtFeed();
#endif
m_sensor.bulkRead(
[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
const sensor_real_t scaledData[]
= {static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[0])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[1])
),
static_cast<sensor_real_t>(
AScale * static_cast<sensor_real_t>(xyz[2])
)};
calibrationRestDetection.updateAcc(imu::AccTs, scaledData);
if (waitForMotion) {
if (!calibrationRestDetection.getRestDetected()) {
waitForMotion = false;
}
return;
}
if (calibrationRestDetection.getRestDetected()) {
const uint16_t i = numCurrentPositionSamples * 3;
accelCalibrationChunk[i + 0] = xyz[0];
accelCalibrationChunk[i + 1] = xyz[1];
accelCalibrationChunk[i + 2] = xyz[2];
numCurrentPositionSamples++;
if (numCurrentPositionSamples >= numSamplesPerPosition) {
for (int i = 0; i < numSamplesPerPosition; i++) {
magneto->sample(
accelCalibrationChunk[i * 3 + 0],
accelCalibrationChunk[i * 3 + 1],
accelCalibrationChunk[i * 3 + 2]
);
}
numPositionsRecorded++;
numCurrentPositionSamples = 0;
if (numPositionsRecorded < expectedPositions) {
ledManager.pattern(50, 50, 2);
ledManager.on();
m_Logger.info(
"Recorded, waiting for position %i...",
numPositionsRecorded + 1
);
waitForMotion = true;
}
}
} else {
numCurrentPositionSamples = 0;
}
if (numPositionsRecorded >= expectedPositions) {
samplesGathered = true;
}
},
[](const RawSensorT xyz[3], const sensor_real_t timeDelta) {}
);
}
ledManager.off();
m_Logger.debug("Calculating accelerometer calibration data...");
accelCalibrationChunk.reset();
float A_BAinv[4][3];
magneto->current_calibration(A_BAinv);
m_Logger.debug("Finished calculating accelerometer calibration");
m_Logger.debug("Accelerometer calibration matrix:");
m_Logger.debug("{");
for (int i = 0; i < 3; i++) {
m_calibration.A_B[i] = A_BAinv[0][i];
m_calibration.A_Ainv[0][i] = A_BAinv[1][i];
m_calibration.A_Ainv[1][i] = A_BAinv[2][i];
m_calibration.A_Ainv[2][i] = A_BAinv[3][i];
m_Logger.debug(
" %f, %f, %f, %f",
A_BAinv[0][i],
A_BAinv[1][i],
A_BAinv[2][i],
A_BAinv[3][i]
);
}
m_Logger.debug("}");
}
void calibrateSampleRate() {
m_Logger.debug(
"Calibrating IMU sample rate in %d second(s)...",
SampleRateCalibDelaySeconds
);
ledManager.on();
eatSamplesForSeconds(SampleRateCalibDelaySeconds);
uint32_t accelSamples = 0;
uint32_t gyroSamples = 0;
const auto calibTarget = millis() + 1000 * SampleRateCalibSeconds;
m_Logger.debug("Counting samples now...");
uint32_t currentTime;
while ((currentTime = millis()) < calibTarget) {
m_sensor.bulkRead(
[&accelSamples](
const RawSensorT xyz[3],
const sensor_real_t timeDelta
) { accelSamples++; },
[&gyroSamples](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
gyroSamples++;
}
);
yield();
}
const auto millisFromStart
= currentTime - (calibTarget - 1000 * SampleRateCalibSeconds);
m_Logger.debug(
"Collected %d gyro, %d acc samples during %d ms",
gyroSamples,
accelSamples,
millisFromStart
);
m_calibration.A_Ts = millisFromStart / (accelSamples * 1000.0);
m_calibration.G_Ts = millisFromStart / (gyroSamples * 1000.0);
m_Logger.debug(
"Gyro frequency %fHz, accel frequency: %fHz",
1.0 / m_calibration.G_Ts,
1.0 / m_calibration.A_Ts
);
ledManager.off();
// fusion needs to be recalculated
recalcFusion();
}
SensorStatus getSensorState() override final { return m_status; }
SensorStatus getSensorState() final { return m_status; }
SensorFusionRestDetect m_fusion;
T<I2CImpl> m_sensor;
SlimeVR::Configuration::SoftFusionSensorConfig m_calibration
= {// let's create here transparent calibration that doesn't affect input data
.ImuType = {imu::Type},
.MotionlessDataLen = {MotionlessCalibDataSize()},
.A_B = {0.0, 0.0, 0.0},
.A_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
.M_B = {0.0, 0.0, 0.0},
.M_Ainv = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
.G_off = {0.0, 0.0, 0.0},
.temperature = 0.0,
.A_Ts = imu::AccTs,
.G_Ts = imu::GyrTs,
.M_Ts = imu::MagTs,
.G_Sens = {1.0, 1.0, 1.0},
.MotionlessData = {}
};
imu m_sensor;
Calib calibrator{
m_fusion,
m_sensor,
sensorId,
m_Logger,
getDefaultTempTs(),
AScale,
GScale
};
SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
uint32_t m_lastPollTime = micros();

7
src/serial/serialcommands.cpp
View File

@@ -406,11 +406,18 @@ void cmdTemperatureCalibration(CmdParser* parser) {
);
}
void cmdDeleteCalibration(CmdParser* parser) {
logger.info("ERASE CALIBRATION");
configuration.eraseSensors();
}
void setUp() {
cmdCallbacks.addCmd("SET", &cmdSet);
cmdCallbacks.addCmd("GET", &cmdGet);
cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
cmdCallbacks.addCmd("REBOOT", &cmdReboot);
cmdCallbacks.addCmd("DELCAL", &cmdDeleteCalibration);
cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
}

23
src/utils.h
View File

@@ -28,4 +28,27 @@
#define UNPACK_VECTOR_ARRAY(V) V[0], V[1], V[2]
#define UNPACK_QUATERNION(Q) Q.x, Q.y, Q.z, Q.w
#include <type_traits>
template <class T, std::enable_if_t<std::is_enum_v<T>, int> = 0>
constexpr T operator|(T lhs, T rhs) {
return static_cast<T>(
static_cast<std::underlying_type<T>::type>(lhs)
| static_cast<std::underlying_type<T>::type>(rhs)
);
}
template <class T, std::enable_if_t<std::is_enum_v<T>, int> = 0>
constexpr T operator&(T lhs, T rhs) {
return static_cast<T>(
static_cast<std::underlying_type<T>::type>(lhs)
& static_cast<std::underlying_type<T>::type>(rhs)
);
}
template <class T, std::enable_if_t<std::is_enum_v<T>, int> = 0>
constexpr bool any(T t) {
return static_cast<std::underlying_type<T>::type>(t) != 0;
}
#endif