1 files changed, 15 insertions, 42 deletions

diff --git a/src/input_common/motion_input.cpp b/src/core/hid/motion_input.cpp
index 1c9d561c0..c25fea966 100644
--- a/src/input_common/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -2,13 +2,21 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included
 
-#include <random>
 #include "common/math_util.h"
-#include "input_common/motion_input.h"
+#include "core/hid/motion_input.h"
 
-namespace InputCommon {
+namespace Core::HID {
 
-MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) : kp(new_kp), ki(new_ki), kd(new_kd) {}
+MotionInput::MotionInput() {
+    // Initialize PID constants with default values
+    SetPID(0.3f, 0.005f, 0.0f);
+}
+
+void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
+    kp = new_kp;
+    ki = new_ki;
+    kd = new_kd;
+}
 
 void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
     accel = acceleration;
@@ -65,6 +73,8 @@ void MotionInput::UpdateRotation(u64 elapsed_time) {
     rotations += gyro * sample_period;
 }
 
+// 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) {
     if (!IsCalibrated(0.1f)) {
         ResetOrientation();
@@ -190,43 +200,6 @@ Common::Vec3f MotionInput::GetRotations() const {
     return rotations;
 }
 
-Input::MotionStatus MotionInput::GetMotion() const {
-    const Common::Vec3f gyroscope = GetGyroscope();
-    const Common::Vec3f accelerometer = GetAcceleration();
-    const Common::Vec3f rotation = GetRotations();
-    const std::array<Common::Vec3f, 3> orientation = GetOrientation();
-    const Common::Quaternion<f32> quaternion = GetQuaternion();
-    return {accelerometer, gyroscope, rotation, orientation, quaternion};
-}
-
-Input::MotionStatus MotionInput::GetRandomMotion(int accel_magnitude, int gyro_magnitude) const {
-    std::random_device device;
-    std::mt19937 gen(device());
-    std::uniform_int_distribution<s16> distribution(-1000, 1000);
-    const Common::Vec3f gyroscope{
-        static_cast<f32>(distribution(gen)) * 0.001f,
-        static_cast<f32>(distribution(gen)) * 0.001f,
-        static_cast<f32>(distribution(gen)) * 0.001f,
-    };
-    const Common::Vec3f accelerometer{
-        static_cast<f32>(distribution(gen)) * 0.001f,
-        static_cast<f32>(distribution(gen)) * 0.001f,
-        static_cast<f32>(distribution(gen)) * 0.001f,
-    };
-    constexpr Common::Vec3f rotation;
-    constexpr std::array orientation{
-        Common::Vec3f{1.0f, 0.0f, 0.0f},
-        Common::Vec3f{0.0f, 1.0f, 0.0f},
-        Common::Vec3f{0.0f, 0.0f, 1.0f},
-    };
-    constexpr Common::Quaternion<f32> quaternion{
-        {0.0f, 0.0f, 0.0f},
-        1.0f,
-    };
-    return {accelerometer * accel_magnitude, gyroscope * gyro_magnitude, rotation, orientation,
-            quaternion};
-}
-
 void MotionInput::ResetOrientation() {
     if (!reset_enabled || only_accelerometer) {
         return;
@@ -304,4 +277,4 @@ void MotionInput::SetOrientationFromAccelerometer() {
         quat = quat.Normalized();
     }
 }
-} // namespace InputCommon
+} // namespace Core::HID