From 97bd6d641811cafc3ec99879fa53beda1a2334b5 Mon Sep 17 00:00:00 2001 From: Narr the Reg Date: Mon, 8 May 2023 11:17:27 -0600 Subject: [PATCH] core: hid: Allow to calibrate gyro sensor --- src/core/hid/emulated_controller.cpp | 6 ++++++ src/core/hid/emulated_controller.h | 3 +++ src/core/hid/motion_input.cpp | 21 ++++++++++++++++++- src/core/hid/motion_input.h | 11 ++++++++++ .../configuration/configure_input_player.cpp | 3 +++ 5 files changed, 43 insertions(+), 1 deletion(-) 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 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)); });