From 6cb22c9d38622225f9f787f483bd73369774cf77 Mon Sep 17 00:00:00 2001 From: Mary Date: Wed, 14 Apr 2021 12:28:43 +0200 Subject: 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 --- Ryujinx.Input/Motion/CemuHook/Client.cs | 473 +++++++++++++++++++++ .../Motion/CemuHook/Protocol/ControllerData.cs | 51 +++ .../Motion/CemuHook/Protocol/ControllerInfo.cs | 21 + Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs | 14 + .../Motion/CemuHook/Protocol/MessageType.cs | 9 + .../Motion/CemuHook/Protocol/SharedResponse.cs | 51 +++ Ryujinx.Input/Motion/MotionInput.cs | 86 ++++ Ryujinx.Input/Motion/MotionSensorFilter.cs | 162 +++++++ 8 files changed, 867 insertions(+) create mode 100644 Ryujinx.Input/Motion/CemuHook/Client.cs create mode 100644 Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs create mode 100644 Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs create mode 100644 Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs create mode 100644 Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs create mode 100644 Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs create mode 100644 Ryujinx.Input/Motion/MotionInput.cs create mode 100644 Ryujinx.Input/Motion/MotionSensorFilter.cs (limited to 'Ryujinx.Input/Motion') diff --git a/Ryujinx.Input/Motion/CemuHook/Client.cs b/Ryujinx.Input/Motion/CemuHook/Client.cs new file mode 100644 index 00000000..395bd0b3 --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Client.cs @@ -0,0 +1,473 @@ +using Force.Crc32; +using Ryujinx.Common; +using Ryujinx.Common.Configuration.Hid; +using Ryujinx.Common.Configuration.Hid.Controller; +using Ryujinx.Common.Configuration.Hid.Controller.Motion; +using Ryujinx.Common.Logging; +using Ryujinx.Configuration; +using Ryujinx.Input.Motion.CemuHook.Protocol; +using System; +using System.Collections.Generic; +using System.IO; +using System.Net; +using System.Net.Sockets; +using System.Numerics; +using System.Threading.Tasks; + +namespace Ryujinx.Input.Motion.CemuHook +{ + public class Client : IDisposable + { + public const uint Magic = 0x43555344; // DSUC + public const ushort Version = 1001; + + private bool _active; + + private readonly Dictionary _hosts; + private readonly Dictionary> _motionData; + private readonly Dictionary _clients; + + private readonly bool[] _clientErrorStatus = new bool[Enum.GetValues(typeof(PlayerIndex)).Length]; + private readonly long[] _clientRetryTimer = new long[Enum.GetValues(typeof(PlayerIndex)).Length]; + + public Client() + { + _hosts = new Dictionary(); + _motionData = new Dictionary>(); + _clients = new Dictionary(); + + CloseClients(); + } + + public void CloseClients() + { + _active = false; + + lock (_clients) + { + foreach (var client in _clients) + { + try + { + client.Value?.Dispose(); + } + catch (SocketException socketException) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to dispose motion client. Error: {socketException.ErrorCode}"); + } + } + + _hosts.Clear(); + _clients.Clear(); + _motionData.Clear(); + } + } + + public void RegisterClient(int player, string host, int port) + { + if (_clients.ContainsKey(player) || !CanConnect(player)) + { + return; + } + + lock (_clients) + { + if (_clients.ContainsKey(player) || !CanConnect(player)) + { + return; + } + + UdpClient client = null; + + try + { + IPEndPoint endPoint = new IPEndPoint(IPAddress.Parse(host), port); + + client = new UdpClient(host, port); + + _clients.Add(player, client); + _hosts.Add(player, endPoint); + + _active = true; + + Task.Run(() => + { + ReceiveLoop(player); + }); + } + catch (FormatException formatException) + { + if (!_clientErrorStatus[player]) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error: {formatException.Message}"); + + _clientErrorStatus[player] = true; + } + } + catch (SocketException socketException) + { + if (!_clientErrorStatus[player]) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error: {socketException.ErrorCode}"); + + _clientErrorStatus[player] = true; + } + + RemoveClient(player); + + client?.Dispose(); + + SetRetryTimer(player); + } + catch (Exception exception) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to register motion client. Error: {exception.Message}"); + + _clientErrorStatus[player] = true; + + RemoveClient(player); + + client?.Dispose(); + + SetRetryTimer(player); + } + } + } + + public bool TryGetData(int player, int slot, out MotionInput input) + { + lock (_motionData) + { + if (_motionData.ContainsKey(player)) + { + if (_motionData[player].TryGetValue(slot, out input)) + { + return true; + } + } + } + + input = null; + + return false; + } + + private void RemoveClient(int clientId) + { + _clients?.Remove(clientId); + + _hosts?.Remove(clientId); + } + + private void Send(byte[] data, int clientId) + { + if (_clients.TryGetValue(clientId, out UdpClient _client)) + { + if (_client != null && _client.Client != null && _client.Client.Connected) + { + try + { + _client?.Send(data, data.Length); + } + catch (SocketException socketException) + { + if (!_clientErrorStatus[clientId]) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to send data request to motion source at {_client.Client.RemoteEndPoint}. Error: {socketException.ErrorCode}"); + } + + _clientErrorStatus[clientId] = true; + + RemoveClient(clientId); + + _client?.Dispose(); + + SetRetryTimer(clientId); + } + catch (ObjectDisposedException) + { + _clientErrorStatus[clientId] = true; + + RemoveClient(clientId); + + _client?.Dispose(); + + SetRetryTimer(clientId); + } + } + } + } + + private byte[] Receive(int clientId, int timeout = 0) + { + if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client)) + { + if (_client != null && _client.Client != null && _client.Client.Connected) + { + _client.Client.ReceiveTimeout = timeout; + + var result = _client?.Receive(ref endPoint); + + if (result.Length > 0) + { + _clientErrorStatus[clientId] = false; + } + + return result; + } + } + + throw new Exception($"Client {clientId} is not registered."); + } + + private void SetRetryTimer(int clientId) + { + var elapsedMs = PerformanceCounter.ElapsedMilliseconds; + + _clientRetryTimer[clientId] = elapsedMs; + } + + private void ResetRetryTimer(int clientId) + { + _clientRetryTimer[clientId] = 0; + } + + private bool CanConnect(int clientId) + { + return _clientRetryTimer[clientId] == 0 || PerformanceCounter.ElapsedMilliseconds - 5000 > _clientRetryTimer[clientId]; + } + + public void ReceiveLoop(int clientId) + { + if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client)) + { + if (_client != null && _client.Client != null && _client.Client.Connected) + { + try + { + while (_active) + { + byte[] data = Receive(clientId); + + if (data.Length == 0) + { + continue; + } + + Task.Run(() => HandleResponse(data, clientId)); + } + } + catch (SocketException socketException) + { + if (!_clientErrorStatus[clientId]) + { + Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to receive data from motion source at {endPoint}. Error: {socketException.ErrorCode}"); + } + + _clientErrorStatus[clientId] = true; + + RemoveClient(clientId); + + _client?.Dispose(); + + SetRetryTimer(clientId); + } + catch (ObjectDisposedException) + { + _clientErrorStatus[clientId] = true; + + RemoveClient(clientId); + + _client?.Dispose(); + + SetRetryTimer(clientId); + } + } + } + } + + public void HandleResponse(byte[] data, int clientId) + { + ResetRetryTimer(clientId); + + MessageType type = (MessageType)BitConverter.ToUInt32(data.AsSpan().Slice(16, 4)); + + data = data.AsSpan()[16..].ToArray(); + + using MemoryStream stream = new MemoryStream(data); + using BinaryReader reader = new BinaryReader(stream); + + switch (type) + { + case MessageType.Protocol: + break; + case MessageType.Info: + ControllerInfoResponse contollerInfo = reader.ReadStruct(); + break; + case MessageType.Data: + ControllerDataResponse inputData = reader.ReadStruct(); + + Vector3 accelerometer = new Vector3() + { + X = -inputData.AccelerometerX, + Y = inputData.AccelerometerZ, + Z = -inputData.AccelerometerY + }; + + Vector3 gyroscrope = new Vector3() + { + X = inputData.GyroscopePitch, + Y = inputData.GyroscopeRoll, + Z = -inputData.GyroscopeYaw + }; + + ulong timestamp = inputData.MotionTimestamp; + + InputConfig config = ConfigurationState.Instance.Hid.InputConfig.Value.Find(x => x.PlayerIndex == (PlayerIndex)clientId); + + lock (_motionData) + { + // Sanity check the configuration state and remove client if needed if needed. + if (config is StandardControllerInputConfig controllerConfig && + controllerConfig.Motion.EnableMotion && + controllerConfig.Motion.MotionBackend == MotionInputBackendType.CemuHook && + controllerConfig.Motion is CemuHookMotionConfigController cemuHookConfig) + { + int slot = inputData.Shared.Slot; + + if (_motionData.ContainsKey(clientId)) + { + if (_motionData[clientId].ContainsKey(slot)) + { + MotionInput previousData = _motionData[clientId][slot]; + + previousData.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone); + } + else + { + MotionInput input = new MotionInput(); + + input.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone); + + _motionData[clientId].Add(slot, input); + } + } + else + { + MotionInput input = new MotionInput(); + + input.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone); + + _motionData.Add(clientId, new Dictionary() { { slot, input } }); + } + } + else + { + RemoveClient(clientId); + } + } + break; + } + } + + public void RequestInfo(int clientId, int slot) + { + if (!_active) + { + return; + } + + Header header = GenerateHeader(clientId); + + using (MemoryStream stream = new MemoryStream()) + using (BinaryWriter writer = new BinaryWriter(stream)) + { + writer.WriteStruct(header); + + ControllerInfoRequest request = new ControllerInfoRequest() + { + Type = MessageType.Info, + PortsCount = 4 + }; + + request.PortIndices[0] = (byte)slot; + + writer.WriteStruct(request); + + header.Length = (ushort)(stream.Length - 16); + + writer.Seek(6, SeekOrigin.Begin); + writer.Write(header.Length); + + header.Crc32 = Crc32Algorithm.Compute(stream.ToArray()); + + writer.Seek(8, SeekOrigin.Begin); + writer.Write(header.Crc32); + + byte[] data = stream.ToArray(); + + Send(data, clientId); + } + } + + public unsafe void RequestData(int clientId, int slot) + { + if (!_active) + { + return; + } + + Header header = GenerateHeader(clientId); + + using (MemoryStream stream = new MemoryStream()) + using (BinaryWriter writer = new BinaryWriter(stream)) + { + writer.WriteStruct(header); + + ControllerDataRequest request = new ControllerDataRequest() + { + Type = MessageType.Data, + Slot = (byte)slot, + SubscriberType = SubscriberType.Slot + }; + + writer.WriteStruct(request); + + header.Length = (ushort)(stream.Length - 16); + + writer.Seek(6, SeekOrigin.Begin); + writer.Write(header.Length); + + header.Crc32 = Crc32Algorithm.Compute(stream.ToArray()); + + writer.Seek(8, SeekOrigin.Begin); + writer.Write(header.Crc32); + + byte[] data = stream.ToArray(); + + Send(data, clientId); + } + } + + private Header GenerateHeader(int clientId) + { + Header header = new Header() + { + Id = (uint)clientId, + MagicString = Magic, + Version = Version, + Length = 0, + Crc32 = 0 + }; + + return header; + } + + public void Dispose() + { + _active = false; + + CloseClients(); + } + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs new file mode 100644 index 00000000..9ff8dc91 --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs @@ -0,0 +1,51 @@ +using System.Runtime.InteropServices; + +namespace Ryujinx.Input.Motion.CemuHook.Protocol +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)] + struct ControllerDataRequest + { + public MessageType Type; + public SubscriberType SubscriberType; + public byte Slot; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public byte[] MacAddress; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ControllerDataResponse + { + public SharedResponse Shared; + public byte Connected; + public uint PacketId; + public byte ExtraButtons; + public byte MainButtons; + public ushort PSExtraInput; + public ushort LeftStickXY; + public ushort RightStickXY; + public uint DPadAnalog; + public ulong MainButtonsAnalog; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public byte[] Touch1; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public byte[] Touch2; + + public ulong MotionTimestamp; + public float AccelerometerX; + public float AccelerometerY; + public float AccelerometerZ; + public float GyroscopePitch; + public float GyroscopeYaw; + public float GyroscopeRoll; + } + + enum SubscriberType : byte + { + All, + Slot, + Mac + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs new file mode 100644 index 00000000..d483633e --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs @@ -0,0 +1,21 @@ +using System.Runtime.InteropServices; + +namespace Ryujinx.Input.Motion.CemuHook.Protocol +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ControllerInfoResponse + { + public SharedResponse Shared; + private byte _zero; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ControllerInfoRequest + { + public MessageType Type; + public int PortsCount; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] + public byte[] PortIndices; + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs new file mode 100644 index 00000000..94cf4bb6 --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs @@ -0,0 +1,14 @@ +using System.Runtime.InteropServices; + +namespace Ryujinx.Input.Motion.CemuHook.Protocol +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct Header + { + public uint MagicString; + public ushort Version; + public ushort Length; + public uint Crc32; + public uint Id; + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs new file mode 100644 index 00000000..de1e5e90 --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs @@ -0,0 +1,9 @@ +namespace Ryujinx.Input.Motion.CemuHook.Protocol +{ + public enum MessageType : uint + { + Protocol = 0x100000, + Info, + Data + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs new file mode 100644 index 00000000..0593286d --- /dev/null +++ b/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs @@ -0,0 +1,51 @@ +using System.Runtime.InteropServices; + +namespace Ryujinx.Input.Motion.CemuHook.Protocol +{ + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct SharedResponse + { + public MessageType Type; + public byte Slot; + public SlotState State; + public DeviceModelType ModelType; + public ConnectionType ConnectionType; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public byte[] MacAddress; + public BatteryStatus BatteryStatus; + } + + public enum SlotState : byte + { + Disconnected, + Reserved, + Connected + } + + public enum DeviceModelType : byte + { + None, + PartialGyro, + FullGyro + } + + public enum ConnectionType : byte + { + None, + USB, + Bluetooth + } + + public enum BatteryStatus : byte + { + NA, + Dying, + Low, + Medium, + High, + Full, + Charging, + Charged + } +} \ No newline at end of file diff --git a/Ryujinx.Input/Motion/MotionInput.cs b/Ryujinx.Input/Motion/MotionInput.cs new file mode 100644 index 00000000..d92c3d7f --- /dev/null +++ b/Ryujinx.Input/Motion/MotionInput.cs @@ -0,0 +1,86 @@ +using Ryujinx.Input.Motion; +using System; +using System.Numerics; + +namespace Ryujinx.Input +{ + public class MotionInput + { + public ulong TimeStamp { get; set; } + public Vector3 Accelerometer { get; set; } + public Vector3 Gyroscrope { get; set; } + public Vector3 Rotation { get; set; } + + private readonly MotionSensorFilter _filter; + private int _calibrationFrame = 0; + + public MotionInput() + { + TimeStamp = 0; + Accelerometer = new Vector3(); + Gyroscrope = new Vector3(); + Rotation = new Vector3(); + + // TODO: RE the correct filter. + _filter = new MotionSensorFilter(0f); + } + + public void Update(Vector3 accel, Vector3 gyro, ulong timestamp, int sensitivity, float deadzone) + { + if (TimeStamp != 0) + { + if (gyro.Length() <= 1f && accel.Length() >= 0.8f && accel.Z >= 0.8f) + { + _calibrationFrame++; + + if (_calibrationFrame >= 90) + { + gyro = Vector3.Zero; + + Rotation = Vector3.Zero; + + _filter.Reset(); + + _calibrationFrame = 0; + } + } + else + { + _calibrationFrame = 0; + } + + Accelerometer = -accel; + + if (gyro.Length() < deadzone) + { + gyro = Vector3.Zero; + } + + gyro *= (sensitivity / 100f); + + Gyroscrope = gyro; + + float deltaTime = MathF.Abs((long)(timestamp - TimeStamp) / 1000000f); + + Vector3 deltaGyro = gyro * deltaTime; + + Rotation += deltaGyro; + + _filter.SamplePeriod = deltaTime; + _filter.Update(accel, DegreeToRad(gyro)); + } + + TimeStamp = timestamp; + } + + public Matrix4x4 GetOrientation() + { + return Matrix4x4.CreateFromQuaternion(_filter.Quaternion); + } + + private static Vector3 DegreeToRad(Vector3 degree) + { + return degree * (MathF.PI / 180); + } + } +} \ No newline at end of file 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 + { + /// + /// 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