diff options
Diffstat (limited to 'src/core/hid/motion_input.cpp')
| -rw-r--r-- | src/core/hid/motion_input.cpp | 14 |
1 files changed, 14 insertions, 0 deletions
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp index eef6edf4ba..0dd66c1ccf 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; } |