Use calibrated ZRO values

SlimeVR · Jan 5, 2025 · 140177c · 140177c
1 parent 365ebb2
commit 140177c
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 1 deletion.
diff --git a/src/sensors/softfusion/CalibrationBase.h b/src/sensors/softfusion/CalibrationBase.h
@@ -98,6 +98,8 @@ class CalibrationBase {
 	virtual void provideGyroSample(const RawSensorT gyroSample[3]) {}
 	virtual void provideTempSample(float tempSample) {}
 
+	virtual float getZROChange() { return IMU::TemperatureZROChange; };
+
 protected:
 	void recalcFusion() {
 		fusion = Sensors::SensorFusionRestDetect(

diff --git a/src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h b/src/sensors/softfusion/nonblockingcalibration/NonBlockingCalibration.h
@@ -179,6 +179,26 @@ class NonBlockingCalibrator
 		}
 	}
 
+	float getZROChange() final {
+		if (activeCalibration.gyroPointsCalibrated < 2) {
+			return 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));
+
+		return 0.1f / maxDiff
+			 / (activeCalibration.gyroMeasurementTemperature2
+				- activeCalibration.gyroMeasurementTemperature1);
+	}
+
 private:
 	enum class CalibrationStepEnum {
 		NONE,

diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h
@@ -102,7 +102,7 @@ class SoftFusionSensor : public Sensor {
 
 	TemperatureGradientCalculator tempGradientCalculator{[&](float gradient) {
 		m_fusion.updateBiasForgettingTime(
-			std::fabs(gradient) * imu::TemperatureZROChange
+			calibrator.getZROChange() / std::fabs(gradient)
 		);
 	}};