aboutsummaryrefslogtreecommitdiff
path: root/Ryujinx.Input/Motion/MotionSensorFilter.cs
diff options
context:
space:
mode:
authorMary <me@thog.eu>2021-04-14 12:28:43 +0200
committerGitHub <noreply@github.com>2021-04-14 12:28:43 +0200
commit6cb22c9d38622225f9f787f483bd73369774cf77 (patch)
tree715a40903ceab05546f7392e5b0f429de75bdd02 /Ryujinx.Input/Motion/MotionSensorFilter.cs
parent978b69b706fc085d66b01e2dd27ef6d4acebf335 (diff)
Miria: The Death of OpenTK 3 (#2194)
* openal: Update to OpenTK 4 * Ryujinx.Graphics.OpenGL: Update to OpenTK 4 * Entirely removed OpenTK 3, still wip * Use SPB for context creation and handling Still need to test on GLX and readd input support * Start implementing a new input system So far only gamepad are supported, no configuration possible via UI but detected via hotplug/removal Button mapping backend is implemented TODO: front end, configuration handling and configuration migration TODO: keyboard support * Enforce RGB only framebuffer on the GLWidget Fix possible transparent window * Implement UI gamepad frontend Also fix bad mapping of minus button and ensure gamepad config is updated in real time * Handle controller being disconnected and reconnected again * Revert "Enforce RGB only framebuffer on the GLWidget" This reverts commit 0949715d1a03ec793e35e37f7b610cbff2d63965. * Fix first color clear * Filter SDL2 events a bit * Start working on the keyboard detail - Rework configuration classes a bit to be more clean. - Integrate fully the keyboard configuration to the front end (TODO: assigner) - Start skeleton for the GTK3 keyboard driver * Add KeyboardStateSnapshot and its integration * Implement keyboard assigner and GTK3 key mapping TODO: controller configuration mapping and IGamepad implementation for keyboard * Add missing SR and SL definitions * Fix copy pasta mistake on config for previous commit * Implement IGamepad interface for GTK3 keyboard * Fix some implementation still being commented in the controller ui for keyboard * Port screen handle code * Remove all configuration management code and move HidNew to Hid * Rename InputConfigNew to InputConfig * Add a version field to the input config * Prepare serialization and deserialization of new input config and migrate profile loading and saving * Support input configuration saving to config and bump config version to 23. * Clean up in ConfigurationState * Reference SPB via a nuget package * Move new input system to Ryujinx.Input project and SDL2 detail to Ryujinx.Input.SDL2 * move GTK3 input to the right directory * Fix triggers on SDL2 * Update to SDL2 2.0.14 via our own fork * Update buttons definition for SDL2 2.0.14 and report gamepad features * Implement motion support again with SDL2 TODO: cemu hooks integration * Switch to latest of nightly SDL2 * SDL2: Fix bugs in gamepad id matching allowing different gamepad to match on the same device index * Ensure values are set in UI when the gamepad get hot plugged * Avoid trying to add controllers in the Update method and don't open SDL2 gamepad instance before checking ids This fixes permanent rumble of pro controller in some hotplug scenario * Fix more UI bugs * Move legcay motion code around before reintegration * gamecontroller UI tweaks here and there * Hide Motion on non motion configurations * Update the TODO grave Some TODO were fixed long time ago or are quite oudated... * Integrate cemu hooks motion configuration * Integrate cemu hooks configuration options to the UI again * cemuhooks => cemuhooks * Add cemu hook support again * Fix regression on normal motion and fix some very nasty bugs around * Fix for XCB multithreads issue on Linux * Enable motion by default * Block inputs in the main view when in the controller configuration window * Some fixes for the controller ui again * Add joycon support and fixes other hints * Bug fixes and clean up - Invert default mapping if not a Nintendo controller - Keep alive the controller being selected on the controller window (allow to avoid big delay for controller needing time to init when doing button assignment) - Clean up hints in use - Remove debug logs around - Fixes potential double free with SDL2Gamepad * Move the button assigner and motion logic to the Ryujinx.Input project * Reimplement raw keyboard hle input Also move out the logic of the hotkeys * Move all remaining Input manager stuffs to the Ryujinx.Input project * Increment configuration version yet again because of master changes * Ensure input config isn't null when not present * Fixes for VS not being nice * Fix broken gamepad caching logic causing crashes on ui * Ensure the background context is destroyed * Update dependencies * Readd retrocompat with old format of the config to avoid parsing and crashes on those versions Also updated the debug Config.json * Document new input APIs * Isolate SDL2Driver to the project and remove external export of it * Add support for external gamepad db mappings on SDL2 * Last clean up before PR * Addresses first part of comments * Address gdkchan's comments * Do not use JsonException * Last comment fixes
Diffstat (limited to 'Ryujinx.Input/Motion/MotionSensorFilter.cs')
-rw-r--r--Ryujinx.Input/Motion/MotionSensorFilter.cs162
1 files changed, 162 insertions, 0 deletions
diff --git a/Ryujinx.Input/Motion/MotionSensorFilter.cs b/Ryujinx.Input/Motion/MotionSensorFilter.cs
new file mode 100644
index 00000000..440fa7ac
--- /dev/null
+++ b/Ryujinx.Input/Motion/MotionSensorFilter.cs
@@ -0,0 +1,162 @@
+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