crafterpika/ryujinx
1
0
Fork 0
mirror of https://git.ryujinx.app/ryubing/ryujinx.git synced 2025-07-28 16:37:13 +02:00
Code Issues Projects Releases Packages Wiki Activity

Move solution and projects to src

Browse source
This commit is contained in:
TSR Berry 2023-04-08 01:22:00 +02:00 • committed by Mary
parent cd124bda58
commit cee7121058
3466 changed files with 55 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

475
src/Ryujinx.Input/Motion/CemuHook/Client.cs Normal file
View file

@ -0,0 +1,475 @@
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.Common.Memory;
using Ryujinx.Input.HLE;
using Ryujinx.Input.Motion.CemuHook.Protocol;
using System;
using System.Collections.Generic;
using System.IO;
using System.IO.Hashing;
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<PlayerIndex>().Length];
private readonly long[] _clientRetryTimer = new long[Enum.GetValues<PlayerIndex>().Length];
private NpadManager _npadManager;
public Client(NpadManager npadManager)
{
_npadManager = npadManager;
_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 = _npadManager.GetPlayerInputConfigByIndex(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 = MemoryStreamManager.Shared.GetStream())
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);
Crc32.Hash(stream.ToArray(), header.Crc32.AsSpan());
writer.Seek(8, SeekOrigin.Begin);
writer.Write(header.Crc32.AsSpan());
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 = MemoryStreamManager.Shared.GetStream())
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);
Crc32.Hash(stream.ToArray(), header.Crc32.AsSpan());
writer.Seek(8, SeekOrigin.Begin);
writer.Write(header.Crc32.AsSpan());
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
};
return header;
}
public void Dispose()
{
_active = false;
CloseClients();
}
}
}

47
src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs Normal file
View file

@ -0,0 +1,47 @@
using Ryujinx.Common.Memory;
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;
public Array6<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;
public Array6<byte> Touch1;
public Array6<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
}
}

20
src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs Normal file
View file

@ -0,0 +1,20 @@
using Ryujinx.Common.Memory;
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;
public Array4<byte> PortIndices;
}
}

15
src/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs Normal file
View file

@ -0,0 +1,15 @@
using Ryujinx.Common.Memory;
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 Array4<byte> Crc32;
public uint Id;
}
}

9
src/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs Normal file
View file

@ -0,0 +1,9 @@
namespace Ryujinx.Input.Motion.CemuHook.Protocol
{
public enum MessageType : uint
{
Protocol = 0x100000,
Info,
Data
}
}

51
src/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs Normal file
View file

@ -0,0 +1,51 @@
using Ryujinx.Common.Memory;
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;
public Array6<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
}
}

65
src/Ryujinx.Input/Motion/MotionInput.cs Normal file
View file

@ -0,0 +1,65 @@
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;
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)
{
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
src/Ryujinx.Input/Motion/MotionSensorFilter.cs Normal file
View 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;
}
}
}