core: hid: Allow to calibrate gyro sensor

1 files changed, 20 insertions, 1 deletions

diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp
index b60478dbb..f56f2ae1d 100644
--- a/src/core/hid/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -37,11 +37,17 @@ void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
     gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue);
     gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue);
 
-    // Auto adjust drift to minimize drift
+    // Auto adjust gyro_bias to minimize drift
     if (!IsMoving(IsAtRestRelaxed)) {
         gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
     }
 
+    // Adjust drift when calibration mode is enabled
+    if (calibration_mode) {
+        gyro_bias = (gyro_bias * 0.99f) + (gyroscope * 0.01f);
+        StopCalibration();
+    }
+
     if (gyro.Length() < gyro_threshold * user_gyro_threshold) {
         gyro = {};
     } else {
@@ -107,6 +113,19 @@ void MotionInput::UpdateRotation(u64 elapsed_time) {
     rotations += gyro * sample_period;
 }
 
+void MotionInput::Calibrate() {
+    calibration_mode = true;
+    calibration_counter = 0;
+}
+
+void MotionInput::StopCalibration() {
+    if (calibration_counter++ > CalibrationSamples) {
+        calibration_mode = false;
+        ResetQuaternion();
+        ResetRotations();
+    }
+}
+
 // Based on Madgwick's implementation of Mayhony's AHRS algorithm.
 // https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs
 void MotionInput::UpdateOrientation(u64 elapsed_time) {