1  using System.Numerics;
  2  
  3  namespace Ryujinx.Input.Motion
  4  {
  5      // MahonyAHRS class. Madgwick's implementation of Mayhony's AHRS algorithm.
  6      // See: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
  7      // 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
  8      class MotionSensorFilter
  9      {
 10          /// <summary>
 11          /// Sample rate coefficient.
 12          /// </summary>
 13          public const float SampleRateCoefficient = 0.45f;
 14  
 15          /// <summary>
 16          /// Gets or sets the sample period.
 17          /// </summary>
 18          public float SamplePeriod { get; set; }
 19  
 20          /// <summary>
 21          /// Gets or sets the algorithm proportional gain.
 22          /// </summary>
 23          public float Kp { get; set; }
 24  
 25          /// <summary>
 26          /// Gets or sets the algorithm integral gain.
 27          /// </summary>
 28          public float Ki { get; set; }
 29  
 30          /// <summary>
 31          /// Gets the Quaternion output.
 32          /// </summary>
 33          public Quaternion Quaternion { get; private set; }
 34  
 35          /// <summary>
 36          /// Integral error.
 37          /// </summary>
 38          private Vector3 _intergralError;
 39  
 40          /// <summary>
 41          /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
 42          /// </summary>
 43          /// <param name="samplePeriod">
 44          /// Sample period.
 45          /// </param>
 46          public MotionSensorFilter(float samplePeriod) : this(samplePeriod, 1f, 0f) { }
 47  
 48          /// <summary>
 49          /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
 50          /// </summary>
 51          /// <param name="samplePeriod">
 52          /// Sample period.
 53          /// </param>
 54          /// <param name="kp">
 55          /// Algorithm proportional gain.
 56          /// </param>
 57          public MotionSensorFilter(float samplePeriod, float kp) : this(samplePeriod, kp, 0f) { }
 58  
 59          /// <summary>
 60          /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
 61          /// </summary>
 62          /// <param name="samplePeriod">
 63          /// Sample period.
 64          /// </param>
 65          /// <param name="kp">
 66          /// Algorithm proportional gain.
 67          /// </param>
 68          /// <param name="ki">
 69          /// Algorithm integral gain.
 70          /// </param>
 71          public MotionSensorFilter(float samplePeriod, float kp, float ki)
 72          {
 73              SamplePeriod = samplePeriod;
 74              Kp = kp;
 75              Ki = ki;
 76  
 77              Reset();
 78  
 79              _intergralError = new Vector3();
 80          }
 81  
 82          /// <summary>
 83          /// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
 84          /// </summary>
 85          /// <param name="accel">
 86          /// Accelerometer measurement in any calibrated units.
 87          /// </param>
 88          /// <param name="gyro">
 89          /// Gyroscope measurement in radians.
 90          /// </param>
 91          public void Update(Vector3 accel, Vector3 gyro)
 92          {
 93              // Normalise accelerometer measurement.
 94              float norm = 1f / accel.Length();
 95  
 96              if (!float.IsFinite(norm))
 97              {
 98                  return;
 99              }
100  
101              accel *= norm;
102  
103              float q2 = Quaternion.X;
104              float q3 = Quaternion.Y;
105              float q4 = Quaternion.Z;
106              float q1 = Quaternion.W;
107  
108              // Estimated direction of gravity.
109              Vector3 gravity = new()
110              {
111                  X = 2f * (q2 * q4 - q1 * q3),
112                  Y = 2f * (q1 * q2 + q3 * q4),
113                  Z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4,
114              };
115  
116              // Error is cross product between estimated direction and measured direction of gravity.
117              Vector3 error = new()
118              {
119                  X = accel.Y * gravity.Z - accel.Z * gravity.Y,
120                  Y = accel.Z * gravity.X - accel.X * gravity.Z,
121                  Z = accel.X * gravity.Y - accel.Y * gravity.X,
122              };
123  
124              if (Ki > 0f)
125              {
126                  _intergralError += error; // Accumulate integral error.
127              }
128              else
129              {
130                  _intergralError = Vector3.Zero; // Prevent integral wind up.
131              }
132  
133              // Apply feedback terms.
134              gyro += (Kp * error) + (Ki * _intergralError);
135  
136              // Integrate rate of change of quaternion.
137              Vector3 delta = new(q2, q3, q4);
138  
139              q1 += (-q2 * gyro.X - q3 * gyro.Y - q4 * gyro.Z) * (SampleRateCoefficient * SamplePeriod);
140              q2 += (q1 * gyro.X + delta.Y * gyro.Z - delta.Z * gyro.Y) * (SampleRateCoefficient * SamplePeriod);
141              q3 += (q1 * gyro.Y - delta.X * gyro.Z + delta.Z * gyro.X) * (SampleRateCoefficient * SamplePeriod);
142              q4 += (q1 * gyro.Z + delta.X * gyro.Y - delta.Y * gyro.X) * (SampleRateCoefficient * SamplePeriod);
143  
144              // Normalise quaternion.
145              Quaternion quaternion = new(q2, q3, q4, q1);
146  
147              norm = 1f / quaternion.Length();
148  
149              if (!float.IsFinite(norm))
150              {
151                  return;
152              }
153  
154              Quaternion = quaternion * norm;
155          }
156  
157          public void Reset()
158          {
159              Quaternion = Quaternion.Identity;
160          }
161      }
162  }