[sfusion, BMI270] Perform gryo motionless calibration before gyro off…

…set calibration (#367)

On a normal reset, we set the CRT (motionless) registers at startup, and then apply the calibrated gryo offset to every gyro sample.

Previously, we performed gyro offset calibration before CRT calibration. This means that we are actually calculating the gyro offset based on a CRT of 0, instead of the actual CRT.

This change performs CRT calibration before gyro calibration, so that the gryo offset is based on the actual CRT.

src/sensors/softfusion/drivers/bmi270.h

@@ -363,6 +363,11 @@ struct BMI270 {
 			gyroSensitivity.z = crt_values[2];
 		}
 
+		// CRT seems to leave some state behind which isn't persisted after
+		// restart. If we continue without restarting, the gyroscope will behave
+		// differently on this run compared to subsequent restarts.
+		restartAndInit();
+
 		setNormalConfig(gyroSensitivity);
 	}
 

src/sensors/softfusion/softfusionsensor.h

@@ -320,7 +320,6 @@ class SoftFusionSensor : public Sensor {
 		if (calibrationType == 0) {
 			// ALL
 			calibrateSampleRate();
-			calibrateGyroOffset();
 			if constexpr (HasMotionlessCalib) {
 				typename imu::MotionlessCalibrationData calibData;
 				m_sensor.motionlessCalibration(calibData);
@@ -330,6 +329,10 @@ class SoftFusionSensor : public Sensor {
 					sizeof(calibData)
 				);
 			}
+			// Gryoscope offset calibration can only happen after any motionless
+			// gyroscope calibration, otherwise we are calculating the offset based
+			// on an incorrect starting point
+			calibrateGyroOffset();
 			calibrateAccel();
 		} else if (calibrationType == 1) {
 			calibrateSampleRate();