From cee712105850ac3385cd0091a923438167433f9f Mon Sep 17 00:00:00 2001 From: TSR Berry <20988865+TSRBerry@users.noreply.github.com> Date: Sat, 8 Apr 2023 01:22:00 +0200 Subject: Move solution and projects to src --- Ryujinx.Input/Motion/MotionSensorFilter.cs | 162 ----------------------------- 1 file changed, 162 deletions(-) delete mode 100644 Ryujinx.Input/Motion/MotionSensorFilter.cs (limited to 'Ryujinx.Input/Motion/MotionSensorFilter.cs') diff --git a/Ryujinx.Input/Motion/MotionSensorFilter.cs b/Ryujinx.Input/Motion/MotionSensorFilter.cs deleted file mode 100644 index 440fa7ac..00000000 --- a/Ryujinx.Input/Motion/MotionSensorFilter.cs +++ /dev/null @@ -1,162 +0,0 @@ -using System.Numerics; - -namespace Ryujinx.Input.Motion -{ - // MahonyAHRS class. Madgwick's implementation of Mayhony's AHRS algorithm. - // See: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ - // Based on: 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 - class MotionSensorFilter - { - /// - /// Sample rate coefficient. - /// - public const float SampleRateCoefficient = 0.45f; - - /// - /// Gets or sets the sample period. - /// - public float SamplePeriod { get; set; } - - /// - /// Gets or sets the algorithm proportional gain. - /// - public float Kp { get; set; } - - /// - /// Gets or sets the algorithm integral gain. - /// - public float Ki { get; set; } - - /// - /// Gets the Quaternion output. - /// - public Quaternion Quaternion { get; private set; } - - /// - /// Integral error. - /// - private Vector3 _intergralError; - - /// - /// Initializes a new instance of the class. - /// - /// - /// Sample period. - /// - public MotionSensorFilter(float samplePeriod) : this(samplePeriod, 1f, 0f) { } - - /// - /// Initializes a new instance of the class. - /// - /// - /// Sample period. - /// - /// - /// Algorithm proportional gain. - /// - public MotionSensorFilter(float samplePeriod, float kp) : this(samplePeriod, kp, 0f) { } - - /// - /// Initializes a new instance of the class. - /// - /// - /// Sample period. - /// - /// - /// Algorithm proportional gain. - /// - /// - /// Algorithm integral gain. - /// - public MotionSensorFilter(float samplePeriod, float kp, float ki) - { - SamplePeriod = samplePeriod; - Kp = kp; - Ki = ki; - - Reset(); - - _intergralError = new Vector3(); - } - - /// - /// Algorithm IMU update method. Requires only gyroscope and accelerometer data. - /// - /// - /// Accelerometer measurement in any calibrated units. - /// - /// - /// Gyroscope measurement in radians. - /// - public void Update(Vector3 accel, Vector3 gyro) - { - // Normalise accelerometer measurement. - float norm = 1f / accel.Length(); - - if (!float.IsFinite(norm)) - { - return; - } - - accel *= norm; - - float q2 = Quaternion.X; - float q3 = Quaternion.Y; - float q4 = Quaternion.Z; - float q1 = Quaternion.W; - - // Estimated direction of gravity. - Vector3 gravity = new Vector3() - { - X = 2f * (q2 * q4 - q1 * q3), - Y = 2f * (q1 * q2 + q3 * q4), - Z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4 - }; - - // Error is cross product between estimated direction and measured direction of gravity. - Vector3 error = new Vector3() - { - X = accel.Y * gravity.Z - accel.Z * gravity.Y, - Y = accel.Z * gravity.X - accel.X * gravity.Z, - Z = accel.X * gravity.Y - accel.Y * gravity.X - }; - - if (Ki > 0f) - { - _intergralError += error; // Accumulate integral error. - } - else - { - _intergralError = Vector3.Zero; // Prevent integral wind up. - } - - // Apply feedback terms. - gyro += (Kp * error) + (Ki * _intergralError); - - // Integrate rate of change of quaternion. - Vector3 delta = new Vector3(q2, q3, q4); - - q1 += (-q2 * gyro.X - q3 * gyro.Y - q4 * gyro.Z) * (SampleRateCoefficient * SamplePeriod); - q2 += (q1 * gyro.X + delta.Y * gyro.Z - delta.Z * gyro.Y) * (SampleRateCoefficient * SamplePeriod); - q3 += (q1 * gyro.Y - delta.X * gyro.Z + delta.Z * gyro.X) * (SampleRateCoefficient * SamplePeriod); - q4 += (q1 * gyro.Z + delta.X * gyro.Y - delta.Y * gyro.X) * (SampleRateCoefficient * SamplePeriod); - - // Normalise quaternion. - Quaternion quaternion = new Quaternion(q2, q3, q4, q1); - - norm = 1f / quaternion.Length(); - - if (!float.IsFinite(norm)) - { - return; - } - - Quaternion = quaternion * norm; - } - - public void Reset() - { - Quaternion = Quaternion.Identity; - } - } -} \ No newline at end of file -- cgit v1.2.3