diff options
| author | Mary <me@thog.eu> | 2021-04-14 12:28:43 +0200 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2021-04-14 12:28:43 +0200 |
| commit | 6cb22c9d38622225f9f787f483bd73369774cf77 (patch) | |
| tree | 715a40903ceab05546f7392e5b0f429de75bdd02 /Ryujinx.Input/Motion/MotionSensorFilter.cs | |
| parent | 978b69b706fc085d66b01e2dd27ef6d4acebf335 (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.cs | 162 |
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 |
