diff options
Diffstat (limited to '')
-rw-r--r-- | src/core/hid/motion_input.cpp | 83 |
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; |