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
This commit is contained in:
Mary
473
Ryujinx.Input/Motion/CemuHook/Client.cs
Normal file
473
Ryujinx.Input/Motion/CemuHook/Client.cs
Normal file
@@ -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<int, IPEndPoint> _hosts;
|
||||
private readonly Dictionary<int, Dictionary<int, MotionInput>> _motionData;
|
||||
private readonly Dictionary<int, UdpClient> _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<int, IPEndPoint>();
|
||||
_motionData = new Dictionary<int, Dictionary<int, MotionInput>>();
|
||||
_clients = new Dictionary<int, UdpClient>();
|
||||
|
||||
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<ControllerInfoResponse>();
|
||||
break;
|
||||
case MessageType.Data:
|
||||
ControllerDataResponse inputData = reader.ReadStruct<ControllerDataResponse>();
|
||||
|
||||
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<int, MotionInput>() { { 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
51
Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
Normal file
51
Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
Normal file
@@ -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
|
||||
}
|
||||
}
|
||||
21
Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
Normal file
21
Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
14
Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
Normal file
14
Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
9
Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
Normal file
9
Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
Normal file
@@ -0,0 +1,9 @@
|
||||
namespace Ryujinx.Input.Motion.CemuHook.Protocol
|
||||
{
|
||||
public enum MessageType : uint
|
||||
{
|
||||
Protocol = 0x100000,
|
||||
Info,
|
||||
Data
|
||||
}
|
||||
}
|
||||
51
Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
Normal file
51
Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
Normal file
@@ -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
|
||||
}
|
||||
}
|
||||
86
Ryujinx.Input/Motion/MotionInput.cs
Normal file
86
Ryujinx.Input/Motion/MotionInput.cs
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
162
Ryujinx.Input/Motion/MotionSensorFilter.cs
Normal file
162
Ryujinx.Input/Motion/MotionSensorFilter.cs
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user