diff --git a/lib/vqf/vqf.cpp b/lib/vqf/vqf.cpp index 3ecd9bdee..a1b45ed6f 100644 --- a/lib/vqf/vqf.cpp +++ b/lib/vqf/vqf.cpp @@ -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 ¶ms, 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) - - double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C; + 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) + + 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; +} diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h index cda5cc105..bde7cd021 100644 --- a/lib/vqf/vqf.h +++ b/lib/vqf/vqf.h @@ -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 #ifndef VQF_HPP @@ -11,16 +11,16 @@ #include #define VQF_SINGLE_PRECISION -#define VQF_NO_MOTION_BIAS_ESTIMATION #define M_PI 3.14159265358979323846 #define M_SQRT2 1.41421356237309504880 /** * @brief Typedef for the floating-point data type used for most operations. * - * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to - * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as - * using floats can cause numeric issues. + * By default, all floating-point calculations are performed using `vqf_real_t`. Set the + * `VQF_SINGLE_PRECISION` define to change this type to `float`. Note that the + * Butterworth filter implementation will always use vqf_real_t precision as using floats + * can cause numeric issues. */ #ifndef VQF_SINGLE_PRECISION typedef double vqf_real_t; @@ -31,988 +31,1117 @@ typedef float vqf_real_t; /** * @brief Struct containing all tuning parameters used by the VQF class. * - * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The - * constructor sets all parameters to the default values. + * The parameters influence the behavior of the algorithm and are independent of the + * sampling rate of the IMU data. The constructor sets all parameters to the default + * values. * - * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and #magDistRejectionEnabled can be used to enable/disable - * the main features of the VQF algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on - * the accelerometer and magnetometer measurements, respectively. The remaining parameters influence bias estimation - * and magnetometer rejection. + * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and + * #magDistRejectionEnabled can be used to enable/disable the main features of the VQF + * algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on + * the accelerometer and magnetometer measurements, respectively. The remaining + * parameters influence bias estimation and magnetometer rejection. */ -struct VQFParams -{ - /** - * @brief Constructor that initializes the struct with the default parameters. - */ - VQFParams(); - - /** - * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds. - * - * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of - * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements. - * - * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the - * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. - * - * Default value: 3.0 s - */ - vqf_real_t tauAcc; - /** - * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. - * - * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of - * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements. - * - * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the - * first-order low-pass filter for the heading correction as follows: - * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. - * - * Default value: 9.0 s - */ - vqf_real_t tauMag; +struct VQFParams { + /** + * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering + * in seconds. + * + * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer + * measurements and while large values of \f$\tau_\mathrm{acc}\f$ imply trust on the + * gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency + * \f$f_\mathrm{c}\f$ of the second-order Butterworth low-pass filter as follows: + * \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. + * + * Default value: 3.0 s + */ + vqf_real_t tauAcc = 3.0; + /** + * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. + * + * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer + * measurements and while large values of \f$\tau_\mathrm{mag}\f$ imply trust on the + * gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency + * \f$f_\mathrm{c}\f$ of the first-order low-pass filter for the heading correction + * as follows: \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. + * + * Default value: 9.0 s + */ + vqf_real_t tauMag = 9.0; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Enables gyroscope bias estimation during motion phases. - * - * If set to true (default), gyroscope bias is estimated based on the inclination correction only, i.e. without - * using magnetometer measurements. - */ - bool motionBiasEstEnabled; + /** + * @brief Enables gyroscope bias estimation during motion phases. + * + * If set to true (default), gyroscope bias is estimated based on the inclination + * correction only, i.e. without using magnetometer measurements. + */ + bool motionBiasEstEnabled = true; #endif - /** - * @brief Enables rest detection and gyroscope bias estimation during rest phases. - * - * If set to true (default), phases in which the IMU is at rest are detected. During rest, the gyroscope bias - * is estimated from the low-pass filtered gyroscope readings. - */ - bool restBiasEstEnabled; - /** - * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. - * - * If set to true (default), the magnetic field is analyzed. For short disturbed phases, the magnetometer-based - * correction is disabled totally. If the magnetic field is always regarded as disturbed or if the duration of - * the disturbances exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with an increased - * time constant. - */ - bool magDistRejectionEnabled; - - /** - * @brief Standard deviation of the initial bias estimation uncertainty (in degrees per second). - * - * Default value: 0.5 °/s - */ - vqf_real_t biasSigmaInit; - /** - * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds). - * - * This value determines the system noise assumed by the Kalman filter. - * - * Default value: 100.0 s - */ - vqf_real_t biasForgettingTime; - /** - * @brief Maximum expected gyroscope bias (in degrees per second). - * - * This value is used to clip the bias estimate and the measurement error in the bias estimation update step. It is - * further used by the rest detection algorithm in order to not regard measurements with a large but constant - * angular rate as rest. - * - * Default value: 2.0 °/s - */ - vqf_real_t biasClip; + /** + * @brief Enables rest detection and gyroscope bias estimation during rest phases. + * + * If set to true (default), phases in which the IMU is at rest are detected. During + * rest, the gyroscope bias is estimated from the low-pass filtered gyroscope + * readings. + */ + bool restBiasEstEnabled = true; + /** + * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. + * + * If set to true (default), the magnetic field is analyzed. For short disturbed + * phases, the magnetometer-based correction is disabled totally. If the magnetic + * field is always regarded as disturbed or if the duration of the disturbances + * exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with + * an increased time constant. + */ + bool magDistRejectionEnabled = true; + + /** + * @brief Standard deviation of the initial bias estimation uncertainty (in degrees + * per second). + * + * Default value: 0.5 °/s + */ + vqf_real_t biasSigmaInit = 0.5; + /** + * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 + * °/s (in seconds). + * + * This value determines the system noise assumed by the Kalman filter. + * + * Default value: 100.0 s + */ + vqf_real_t biasForgettingTime = 100.0; + /** + * @brief Maximum expected gyroscope bias (in degrees per second). + * + * This value is used to clip the bias estimate and the measurement error in the + * bias estimation update step. It is further used by the rest detection algorithm + * in order to not regard measurements with a large but constant angular rate as + * rest. + * + * Default value: 2.0 °/s + */ + vqf_real_t biasClip = 2.0; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second). - * - * This value determines the trust on motion bias estimation updates. A small value leads to fast convergence. - * - * Default value: 0.1 °/s - */ - vqf_real_t biasSigmaMotion; - /** - * @brief Forgetting factor for unobservable bias in vertical direction during motion. - * - * As magnetometer measurements are deliberately not used during motion bias estimation, gyroscope bias is not - * observable in vertical direction. This value is the relative weight of an artificial zero measurement that - * ensures that the bias estimate in the unobservable direction will eventually decay to zero. - * - * Default value: 0.0001 - */ - vqf_real_t biasVerticalForgettingFactor; + /** + * @brief Standard deviation of the converged bias estimation uncertainty during + * motion (in degrees per second). + * + * This value determines the trust on motion bias estimation updates. A small value + * leads to fast convergence. + * + * Default value: 0.1 °/s + */ + vqf_real_t biasSigmaMotion = 0.1; + /** + * @brief Forgetting factor for unobservable bias in vertical direction during + * motion. + * + * As magnetometer measurements are deliberately not used during motion bias + * estimation, gyroscope bias is not observable in vertical direction. This value is + * the relative weight of an artificial zero measurement that ensures that the bias + * estimate in the unobservable direction will eventually decay to zero. + * + * Default value: 0.0001 + */ + vqf_real_t biasVerticalForgettingFactor = 0.0001; #endif - /** - * @brief Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second). - * - * This value determines the trust on rest bias estimation updates. A small value leads to fast convergence. - * - * Default value: 0.03 ° - */ - vqf_real_t biasSigmaRest; - - /** - * @brief Time threshold for rest detection (in seconds). - * - * Rest is detected when the measurements have been close to the low-pass filtered reference for the given time. - * - * Default value: 1.5 s - */ - vqf_real_t restMinT; - /** - * @brief Time constant for the low-pass filter used in rest detection (in seconds). - * - * This time constant characterizes a second-order Butterworth low-pass filter used to obtain the reference for - * rest detection. - * - * Default value: 0.5 s - */ - vqf_real_t restFilterTau; - /** - * @brief Angular velocity threshold for rest detection (in °/s). - * - * For rest to be detected, the norm of the deviation between measurement and reference must be below the given - * threshold. (Furthermore, the absolute value of each component must be below #biasClip). - * - * Default value: 2.0 °/s - */ - vqf_real_t restThGyr; - /** - * @brief Acceleration threshold for rest detection (in m/s²). - * - * For rest to be detected, the norm of the deviation between measurement and reference must be below the given - * threshold. - * - * Default value: 0.5 m/s² - */ - vqf_real_t restThAcc; - - /** - * @brief Time constant for current norm/dip value in magnetic disturbance detection (in seconds). - * - * This (very fast) low-pass filter is intended to provide additional robustness when the magnetometer measurements - * are noisy or not sampled perfectly in sync with the gyroscope measurements. Set to -1 to disable the low-pass - * filter and directly use the magnetometer measurements. - * - * Default value: 0.05 s - */ - vqf_real_t magCurrentTau; - /** - * @brief Time constant for the adjustment of the magnetic field reference (in seconds). - * - * This adjustment allows the reference estimate to converge to the observed undisturbed field. - * - * Default value: 20.0 s - */ - vqf_real_t magRefTau; - /** - * @brief Relative threshold for the magnetic field strength for magnetic disturbance detection. - * - * This value is relative to the reference norm. - * - * Default value: 0.1 (10%) - */ - vqf_real_t magNormTh; - /** - * @brief Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees). - * - * Default vaule: 10 ° - */ - vqf_real_t magDipTh; - /** - * @brief Duration after which to accept a different homogeneous magnetic field (in seconds). - * - * A different magnetic field reference is accepted as the new field when the measurements are within the thresholds - * #magNormTh and #magDipTh for the given time. Additionally, only phases with sufficient movement, specified by - * #magNewMinGyr, count. - * - * Default value: 20.0 - */ - vqf_real_t magNewTime; - /** - * @brief Duration after which to accept a homogeneous magnetic field for the first time (in seconds). - * - * This value is used instead of #magNewTime when there is no current estimate in order to allow for the initial - * magnetic field reference to be obtained faster. - * - * Default value: 5.0 - */ - vqf_real_t magNewFirstTime; - /** - * @brief Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s). - * - * Durations for which the angular velocity norm is below this threshold do not count towards reaching #magNewTime. - * - * Default value: 20.0 °/s - */ - vqf_real_t magNewMinGyr; - /** - * @brief Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds). - * - * Default value: 0.5 s - */ - vqf_real_t magMinUndisturbedTime; - /** - * @brief Maximum duration of full magnetic disturbance rejection (in seconds). - * - * For magnetic disturbances up to this duration, heading correction is fully disabled and heading changes are - * tracked by gyroscope only. After this duration (or for many small disturbed phases without sufficient time in the - * undisturbed field in between), the heading correction is performed with an increased time constant (see - * #magRejectionFactor). - * - * Default value: 60.0 s - */ - vqf_real_t magMaxRejectionTime; - /** - * @brief Factor by which to slow the heading correction during long disturbed phases. - * - * After #magMaxRejectionTime of full magnetic disturbance rejection, heading correction is performed with an - * increased time constant. This parameter (approximately) specifies the factor of the increase. - * - * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in an undisturbed magnetic field, - * the time is reset and full magnetic disturbance rejection will be performed for up to #magMaxRejectionTime again. - * - * Default value: 2.0 - */ - vqf_real_t magRejectionFactor; + /** + * @brief Standard deviation of the converged bias estimation uncertainty during + * rest (in degrees per second). + * + * This value determines the trust on rest bias estimation updates. A small value + * leads to fast convergence. + * + * Default value: 0.03 ° + */ + vqf_real_t biasSigmaRest = 0.03; + + /** + * @brief Time threshold for rest detection (in seconds). + * + * Rest is detected when the measurements have been close to the low-pass filtered + * reference for the given time. + * + * Default value: 1.5 s + */ + vqf_real_t restMinT = 1.5; + /** + * @brief Time constant for the low-pass filter used in rest detection (in seconds). + * + * This time constant characterizes a second-order Butterworth low-pass filter used + * to obtain the reference for rest detection. + * + * Default value: 0.5 s + */ + vqf_real_t restFilterTau = 0.5; + /** + * @brief Angular velocity threshold for rest detection (in °/s). + * + * For rest to be detected, the norm of the deviation between measurement and + * reference must be below the given threshold. (Furthermore, the absolute value of + * each component must be below #biasClip). + * + * Default value: 2.0 °/s + */ + vqf_real_t restThGyr = 2.0; + /** + * @brief Acceleration threshold for rest detection (in m/s²). + * + * For rest to be detected, the norm of the deviation between measurement and + * reference must be below the given threshold. + * + * Default value: 0.5 m/s² + */ + vqf_real_t restThAcc = 0.5; + + /** + * @brief Time constant for current norm/dip value in magnetic disturbance detection + * (in seconds). + * + * This (very fast) low-pass filter is intended to provide additional robustness + * when the magnetometer measurements are noisy or not sampled perfectly in sync + * with the gyroscope measurements. Set to -1 to disable the low-pass filter and + * directly use the magnetometer measurements. + * + * Default value: 0.05 s + */ + vqf_real_t magCurrentTau = 0.05; + /** + * @brief Time constant for the adjustment of the magnetic field reference (in + * seconds). + * + * This adjustment allows the reference estimate to converge to the observed + * undisturbed field. + * + * Default value: 20.0 s + */ + vqf_real_t magRefTau = 20.0; + /** + * @brief Relative threshold for the magnetic field strength for magnetic + * disturbance detection. + * + * This value is relative to the reference norm. + * + * Default value: 0.1 (10%) + */ + vqf_real_t magNormTh = 0.1; + /** + * @brief Threshold for the magnetic field dip angle for magnetic disturbance + * detection (in degrees). + * + * Default vaule: 10 ° + */ + vqf_real_t magDipTh = 10.0; + /** + * @brief Duration after which to accept a different homogeneous magnetic field (in + * seconds). + * + * A different magnetic field reference is accepted as the new field when the + * measurements are within the thresholds #magNormTh and #magDipTh for the given + * time. Additionally, only phases with sufficient movement, specified by + * #magNewMinGyr, count. + * + * Default value: 20.0 + */ + vqf_real_t magNewTime = 20.0; + /** + * @brief Duration after which to accept a homogeneous magnetic field for the first + * time (in seconds). + * + * This value is used instead of #magNewTime when there is no current estimate in + * order to allow for the initial magnetic field reference to be obtained faster. + * + * Default value: 5.0 + */ + vqf_real_t magNewFirstTime = 5.0; + /** + * @brief Minimum angular velocity needed in order to count time for new magnetic + * field acceptance (in °/s). + * + * Durations for which the angular velocity norm is below this threshold do not + * count towards reaching #magNewTime. + * + * Default value: 20.0 °/s + */ + vqf_real_t magNewMinGyr = 20.0; + /** + * @brief Minimum duration within thresholds after which to regard the field as + * undisturbed again (in seconds). + * + * Default value: 0.5 s + */ + vqf_real_t magMinUndisturbedTime = 0.5; + /** + * @brief Maximum duration of full magnetic disturbance rejection (in seconds). + * + * For magnetic disturbances up to this duration, heading correction is fully + * disabled and heading changes are tracked by gyroscope only. After this duration + * (or for many small disturbed phases without sufficient time in the undisturbed + * field in between), the heading correction is performed with an increased time + * constant (see #magRejectionFactor). + * + * Default value: 60.0 s + */ + vqf_real_t magMaxRejectionTime = 60.0; + /** + * @brief Factor by which to slow the heading correction during long disturbed + * phases. + * + * After #magMaxRejectionTime of full magnetic disturbance rejection, heading + * correction is performed with an increased time constant. This parameter + * (approximately) specifies the factor of the increase. + * + * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in + * an undisturbed magnetic field, the time is reset and full magnetic disturbance + * rejection will be performed for up to #magMaxRejectionTime again. + * + * Default value: 2.0 + */ + vqf_real_t magRejectionFactor = 2.0; }; /** * @brief Struct containing the filter state of the VQF class. * - * The relevant parts of the state can be accessed via functions of the VQF class, e.g. VQF::getQuat6D(), - * VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), VQF::getRestDetected() and - * VQF::getMagDistDetected(). To reset the state to the initial values, use VQF::resetState(). + * The relevant parts of the state can be accessed via functions of the VQF class, e.g. + * VQF::getQuat6D(), VQF::getQuat9D(), VQF::getGyrBiasEstimate(), + * VQF::setGyrBiasEstimate(), VQF::getRestDetected() and VQF::getMagDistDetected(). To + * reset the state to the initial values, use VQF::resetState(). * - * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this - * purpose, the state can be accessed by VQF::getState() and set by VQF::setState(). + * Direct access to the full state is typically not needed but can be useful in some + * cases, e.g. for debugging. For this purpose, the state can be accessed by + * VQF::getState() and set by VQF::setState(). */ struct VQFState { - /** - * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. - */ - vqf_real_t gyrQuat[4]; - /** - * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. - */ - vqf_real_t accQuat[4]; - /** - * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. - * - * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. - */ - vqf_real_t delta; - /** - * @brief True if it has been detected that the IMU is currently at rest. - * - * Used to switch between rest and motion gyroscope bias estimation. - */ - bool restDetected; - /** - * @brief True if magnetic disturbances have been detected. - */ - bool magDistDetected; - - /** - * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. - */ - vqf_real_t lastAccLp[3]; - /** - * @brief Internal low-pass filter state for #lastAccLp. - */ - double accLpState[3*2]; - /** - * @brief Last inclination correction angular rate. - * - * Change to inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the - * last accelerometer update, expressed as an angular rate (in rad/s). - */ - vqf_real_t lastAccCorrAngularRate; - - /** - * @brief Gain used for heading correction to ensure fast initial convergence. - * - * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter - * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is - * set to zero. - */ - vqf_real_t kMagInit; - /** - * @brief Last heading disagreement angle. - * - * Disagreement between the heading \f$\hat\delta\f$ estimated from the last magnetometer sample and the state - * \f$\delta\f$ (in rad). - */ - vqf_real_t lastMagDisAngle; - /** - * @brief Last heading correction angular rate. - * - * Change to heading \f$\delta\f$ performed in the last magnetometer update, - * expressed as an angular rate (in rad/s). - */ - vqf_real_t lastMagCorrAngularRate; - - /** - * @brief Current gyroscope bias estimate (in rad/s). - */ - vqf_real_t bias[3]; + /** + * @brief Angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + */ + vqf_real_t gyrQuat[4]; + /** + * @brief Inclination correction quaternion + * \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + */ + vqf_real_t accQuat[4]; + /** + * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and + * \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} + * & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + */ + vqf_real_t delta; + /** + * @brief True if it has been detected that the IMU is currently at rest. + * + * Used to switch between rest and motion gyroscope bias estimation. + */ + bool restDetected; + /** + * @brief True if magnetic disturbances have been detected. + */ + bool magDistDetected; + + /** + * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. + */ + vqf_real_t lastAccLp[3]; + /** + * @brief Internal low-pass filter state for #lastAccLp. + */ + vqf_real_t accLpState[3 * 2]; + /** + * @brief Last inclination correction angular rate. + * + * Change to inclination correction quaternion + * \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the last + * accelerometer update, expressed as an angular rate (in rad/s). + */ + vqf_real_t lastAccCorrAngularRate; + + /** + * @brief Gain used for heading correction to ensure fast initial convergence. + * + * This value is used as the gain for heading correction in the beginning if it is + * larger than the normal filter gain. It is initialized to 1 and then updated to + * 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is set to zero. + */ + vqf_real_t kMagInit; + /** + * @brief Last heading disagreement angle. + * + * Disagreement between the heading \f$\hat\delta\f$ estimated from the last + * magnetometer sample and the state \f$\delta\f$ (in rad). + */ + vqf_real_t lastMagDisAngle; + /** + * @brief Last heading correction angular rate. + * + * Change to heading \f$\delta\f$ performed in the last magnetometer update, + * expressed as an angular rate (in rad/s). + */ + vqf_real_t lastMagCorrAngularRate; + + /** + * @brief Current gyroscope bias estimate (in rad/s). + */ + vqf_real_t bias[3]; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Covariance matrix of the gyroscope bias estimate. - * - * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the internal unit used is 0.01 °/s, - * i.e. to get the standard deviation in degrees per second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. - */ - vqf_real_t biasP[9]; + /** + * @brief Covariance matrix of the gyroscope bias estimate. + * + * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the + * internal unit used is 0.01 °/s, i.e. to get the standard deviation in degrees per + * second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. + */ + vqf_real_t biasP[9]; #else - // If only rest gyr bias estimation is enabled, P and K of the KF are always diagonal - // and matrix inversion is not needed. If motion bias estimation is disabled at compile - // time, storing the full P matrix is not necessary. - vqf_real_t biasP; + // If only rest gyr bias estimation is enabled, P and K of the KF are always + // diagonal and matrix inversion is not needed. If motion bias estimation is + // disabled at compile time, storing the full P matrix is not necessary. + vqf_real_t biasP; #endif #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion - * bias estimation. - */ - double motionBiasEstRLpState[9*2]; - /** - * @brief Internal low-pass filter state for the rotated bias estimate used in motion bias estimation. - */ - double motionBiasEstBiasLpState[2*2]; + /** + * @brief Internal state of the Butterworth low-pass filter for the rotation matrix + * coefficients used in motion bias estimation. + */ + vqf_real_t motionBiasEstRLpState[9 * 2]; + /** + * @brief Internal low-pass filter state for the rotated bias estimate used in + * motion bias estimation. + */ + vqf_real_t motionBiasEstBiasLpState[2 * 2]; #endif - /** - * @brief Last (squared) deviations from the reference of the last sample used in rest detection. - * - * Looking at those values can be useful to understand how rest detection is working and which thresholds are - * suitable. The array contains the last values for gyroscope and accelerometer in the respective - * units. Note that the values are squared. - * - * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values. - */ - vqf_real_t restLastSquaredDeviations[2]; - /** - * @brief The current duration for which all sensor readings are within the rest detection thresholds. - * - * Rest is detected if this value is larger or equal to VQFParams::restMinT. - */ - vqf_real_t restT; - /** - * @brief Last low-pass filtered gyroscope measurement used as the reference for rest detection. - * - * Note that this value is also used for gyroscope bias estimation when rest is detected. - */ - vqf_real_t restLastGyrLp[3]; - /** - * @brief Internal low-pass filter state for #restLastGyrLp. - */ - double restGyrLpState[3*2]; - /** - * @brief Last low-pass filtered accelerometer measurement used as the reference for rest detection. - */ - vqf_real_t restLastAccLp[3]; - /** - * @brief Internal low-pass filter state for #restLastAccLp. - */ - double restAccLpState[3*2]; - - /** - * @brief Norm of the currently accepted magnetic field reference. - * - * A value of -1 indicates that no homogeneous field is found yet. - */ - vqf_real_t magRefNorm; - /** - * @brief Dip angle of the currently accepted magnetic field reference. - */ - vqf_real_t magRefDip; - /** - * @brief The current duration for which the current norm and dip are close to the reference. - * - * The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime. - */ - vqf_real_t magUndisturbedT; - /** - * @brief The current duration for which the magnetic field was rejected. - * - * If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading - * correction updates are fully disabled. - */ - vqf_real_t magRejectT; - /** - * @brief Norm of the alternative magnetic field reference currently being evaluated. - */ - vqf_real_t magCandidateNorm; - /** - * @brief Dip angle of the alternative magnetic field reference currently being evaluated. - */ - vqf_real_t magCandidateDip; - /** - * @brief The current duration for which the norm and dip are close to the candidate. - * - * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if #magRefNorm < 0), the current - * candidate is accepted as the new reference. - */ - vqf_real_t magCandidateT; - /** - * @brief Norm and dip angle of the current magnetometer measurements. - * - * Slightly low-pass filtered, see VQFParams::magCurrentTau. - */ - vqf_real_t magNormDip[2]; - /** - * @brief Internal low-pass filter state for the current norm and dip angle. - */ - double magNormDipLpState[2*2]; + /** + * @brief Last (squared) deviations from the reference of the last sample used in + * rest detection. + * + * Looking at those values can be useful to understand how rest detection is working + * and which thresholds are suitable. The array contains the last values for + * gyroscope and accelerometer in the respective units. Note that the values are + * squared. + * + * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and + * interpret those values. + */ + vqf_real_t restLastSquaredDeviations[2]; + /** + * @brief The current duration for which all sensor readings are within the rest + * detection thresholds. + * + * Rest is detected if this value is larger or equal to VQFParams::restMinT. + */ + vqf_real_t restT; + /** + * @brief Last low-pass filtered gyroscope measurement used as the reference for + * rest detection. + * + * Note that this value is also used for gyroscope bias estimation when rest is + * detected. + */ + vqf_real_t restLastGyrLp[3]; + /** + * @brief Internal low-pass filter state for #restLastGyrLp. + */ + vqf_real_t restGyrLpState[3 * 2]; + /** + * @brief Last low-pass filtered accelerometer measurement used as the reference for + * rest detection. + */ + vqf_real_t restLastAccLp[3]; + /** + * @brief Internal low-pass filter state for #restLastAccLp. + */ + vqf_real_t restAccLpState[3 * 2]; + + /** + * @brief Norm of the currently accepted magnetic field reference. + * + * A value of -1 indicates that no homogeneous field is found yet. + */ + vqf_real_t magRefNorm; + /** + * @brief Dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t magRefDip; + /** + * @brief The current duration for which the current norm and dip are close to the + * reference. + * + * The magnetic field is regarded as undisturbed when this value reaches + * VQFParams::magMinUndisturbedTime. + */ + vqf_real_t magUndisturbedT; + /** + * @brief The current duration for which the magnetic field was rejected. + * + * If the magnetic field is disturbed and this value is smaller than + * VQFParams::magMaxRejectionTime, heading correction updates are fully disabled. + */ + vqf_real_t magRejectT; + /** + * @brief Norm of the alternative magnetic field reference currently being + * evaluated. + */ + vqf_real_t magCandidateNorm; + /** + * @brief Dip angle of the alternative magnetic field reference currently being + * evaluated. + */ + vqf_real_t magCandidateDip; + /** + * @brief The current duration for which the norm and dip are close to the + * candidate. + * + * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if + * #magRefNorm < 0), the current candidate is accepted as the new reference. + */ + vqf_real_t magCandidateT; + /** + * @brief Norm and dip angle of the current magnetometer measurements. + * + * Slightly low-pass filtered, see VQFParams::magCurrentTau. + */ + vqf_real_t magNormDip[2]; + /** + * @brief Internal low-pass filter state for the current norm and dip angle. + */ + vqf_real_t magNormDipLpState[2 * 2]; }; /** * @brief Struct containing coefficients used by the VQF class. * - * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. - * They are calculated in VQF::setup(). + * Coefficients are values that depend on the parameters and the sampling times, but do + * not change during update steps. They are calculated in VQF::setup(). */ -struct VQFCoefficients -{ - /** - * @brief Sampling time of the gyroscope measurements (in seconds). - */ - vqf_real_t gyrTs; - /** - * @brief Sampling time of the accelerometer measurements (in seconds). - */ - vqf_real_t accTs; - /** - * @brief Sampling time of the magnetometer measurements (in seconds). - */ - vqf_real_t magTs; - - /** - * @brief Numerator coefficients of the acceleration low-pass filter. - * - * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. - */ - double accLpB[3]; - /** - * @brief Denominator coefficients of the acceleration low-pass filter. - * - * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. - */ - double accLpA[2]; - - /** - * @brief Gain of the first-order filter used for heading correction. - */ - vqf_real_t kMag; - - /** - * @brief Variance of the initial gyroscope bias estimate. - */ - vqf_real_t biasP0; - /** - * @brief System noise variance used in gyroscope bias estimation. - */ - vqf_real_t biasV; +struct VQFCoefficients { + /** + * @brief Sampling time of the gyroscope measurements (in seconds). + */ + vqf_real_t gyrTs; + /** + * @brief Sampling time of the accelerometer measurements (in seconds). + */ + vqf_real_t accTs; + /** + * @brief Sampling time of the magnetometer measurements (in seconds). + */ + vqf_real_t magTs; + + /** + * @brief Numerator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. + */ + vqf_real_t accLpB[3]; + /** + * @brief Denominator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. + */ + vqf_real_t accLpA[2]; + + /** + * @brief Gain of the first-order filter used for heading correction. + */ + vqf_real_t kMag; + + /** + * @brief Variance of the initial gyroscope bias estimate. + */ + vqf_real_t biasP0; + /** + * @brief System noise variance used in gyroscope bias estimation. + */ + vqf_real_t biasV; #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Measurement noise variance for the motion gyroscope bias estimation update. - */ - vqf_real_t biasMotionW; - /** - * @brief Measurement noise variance for the motion gyroscope bias estimation update in vertical direction. - */ - vqf_real_t biasVerticalW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation + * update. + */ + vqf_real_t biasMotionW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update + * in vertical direction. + */ + vqf_real_t biasVerticalW; #endif - /** - * @brief Measurement noise variance for the rest gyroscope bias estimation update. - */ - vqf_real_t biasRestW; - - /** - * @brief Numerator coefficients of the gyroscope measurement low-pass filter for rest detection. - */ - double restGyrLpB[3]; - /** - * @brief Denominator coefficients of the gyroscope measurement low-pass filter for rest detection. - */ - double restGyrLpA[2]; - /** - * @brief Numerator coefficients of the accelerometer measurement low-pass filter for rest detection. - */ - double restAccLpB[3]; - /** - * @brief Denominator coefficients of the accelerometer measurement low-pass filter for rest detection. - */ - double restAccLpA[2]; - - /** - * @brief Gain of the first-order filter used for to update the magnetic field reference and candidate. - */ - vqf_real_t kMagRef; - /** - * @brief Numerator coefficients of the low-pass filter for the current magnetic norm and dip. - */ - double magNormDipLpB[3]; - /** - * @brief Denominator coefficients of the low-pass filter for the current magnetic norm and dip. - */ - double magNormDipLpA[2]; + /** + * @brief Measurement noise variance for the rest gyroscope bias estimation update. + */ + vqf_real_t biasRestW; + + /** + * @brief Numerator coefficients of the gyroscope measurement low-pass filter for + * rest detection. + */ + vqf_real_t restGyrLpB[3]; + /** + * @brief Denominator coefficients of the gyroscope measurement low-pass filter for + * rest detection. + */ + vqf_real_t restGyrLpA[2]; + /** + * @brief Numerator coefficients of the accelerometer measurement low-pass filter + * for rest detection. + */ + vqf_real_t restAccLpB[3]; + /** + * @brief Denominator coefficients of the accelerometer measurement low-pass filter + * for rest detection. + */ + vqf_real_t restAccLpA[2]; + + /** + * @brief Gain of the first-order filter used for to update the magnetic field + * reference and candidate. + */ + vqf_real_t kMagRef; + /** + * @brief Numerator coefficients of the low-pass filter for the current magnetic + * norm and dip. + */ + vqf_real_t magNormDipLpB[3]; + /** + * @brief Denominator coefficients of the low-pass filter for the current magnetic + * norm and dip. + */ + vqf_real_t magNormDipLpA[2]; }; /** * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation. * * \rst - * This class implements the orientation estimation filter described in the following publication: + * This class implements the orientation estimation filter described in the following + * publication: * * - * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic - * Disturbance Rejection." Information Fusion 2023, 91, 187--204. - * `doi:10.1016/j.inffus.2022.10.014 `_. - * [Accepted manuscript available at `arXiv:2203.17024 `_.] + * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias + * Estimation and Magnetic Disturbance Rejection." Information Fusion 2023, 91, + * 187--204. `doi:10.1016/j.inffus.2022.10.014 + * `_. [Accepted manuscript available at + * `arXiv:2203.17024 `_.] * - * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used - * without magnetometer data. It performs rest detection, gyroscope bias estimation during rest and motion, and magnetic - * disturbance detection and rejection. Different sampling rates for gyroscopes, accelerometers, and magnetometers are - * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via a - * number of tuning parameters. + * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) + * sensor fusion and can also be used without magnetometer data. It performs rest + * detection, gyroscope bias estimation during rest and motion, and magnetic disturbance + * detection and rejection. Different sampling rates for gyroscopes, accelerometers, and + * magnetometers are supported as well. While in most cases, the defaults will be + * reasonable, the algorithm can be influenced via a number of tuning parameters. * * To use this C++ implementation, * - * 1. create a instance of the class and provide the sampling time and, optionally, parameters - * 2. for every sample, call one of the update functions to feed the algorithm with IMU data - * 3. access the estimation results with :meth:`getQuat6D() `, :meth:`getQuat9D() ` and - * the other getter methods. + * 1. create a instance of the class and provide the sampling time and, optionally, + * parameters + * 2. for every sample, call one of the update functions to feed the algorithm with IMU + * data + * 3. access the estimation results with :meth:`getQuat6D() `, + * :meth:`getQuat9D() ` and the other getter methods. * - * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() `. + * If the full data is available in (row-major) data buffers, you can use + * :meth:`updateBatch() `. * - * This class is the main C++ implementation of the algorithm. Depending on use case and programming language of choice, - * the following alternatives might be useful: + * This class is the main C++ implementation of the algorithm. Depending on use case and + * programming language of choice, the following alternatives might be useful: * * +------------------------+--------------------------+--------------------------+---------------------------+ - * | | Full Version | Basic Version | Offline Version | - * | | | | | + * | | Full Version | Basic Version | + * Offline Version | | | | | | * +========================+==========================+==========================+===========================+ - * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | :cpp:func:`offlineVQF` | + * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | + * :cpp:func:`offlineVQF` | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | :py:meth:`vqf.offlineVQF` | + * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | + * :py:meth:`vqf.offlineVQF` | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | + * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | * +------------------------+--------------------------+--------------------------+---------------------------+ - * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | + * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | * +------------------------+--------------------------+--------------------------+---------------------------+ * \endrst */ -class VQF -{ +class VQF { public: - /** - * Initializes the object with default parameters. - * - * In the most common case (using the default parameters and all data being sampled with the same frequency, - * create the class like this: - * \rst - * .. code-block:: c++ - * - * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz - * \endrst - * - * @param gyrTs sampling time of the gyroscope measurements in seconds - * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * - */ - VQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); - /** - * @brief Initializes the object with custom parameters. - * - * Example code to create an object with magnetic disturbance rejection disabled: - * \rst - * .. code-block:: c++ - * - * VQFParams params; - * params.magDistRejectionEnabled = false; - * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz - * \endrst - * - * @param params VQFParams struct containing the desired parameters - * @param gyrTs sampling time of the gyroscope measurements in seconds - * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) - * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) - */ - VQF(const VQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); - - /** - * @brief Performs gyroscope update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * @param gyr gyroscope measurement in rad/s - */ - void updateGyr(const vqf_real_t gyr[3], double gyrTs); - /** - * @brief Performs accelerometer update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * Should be called after #updateGyr and before #updateMag. - * - * @param acc accelerometer measurement in m/s² - */ - void updateAcc(const vqf_real_t acc[3]); - /** - * @brief Performs magnetometer update step. - * - * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have - * different sampling rates. Otherwise, simply use #update(). - * - * Should be called after #updateAcc. - * - * @param mag magnetometer measurement in arbitrary units - */ - void updateMag(const vqf_real_t mag[3]); - - /** - * @brief Returns the angular velocity strapdown integration quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat3D(vqf_real_t out[4]) const; - /** - * @brief Returns the 6D (magnetometer-free) orientation quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat6D(vqf_real_t out[4]) const; - /** - * @brief Returns the 9D (with magnetometers) orientation quaternion - * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. - * @param out output array for the quaternion - */ - void getQuat9D(vqf_real_t out[4]) const; - /** - * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. - * - * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. - * - * @return delta angle in rad (VQFState::delta) - */ - vqf_real_t getDelta() const; - - /** - * @brief Returns the current gyroscope bias estimate and the uncertainty. - * - * The returned standard deviation sigma represents the estimation uncertainty in the worst direction and is based - * on an upper bound of the largest eigenvalue of the covariance matrix. - * - * @param out output array for the gyroscope bias estimate (rad/s) - * @return standard deviation sigma of the estimation uncertainty (rad/s) - */ - vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; - /** - * @brief Sets the current gyroscope bias estimate and the uncertainty. - * - * If a value for the uncertainty sigma is given, the covariance matrix is set to a corresponding scaled identity - * matrix. - * - * @param bias gyroscope bias estimate (rad/s) - * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 (default) in order to not - * change the estimation covariance matrix - */ - void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma=-1.0); - /** - * @brief Returns true if rest was detected. - */ - bool getRestDetected() const; - /** - * @brief Returns true if a disturbed magnetic field was detected. - */ - bool getMagDistDetected() const; - /** - * @brief Returns the relative deviations used in rest detection. - * - * Looking at those values can be useful to understand how rest detection is working and which thresholds are - * suitable. The output array is filled with the last values for gyroscope and accelerometer, - * relative to the threshold. In order for rest to be detected, both values must stay below 1. - * - * @param out output array of size 2 for the relative rest deviations - */ - void getRelativeRestDeviations(vqf_real_t out[2]) const; - /** - * @brief Returns the norm of the currently accepted magnetic field reference. - */ - vqf_real_t getMagRefNorm() const; - /** - * @brief Returns the dip angle of the currently accepted magnetic field reference. - */ - vqf_real_t getMagRefDip() const; - /** - * @brief Overwrites the current magnetic field reference. - * @param norm norm of the magnetic field reference - * @param dip dip angle of the magnetic field reference - */ - void setMagRef(vqf_real_t norm, vqf_real_t dip); - - /** - * @brief Sets the time constant for accelerometer low-pass filtering. - * - * For more details, see VQFParams.tauAcc. - * - * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds - */ - void setTauAcc(vqf_real_t tauAcc); - /** - * @brief Sets the time constant for the magnetometer update. - * - * For more details, see VQFParams.tauMag. - * - * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds - */ - void setTauMag(vqf_real_t tauMag); + /** + * Initializes the object with default parameters. + * + * In the most common case (using the default parameters and all data being sampled + * with the same frequency, create the class like this: \rst + * .. code-block:: c++ + * + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the + * value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value + * of `gyrTs` is used if set to -1) + * + */ + VQF(vqf_real_t gyrTs, vqf_real_t accTs = -1.0, vqf_real_t magTs = -1.0); + /** + * @brief Initializes the object with custom parameters. + * + * Example code to create an object with magnetic disturbance rejection disabled: + * \rst + * .. code-block:: c++ + * + * VQFParams params; + * params.magDistRejectionEnabled = false; + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param params VQFParams struct containing the desired parameters + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the + * value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value + * of `gyrTs` is used if set to -1) + */ + VQF(const VQFParams& params, + vqf_real_t gyrTs, + vqf_real_t accTs = -1.0, + vqf_real_t magTs = -1.0); + + /** + * @brief Performs gyroscope update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * @param gyr gyroscope measurement in rad/s + */ + void updateGyr(const vqf_real_t gyr[3], vqf_real_t gyrTs); + /** + * @brief Performs accelerometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateGyr and before #updateMag. + * + * @param acc accelerometer measurement in m/s² + */ + void updateAcc(const vqf_real_t acc[3]); + /** + * @brief Performs magnetometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers + * and magnetometers have different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateAcc. + * + * @param mag magnetometer measurement in arbitrary units + */ + void updateMag(const vqf_real_t mag[3]); + + /** + * @brief Returns the angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat3D(vqf_real_t out[4]) const; + /** + * @brief Returns the 6D (magnetometer-free) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat6D(vqf_real_t out[4]) const; + /** + * @brief Returns the 9D (with magnetometers) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat9D(vqf_real_t out[4]) const; + /** + * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ + * and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} + * & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + * + * @return delta angle in rad (VQFState::delta) + */ + vqf_real_t getDelta() const; + + /** + * @brief Returns the current gyroscope bias estimate and the uncertainty. + * + * The returned standard deviation sigma represents the estimation uncertainty in + * the worst direction and is based on an upper bound of the largest eigenvalue of + * the covariance matrix. + * + * @param out output array for the gyroscope bias estimate (rad/s) + * @return standard deviation sigma of the estimation uncertainty (rad/s) + */ + vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; + /** + * @brief Sets the current gyroscope bias estimate and the uncertainty. + * + * If a value for the uncertainty sigma is given, the covariance matrix is set to a + * corresponding scaled identity matrix. + * + * @param bias gyroscope bias estimate (rad/s) + * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 + * (default) in order to not change the estimation covariance matrix + */ + void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma = -1.0); + /** + * @brief Returns true if rest was detected. + */ + bool getRestDetected() const; + /** + * @brief Returns true if a disturbed magnetic field was detected. + */ + bool getMagDistDetected() const; + /** + * @brief Returns the relative deviations used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working + * and which thresholds are suitable. The output array is filled with the last + * values for gyroscope and accelerometer, relative to the threshold. In order for + * rest to be detected, both values must stay below 1. + * + * @param out output array of size 2 for the relative rest deviations + */ + void getRelativeRestDeviations(vqf_real_t out[2]) const; + /** + * @brief Returns the norm of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefNorm() const; + /** + * @brief Returns the dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefDip() const; + /** + * @brief Overwrites the current magnetic field reference. + * @param norm norm of the magnetic field reference + * @param dip dip angle of the magnetic field reference + */ + void setMagRef(vqf_real_t norm, vqf_real_t dip); + + /** + * @brief Sets the time constant for accelerometer low-pass filtering. + * + * For more details, see VQFParams.tauAcc. + * + * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds + */ + void setTauAcc(vqf_real_t tauAcc); + /** + * @brief Sets the time constant for the magnetometer update. + * + * For more details, see VQFParams.tauMag. + * + * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds + */ + void setTauMag(vqf_real_t tauMag); #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Enables/disabled gyroscope bias estimation during motion. - */ - void setMotionBiasEstEnabled(bool enabled); + /** + * @brief Enables/disabled gyroscope bias estimation during motion. + */ + void setMotionBiasEstEnabled(bool enabled); #endif - /** - * @brief Enables/disables rest detection and bias estimation during rest. - */ - void setRestBiasEstEnabled(bool enabled); - /** - * @brief Enables/disables magnetic disturbance detection and rejection. - */ - void setMagDistRejectionEnabled(bool enabled); - /** - * @brief Sets the current thresholds for rest detection. - * - * For details about the parameters, see VQFParams.restThGyr and VQFParams.restThAcc. - */ - void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); - - /** - * @brief Returns the current parameters. - */ - const VQFParams& getParams() const; - /** - * @brief Returns the coefficients used by the algorithm. - */ - const VQFCoefficients& getCoeffs() const; - /** - * @brief Returns the current state. - */ - const VQFState& getState() const; - /** - * @brief Overwrites the current state. - * - * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In - * combination with #getState, individual elements of the state can be modified. - * - * @param state A VQFState struct containing the new state - */ - void setState(const VQFState& state); - /** - * @brief Resets the state to the default values at initialization. - * - * Resetting the state is equivalent to creating a new instance of this class. - */ - void resetState(); - - /** - * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$). - */ - static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); - /** - * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$). - */ - static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); - /** - * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} = - * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). - */ - static void quatSetToIdentity(vqf_real_t out[4]); - /** - * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. - * - * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & - * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ - */ - static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); - /** - * @brief Rotates a vector with a given quaternion. - * - * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = - * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$ - */ - static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); - /** - * @brief Calculates the Euclidean norm of a vector. - * @param vec pointer to an array of N elements - * @param N number of elements - */ - static vqf_real_t norm(const vqf_real_t vec[], size_t N); - /** - * @brief Normalizes a vector in-place. - * @param vec pointer to an array of N elements that will be normalized - * @param N number of elements - */ - static void normalize(vqf_real_t vec[], size_t N); - /** - * @brief Clips a vector in-place. - * @param vec pointer to an array of N elements that will be clipped - * @param N number of elements - * @param min smallest allowed value - * @param max largest allowed value - */ - static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); - /** - * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant. - * - * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ - * - * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$. - * - * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain - * unfiltered values (\f$k=1\f$) - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds - * @return filter gain *k* - */ - static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); - /** - * @brief Calculates coefficients for a second-order Butterworth low-pass filter. - * - * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the - * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$. - * - * @param tau time constant \f$\tau\f$ in seconds - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds - * @param outB output array for numerator coefficients - * @param outA output array for denominator coefficients (without \f$a_0=1\f$) - */ - static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); - /** - * @brief Calculates the initial filter state for a given steady-state value. - * @param x0 steady state value - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param out output array for filter state - */ - static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2]); - /** - * @brief Adjusts the filter state when changing coefficients. - * - * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last - * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the - * output does not jump. - * - * @param last_y last filter output values (array of size N) - * @param N number of values in vector-valued signal - * @param b_old previous numerator coefficients - * @param a_old previous denominator coefficients (without \f$a_0=1\f$) - * @param b_new new numerator coefficients - * @param a_new new denominator coefficients (without \f$a_0=1\f$) - * @param state filter state (array of size N*2, will be modified) - */ - static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], - const double a_old[2], const double b_new[3], - const double a_new[2], double state[]); - /** - * @brief Performs a filter step for a scalar value. - * @param x input value - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param state filter state array (will be modified) - * @return filtered value - */ - static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); - /** - * @brief Performs filter step for vector-valued signal with averaging-based initialization. - * - * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the - * initial conditions for the low-pass filter are calculated based on the current mean value and from then on, - * regular filtering with the rational transfer function described by the coefficients b and a is performed. - * - * @param x input values (array of size N) - * @param N number of values in vector-valued signal - * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) - * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) - * @param b numerator coefficients - * @param a denominator coefficients (without \f$a_0=1\f$) - * @param state filter state (array of size N*2, will be modified) - * @param out output array for filtered values (size N) - */ - static void 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[]); + /** + * @brief Enables/disables rest detection and bias estimation during rest. + */ + void setRestBiasEstEnabled(bool enabled); + /** + * @brief Enables/disables magnetic disturbance detection and rejection. + */ + void setMagDistRejectionEnabled(bool enabled); + /** + * @brief Sets the current thresholds for rest detection. + * + * For details about the parameters, see VQFParams.restThGyr and + * VQFParams.restThAcc. + */ + void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); + + /** + * @brief Returns the current parameters. + */ + const VQFParams& getParams() const; + /** + * @brief Returns the coefficients used by the algorithm. + */ + const VQFCoefficients& getCoeffs() const; + /** + * @brief Returns the current state. + */ + const VQFState& getState() const; + /** + * @brief Overwrites the current state. + * + * This method allows to set a completely arbitrary filter state and is intended for + * debugging purposes. In combination with #getState, individual elements of the + * state can be modified. + * + * @param state A VQFState struct containing the new state + */ + void setState(const VQFState& state); + /** + * @brief Resets the state to the default values at initialization. + * + * Resetting the state is equivalent to creating a new instance of this class. + */ + void resetState(); + + /** + * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = + * \mathbf{q}_1 \otimes \mathbf{q}_2\f$). + */ + static void + quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); + /** + * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = + * \mathbf{q}^*\f$). + */ + static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); + /** + * @brief Sets the output quaternion to the identity quaternion + * (\f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). + */ + static void quatSetToIdentity(vqf_real_t out[4]); + /** + * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. + * + * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ + */ + static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); + /** + * @brief Rotates a vector with a given quaternion. + * + * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = + * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes + * \mathbf{q}^*\f$ + */ + static void + quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); + /** + * @brief Calculates the Euclidean norm of a vector. + * @param vec pointer to an array of N elements + * @param N number of elements + */ + static vqf_real_t norm(const vqf_real_t vec[], size_t N); + /** + * @brief Normalizes a vector in-place. + * @param vec pointer to an array of N elements that will be normalized + * @param N number of elements + */ + static void normalize(vqf_real_t vec[], size_t N); + /** + * @brief Clips a vector in-place. + * @param vec pointer to an array of N elements that will be clipped + * @param N number of elements + * @param min smallest allowed value + * @param max largest allowed value + */ + static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); + /** + * @brief Calculates the gain for a first-order low-pass filter from the 1/e time + * constant. + * + * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ + * + * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = + * \frac{1}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update + * (\f$k=0\f$) or 0 to obtain unfiltered values (\f$k=1\f$) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @return filter gain *k* + */ + static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); + /** + * @brief Calculates coefficients for a second-order Butterworth low-pass filter. + * + * The filter is parametrized via the time constant of the dampened, non-oscillating + * part of step response and the resulting cutoff frequency is \f$f_\mathrm{c} = + * \frac{\sqrt{2}}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @param outB output array for numerator coefficients + * @param outA output array for denominator coefficients (without \f$a_0=1\f$) + */ + static void + filterCoeffs(vqf_real_t tau, vqf_real_t Ts, vqf_real_t outB[3], vqf_real_t outA[2]); + /** + * @brief Calculates the initial filter state for a given steady-state value. + * @param x0 steady state value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param out output array for filter state + */ + static void filterInitialState( + vqf_real_t x0, + const vqf_real_t b[], + const vqf_real_t a[], + vqf_real_t out[2] + ); + /** + * @brief Adjusts the filter state when changing coefficients. + * + * This function assumes that the filter is currently in a steady state, i.e. the + * last input values and the last output values are all equal. Based on this, the + * filter state is adjusted to new filter coefficients so that the output does not + * jump. + * + * @param last_y last filter output values (array of size N) + * @param N number of values in vector-valued signal + * @param b_old previous numerator coefficients + * @param a_old previous denominator coefficients (without \f$a_0=1\f$) + * @param b_new new numerator coefficients + * @param a_new new denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + */ + static void filterAdaptStateForCoeffChange( + vqf_real_t last_y[], + size_t N, + const vqf_real_t b_old[3], + const vqf_real_t a_old[2], + const vqf_real_t b_new[3], + const vqf_real_t a_new[2], + vqf_real_t state[] + ); + /** + * @brief Performs a filter step for a scalar value. + * @param x input value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state array (will be modified) + * @return filtered value + */ + static vqf_real_t + filterStep(vqf_real_t x, const vqf_real_t b[3], const vqf_real_t a[2], vqf_real_t state[2]); + /** + * @brief Performs filter step for vector-valued signal with averaging-based + * initialization. + * + * During the first \f$\tau\f$ seconds, the filter output is the mean of the + * previous samples. At \f$t=\tau\f$, the initial conditions for the low-pass filter + * are calculated based on the current mean value and from then on, regular + * filtering with the rational transfer function described by the coefficients b and + * a is performed. + * + * @param x input values (array of size N) + * @param N number of values in vector-valued signal + * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + * @param out output array for filtered values (size N) + */ + static void 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[] + ); #ifndef VQF_NO_MOTION_BIAS_ESTIMATION - /** - * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. - * @param scale value of diagonal elements - * @param out output array of size 9 (3x3 matrix stored in row-major order) - */ - static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication after transposing the first matrix - * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Performs 3x3 matrix multiplication after transposing the second matrix - * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). - * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) - * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static void matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); - /** - * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\f$). - * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) - * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) - */ - static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); + /** + * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. + * @param scale value of diagonal elements + * @param out output array of size 9 (3x3 matrix stored in row-major order) + */ + static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = + * \mathbf{M}_1\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3Multiply( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Performs 3x3 matrix multiplication after transposing the first matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3MultiplyTpsFirst( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Performs 3x3 matrix multiplication after transposing the second matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static void matrix3MultiplyTpsSecond( + const vqf_real_t in1[9], + const vqf_real_t in2[9], + vqf_real_t out[9] + ); + /** + * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = + * \mathbf{M}^{-1}\f$). + * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major + * order) + */ + static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); #endif + void updateBiasForgettingTime(float biasForgettingTime); + protected: - /** - * @brief Calculates coefficients based on parameters and sampling rates. - */ - void setup(); - - /** - * @brief Contains the current parameters. - * - * See #getParams. To set parameters, pass them to the constructor. Part of the parameters can be changed with - * #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and - * #setRestDetectionThresholds. - */ - VQFParams params; - /** - * @brief Contains the current state. - * - * See #getState, #getState and #resetState. - */ - VQFState state; - /** - * @brief Contains the current coefficients (calculated in #setup). - * - * See #getCoeffs. - */ - VQFCoefficients coeffs; + /** + * @brief Calculates coefficients based on parameters and sampling rates. + */ + void setup(); + + /** + * @brief Contains the current parameters. + * + * See #getParams. To set parameters, pass them to the constructor. Part of the + * parameters can be changed with #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, + * #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and + * #setRestDetectionThresholds. + */ + VQFParams params; + /** + * @brief Contains the current state. + * + * See #getState, #getState and #resetState. + */ + VQFState state; + /** + * @brief Contains the current coefficients (calculated in #setup). + * + * See #getCoeffs. + */ + VQFCoefficients coeffs; }; -#endif // VQF_HPP +#endif // VQF_HPP diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index c4ca47c1e..2f123dc33 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -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; diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index 00cd4c78b..f246b2234 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -46,6 +46,7 @@ class Configuration { 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, diff --git a/src/configuration/SensorConfig.h b/src/configuration/SensorConfig.h index b85679b37..7013b0bae 100644 --- a/src/configuration/SensorConfig.h +++ b/src/configuration/SensorConfig.h @@ -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 { + SensorTypeID 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; }; diff --git a/src/debug.h b/src/debug.h index 9e0c7aa2e..06da9775f 100644 --- a/src/debug.h +++ b/src/debug.h @@ -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 false + +#ifndef DEBUG_MEASURE_SENSOR_TIME_TAKEN +#define DEBUG_MEASURE_SENSOR_TIME_TAKEN false +#endif + #endif // SLIMEVR_DEBUG_H_ diff --git a/src/debugging/TimeTaken.cpp b/src/debugging/TimeTaken.cpp new file mode 100644 index 000000000..1adfcccb6 --- /dev/null +++ b/src/debugging/TimeTaken.cpp @@ -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(SecondsBetweenReports * 1e3)) { + return; + } + + float usedPercentage = static_cast(timeTakenMicros) / 1e3f + / static_cast(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 diff --git a/src/debugging/TimeTaken.h b/src/debugging/TimeTaken.h new file mode 100644 index 000000000..0c6dd00db --- /dev/null +++ b/src/debugging/TimeTaken.h @@ -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 + +#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 diff --git a/src/defines.h b/src/defines.h index f1c5ecc15..7b5223b89 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,7 +26,7 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_BNO085 +#define IMU IMU_ICM45686 #define SECOND_IMU IMU #define BOARD BOARD_SLIMEVR #define IMU_ROTATION DEG_270 @@ -54,24 +54,25 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ */ #ifndef SENSOR_DESC_LIST -#define SENSOR_DESC_LIST \ - SENSOR_DESC_ENTRY( \ - IMU, \ - PRIMARY_IMU_ADDRESS_ONE, \ - IMU_ROTATION, \ - DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \ - PRIMARY_IMU_OPTIONAL, \ - DIRECT_PIN(PIN_IMU_INT), \ - 0 \ - ) \ - SENSOR_DESC_ENTRY( \ - SECOND_IMU, \ - SECONDARY_IMU_ADDRESS_TWO, \ - SECOND_IMU_ROTATION, \ - DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \ - SECONDARY_IMU_OPTIONAL, \ - DIRECT_PIN(PIN_IMU_INT_2), \ - 0 \ +#define SENSOR_DESC_LIST \ + SENSOR_DESC_ENTRY_SPI( \ + IMU, \ + DIRECT_SPI(24'000'000, MSBFIRST, SPI_MODE3, DIRECT_PIN(15)), \ + IMU_ROTATION, \ + DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \ + PRIMARY_IMU_OPTIONAL, \ + DIRECT_PIN(PIN_IMU_INT), \ + 0 \ + \ + ) \ + SENSOR_DESC_ENTRY( \ + SECOND_IMU, \ + SECONDARY_IMU_ADDRESS_TWO, \ + SECOND_IMU_ROTATION, \ + DIRECT_WIRE(PIN_IMU_SCL, PIN_IMU_SDA), \ + SECONDARY_IMU_OPTIONAL, \ + DIRECT_PIN(PIN_IMU_INT_2), \ + 0 \ ) #endif @@ -239,10 +240,10 @@ PIN_IMU_SDA, PRIMARY_IMU_OPTIONAL, BMI160_QMC_REMAP) \ // Board-specific configurations #if BOARD == BOARD_SLIMEVR -#define PIN_IMU_SDA 14 -#define PIN_IMU_SCL 12 -#define PIN_IMU_INT 16 -#define PIN_IMU_INT_2 13 +#define PIN_IMU_SDA 4 +#define PIN_IMU_SCL 5 +#define PIN_IMU_INT 2 +#define PIN_IMU_INT_2 16 #define PIN_BATTERY_LEVEL 17 #define LED_PIN 2 #define LED_INVERTED true diff --git a/src/logging/Logger.cpp b/src/logging/Logger.cpp index be8d68b9b..4508369be 100644 --- a/src/logging/Logger.cpp +++ b/src/logging/Logger.cpp @@ -7,49 +7,49 @@ void Logger::setTag(const char* tag) { strcpy(m_Tag, tag); } -void Logger::trace(const char* format, ...) { +void Logger::trace(const char* format, ...) const { va_list args; va_start(args, format); log(TRACE, format, args); va_end(args); } -void Logger::debug(const char* format, ...) { +void Logger::debug(const char* format, ...) const { va_list args; va_start(args, format); log(DEBUG, format, args); va_end(args); } -void Logger::info(const char* format, ...) { +void Logger::info(const char* format, ...) const { va_list args; va_start(args, format); log(INFO, format, args); va_end(args); } -void Logger::warn(const char* format, ...) { +void Logger::warn(const char* format, ...) const { va_list args; va_start(args, format); log(WARN, format, args); va_end(args); } -void Logger::error(const char* format, ...) { +void Logger::error(const char* format, ...) const { va_list args; va_start(args, format); log(ERROR, format, args); va_end(args); } -void Logger::fatal(const char* format, ...) { +void Logger::fatal(const char* format, ...) const { va_list args; va_start(args, format); log(FATAL, format, args); va_end(args); } -void Logger::log(Level level, const char* format, va_list args) { +void Logger::log(Level level, const char* format, va_list args) const { if (level < LOG_LEVEL) { return; } diff --git a/src/logging/Logger.h b/src/logging/Logger.h index 09d1d2b9f..14e69d802 100644 --- a/src/logging/Logger.h +++ b/src/logging/Logger.h @@ -27,48 +27,48 @@ class Logger { void setTag(const char* tag); - void trace(const char* str, ...); - void debug(const char* str, ...); - void info(const char* str, ...); - void warn(const char* str, ...); - void error(const char* str, ...); - void fatal(const char* str, ...); + void trace(const char* str, ...) const; + void debug(const char* str, ...) const; + void info(const char* str, ...) const; + void warn(const char* str, ...) const; + void error(const char* str, ...) const; + void fatal(const char* str, ...) const; template - inline void traceArray(const char* str, const T* array, size_t size) { + inline void traceArray(const char* str, const T* array, size_t size) const { logArray(TRACE, str, array, size); } template - inline void debugArray(const char* str, const T* array, size_t size) { + inline void debugArray(const char* str, const T* array, size_t size) const { logArray(DEBUG, str, array, size); } template - inline void infoArray(const char* str, const T* array, size_t size) { + inline void infoArray(const char* str, const T* array, size_t size) const { logArray(INFO, str, array, size); } template - inline void warnArray(const char* str, const T* array, size_t size) { + inline void warnArray(const char* str, const T* array, size_t size) const { logArray(WARN, str, array, size); } template - inline void errorArray(const char* str, const T* array, size_t size) { + inline void errorArray(const char* str, const T* array, size_t size) const { logArray(ERROR, str, array, size); } template - inline void fatalArray(const char* str, const T* array, size_t size) { + inline void fatalArray(const char* str, const T* array, size_t size) const { logArray(FATAL, str, array, size); } private: - void log(Level level, const char* str, va_list args); + void log(Level level, const char* str, va_list args) const; template - void logArray(Level level, const char* str, const T* array, size_t size) { + void logArray(Level level, const char* str, const T* array, size_t size) const { if (level < LOG_LEVEL) { return; } diff --git a/src/main.cpp b/src/main.cpp index a5f4eb88c..1dca40bfb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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; @@ -119,7 +124,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(); diff --git a/src/sensorinterface/RegisterInterface.h b/src/sensorinterface/RegisterInterface.h new file mode 100644 index 000000000..ffd756268 --- /dev/null +++ b/src/sensorinterface/RegisterInterface.h @@ -0,0 +1,44 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & 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 + +#include "I2Cdev.h" + +namespace SlimeVR::Sensors::SoftFusion { + +struct RegisterInterface { + static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; + + virtual uint8_t readReg(uint8_t regAddr) const = 0; + virtual uint16_t readReg16(uint8_t regAddr) const = 0; + virtual void writeReg(uint8_t regAddr, uint8_t value) const = 0; + virtual void writeReg16(uint8_t regAddr, uint16_t value) const = 0; + virtual void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0; + virtual void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const = 0; + virtual uint8_t getAddress() const = 0; + virtual bool hasSensorOnBus() = 0; +}; + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensorinterface/SPIImpl.h b/src/sensorinterface/SPIImpl.h new file mode 100644 index 000000000..1c3c6ff8e --- /dev/null +++ b/src/sensorinterface/SPIImpl.h @@ -0,0 +1,156 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2025 Eiren Rain & 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 +#include + +#include + +#include "../logging/Logger.h" +#include "RegisterInterface.h" + +#define ICM_READ_FLAG 0x80 + +namespace SlimeVR::Sensors::SoftFusion { + +struct SPIImpl : public RegisterInterface { + SPIImpl(uint8_t address) + : m_spiClass(SPI) + , m_spiSettings(SPISettings()) + , m_csPin(nullptr) { + static_assert("SPI requires explicit declaration"); + } + + SPIImpl(SPIClass& spiClass, SPISettings spiSettings, PinInterface* csPin) + : m_spiClass(spiClass) + , m_spiSettings(spiSettings) + , m_csPin(csPin) { + m_Logger.info( + "SPI settings: clock: %d, bit order: 0x%02X, data mode: 0x%02X", + spiSettings._clock, + spiSettings._bitOrder, + spiSettings._dataMode + ); + csPin->pinMode(OUTPUT); + csPin->digitalWrite(HIGH); + spiClass.begin(); + } + + uint8_t readReg(uint8_t regAddr) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + + m_spiClass.transfer(regAddr | ICM_READ_FLAG); + uint8_t buffer = m_spiClass.transfer(0); + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + + return buffer; + } + + uint16_t readReg16(uint8_t regAddr) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + + m_spiClass.transfer(regAddr | ICM_READ_FLAG); + uint8_t b1 = m_spiClass.transfer(0); + uint8_t b2 = m_spiClass.transfer(0); + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + return b2 << 8 | b1; + } + + void writeReg(uint8_t regAddr, uint8_t value) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + + m_spiClass.transfer(regAddr); + m_spiClass.transfer(value); + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + } + + void writeReg16(uint8_t regAddr, uint16_t value) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + + m_spiClass.transfer(regAddr); + m_spiClass.transfer(value & 0xFF); + m_spiClass.transfer(value >> 8); + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + } + + void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + ; + + m_spiClass.transfer(regAddr | ICM_READ_FLAG); + for (uint8_t i = 0; i < size; ++i) { + buffer[i] = m_spiClass.transfer(0); + } + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + } + + void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override { + m_spiClass.beginTransaction(m_spiSettings); + m_csPin->digitalWrite(LOW); + + m_spiClass.transfer(regAddr); + for (uint8_t i = 0; i < size; ++i) { + m_spiClass.transfer(buffer[i]); + } + + m_csPin->digitalWrite(HIGH); + m_spiClass.endTransaction(); + } + + bool hasSensorOnBus() { + return true; // TODO + } + + uint8_t getAddress() const override { + static_assert("SPI doesn't have addresses, you're using incompatible sensors"); + return 0; + } + + operator uint8_t() const { return getAddress(); } + + operator std::string() const { return std::string("SPI"); } + +private: + SPIClass& m_spiClass; + SPISettings m_spiSettings; + PinInterface* m_csPin; + SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("SPI"); +}; + +} // namespace SlimeVR::Sensors::SoftFusion diff --git a/src/sensors/softfusion/i2cimpl.h b/src/sensorinterface/i2cimpl.h similarity index 77% rename from src/sensors/softfusion/i2cimpl.h rename to src/sensorinterface/i2cimpl.h index 80ac421de..7694c9a27 100644 --- a/src/sensors/softfusion/i2cimpl.h +++ b/src/sensorinterface/i2cimpl.h @@ -26,22 +26,21 @@ #include #include "I2Cdev.h" +#include "RegisterInterface.h" namespace SlimeVR::Sensors::SoftFusion { -struct I2CImpl { - static constexpr size_t MaxTransactionLength = I2C_BUFFER_LENGTH - 2; - +struct I2CImpl : public RegisterInterface { I2CImpl(uint8_t devAddr) : m_devAddr(devAddr) {} - uint8_t readReg(uint8_t regAddr) const { + uint8_t readReg(uint8_t regAddr) const override { uint8_t buffer = 0; I2Cdev::readByte(m_devAddr, regAddr, &buffer); return buffer; } - uint16_t readReg16(uint8_t regAddr) const { + uint16_t readReg16(uint8_t regAddr) const override { uint16_t buffer = 0; I2Cdev::readBytes( m_devAddr, @@ -52,11 +51,11 @@ struct I2CImpl { return buffer; } - void writeReg(uint8_t regAddr, uint8_t value) const { + void writeReg(uint8_t regAddr, uint8_t value) const override { I2Cdev::writeByte(m_devAddr, regAddr, value); } - void writeReg16(uint8_t regAddr, uint16_t value) const { + void writeReg16(uint8_t regAddr, uint16_t value) const override { I2Cdev::writeBytes( m_devAddr, regAddr, @@ -65,14 +64,24 @@ struct I2CImpl { ); } - void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + void readBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override { I2Cdev::readBytes(m_devAddr, regAddr, size, buffer); } - void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const { + void writeBytes(uint8_t regAddr, uint8_t size, uint8_t* buffer) const override { I2Cdev::writeBytes(m_devAddr, regAddr, size, buffer); } + bool hasSensorOnBus() { return I2CSCAN::hasDevOnBus(m_devAddr); } + + uint8_t getAddress() const override { return m_devAddr; } + + operator uint8_t() const { return getAddress(); } + + operator std::string() const { + return std::string("I2C"); + } + private: uint8_t m_devAddr; }; diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index c608d488f..4502acf7a 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -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 diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h index c60b02f2d..7e82164cf 100644 --- a/src/sensors/SensorFusion.h +++ b/src/sensors/SensorFusion.h @@ -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 @@ class SensorFusion { : 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 @@ class SensorFusion { { } + 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 @@ class SensorFusion { 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 @@ class SensorFusion { #elif SENSOR_USE_BASICVQF BasicVQF basicvqf; #elif SENSOR_USE_VQF - SensorVQFParams vqfParams{}; + VQFParams vqfParams; VQF vqf; #endif diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 95c396372..5cf62309a 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -33,6 +33,21 @@ class SensorFusionRestDetect : public SensorFusion { { } +#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 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 2f80dcfb9..ea346d42a 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -24,15 +24,19 @@ #include "SensorManager.h" #include +#include #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" @@ -42,33 +46,47 @@ #include "softfusion/drivers/lsm6dsr.h" #include "softfusion/drivers/lsm6dsv.h" #include "softfusion/drivers/mpu6050.h" -#include "softfusion/i2cimpl.h" #include "softfusion/softfusionsensor.h" #if ESP32 #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 { +template using SoftFusionLSM6DS3TRC - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionICM42688 - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionBMI270 - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSV - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSO - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionLSM6DSR - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionMPU6050 - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionICM45686 - = SoftFusionSensor; + = SoftFusionSensor; +template using SoftFusionICM45605 - = SoftFusionSensor; + = SoftFusionSensor; void SensorManager::setup() { std::map directPinInterfaces; @@ -116,20 +134,40 @@ void SensorManager::setup() { } #define NO_PIN nullptr +#define NO_WIRE new EmptySensorInterface #define DIRECT_PIN(pin) directPin(pin) #define DIRECT_WIRE(scl, sda) directWire(scl, sda) #define MCP_PIN(pin) mcpPin(pin) #define PCA_WIRE(scl, sda, addr, ch) pcaWire(scl, sda, addr, ch) +#define DIRECT_SPI(clockfreq, bitorder, datamode, CS_PIN) \ + SoftFusion::SPIImpl(SPI, SPISettings(clockfreq, bitorder, datamode), CS_PIN) + +#define SENSOR_DESC_ENTRY(ImuType, ...) \ + { \ + auto sensor = buildSensor, SoftFusion::I2CImpl>( \ + sensorID, \ + __VA_ARGS__ \ + ); \ + if (sensor->isWorking()) { \ + m_Logger.info("Sensor %d configured", sensorID + 1); \ + activeSensorCount++; \ + } \ + m_Sensors.push_back(std::move(sensor)); \ + sensorID++; \ + } -#define SENSOR_DESC_ENTRY(ImuType, ...) \ - { \ - auto sensor = buildSensor(sensorID, __VA_ARGS__); \ - if (sensor->isWorking()) { \ - m_Logger.info("Sensor %d configured", sensorID + 1); \ - activeSensorCount++; \ - } \ - m_Sensors.push_back(std::move(sensor)); \ - sensorID++; \ +#define SENSOR_DESC_ENTRY_SPI(ImuType, ...) \ + { \ + auto sensor = buildSensor, SoftFusion::SPIImpl>( \ + sensorID, \ + __VA_ARGS__ \ + ); \ + if (sensor->isWorking()) { \ + m_Logger.info("Sensor %d configured", sensorID + 1); \ + activeSensorCount++; \ + } \ + m_Sensors.push_back(std::move(sensor)); \ + sensorID++; \ } // Apply descriptor list and expand to entries @@ -139,10 +177,6 @@ void SensorManager::setup() { { m_Sensors[SensorTypeID]->setSensorInfo(__VA_ARGS__); } SENSOR_INFO_LIST; -#undef SENSOR_DESC_ENTRY -#undef NO_PIN -#undef DIRECT_PIN -#undef DIRECT_WIRE m_Logger.info("%d sensor(s) configured", activeSensorCount); // Check and scan i2c if no sensors active if (activeSensorCount == 0) { diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index da590f1ec..0e7970b51 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -45,6 +45,8 @@ #include "sensorinterface/I2CPCAInterface.h" #include "sensorinterface/I2CWireSensorInterface.h" #include "sensorinterface/MCP23X17PinInterface.h" +#include "sensorinterface/SPIImpl.h" +#include "sensorinterface/i2cimpl.h" namespace SlimeVR { namespace Sensors { @@ -57,7 +59,7 @@ class SensorManager { void update(); - std::vector>& getSensors() { return m_Sensors; }; + std::vector>& getSensors() { return m_Sensors; }; SensorTypeID getSensorType(size_t id) { if (id < m_Sensors.size()) { return m_Sensors[id]->getSensorType(); @@ -68,26 +70,28 @@ class SensorManager { private: SlimeVR::Logging::Logger m_Logger; - std::vector> m_Sensors; + std::vector> m_Sensors; Adafruit_MCP23X17 m_MCP; - template - std::unique_ptr buildSensor( + template + std::unique_ptr<::Sensor> buildSensor( uint8_t sensorID, - std::optional imuAddress, + std::optional imuInterface, float rotation, SensorInterface* sensorInterface, bool optional = false, PinInterface* intPin = nullptr, int extraParam = 0 ) { - uint8_t i2cAddress = imuAddress.value_or(ImuType::Address + sensorID); + RegInterface interface = imuInterface.value_or( + RegInterface(ImuType::Address + sensorID) + ); m_Logger.trace( "Building IMU with: id=%d,\n\ address=0x%02X, rotation=%f,\n\ interface=%s, int=%s, extraParam=%d, optional=%d", sensorID, - i2cAddress, + interface, rotation, sensorInterface, intPin, @@ -96,29 +100,25 @@ class SensorManager { ); // Now start detecting and building the IMU - std::unique_ptr sensor; + std::unique_ptr<::Sensor> sensor; // Init I2C bus for each sensor upon startup sensorInterface->init(); sensorInterface->swapIn(); - if (I2CSCAN::hasDevOnBus(i2cAddress)) { - m_Logger - .trace("Sensor %d found at address 0x%02X", sensorID + 1, i2cAddress); + if (interface.hasSensorOnBus()) { + m_Logger.trace("Sensor %d found at address 0x%s", sensorID + 1, interface); } else { if (!optional) { - m_Logger.error( - "Mandatory sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress + m_Logger + .error("Mandatory sensor %d not found at address 0x%s", sensorID + 1, interface); + sensor = std::make_unique( + sensorID, + ImuType::SensorTypeID ); - sensor = std::make_unique(sensorID, ImuType::TypeID); } else { - m_Logger.debug( - "Optional sensor %d not found at address 0x%02X", - sensorID + 1, - i2cAddress - ); + m_Logger + .debug("Optional sensor %d not found at address 0x%s", sensorID + 1, interface); sensor = std::make_unique(sensorID); } return sensor; @@ -126,7 +126,7 @@ class SensorManager { sensor = std::make_unique( sensorID, - i2cAddress, + interface, rotation, sensorInterface, intPin, diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h new file mode 100644 index 000000000..8e9638dc8 --- /dev/null +++ b/src/sensors/softfusion/CalibrationBase.h @@ -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 +#include +#include + +#include "configuration/SensorConfig.h" +#include "motionprocessing/types.h" +#include "sensors/SensorFusionRestDetect.h" + +namespace SlimeVR::Sensor { + +template +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; + using ReturnLastFn + = std::function(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 diff --git a/src/sensors/softfusion/SoftfusionCalibration.h b/src/sensors/softfusion/SoftfusionCalibration.h new file mode 100644 index 000000000..04e316f80 --- /dev/null +++ b/src/sensors/softfusion/SoftfusionCalibration.h @@ -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 +#include + +#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 +class SoftfusionCalibrator : public CalibrationBase { +public: + static constexpr bool HasUpsideDownCalibration = true; + + using Base = CalibrationBase; + + 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( + GScale * (gyroSample[0] - calibration.G_off[0]) + ); + gyroSample[1] = static_cast( + GScale * (gyroSample[1] - calibration.G_off[1]) + ); + gyroSample[2] = static_cast( + 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(sumXYZ[0]) / static_cast(sampleCount); + calibration.G_off[1] + = static_cast(sumXYZ[1]) / static_cast(sampleCount); + calibration.G_off[2] + = static_cast(sumXYZ[2]) / static_cast(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(); + 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 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( + AScale * static_cast(xyz[0]) + ), + static_cast( + AScale * static_cast(xyz[1]) + ), + static_cast( + AScale * static_cast(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( + currentTime - (calibTarget - 1000 * SampleRateCalibSeconds) + ); + logger.debug( + "Collected %d gyro, %d acc samples during %d ms", + gyroSamples, + accelSamples, + millisFromStart + ); + calibration.A_Ts + = millisFromStart / (static_cast(accelSamples) * 1000.0f); + calibration.G_Ts + = millisFromStart / (static_cast(gyroSamples) * 1000.0f); + calibration.T_Ts + = millisFromStart / (static_cast(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 diff --git a/src/sensors/softfusion/TempGradientCalculator.cpp b/src/sensors/softfusion/TempGradientCalculator.cpp new file mode 100644 index 000000000..fabad38e6 --- /dev/null +++ b/src/sensors/softfusion/TempGradientCalculator.cpp @@ -0,0 +1,27 @@ +#include "TempGradientCalculator.h" + +#include + +TemperatureGradientCalculator::TemperatureGradientCalculator( + const std::function& callback +) + : callback{callback} {} + +void TemperatureGradientCalculator::feedSample(float sample, float timeStep) { + tempSum += sample * timeStep; +} + +void TemperatureGradientCalculator::tick() { + uint64_t nextCheck + = lastAverageSentMillis + static_cast(AveragingTimeSeconds * 1e3); + + if (millis() < nextCheck) { + return; + } + + lastAverageSentMillis = nextCheck; + float average = tempSum / AveragingTimeSeconds; + callback((average - lastTempAverage) / AveragingTimeSeconds); + lastTempAverage = average; + tempSum = 0; +} diff --git a/src/sensors/softfusion/TempGradientCalculator.h b/src/sensors/softfusion/TempGradientCalculator.h new file mode 100644 index 000000000..1aad1ed0f --- /dev/null +++ b/src/sensors/softfusion/TempGradientCalculator.h @@ -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 + +#include +#include + +class TemperatureGradientCalculator { +public: + explicit TemperatureGradientCalculator( + const std::function& callback + ); + void feedSample(float sample, float timeStep); + void tick(); + +private: + std::function callback; + + static constexpr float AveragingTimeSeconds = 5.0f; + float tempSum = 0; + uint64_t lastAverageSentMillis = millis(); + float lastTempAverage = 0; +}; diff --git a/src/sensors/softfusion/drivers/bmi270.h b/src/sensors/softfusion/drivers/bmi270.h index d5d1099b0..782e84e46 100644 --- a/src/sensors/softfusion/drivers/bmi270.h +++ b/src/sensors/softfusion/drivers/bmi270.h @@ -30,6 +30,7 @@ #include #include "bmi270fw.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -38,7 +39,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 400Hz, accel ODR = 100Hz // Timestamps reading are not used -template +template struct BMI270 { static constexpr uint8_t Address = 0x68; static constexpr auto Name = "BMI270"; @@ -52,18 +53,28 @@ 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; }; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - int8_t zxFactor; - BMI270(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) - , zxFactor(0) {} + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + int8_t m_zxFactor; + BMI270(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) + , m_zxFactor(0) {} struct Regs { struct WhoAmI { @@ -233,14 +244,14 @@ struct BMI270 { bool restartAndInit() { // perform initialization step - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); + m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueSwReset); delay(12); // disable power saving - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); + m_RegisterInterface.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueNoPowerSaving); delay(1); // firmware upload - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); + m_RegisterInterface.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueStartInit); for (uint16_t pos = 0; pos < sizeof(bmi270_firmware);) { // tell the device current position @@ -250,57 +261,57 @@ struct BMI270 { const uint16_t pos_words = pos >> 1; // convert current position to words const uint16_t position = (pos_words & 0x0F) | ((pos_words << 4) & 0xff00); - i2c.writeReg16(Regs::InitAddr, position); + m_RegisterInterface.writeReg16(Regs::InitAddr, position); // write actual payload chunk const uint16_t burstWrite = std::min( sizeof(bmi270_firmware) - pos, - I2CImpl::MaxTransactionLength + RegisterInterface::MaxTransactionLength ); - i2c.writeBytes( + m_RegisterInterface.writeBytes( Regs::InitData, burstWrite, const_cast(bmi270_firmware + pos) ); pos += burstWrite; } - i2c.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); + m_RegisterInterface.writeReg(Regs::InitCtrl::reg, Regs::InitCtrl::valueEndInit); delay(140); // leave fifo_self_wakeup enabled - i2c.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); + m_RegisterInterface.writeReg(Regs::PwrConf::reg, Regs::PwrConf::valueFifoSelfWakeup); // check if IMU initialized correctly - if (!(i2c.readReg(Regs::InternalStatus::reg) + if (!(m_RegisterInterface.readReg(Regs::InternalStatus::reg) & Regs::InternalStatus::initializedBit)) { // firmware upload fail or sensor not initialized return false; } // read zx factor used to reduce gyro cross-sensitivity error - const uint8_t zx_factor_reg = i2c.readReg(Regs::RaGyrCas); + const uint8_t zx_factor_reg = m_RegisterInterface.readReg(Regs::RaGyrCas); const uint8_t sign_byte = (zx_factor_reg << 1) & 0x80; - zxFactor = static_cast(zx_factor_reg | sign_byte); + m_zxFactor = static_cast(zx_factor_reg | sign_byte); return true; } void setNormalConfig(MotionlessCalibrationData& gyroSensitivity) { - i2c.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); - i2c.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); + m_RegisterInterface.writeReg(Regs::GyrConf::reg, Regs::GyrConf::value); + m_RegisterInterface.writeReg(Regs::GyrRange::reg, Regs::GyrRange::value); - i2c.writeReg(Regs::AccConf::reg, Regs::AccConf::value); - i2c.writeReg(Regs::AccRange::reg, Regs::AccRange::value); + m_RegisterInterface.writeReg(Regs::AccConf::reg, Regs::AccConf::value); + m_RegisterInterface.writeReg(Regs::AccRange::reg, Regs::AccRange::value); if (gyroSensitivity.valid) { - i2c.writeReg(Regs::Offset6::reg, Regs::Offset6::value); - i2c.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); + m_RegisterInterface.writeReg(Regs::Offset6::reg, Regs::Offset6::value); + m_RegisterInterface.writeBytes(Regs::GyrUserGain, 3, &gyroSensitivity.x); } - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); delay(100); // power up delay - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); delay(4); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); + m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueFifoFlush); delay(2); } @@ -314,44 +325,48 @@ 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(); // only Accel ON - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); + m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueAccOn); delay(100); - i2c.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); - i2c.writeReg(Regs::FeatPage, 1); - i2c.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); - i2c.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); + m_RegisterInterface.writeReg(Regs::GyrCrtConf::reg, Regs::GyrCrtConf::valueRunning); + m_RegisterInterface.writeReg(Regs::FeatPage, 1); + m_RegisterInterface.writeReg16(Regs::GTrig1::reg, Regs::GTrig1::valueTriggerCRT); + m_RegisterInterface.writeReg(Regs::Cmd::reg, Regs::Cmd::valueGTrigger); delay(200); - while (i2c.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { - logger.info("CRT running. Do not move tracker!"); + while (m_RegisterInterface.readReg(Regs::GyrCrtConf::reg) == Regs::GyrCrtConf::valueRunning) { + m_Logger.info("CRT running. Do not move tracker!"); delay(200); } - i2c.writeReg(Regs::FeatPage, 0); + m_RegisterInterface.writeReg(Regs::FeatPage, 0); - uint8_t status = i2c.readReg(Regs::GyrGainStatus::reg) + uint8_t status = m_RegisterInterface.readReg(Regs::GyrGainStatus::reg) >> Regs::GyrGainStatus::statusOffset; // turn gyroscope back on - i2c.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); + m_RegisterInterface.writeReg(Regs::PwrCtrl::reg, Regs::PwrCtrl::valueGyrAccTempOn); delay(100); + bool success; + if (status != 0) { - logger.error( + m_Logger.error( "CRT failed with status 0x%x. Recalibrate again to enable CRT.", status ); if (status == 0x03) { - logger.error("Reason: tracker was moved during CRT!"); + m_Logger.error("Reason: tracker was moved during CRT!"); } + + success = false; } else { std::array crt_values; - i2c.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); - logger.debug( + m_RegisterInterface.readBytes(Regs::GyrUserGain, crt_values.size(), crt_values.data()); + m_Logger.debug( "CRT finished successfully, result 0x%x, 0x%x, 0x%x", crt_values[0], crt_values[1], @@ -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 { @@ -376,11 +395,11 @@ struct BMI270 { // temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 // degrees C (0x7FFF) constexpr float TempStep = 128. / 65535; - const auto value = static_cast(i2c.readReg16(Regs::TempData)); + const auto value = static_cast(m_RegisterInterface.readReg16(Regs::TempData)); return static_cast(value) * TempStep + 23.0f; } - using FifoBuffer = std::array; + using FifoBuffer = std::array; FifoBuffer read_buffer; template @@ -391,15 +410,19 @@ struct BMI270 { return to_ret; } - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount); const auto bytes_to_read = std::min( static_cast(read_buffer.size()), static_cast(fifo_bytes) ); - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + m_RegisterInterface.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); for (uint32_t i = 0u; i < bytes_to_read;) { const uint8_t header = getFromFifo(i, read_buffer); @@ -428,7 +451,7 @@ struct BMI270 { gyro[0] = std::clamp( static_cast(gyro[0]) - static_cast( - (static_cast(zxFactor) * gyro[2]) / 512 + (static_cast(m_zxFactor) * gyro[2]) / 512 ), static_cast(ShortLimit::min()), static_cast(ShortLimit::max()) diff --git a/src/sensors/softfusion/drivers/icm42688.h b/src/sensors/softfusion/drivers/icm42688.h index eab654eab..a8d4e8b99 100644 --- a/src/sensors/softfusion/drivers/icm42688.h +++ b/src/sensors/softfusion/drivers/icm42688.h @@ -27,6 +27,8 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g @@ -34,7 +36,7 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 500Hz, accel ODR = 100Hz // Timestamps reading not used, as they're useless (constant predefined increment) -template +template struct ICM42688 { static constexpr uint8_t Address = 0x68; static constexpr auto Name = "ICM-42688"; @@ -42,24 +44,39 @@ 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; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM42688(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + 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, + }; + + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM42688(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct Regs { struct WhoAmI { 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,41 +128,42 @@ 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 - i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); + m_RegisterInterface.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset); delay(20); - i2c.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value); - i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); - i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); - i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); - i2c.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); - i2c.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value); + m_RegisterInterface.writeReg(Regs::IntfConfig0::reg, Regs::IntfConfig0::value); + m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + m_RegisterInterface.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value); + m_RegisterInterface.writeReg(Regs::FifoConfig1::reg, Regs::FifoConfig1::value); + m_RegisterInterface.writeReg(Regs::PwrMgmt::reg, Regs::PwrMgmt::value); delay(1); return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::TempData)); - float result = ((float)value / 132.48f) + 25.0f; - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto fifo_bytes = i2c.readReg16(Regs::FifoCount); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { + const auto fifo_bytes = m_RegisterInterface.readReg16(Regs::FifoCount); std::array read_buffer; // max 8 readings const auto bytes_to_read = std::min( @@ -153,7 +171,7 @@ struct ICM42688 { static_cast(fifo_bytes) ) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + m_RegisterInterface.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { FifoEntryAligned entry; memcpy( @@ -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(entry.part.gyro[0]) << 4 | (entry.part.xlsb & 0xf), + static_cast(entry.part.gyro[1]) << 4 | (entry.part.ylsb & 0xf), + static_cast(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(entry.part.accel[0]) << 4 + | (static_cast(entry.part.xlsb) & 0xf0 >> 4), + static_cast(entry.part.accel[1]) << 4 + | (static_cast(entry.part.ylsb) & 0xf0 >> 4), + static_cast(entry.part.accel[2]) << 4 + | (static_cast(entry.part.zlsb) & 0xf0 >> 4), + }; + processAccelSample(accelData, AccTs); + } + + if (entry.part.temp != 0x8000) { + processTemperatureSample(static_cast(entry.part.temp), TempTs); } } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/icm45605.h b/src/sensors/softfusion/drivers/icm45605.h index 8bf00c3d4..8296c0110 100644 --- a/src/sensors/softfusion/drivers/icm45605.h +++ b/src/sensors/softfusion/drivers/icm45605.h @@ -24,6 +24,7 @@ #pragma once #include "icm45base.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -34,13 +35,21 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz // Timestamps reading not used, as they're useless (constant predefined increment) -template -struct ICM45605 : public ICM45Base { +template +struct ICM45605 : public ICM45Base { static constexpr auto Name = "ICM-45605"; static constexpr auto Type = SensorTypeID::ICM45605; - ICM45605(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.3f, + .biasClip = 0.6f, + .restThGyr = 0.3f, + .restThAcc = 0.0098f, + }; + + ICM45605(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -49,11 +58,9 @@ struct ICM45605 : public ICM45Base { }; }; - float getDirectTemp() const { return ICM45Base::getDirectTemp(); } - bool initialize() { - ICM45Base::softResetIMU(); - return ICM45Base::initializeBase(); + ICM45Base::softResetIMU(); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h index c78e12890..efea87530 100644 --- a/src/sensors/softfusion/drivers/icm45686.h +++ b/src/sensors/softfusion/drivers/icm45686.h @@ -24,6 +24,7 @@ #pragma once #include "icm45base.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -34,13 +35,21 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // Gyroscope ODR = 409.6Hz, accel ODR = 204.8Hz // Timestamps reading not used, as they're useless (constant predefined increment) -template -struct ICM45686 : public ICM45Base { +template +struct ICM45686 : public ICM45Base { static constexpr auto Name = "ICM-45686"; static constexpr auto Type = SensorTypeID::ICM45686; - ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : ICM45Base{i2c, logger} {} + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 0.5f, + .biasClip = 1.0f, + .restThGyr = 0.5f, + .restThAcc = 0.196f, + }; + + ICM45686(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : ICM45Base{registerInterface, logger} {} struct Regs { struct WhoAmI { @@ -59,15 +68,13 @@ struct ICM45686 : public ICM45Base { }; }; - float getDirectTemp() const { return ICM45Base::getDirectTemp(); } - - using ICM45Base::i2c; + using ICM45Base::m_RegisterInterface; bool initialize() { - ICM45Base::softResetIMU(); - i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); - i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); - return ICM45Base::initializeBase(); + ICM45Base::softResetIMU(); + m_RegisterInterface.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value); + m_RegisterInterface.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value); + return ICM45Base::initializeBase(); } }; diff --git a/src/sensors/softfusion/drivers/icm45base.h b/src/sensors/softfusion/drivers/icm45base.h index 47dc5b1cd..91eccdead 100644 --- a/src/sensors/softfusion/drivers/icm45base.h +++ b/src/sensors/softfusion/drivers/icm45base.h @@ -30,15 +30,15 @@ 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 +template 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,13 +49,18 @@ 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; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - ICM45Base(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + static constexpr int fifoReadSize + = 8; // Can't be more than 12 or it will overflow i2c read size, default 8 + + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + ICM45Base(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct BaseRegs { static constexpr uint8_t TempData = 0x0c; @@ -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,86 +111,81 @@ 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); + m_RegisterInterface.writeReg(BaseRegs::DeviceConfig::reg, BaseRegs::DeviceConfig::valueSwReset); delay(35); } bool initializeBase() { // perform initialization step - i2c.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value); - i2c.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value); - i2c.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value); - i2c.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value); - i2c.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value); + m_RegisterInterface.writeReg(BaseRegs::GyroConfig::reg, BaseRegs::GyroConfig::value); + m_RegisterInterface.writeReg(BaseRegs::AccelConfig::reg, BaseRegs::AccelConfig::value); + m_RegisterInterface.writeReg(BaseRegs::FifoConfig0::reg, BaseRegs::FifoConfig0::value); + m_RegisterInterface.writeReg(BaseRegs::FifoConfig3::reg, BaseRegs::FifoConfig3::value); + m_RegisterInterface.writeReg(BaseRegs::PwrMgmt0::reg, BaseRegs::PwrMgmt0::value); delay(1); return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(BaseRegs::TempData)); - float result = ((float)value / 132.48f) + 25.0f; - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto fifo_packets = i2c.readReg16(BaseRegs::FifoCount); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { + const auto fifo_packets = m_RegisterInterface.readReg16(BaseRegs::FifoCount); const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize); - std::array read_buffer; // max 8 readings + FifoBuffer read_buffer; const auto bytes_to_read = std::min( - static_cast(read_buffer.size()), + static_cast(FullFifoBufferSize), static_cast(fifo_bytes) ) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(BaseRegs::FifoData, bytes_to_read, read_buffer.data()); + m_RegisterInterface.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(entry.part.gyro[0]) << 4 - | (entry.part.lsb[0] & 0xf), - static_cast(entry.part.gyro[1]) << 4 - | (entry.part.lsb[1] & 0xf), - static_cast(entry.part.gyro[2]) << 4 - | (entry.part.lsb[2] & 0xf), + static_cast(entry.gyro[0]) << 4 | (entry.lsb[0] & 0xf), + static_cast(entry.gyro[1]) << 4 | (entry.lsb[1] & 0xf), + static_cast(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(entry.part.accel[0]) << 4 - | (static_cast(entry.part.lsb[0]) & 0xf0 >> 4), - static_cast(entry.part.accel[1]) << 4 - | (static_cast(entry.part.lsb[1]) & 0xf0 >> 4), - static_cast(entry.part.accel[2]) << 4 - | (static_cast(entry.part.lsb[2]) & 0xf0 >> 4), + static_cast(entry.accel[0]) << 4 + | (static_cast(entry.lsb[0]) & 0xf0 >> 4), + static_cast(entry.accel[1]) << 4 + | (static_cast(entry.lsb[1]) & 0xf0 >> 4), + static_cast(entry.accel[2]) << 4 + | (static_cast(entry.lsb[2]) & 0xf0 >> 4), }; processAccelSample(accelData, AccTs); } + + if (entry.temp != 0x8000) { + processTemperatureSample(static_cast(entry.temp), TempTs); + } } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers +}; // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds-common.h b/src/sensors/softfusion/drivers/lsm6ds-common.h index 382103b9f..57705338d 100644 --- a/src/sensors/softfusion/drivers/lsm6ds-common.h +++ b/src/sensors/softfusion/drivers/lsm6ds-common.h @@ -29,22 +29,14 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -template +template struct LSM6DSOutputHandler { - LSM6DSOutputHandler(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + LSM6DSOutputHandler(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - - template - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; #pragma pack(push, 1) struct FifoEntryAligned { @@ -57,22 +49,24 @@ struct LSM6DSOutputHandler { static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1; - template + template 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; - const auto fifo_status = i2c.readReg16(Regs::FifoStatus); + const auto fifo_status = m_RegisterInterface.readReg16(Regs::FifoStatus); const auto available_axes = fifo_status & FIFO_SAMPLES_MASK; const auto fifo_bytes = available_axes * FullFifoEntrySize; if (fifo_status & FIFO_OVERRUN_LATCHED_MASK) { // FIFO overrun is expected to happen during startup and calibration - logger.error("FIFO OVERRUN! This occuring during normal usage is an issue." + m_Logger.error("FIFO OVERRUN! This occuring during normal usage is an issue." ); } @@ -82,7 +76,7 @@ struct LSM6DSOutputHandler { static_cast(fifo_bytes) ) / FullFifoEntrySize * FullFifoEntrySize; - i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); + m_RegisterInterface.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data()); for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) { FifoEntryAligned entry; uint8_t tag = read_buffer[i] >> 3; @@ -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 \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6ds3trc.h b/src/sensors/softfusion/drivers/lsm6ds3trc.h index cf0f31975..707838bc3 100644 --- a/src/sensors/softfusion/drivers/lsm6ds3trc.h +++ b/src/sensors/softfusion/drivers/lsm6ds3trc.h @@ -27,13 +27,15 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 416Hz -template +template struct LSM6DS3TRC { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DS3TR-C"; @@ -44,22 +46,35 @@ 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; - I2CImpl i2c; - SlimeVR::Logging::Logger logger; - LSM6DS3TRC(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + 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, + }; + + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger m_Logger; + LSM6DS3TRC(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(registerInterface) + , m_Logger(logger) {} struct Regs { struct WhoAmI { 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 @@ -92,31 +111,29 @@ struct LSM6DS3TRC { bool initialize() { // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); delay(20); - 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::FifoCtrl3::reg, Regs::FifoCtrl3::value); - i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + m_RegisterInterface.writeReg(Regs::Ctrl2G::reg, Regs::Ctrl2G::value); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl2::reg, Regs::FifoCtrl2::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl3::reg, Regs::FifoCtrl3::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); return true; } - float getDirectTemp() const { - const auto value = static_cast(i2c.readReg16(Regs::OutTemp)); - float result = ((float)value / 256.0f) + 25.0f; - - return result; - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto read_result = i2c.readReg16(Regs::FifoStatus); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTemperatureSample + ) { + const auto read_result = m_RegisterInterface.readReg16(Regs::FifoStatus); if (read_result & 0x4000) { // overrun! // disable and re-enable fifo to clear it - logger.debug("Fifo overrun, resetting..."); - i2c.writeReg(Regs::FifoCtrl5::reg, 0); - i2c.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); + m_Logger.debug("Fifo overrun, resetting..."); + m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, 0); + m_RegisterInterface.writeReg(Regs::FifoCtrl5::reg, Regs::FifoCtrl5::value); return; } const auto unread_entries = read_result & 0x7ff; @@ -133,7 +150,7 @@ struct LSM6DS3TRC { * sizeof(uint16_t) / single_measurement_bytes * single_measurement_bytes; - i2c.readBytes( + m_RegisterInterface.readBytes( Regs::FifoData, bytes_to_read, reinterpret_cast(read_buffer.data()) @@ -145,8 +162,9 @@ struct LSM6DS3TRC { reinterpret_cast(&read_buffer[i + 3]), AccTs ); + processTemperatureSample(read_buffer[i + 9], TempTs); } } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dso.h b/src/sensors/softfusion/drivers/lsm6dso.h index e2b03d9e6..05e3af6ee 100644 --- a/src/sensors/softfusion/drivers/lsm6dso.h +++ b/src/sensors/softfusion/drivers/lsm6dso.h @@ -28,6 +28,7 @@ #include #include "lsm6ds-common.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -35,8 +36,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSO : LSM6DSOutputHandler { +template +struct LSM6DSO : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSO"; static constexpr auto Type = SensorTypeID::LSM6DSO; @@ -44,22 +45,36 @@ struct LSM6DSO : LSM6DSOutputHandler { 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; - using LSM6DSOutputHandler::i2c; + 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::m_RegisterInterface; struct Regs { struct WhoAmI { 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,41 +96,45 @@ struct LSM6DSO : LSM6DSOutputHandler { }; 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; static constexpr uint8_t FifoData = 0x78; }; - LSM6DSO(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSO(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(registerInterface, logger) {} bool initialize() { // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); delay(20); - i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); - i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dsr.h b/src/sensors/softfusion/drivers/lsm6dsr.h index a91e24262..0a5fe268d 100644 --- a/src/sensors/softfusion/drivers/lsm6dsr.h +++ b/src/sensors/softfusion/drivers/lsm6dsr.h @@ -35,8 +35,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 416Hz, accel ODR = 104Hz -template -struct LSM6DSR : LSM6DSOutputHandler { +template +struct LSM6DSR : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSR"; static constexpr auto Type = SensorTypeID::LSM6DSR; @@ -44,22 +44,36 @@ struct LSM6DSR : LSM6DSOutputHandler { 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; - using LSM6DSOutputHandler::i2c; + 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::m_RegisterInterface; struct Regs { struct WhoAmI { 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,41 +95,45 @@ struct LSM6DSR : LSM6DSOutputHandler { }; 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; static constexpr uint8_t FifoData = 0x78; }; - LSM6DSR(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSR(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(registerInterface, logger) {} bool initialize() { // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); delay(20); - i2c.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); - i2c.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + m_RegisterInterface.writeReg(Regs::Ctrl1XL::reg, Regs::Ctrl1XL::value); + m_RegisterInterface.writeReg(Regs::Ctrl2GY::reg, Regs::Ctrl2GY::value); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/lsm6dsv.h b/src/sensors/softfusion/drivers/lsm6dsv.h index 6127d62fa..1317549be 100644 --- a/src/sensors/softfusion/drivers/lsm6dsv.h +++ b/src/sensors/softfusion/drivers/lsm6dsv.h @@ -28,6 +28,7 @@ #include #include "lsm6ds-common.h" +#include "vqf.h" namespace SlimeVR::Sensors::SoftFusion::Drivers { @@ -35,8 +36,8 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { // and gyroscope range at 1000dps // Gyroscope ODR = 480Hz, accel ODR = 120Hz -template -struct LSM6DSV : LSM6DSOutputHandler { +template +struct LSM6DSV : LSM6DSOutputHandler { static constexpr uint8_t Address = 0x6a; static constexpr auto Name = "LSM6DSV"; static constexpr auto Type = SensorTypeID::LSM6DSV; @@ -44,22 +45,36 @@ struct LSM6DSV : LSM6DSOutputHandler { 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; - using LSM6DSOutputHandler::i2c; + 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::m_RegisterInterface; struct Regs { struct WhoAmI { 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,44 +108,48 @@ struct LSM6DSV : LSM6DSOutputHandler { }; 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; static constexpr uint8_t FifoData = 0x78; }; - LSM6DSV(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : LSM6DSOutputHandler(i2c, logger) {} + LSM6DSV(RegInterface registerInterface, SlimeVR::Logging::Logger& logger) + : LSM6DSOutputHandler(registerInterface, logger) {} bool initialize() { // perform initialization step - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::valueSwReset); delay(20); - i2c.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); - i2c.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); - i2c.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); - i2c.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); - i2c.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); - i2c.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); - i2c.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); - i2c.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); + m_RegisterInterface.writeReg(Regs::HAODRCFG::reg, Regs::HAODRCFG::value); + m_RegisterInterface.writeReg(Regs::Ctrl1XLODR::reg, Regs::Ctrl1XLODR::value); + m_RegisterInterface.writeReg(Regs::Ctrl2GODR::reg, Regs::Ctrl2GODR::value); + m_RegisterInterface.writeReg(Regs::Ctrl3C::reg, Regs::Ctrl3C::value); + m_RegisterInterface.writeReg(Regs::Ctrl6GFS::reg, Regs::Ctrl6GFS::value); + m_RegisterInterface.writeReg(Regs::Ctrl8XLFS::reg, Regs::Ctrl8XLFS::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl3BDR::reg, Regs::FifoCtrl3BDR::value); + m_RegisterInterface.writeReg(Regs::FifoCtrl4Mode::reg, Regs::FifoCtrl4Mode::value); return true; } - float getDirectTemp() const { - return LSM6DSOutputHandler::template getDirectTemp(); - } - - template - void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - LSM6DSOutputHandler::template bulkRead( - processAccelSample, - processGyroSample, - GyrTs, - AccTs - ); + template + void bulkRead( + AccelCall&& processAccelSample, + GyroCall&& processGyroSample, + TempCall&& processTempSample + ) { + LSM6DSOutputHandler:: + template bulkRead( + processAccelSample, + processGyroSample, + processTempSample, + GyrTs, + AccTs, + TempTs + ); } }; -} // namespace SlimeVR::Sensors::SoftFusion::Drivers \ No newline at end of file +} // namespace SlimeVR::Sensors::SoftFusion::Drivers diff --git a/src/sensors/softfusion/drivers/mpu6050.h b/src/sensors/softfusion/drivers/mpu6050.h index 8fb2ce9b9..aa58d9958 100644 --- a/src/sensors/softfusion/drivers/mpu6050.h +++ b/src/sensors/softfusion/drivers/mpu6050.h @@ -29,13 +29,15 @@ #include #include +#include "vqf.h" + namespace SlimeVR::Sensors::SoftFusion::Drivers { // Driver uses acceleration range at 8g // and gyroscope range at 1000dps // Gyroscope ODR = accel ODR = 250Hz -template +template struct MPU6050 { struct FifoSample { uint8_t accel_x_h, accel_x_l; @@ -65,11 +67,21 @@ struct MPU6050 { static constexpr float GyroSensitivity = 32.8f; static constexpr float AccelSensitivity = 4096.0f; - I2CImpl i2c; - SlimeVR::Logging::Logger& logger; - MPU6050(I2CImpl i2c, SlimeVR::Logging::Logger& logger) - : i2c(i2c) - , logger(logger) {} + static constexpr float TemperatureZROChange = 1.6f; + + static constexpr VQFParams SensorVQFParams{ + .motionBiasEstEnabled = true, + .biasSigmaInit = 20.0f, + .biasClip = 40.0f, + .restThGyr = 20.0f, + .restThAcc = 0.784f, + }; + + RegInterface m_RegisterInterface; + SlimeVR::Logging::Logger& m_Logger; + MPU6050(RegInterface i2c, SlimeVR::Logging::Logger& logger) + : m_RegisterInterface(i2c) + , m_Logger(logger) {} struct Regs { struct WhoAmI { @@ -108,49 +120,49 @@ struct MPU6050 { } void resetFIFO() { - i2c.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); + m_RegisterInterface.writeReg(Regs::UserCtrl::reg, Regs::UserCtrl::fifoResetValue); } bool initialize() { // Reset - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_PWR_MGMT_1, 0x80 ); // PWR_MGMT_1: reset with 100ms delay (also disables sleep) delay(100); - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_SIGNAL_PATH_RESET, 0x07 ); // full SIGNAL_PATH_RESET: with another 100ms delay delay(100); // Configure - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_PWR_MGMT_1, 0x01 ); // 0000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_USER_CTRL, 0x00 ); // 0000 0000 USER_CTRL: Disable FIFO / I2C master / DMP - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_INT_ENABLE, 0x10 ); // 0001 0000 INT_ENABLE: only FIFO overflow interrupt - i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); - i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); - i2c.writeReg( + m_RegisterInterface.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value); + m_RegisterInterface.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value); + m_RegisterInterface.writeReg( MPU6050_RA_CONFIG, 0x02 ); // 0000 0010 CONFIG: No EXT_SYNC_SET, DLPF set to 98Hz(also lowers gyro // output rate to 1KHz) - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_SMPLRT_DIV, 0x03 ); // 0000 0011 SMPLRT_DIV: Divides the internal sample rate 250Hz (Sample Rate // = Gyroscope Output Rate / (1 + SMPLRT_DIV)) - i2c.writeReg( + m_RegisterInterface.writeReg( MPU6050_RA_FIFO_EN, 0x78 ); // 0111 1000 FIFO_EN: All gyro axes + Accel @@ -161,26 +173,26 @@ struct MPU6050 { } float getDirectTemp() const { - auto value = byteSwap(i2c.readReg16(Regs::OutTemp)); + auto value = byteSwap(m_RegisterInterface.readReg16(Regs::OutTemp)); float result = (static_cast(value) / 340.0f) + 36.53f; return result; } template void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) { - const auto status = i2c.readReg(Regs::IntStatus); + const auto status = m_RegisterInterface.readReg(Regs::IntStatus); if (status & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) { // Overflows make it so we lose track of which packet is which // This necessitates a reset - logger.debug("Fifo overrun, resetting..."); + m_Logger.debug("Fifo overrun, resetting..."); resetFIFO(); return; } std::array readBuffer; // max 10 packages of 12byte values (sample) of data form fifo - auto byteCount = byteSwap(i2c.readReg16(Regs::FifoCount)); + auto byteCount = byteSwap(m_RegisterInterface.readReg16(Regs::FifoCount)); auto readBytes = min(static_cast(byteCount), readBuffer.size()) / sizeof(FifoSample) * sizeof(FifoSample); @@ -188,7 +200,7 @@ struct MPU6050 { return; } - i2c.readBytes(Regs::FifoData, readBytes, readBuffer.data()); + m_RegisterInterface.readBytes(Regs::FifoData, readBytes, readBuffer.data()); for (auto i = 0u; i < readBytes; i += sizeof(FifoSample)) { const FifoSample* sample = reinterpret_cast(&readBuffer[i]); diff --git a/src/sensors/softfusion/nonblockingcalibration/AccelBiasCalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/AccelBiasCalibrationStep.h new file mode 100644 index 000000000..796731ca0 --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/AccelBiasCalibrationStep.h @@ -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 +#include + +#include "../../../consts.h" +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class AccelBiasCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + AccelBiasCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig, + float accelScale + ) + : CalibrationStep{sensorConfig} + , accelScale{accelScale} {} + + void start() override final { + CalibrationStep::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(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(accelSample[0])), + std::abs(static_cast(accelSample[1])), + std::abs(static_cast(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; + float accelScale; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/nonblockingcalibration/CalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/CalibrationStep.h new file mode 100644 index 000000000..c9f3512e6 --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/CalibrationStep.h @@ -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 +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 diff --git a/src/sensors/softfusion/nonblockingcalibration/GyroBiasCalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/GyroBiasCalibrationStep.h new file mode 100644 index 000000000..fcf958bcd --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/GyroBiasCalibrationStep.h @@ -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 +#include + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class GyroBiasCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + GyroBiasCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig + ) + : CalibrationStep{sensorConfig} {} + + void start() override final { + CalibrationStep::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(calibrationData.value().sampleCount); + float gyroOffsetY = calibrationData.value().gyroSums[1] + / static_cast(calibrationData.value().sampleCount); + float gyroOffsetZ = calibrationData.value().gyroSums[2] + / static_cast(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; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/nonblockingcalibration/MotionlessCalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/MotionlessCalibrationStep.h new file mode 100644 index 000000000..1ac1fa929 --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/MotionlessCalibrationStep.h @@ -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 +#include + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class MotionlessCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + MotionlessCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig, + IMU& imu + ) + : CalibrationStep{sensorConfig} + , imu{imu} {} + + void start() override final { + CalibrationStep::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; + IMU& imu; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h b/src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h new file mode 100644 index 000000000..707d19556 --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h @@ -0,0 +1,434 @@ +/* + 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 + +#include + +#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 +class NonBlockingCalibrator + : public Sensor::CalibrationBase { +public: + static constexpr bool HasUpsideDownCalibration = false; + + using Base = Sensor::CalibrationBase; + + 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::TickResult::DONE: + stepCalibrationForward(); + break; + case CalibrationStep::TickResult::SKIP: + stepCalibrationForward(false); + break; + case CalibrationStep::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( + GScale * (gyroSample[0] - activeCalibration.G_off1[0]) + ); + gyroSample[1] = static_cast( + GScale * (gyroSample[1] - activeCalibration.G_off1[1]) + ); + gyroSample[2] = static_cast( + 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 sampleRateCalibrationStep{calibration}; + MotionlessCalibrationStep motionlessCalibrationStep{ + calibration, + sensor + }; + GyroBiasCalibrationStep gyroBiasCalibrationStep{calibration}; + AccelBiasCalibrationStep accelBiasCalibrationStep{ + calibration, + static_cast(Base::AScale) + }; + NullCalibrationStep nullCalibrationStep{calibration}; + + CalibrationStep* 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 diff --git a/src/sensors/softfusion/nonblockingcalibration/NullCalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/NullCalibrationStep.h new file mode 100644 index 000000000..11b52b23d --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/NullCalibrationStep.h @@ -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 +class NullCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + NullCalibrationStep(SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig) + : CalibrationStep{sensorConfig} {} + + void start() override final { CalibrationStep::start(); } + + TickResult tick() override final { return TickResult::CONTINUE; } + + void cancel() override final {} +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/nonblockingcalibration/SampleRateCalibrationStep.h b/src/sensors/softfusion/nonblockingcalibration/SampleRateCalibrationStep.h new file mode 100644 index 000000000..85c7d0344 --- /dev/null +++ b/src/sensors/softfusion/nonblockingcalibration/SampleRateCalibrationStep.h @@ -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 + +#include "CalibrationStep.h" + +namespace SlimeVR::Sensors::NonBlockingCalibration { + +template +class SampleRateCalibrationStep : public CalibrationStep { + using CalibrationStep::sensorConfig; + using typename CalibrationStep::TickResult; + +public: + SampleRateCalibrationStep( + SlimeVR::Configuration::NonBlockingSensorConfig& sensorConfig + ) + : CalibrationStep{sensorConfig} {} + + void start() override final { + CalibrationStep::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; +}; + +} // namespace SlimeVR::Sensors::NonBlockingCalibration diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 1093ce97f..b495fe63a 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -23,54 +23,65 @@ #pragma once +#include +#include +#include + #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