Merge pull request #9757 from german77/gyro

core: hid: Use gyro thresholds modes set by the game

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) {

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};
 

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 {

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;

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;
 

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);

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 {

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);