From 5e9fa5def5cf5ae3f00cc354a0b1363123dc6930 Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
Date: Thu, 9 Feb 2023 19:05:20 -0600
Subject: [PATCH] core: hid: Use gyro thresholds modes set by the game

---
 src/core/hid/emulated_controller.cpp          | 22 ++++++++++++++++++-
 src/core/hid/emulated_controller.h            |  5 ++++-
 src/core/hid/hid_types.h                      |  7 ++++++
 src/core/hid/motion_input.cpp                 | 12 ++++++----
 src/core/hid/motion_input.h                   | 15 +++++++++++++
 src/core/hle/service/hid/controllers/npad.cpp |  7 ++++--
 src/core/hle/service/hid/controllers/npad.h   | 14 ++++--------
 src/core/hle/service/hid/hid.cpp              |  6 ++---
 8 files changed, 67 insertions(+), 21 deletions(-)

diff --git a/src/core/hid/emulated_controller.cpp b/src/core/hid/emulated_controller.cpp
index 631aa6ad2..6d5a3dead 100644
--- a/src/core/hid/emulated_controller.cpp
+++ b/src/core/hid/emulated_controller.cpp
@@ -957,7 +957,7 @@ void EmulatedController::SetMotion(const Common::Input::CallbackStatus& callback
         raw_status.gyro.y.value,
         raw_status.gyro.z.value,
     });
-    emulated.SetGyroThreshold(raw_status.gyro.x.properties.threshold);
+    emulated.SetUserGyroThreshold(raw_status.gyro.x.properties.threshold);
     emulated.UpdateRotation(raw_status.delta_timestamp);
     emulated.UpdateOrientation(raw_status.delta_timestamp);
     force_update_motion = raw_status.force_update;
@@ -1284,6 +1284,26 @@ void EmulatedController::SetLedPattern() {
     }
 }
 
+void EmulatedController::SetGyroscopeZeroDriftMode(GyroscopeZeroDriftMode mode) {
+    for (auto& motion : controller.motion_values) {
+        switch (mode) {
+        case GyroscopeZeroDriftMode::Loose:
+            motion_sensitivity = motion.emulated.IsAtRestLoose;
+            motion.emulated.SetGyroThreshold(motion.emulated.ThresholdLoose);
+            break;
+        case GyroscopeZeroDriftMode::Tight:
+            motion_sensitivity = motion.emulated.IsAtRestThight;
+            motion.emulated.SetGyroThreshold(motion.emulated.ThresholdThight);
+            break;
+        case GyroscopeZeroDriftMode::Standard:
+        default:
+            motion_sensitivity = motion.emulated.IsAtRestStandard;
+            motion.emulated.SetGyroThreshold(motion.emulated.ThresholdStandard);
+            break;
+        }
+    }
+}
+
 void EmulatedController::SetSupportedNpadStyleTag(NpadStyleTag supported_styles) {
     supported_style_tag = supported_styles;
     if (!is_connected) {
diff --git a/src/core/hid/emulated_controller.h b/src/core/hid/emulated_controller.h
index b02bf35c4..a9da465a2 100644
--- a/src/core/hid/emulated_controller.h
+++ b/src/core/hid/emulated_controller.h
@@ -398,6 +398,9 @@ public:
     /// Asks the output device to change the player led pattern
     void SetLedPattern();
 
+    /// Changes sensitivity of the motion sensor
+    void SetGyroscopeZeroDriftMode(GyroscopeZeroDriftMode mode);
+
     /**
      * Adds a callback to the list of events
      * @param update_callback A ConsoleUpdateCallback that will be triggered
@@ -523,7 +526,7 @@ private:
     bool is_connected{false};
     bool is_configuring{false};
     bool system_buttons_enabled{true};
-    f32 motion_sensitivity{0.01f};
+    f32 motion_sensitivity{Core::HID::MotionInput::IsAtRestStandard};
     bool force_update_motion{false};
     u32 turbo_button_state{0};
 
diff --git a/src/core/hid/hid_types.h b/src/core/hid/hid_types.h
index e3b1cfbc6..6b35f448c 100644
--- a/src/core/hid/hid_types.h
+++ b/src/core/hid/hid_types.h
@@ -282,6 +282,13 @@ enum class VibrationGcErmCommand : u64 {
     StopHard = 2,
 };
 
+// This is nn::hid::GyroscopeZeroDriftMode
+enum class GyroscopeZeroDriftMode : u32 {
+    Loose = 0,
+    Standard = 1,
+    Tight = 2,
+};
+
 // This is nn::hid::NpadStyleTag
 struct NpadStyleTag {
     union {
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp
index b1f658e62..eef6edf4b 100644
--- a/src/core/hid/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -9,7 +9,7 @@ namespace Core::HID {
 MotionInput::MotionInput() {
     // Initialize PID constants with default values
     SetPID(0.3f, 0.005f, 0.0f);
-    SetGyroThreshold(0.007f);
+    SetGyroThreshold(ThresholdStandard);
 }
 
 void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
@@ -26,11 +26,11 @@ void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
     gyro = gyroscope - gyro_bias;
 
     // Auto adjust drift to minimize drift
-    if (!IsMoving(0.1f)) {
+    if (!IsMoving(IsAtRestRelaxed)) {
         gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
     }
 
-    if (gyro.Length() < gyro_threshold) {
+    if (gyro.Length() < gyro_threshold * user_gyro_threshold) {
         gyro = {};
     } else {
         only_accelerometer = false;
@@ -49,6 +49,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;
 }
@@ -208,7 +212,7 @@ 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;
diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h
index f5fd90db5..9180bb9aa 100644
--- a/src/core/hid/motion_input.h
+++ b/src/core/hid/motion_input.h
@@ -11,6 +11,15 @@ namespace Core::HID {
 
 class MotionInput {
 public:
+    static constexpr float ThresholdLoose = 0.01f;
+    static constexpr float ThresholdStandard = 0.007f;
+    static constexpr float ThresholdThight = 0.002f;
+
+    static constexpr float IsAtRestRelaxed = 0.05f;
+    static constexpr float IsAtRestLoose = 0.02f;
+    static constexpr float IsAtRestStandard = 0.01f;
+    static constexpr float IsAtRestThight = 0.005f;
+
     explicit MotionInput();
 
     MotionInput(const MotionInput&) = default;
@@ -26,6 +35,9 @@ public:
     void SetGyroBias(const Common::Vec3f& bias);
     void SetGyroThreshold(f32 threshold);
 
+    /// Applies a modifier on top of the normal gyro threshold
+    void SetUserGyroThreshold(f32 threshold);
+
     void EnableReset(bool reset);
     void ResetRotations();
 
@@ -74,6 +86,9 @@ private:
     // Minimum gyro amplitude to detect if the device is moving
     f32 gyro_threshold = 0.0f;
 
+    // Multiplies gyro_threshold by this value
+    f32 user_gyro_threshold = 0.0f;
+
     // Number of invalid sequential data
     u32 reset_counter = 0;
 
diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp
index 80eba22e8..ba6f04d8d 100644
--- a/src/core/hle/service/hid/controllers/npad.cpp
+++ b/src/core/hle/service/hid/controllers/npad.cpp
@@ -1132,7 +1132,8 @@ Result Controller_NPad::DisconnectNpad(Core::HID::NpadIdType npad_id) {
     return ResultSuccess;
 }
 Result Controller_NPad::SetGyroscopeZeroDriftMode(
-    const Core::HID::SixAxisSensorHandle& sixaxis_handle, GyroscopeZeroDriftMode drift_mode) {
+    const Core::HID::SixAxisSensorHandle& sixaxis_handle,
+    Core::HID::GyroscopeZeroDriftMode drift_mode) {
     const auto is_valid = VerifyValidSixAxisSensorHandle(sixaxis_handle);
     if (is_valid.IsError()) {
         LOG_ERROR(Service_HID, "Invalid handle, error_code={}", is_valid.raw);
@@ -1140,14 +1141,16 @@ Result Controller_NPad::SetGyroscopeZeroDriftMode(
     }
 
     auto& sixaxis = GetSixaxisState(sixaxis_handle);
+    auto& controller = GetControllerFromHandle(sixaxis_handle);
     sixaxis.gyroscope_zero_drift_mode = drift_mode;
+    controller.device->SetGyroscopeZeroDriftMode(drift_mode);
 
     return ResultSuccess;
 }
 
 Result Controller_NPad::GetGyroscopeZeroDriftMode(
     const Core::HID::SixAxisSensorHandle& sixaxis_handle,
-    GyroscopeZeroDriftMode& drift_mode) const {
+    Core::HID::GyroscopeZeroDriftMode& drift_mode) const {
     const auto is_valid = VerifyValidSixAxisSensorHandle(sixaxis_handle);
     if (is_valid.IsError()) {
         LOG_ERROR(Service_HID, "Invalid handle, error_code={}", is_valid.raw);
diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h
index 02cc00920..a5998c453 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -52,13 +52,6 @@ public:
     // When the controller is requesting a motion update for the shared memory
     void OnMotionUpdate(const Core::Timing::CoreTiming& core_timing) override;
 
-    // This is nn::hid::GyroscopeZeroDriftMode
-    enum class GyroscopeZeroDriftMode : u32 {
-        Loose = 0,
-        Standard = 1,
-        Tight = 2,
-    };
-
     // This is nn::hid::NpadJoyHoldType
     enum class NpadJoyHoldType : u64 {
         Vertical = 0,
@@ -146,9 +139,9 @@ public:
     Result DisconnectNpad(Core::HID::NpadIdType npad_id);
 
     Result SetGyroscopeZeroDriftMode(const Core::HID::SixAxisSensorHandle& sixaxis_handle,
-                                     GyroscopeZeroDriftMode drift_mode);
+                                     Core::HID::GyroscopeZeroDriftMode drift_mode);
     Result GetGyroscopeZeroDriftMode(const Core::HID::SixAxisSensorHandle& sixaxis_handle,
-                                     GyroscopeZeroDriftMode& drift_mode) const;
+                                     Core::HID::GyroscopeZeroDriftMode& drift_mode) const;
     Result IsSixAxisSensorAtRest(const Core::HID::SixAxisSensorHandle& sixaxis_handle,
                                  bool& is_at_rest) const;
     Result IsFirmwareUpdateAvailableForSixAxisSensor(
@@ -489,7 +482,8 @@ private:
         Core::HID::SixAxisSensorFusionParameters fusion{};
         Core::HID::SixAxisSensorCalibrationParameter calibration{};
         Core::HID::SixAxisSensorIcInformation ic_information{};
-        GyroscopeZeroDriftMode gyroscope_zero_drift_mode{GyroscopeZeroDriftMode::Standard};
+        Core::HID::GyroscopeZeroDriftMode gyroscope_zero_drift_mode{
+            Core::HID::GyroscopeZeroDriftMode::Standard};
     };
 
     struct NpadControllerData {
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index ac2c0c76d..5a1aa0903 100644
--- a/src/core/hle/service/hid/hid.cpp
+++ b/src/core/hle/service/hid/hid.cpp
@@ -712,7 +712,7 @@ void Hid::ResetSixAxisSensorFusionParameters(Kernel::HLERequestContext& ctx) {
 void Hid::SetGyroscopeZeroDriftMode(Kernel::HLERequestContext& ctx) {
     IPC::RequestParser rp{ctx};
     const auto sixaxis_handle{rp.PopRaw<Core::HID::SixAxisSensorHandle>()};
-    const auto drift_mode{rp.PopEnum<Controller_NPad::GyroscopeZeroDriftMode>()};
+    const auto drift_mode{rp.PopEnum<Core::HID::GyroscopeZeroDriftMode>()};
     const auto applet_resource_user_id{rp.Pop<u64>()};
 
     auto& controller = GetAppletResource()->GetController<Controller_NPad>(HidController::NPad);
@@ -739,7 +739,7 @@ void Hid::GetGyroscopeZeroDriftMode(Kernel::HLERequestContext& ctx) {
 
     const auto parameters{rp.PopRaw<Parameters>()};
 
-    auto drift_mode{Controller_NPad::GyroscopeZeroDriftMode::Standard};
+    auto drift_mode{Core::HID::GyroscopeZeroDriftMode::Standard};
     auto& controller = GetAppletResource()->GetController<Controller_NPad>(HidController::NPad);
     const auto result = controller.GetGyroscopeZeroDriftMode(parameters.sixaxis_handle, drift_mode);
 
@@ -764,7 +764,7 @@ void Hid::ResetGyroscopeZeroDriftMode(Kernel::HLERequestContext& ctx) {
 
     const auto parameters{rp.PopRaw<Parameters>()};
 
-    const auto drift_mode{Controller_NPad::GyroscopeZeroDriftMode::Standard};
+    const auto drift_mode{Core::HID::GyroscopeZeroDriftMode::Standard};
     auto& controller = GetAppletResource()->GetController<Controller_NPad>(HidController::NPad);
     const auto result = controller.SetGyroscopeZeroDriftMode(parameters.sixaxis_handle, drift_mode);