Merge pull request #10203 from german77/calibration

core: hid: Allow to calibrate gyro sensor

6 files changed, 53 insertions, 11 deletions

diff --git a/src/core/hid/emulated_controller.cpp b/src/core/hid/emulated_controller.cpp
index c7f0af71f..366880711 100644
--- a/src/core/hid/emulated_controller.cpp
+++ b/src/core/hid/emulated_controller.cpp
@@ -688,6 +688,12 @@ void EmulatedController::SetMotionParam(std::size_t index, Common::ParamPackage
     ReloadInput();
 }
 
+void EmulatedController::StartMotionCalibration() {
+    for (ControllerMotionInfo& motion : controller.motion_values) {
+        motion.emulated.Calibrate();
+    }
+}
+
 void EmulatedController::SetButton(const Common::Input::CallbackStatus& callback, std::size_t index,
                                    Common::UUID uuid) {
     if (index >= controller.button_values.size()) {
diff --git a/src/core/hid/emulated_controller.h b/src/core/hid/emulated_controller.h
index fa4ad7fae..88fad2f56 100644
--- a/src/core/hid/emulated_controller.h
+++ b/src/core/hid/emulated_controller.h
@@ -290,6 +290,9 @@ public:
      */
     void SetMotionParam(std::size_t index, Common::ParamPackage param);
 
+    /// Auto calibrates the current motion devices
+    void StartMotionCalibration();
+
     /// Returns the latest button status from the controller with parameters
     ButtonValues GetButtonsValues() const;
 
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) {
diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h
index 482719359..11678983d 100644
--- a/src/core/hid/motion_input.h
+++ b/src/core/hid/motion_input.h
@@ -23,6 +23,8 @@ public:
     static constexpr float GyroMaxValue = 5.0f;
     static constexpr float AccelMaxValue = 7.0f;
 
+    static constexpr std::size_t CalibrationSamples = 300;
+
     explicit MotionInput();
 
     MotionInput(const MotionInput&) = default;
@@ -49,6 +51,8 @@ public:
     void UpdateRotation(u64 elapsed_time);
     void UpdateOrientation(u64 elapsed_time);
 
+    void Calibrate();
+
     [[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const;
     [[nodiscard]] Common::Vec3f GetAcceleration() const;
     [[nodiscard]] Common::Vec3f GetGyroscope() const;
@@ -61,6 +65,7 @@ public:
     [[nodiscard]] bool IsCalibrated(f32 sensitivity) const;
 
 private:
+    void StopCalibration();
     void ResetOrientation();
     void SetOrientationFromAccelerometer();
 
@@ -103,6 +108,12 @@ private:
 
     // Use accelerometer values to calculate position
     bool only_accelerometer = true;
+
+    // When enabled it will aggressively adjust for gyro drift
+    bool calibration_mode = false;
+
+    // Used to auto disable calibration mode
+    std::size_t calibration_counter = 0;
 };
 
 } // namespace Core::HID
diff --git a/src/yuzu/configuration/configure_input_player.cpp b/src/yuzu/configuration/configure_input_player.cpp
index 561a08dc5..2c2e7e47b 100644
--- a/src/yuzu/configuration/configure_input_player.cpp
+++ b/src/yuzu/configuration/configure_input_player.cpp
@@ -479,6 +479,9 @@ ConfigureInputPlayer::ConfigureInputPlayer(QWidget* parent, std::size_t player_i
                             param.Set("threshold", new_threshold / 1000.0f);
                             emulated_controller->SetMotionParam(motion_id, param);
                         });
+                        context_menu.addAction(tr("Calibrate sensor"), [&] {
+                            emulated_controller->StartMotionCalibration();
+                        });
                     }
                     context_menu.exec(motion_map[motion_id]->mapToGlobal(menu_location));
                 });
diff --git a/src/yuzu/configuration/configure_input_player_widget.cpp b/src/yuzu/configuration/configure_input_player_widget.cpp
index fe1ee2289..a188eef92 100644
--- a/src/yuzu/configuration/configure_input_player_widget.cpp
+++ b/src/yuzu/configuration/configure_input_player_widget.cpp
@@ -582,9 +582,9 @@ void PlayerControlPreview::DrawDualController(QPainter& p, const QPointF center)
         using namespace Settings::NativeMotion;
         p.setPen(colors.outline);
         p.setBrush(colors.transparent);
-        Draw3dCube(p, center + QPointF(-180, -5),
+        Draw3dCube(p, center + QPointF(-180, 90),
                    motion_values[Settings::NativeMotion::MotionLeft].euler, 20.0f);
-        Draw3dCube(p, center + QPointF(180, -5),
+        Draw3dCube(p, center + QPointF(180, 90),
                    motion_values[Settings::NativeMotion::MotionRight].euler, 20.0f);
     }
 
@@ -2926,14 +2926,14 @@ void PlayerControlPreview::DrawArrow(QPainter& p, const QPointF center, const Di
 void PlayerControlPreview::Draw3dCube(QPainter& p, QPointF center, const Common::Vec3f& euler,
                                       float size) {
     std::array<Common::Vec3f, 8> cube{
-        Common::Vec3f{-1, -1, -1},
-        {-1, 1, -1},
-        {1, 1, -1},
-        {1, -1, -1},
-        {-1, -1, 1},
-        {-1, 1, 1},
-        {1, 1, 1},
-        {1, -1, 1},
+        Common::Vec3f{-0.7f, -1, -0.5f},
+        {-0.7f, 1, -0.5f},
+        {0.7f, 1, -0.5f},
+        {0.7f, -1, -0.5f},
+        {-0.7f, -1, 0.5f},
+        {-0.7f, 1, 0.5f},
+        {0.7f, 1, 0.5f},
+        {0.7f, -1, 0.5f},
     };
 
     for (Common::Vec3f& point : cube) {