Increase sensitivity when gyro is calibrated + skip accel calibration…

… for now
kounocom · Oct 8, 2024 · 1e47ff7 · 1e47ff7
1 parent 6f74905
commit 1e47ff7
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 11 deletions.
diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp
@@ -216,6 +216,10 @@ void SensorFusion::calcLinearAcc(
 void SensorFusion::updateBiasForgettingTime(float biasForgettingTime) {
 	vqf.updateBiasForgettingTime(biasForgettingTime);
 }
+
+void SensorFusion::updateRestDetectionParams(float restThGyr, float restThAcc) {
+	vqf.setRestDetectionThresholds(restThGyr, restThAcc);
+}
 #endif
 }  // namespace Sensors
 }  // namespace SlimeVR
diff --git a/src/sensors/SensorFusion.h b/src/sensors/SensorFusion.h
@@ -125,6 +125,8 @@ class SensorFusion {
 
 #if SENSOR_USE_VQF
 	void updateBiasForgettingTime(float biasForgettingTime);
+
+	void updateRestDetectionParams(float restThGyr, float restThAcc);
 #endif
 
 protected:

diff --git a/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h b/src/sensors/nonblockingcalibration/AccelBiasCalibrationStep.h
@@ -144,7 +144,7 @@ class AccelBiasCalibrationStep : public CalibrationStep<SensorRawT> {
 	struct CalibrationData {
 		bool axisDetermined = false;
 		int16_t currentAxis[3]{0, 0, 0};
-		size_t largestAxis = -1;
+		int32_t largestAxis = -1;
 		int32_t accelSum = 0;
 		size_t sampleCount = 0;
 	};

diff --git a/src/sensors/nonblockingcalibration/NonBlockingCalibration.h b/src/sensors/nonblockingcalibration/NonBlockingCalibration.h
@@ -53,8 +53,8 @@ class NonBlockingCalibrator {
 		uint8_t sensorId
 	)
 		: fusion{fusion}
-		, accelScale{accelScale}
 		, imu{imu}
+		, accelScale{accelScale}
 		, sensorId{sensorId} {}
 
 	void setup(Configuration::NonBlockingCalibrationConfig& baseCalibrationConfig) {
@@ -151,9 +151,9 @@ class NonBlockingCalibrator {
 		} else if (calibrationConfig.gyroPointsCalibrated == 0) {
 			nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 			currentStep = &gyroBiasCalibrationStep;
-		} else if (!accelBiasCalibrationStep.allAxesCalibrated()) {
-			nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS;
-			currentStep = &accelBiasCalibrationStep;
+			// } else if (!accelBiasCalibrationStep.allAxesCalibrated()) {
+			// 	nextCalibrationStep = CalibrationStepEnum::ACCEL_BIAS;
+			// 	currentStep = &accelBiasCalibrationStep;
 		} else {
 			nextCalibrationStep = CalibrationStepEnum::GYRO_BIAS;
 			currentStep = &gyroBiasCalibrationStep;

diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h
@@ -107,11 +107,6 @@ class SoftFusionSensor : public Sensor {
 
 	float zroChangeOverTemperature = 0;
 
-#ifdef USE_NONBLOCKING_CALIBRATION
-	NonBlockingCalibration::NonBlockingCalibrator<imu, RawSensorT>
-		nonBlockingCalibrator;
-#endif
-
 	void handleTemperatureMeasurement(float temperature, float timeStep) {
 #ifdef USE_NONBLOCKING_CALIBRATION
 		nonBlockingCalibrator.provideTempSample(temperature);
@@ -170,12 +165,31 @@ class SoftFusionSensor : public Sensor {
 				m_calibration.A_Ts,
 				m_calibration.M_Ts
 			);
+
+#ifdef USE_NONBLOCKING_CALIBRATION
+			if (m_calibration.gyroPointsCalibrated > 0) {
+				m_fusion.updateRestDetectionParams(
+					imu::SensorVQFParams.restThGyr / 2,
+					imu::SensorVQFParams.restThAcc
+				);
+			}
+#endif
 		} else {
 			m_fusion = SensorFusionRestDetect(
 				m_calibration.G_Ts,
 				m_calibration.A_Ts,
 				m_calibration.M_Ts
 			);
+
+#ifdef USE_NONBLOCKING_CALIBRATION
+			if (m_calibration.gyroPointsCalibrated > 0) {
+				VQFParams defaultParams;
+				m_fusion.updateRestDetectionParams(
+					defaultParams.restThGyr / 2,
+					defaultParams.restThAcc
+				);
+			}
+#endif
 		}
 	}
 
@@ -341,7 +355,6 @@ SoftFusionSensor(
 		sclPin,
 		sdaPin
 	)
-	, m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger)
 	, m_fusion(
 		  HasPerSensorVQFParams
 			  ? SensorFusionRestDetect(
@@ -352,6 +365,7 @@ SoftFusionSensor(
 			  )
 			  : SensorFusionRestDetect(imu::GyrTs, imu::AccTs, imu::MagTs)
 	  )
+	, m_sensor(I2CImpl(imu::Address + addrSuppl), m_Logger)
 #ifdef USE_NONBLOCKING_CALIBRATION
 	, nonBlockingCalibrator(m_fusion, AScale, m_sensor, sensorId)
 #endif
@@ -803,6 +817,10 @@ SensorStatus getSensorState() override final { return m_status; }
 
 SensorFusionRestDetect m_fusion;
 T<I2CImpl> m_sensor;
+#ifdef USE_NONBLOCKING_CALIBRATION
+NonBlockingCalibration::NonBlockingCalibrator<imu, RawSensorT> nonBlockingCalibrator;
+#endif
+
 #ifndef USE_NONBLOCKING_CALIBRATION
 SlimeVR::Configuration::SoftFusionCalibrationConfig m_calibration
 	= {  // let's create here transparent calibration that doesn't affect input data