aboutsummaryrefslogtreecommitdiff
path: root/Ryujinx.Input/Motion/MotionSensorFilter.cs
diff options
context:
space:
mode:
Diffstat (limited to 'Ryujinx.Input/Motion/MotionSensorFilter.cs')
-rw-r--r--Ryujinx.Input/Motion/MotionSensorFilter.cs162
1 files changed, 0 insertions, 162 deletions
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
- {
- /// <summary>
- /// Sample rate coefficient.
- /// </summary>
- public const float SampleRateCoefficient = 0.45f;
-
- /// <summary>
- /// Gets or sets the sample period.
- /// </summary>
- public float SamplePeriod { get; set; }
-
- /// <summary>
- /// Gets or sets the algorithm proportional gain.
- /// </summary>
- public float Kp { get; set; }
-
- /// <summary>
- /// Gets or sets the algorithm integral gain.
- /// </summary>
- public float Ki { get; set; }
-
- /// <summary>
- /// Gets the Quaternion output.
- /// </summary>
- public Quaternion Quaternion { get; private set; }
-
- /// <summary>
- /// Integral error.
- /// </summary>
- private Vector3 _intergralError;
-
- /// <summary>
- /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
- /// </summary>
- /// <param name="samplePeriod">
- /// Sample period.
- /// </param>
- public MotionSensorFilter(float samplePeriod) : this(samplePeriod, 1f, 0f) { }
-
- /// <summary>
- /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
- /// </summary>
- /// <param name="samplePeriod">
- /// Sample period.
- /// </param>
- /// <param name="kp">
- /// Algorithm proportional gain.
- /// </param>
- public MotionSensorFilter(float samplePeriod, float kp) : this(samplePeriod, kp, 0f) { }
-
- /// <summary>
- /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
- /// </summary>
- /// <param name="samplePeriod">
- /// Sample period.
- /// </param>
- /// <param name="kp">
- /// Algorithm proportional gain.
- /// </param>
- /// <param name="ki">
- /// Algorithm integral gain.
- /// </param>
- public MotionSensorFilter(float samplePeriod, float kp, float ki)
- {
- SamplePeriod = samplePeriod;
- Kp = kp;
- Ki = ki;
-
- Reset();
-
- _intergralError = new Vector3();
- }
-
- /// <summary>
- /// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
- /// </summary>
- /// <param name="accel">
- /// Accelerometer measurement in any calibrated units.
- /// </param>
- /// <param name="gyro">
- /// Gyroscope measurement in radians.
- /// </param>
- 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