diff options
| -rw-r--r-- | src/core/hid/emulated_controller.cpp | 12 | ||||
| -rw-r--r-- | src/core/hid/motion_input.cpp | 14 | ||||
| -rw-r--r-- | src/core/hid/motion_input.h | 6 |
3 files changed, 30 insertions, 2 deletions
diff --git a/src/core/hid/emulated_controller.cpp b/src/core/hid/emulated_controller.cpp index 6d5a3dead..a29c9a6f8 100644 --- a/src/core/hid/emulated_controller.cpp +++ b/src/core/hid/emulated_controller.cpp @@ -363,7 +363,17 @@ void EmulatedController::ReloadInput() { SetMotion(callback, index); }, }); - motion_devices[index]->ForceUpdate(); + + // Restore motion state + auto& emulated_motion = controller.motion_values[index].emulated; + auto& motion = controller.motion_state[index]; + emulated_motion.ResetRotations(); + emulated_motion.ResetQuaternion(); + motion.accel = emulated_motion.GetAcceleration(); + motion.gyro = emulated_motion.GetGyroscope(); + motion.rotation = emulated_motion.GetRotations(); + motion.orientation = emulated_motion.GetOrientation(); + motion.is_at_rest = !emulated_motion.IsMoving(motion_sensitivity); } for (std::size_t index = 0; index < camera_devices.size(); ++index) { diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp index eef6edf4b..0dd66c1cc 100644 --- a/src/core/hid/motion_input.cpp +++ b/src/core/hid/motion_input.cpp @@ -10,6 +10,8 @@ MotionInput::MotionInput() { // Initialize PID constants with default values SetPID(0.3f, 0.005f, 0.0f); SetGyroThreshold(ThresholdStandard); + ResetQuaternion(); + ResetRotations(); } void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) { @@ -20,11 +22,19 @@ 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; + 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 drift to minimize drift if (!IsMoving(IsAtRestRelaxed)) { gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f); @@ -61,6 +71,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; } diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h index 9180bb9aa..e2c1bbf95 100644 --- a/src/core/hid/motion_input.h +++ b/src/core/hid/motion_input.h @@ -20,6 +20,9 @@ public: static constexpr float IsAtRestStandard = 0.01f; static constexpr float IsAtRestThight = 0.005f; + static constexpr float GyroMaxValue = 5.0f; + static constexpr float AccelMaxValue = 7.0f; + explicit MotionInput(); MotionInput(const MotionInput&) = default; @@ -40,6 +43,7 @@ public: void EnableReset(bool reset); void ResetRotations(); + void ResetQuaternion(); void UpdateRotation(u64 elapsed_time); void UpdateOrientation(u64 elapsed_time); @@ -69,7 +73,7 @@ private: Common::Vec3f derivative_error; // Quaternion containing the device orientation - Common::Quaternion<f32> quat{{0.0f, 0.0f, -1.0f}, 0.0f}; + Common::Quaternion<f32> quat; // Number of full rotations in each axis Common::Vec3f rotations; |