1 files changed, 78 insertions, 5 deletions

diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp
index b1f658e62..f56f2ae1d 100644
--- a/src/core/hid/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -1,6 +1,8 @@
 // SPDX-FileCopyrightText: Copyright 2020 yuzu Emulator Project
 // SPDX-License-Identifier: GPL-2.0-or-later
 
+#include <cmath>
+
 #include "common/math_util.h"
 #include "core/hid/motion_input.h"
 
@@ -9,7 +11,9 @@ namespace Core::HID {
 MotionInput::MotionInput() {
     // Initialize PID constants with default values
     SetPID(0.3f, 0.005f, 0.0f);
-    SetGyroThreshold(0.007f);
+    SetGyroThreshold(ThresholdStandard);
+    ResetQuaternion();
+    ResetRotations();
 }
 
 void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
@@ -20,17 +24,31 @@ void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
 
 void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
     accel = acceleration;
+
+    accel.x = std::clamp(accel.x, -AccelMaxValue, AccelMaxValue);
+    accel.y = std::clamp(accel.y, -AccelMaxValue, AccelMaxValue);
+    accel.z = std::clamp(accel.z, -AccelMaxValue, AccelMaxValue);
 }
 
 void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
     gyro = gyroscope - gyro_bias;
 
-    // Auto adjust drift to minimize drift
-    if (!IsMoving(0.1f)) {
+    gyro.x = std::clamp(gyro.x, -GyroMaxValue, GyroMaxValue);
+    gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue);
+    gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue);
+
+    // Auto adjust gyro_bias to minimize drift
+    if (!IsMoving(IsAtRestRelaxed)) {
         gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
     }
 
-    if (gyro.Length() < gyro_threshold) {
+    // 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 {
         only_accelerometer = false;
@@ -41,6 +59,20 @@ void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) {
     quat = quaternion;
 }
 
+void MotionInput::SetEulerAngles(const Common::Vec3f& euler_angles) {
+    const float cr = std::cos(euler_angles.x * 0.5f);
+    const float sr = std::sin(euler_angles.x * 0.5f);
+    const float cp = std::cos(euler_angles.y * 0.5f);
+    const float sp = std::sin(euler_angles.y * 0.5f);
+    const float cy = std::cos(euler_angles.z * 0.5f);
+    const float sy = std::sin(euler_angles.z * 0.5f);
+
+    quat.w = cr * cp * cy + sr * sp * sy;
+    quat.xyz.x = sr * cp * cy - cr * sp * sy;
+    quat.xyz.y = cr * sp * cy + sr * cp * sy;
+    quat.xyz.z = cr * cp * sy - sr * sp * cy;
+}
+
 void MotionInput::SetGyroBias(const Common::Vec3f& bias) {
     gyro_bias = bias;
 }
@@ -49,6 +81,10 @@ void MotionInput::SetGyroThreshold(f32 threshold) {
     gyro_threshold = threshold;
 }
 
+void MotionInput::SetUserGyroThreshold(f32 threshold) {
+    user_gyro_threshold = threshold / ThresholdStandard;
+}
+
 void MotionInput::EnableReset(bool reset) {
     reset_enabled = reset;
 }
@@ -57,6 +93,10 @@ void MotionInput::ResetRotations() {
     rotations = {};
 }
 
+void MotionInput::ResetQuaternion() {
+    quat = {{0.0f, 0.0f, -1.0f}, 0.0f};
+}
+
 bool MotionInput::IsMoving(f32 sensitivity) const {
     return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
 }
@@ -73,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) {
@@ -204,11 +257,31 @@ Common::Vec3f MotionInput::GetRotations() const {
     return rotations;
 }
 
+Common::Vec3f MotionInput::GetEulerAngles() const {
+    // roll (x-axis rotation)
+    const float sinr_cosp = 2 * (quat.w * quat.xyz.x + quat.xyz.y * quat.xyz.z);
+    const float cosr_cosp = 1 - 2 * (quat.xyz.x * quat.xyz.x + quat.xyz.y * quat.xyz.y);
+
+    // pitch (y-axis rotation)
+    const float sinp = std::sqrt(1 + 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z));
+    const float cosp = std::sqrt(1 - 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z));
+
+    // yaw (z-axis rotation)
+    const float siny_cosp = 2 * (quat.w * quat.xyz.z + quat.xyz.x * quat.xyz.y);
+    const float cosy_cosp = 1 - 2 * (quat.xyz.y * quat.xyz.y + quat.xyz.z * quat.xyz.z);
+
+    return {
+        std::atan2(sinr_cosp, cosr_cosp),
+        2 * std::atan2(sinp, cosp) - Common::PI / 2,
+        std::atan2(siny_cosp, cosy_cosp),
+    };
+}
+
 void MotionInput::ResetOrientation() {
     if (!reset_enabled || only_accelerometer) {
         return;
     }
-    if (!IsMoving(0.5f) && accel.z <= -0.9f) {
+    if (!IsMoving(IsAtRestRelaxed) && accel.z <= -0.9f) {
         ++reset_counter;
         if (reset_counter > 900) {
             quat.w = 0;