Add project files.
This commit is contained in:
David Khachaturov
parent
b986cc9e2e
commit
c8bfdb2220
11 changed files with 2378 additions and 0 deletions
37
BetterJoyForCemu.sln
Normal file
37
BetterJoyForCemu.sln
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio 15
|
||||
VisualStudioVersion = 15.0.27130.2036
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "BetterJoyForCemu", "BetterJoyForCemu\BetterJoyForCemu.csproj", "{1BF709E9-C133-41DF-933A-C9FF3F664C7B}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|Any CPU = Debug|Any CPU
|
||||
Debug|x64 = Debug|x64
|
||||
Debug|x86 = Debug|x86
|
||||
Release|Any CPU = Release|Any CPU
|
||||
Release|x64 = Release|x64
|
||||
Release|x86 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x64.Build.0 = Debug|x64
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x86.ActiveCfg = Debug|x86
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Debug|x86.Build.0 = Debug|x86
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x64.ActiveCfg = Release|x64
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x64.Build.0 = Release|x64
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x86.ActiveCfg = Release|x86
|
||||
{1BF709E9-C133-41DF-933A-C9FF3F664C7B}.Release|x86.Build.0 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
EndGlobalSection
|
||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||
SolutionGuid = {F928F866-3D27-4531-970D-25ADE4DDD979}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
6
BetterJoyForCemu/App.config
Normal file
6
BetterJoyForCemu/App.config
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version="1.0" encoding="utf-8" ?>
|
||||
<configuration>
|
||||
<startup>
|
||||
<supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.6.1" />
|
||||
</startup>
|
||||
</configuration>
|
||||
106
BetterJoyForCemu/BetterJoyForCemu.csproj
Normal file
106
BetterJoyForCemu/BetterJoyForCemu.csproj
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
|
||||
<PropertyGroup>
|
||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
||||
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
||||
<ProjectGuid>{1BF709E9-C133-41DF-933A-C9FF3F664C7B}</ProjectGuid>
|
||||
<OutputType>Exe</OutputType>
|
||||
<RootNamespace>BetterJoyForCemu</RootNamespace>
|
||||
<AssemblyName>BetterJoyForCemu</AssemblyName>
|
||||
<TargetFrameworkVersion>v4.6.1</TargetFrameworkVersion>
|
||||
<FileAlignment>512</FileAlignment>
|
||||
<AutoGenerateBindingRedirects>true</AutoGenerateBindingRedirects>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
||||
<PlatformTarget>AnyCPU</PlatformTarget>
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<DebugType>full</DebugType>
|
||||
<Optimize>false</Optimize>
|
||||
<OutputPath>bin\Debug\</OutputPath>
|
||||
<DefineConstants>DEBUG;TRACE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
||||
<PlatformTarget>AnyCPU</PlatformTarget>
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<Optimize>true</Optimize>
|
||||
<OutputPath>bin\Release\</OutputPath>
|
||||
<DefineConstants>TRACE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x86'">
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<OutputPath>bin\x86\Debug\</OutputPath>
|
||||
<DefineConstants>DEBUG;TRACE</DefineConstants>
|
||||
<DebugType>full</DebugType>
|
||||
<PlatformTarget>x86</PlatformTarget>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
|
||||
<Prefer32Bit>true</Prefer32Bit>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x86'">
|
||||
<OutputPath>bin\x86\Release\</OutputPath>
|
||||
<DefineConstants>TRACE</DefineConstants>
|
||||
<Optimize>true</Optimize>
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<PlatformTarget>x86</PlatformTarget>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
|
||||
<Prefer32Bit>true</Prefer32Bit>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'">
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<OutputPath>bin\x64\Debug\</OutputPath>
|
||||
<DefineConstants>DEBUG;TRACE</DefineConstants>
|
||||
<DebugType>full</DebugType>
|
||||
<PlatformTarget>x64</PlatformTarget>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
|
||||
<Prefer32Bit>true</Prefer32Bit>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x64'">
|
||||
<OutputPath>bin\x64\Release\</OutputPath>
|
||||
<DefineConstants>TRACE</DefineConstants>
|
||||
<Optimize>true</Optimize>
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<PlatformTarget>x64</PlatformTarget>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
|
||||
<Prefer32Bit>true</Prefer32Bit>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<Reference Include="Crc32.NET, Version=1.0.0.0, Culture=neutral, PublicKeyToken=dc0b95cf99bf4e99, processorArchitecture=MSIL">
|
||||
<HintPath>..\packages\Crc32.NET.1.2.0\lib\net20\Crc32.NET.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="System" />
|
||||
<Reference Include="System.Core" />
|
||||
<Reference Include="System.Numerics" />
|
||||
<Reference Include="System.Xml.Linq" />
|
||||
<Reference Include="System.Data.DataSetExtensions" />
|
||||
<Reference Include="Microsoft.CSharp" />
|
||||
<Reference Include="System.Data" />
|
||||
<Reference Include="System.Net.Http" />
|
||||
<Reference Include="System.Xml" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Compile Include="HIDapi.cs" />
|
||||
<Compile Include="Joycon.cs" />
|
||||
<Compile Include="MyQuaternion.cs" />
|
||||
<Compile Include="Program.cs" />
|
||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||
<Compile Include="UpdServer.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<None Include="App.config" />
|
||||
<None Include="packages.config" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Content Include="hidapi.dll">
|
||||
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
|
||||
</Content>
|
||||
</ItemGroup>
|
||||
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
||||
</Project>
|
||||
115
BetterJoyForCemu/HIDapi.cs
Normal file
115
BetterJoyForCemu/HIDapi.cs
Normal file
|
|
@ -0,0 +1,115 @@
|
|||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace BetterJoyForCemu {
|
||||
public class HIDapi {
|
||||
#if LINUX
|
||||
const string dll = "libhidapi.so";
|
||||
#else
|
||||
const string dll = "hidapi.dll";
|
||||
#endif
|
||||
|
||||
public struct hid_device_info {
|
||||
[MarshalAs(UnmanagedType.LPStr)]
|
||||
public string path;
|
||||
public ushort vendor_id;
|
||||
public ushort product_id;
|
||||
[MarshalAs(UnmanagedType.LPWStr)]
|
||||
public string serial_number;
|
||||
public ushort release_number;
|
||||
[MarshalAs(UnmanagedType.LPWStr)]
|
||||
public string manufacturer_string;
|
||||
[MarshalAs(UnmanagedType.LPWStr)]
|
||||
public string product_string;
|
||||
public ushort usage_page;
|
||||
public ushort usage;
|
||||
public int interface_number;
|
||||
public IntPtr next;
|
||||
};
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_init();
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_exit();
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr hid_enumerate(ushort vendor_id, ushort product_id);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void hid_free_enumeration(IntPtr phid_device_info);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr hid_open(ushort vendor_id, ushort product_id, [MarshalAs(UnmanagedType.LPWStr)]string serial_number);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr hid_open_path([MarshalAs(UnmanagedType.LPStr)]string path);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_write(IntPtr device, byte[] data, UIntPtr length);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_read_timeout(IntPtr dev, byte[] data, UIntPtr length, int milliseconds);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_read(IntPtr device, byte[] data, UIntPtr length);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_set_nonblocking(IntPtr device, int nonblock);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_send_feature_report(IntPtr device, byte[] data, UIntPtr length);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_get_feature_report(IntPtr device, byte[] data, UIntPtr length);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void hid_close(IntPtr device);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_get_manufacturer_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_get_product_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_get_serial_number_string(IntPtr device, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern int hid_get_indexed_string(IntPtr device, int string_index, [MarshalAs(UnmanagedType.LPWStr)]string string_, UIntPtr maxlen);
|
||||
|
||||
[DllImport(dll, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.LPWStr)]
|
||||
public static extern string hid_error(IntPtr device);
|
||||
|
||||
static void PrintEnumeration(IntPtr phid_device_info) {
|
||||
if (!phid_device_info.Equals(IntPtr.Zero)) {
|
||||
hid_device_info hdev = (hid_device_info)Marshal.PtrToStructure(phid_device_info, typeof(hid_device_info));
|
||||
|
||||
Console.WriteLine(string.Format("path: {0}", hdev.path));
|
||||
Console.WriteLine(string.Format("vendor id: {0:X}", hdev.vendor_id));
|
||||
Console.WriteLine(string.Format("product id: {0:X}", hdev.product_id));
|
||||
Console.WriteLine(string.Format("usage page: {0:X}", hdev.usage_page));
|
||||
Console.WriteLine(string.Format("usage: {0:X}", hdev.usage));
|
||||
Console.WriteLine("");
|
||||
|
||||
PrintEnumeration(hdev.next);
|
||||
}
|
||||
}
|
||||
|
||||
static string _getDevicePath(IntPtr phid_device_info, ushort usagePage, ushort usage) {
|
||||
if (!phid_device_info.Equals(IntPtr.Zero)) {
|
||||
hid_device_info hdev = (hid_device_info)Marshal.PtrToStructure(phid_device_info, typeof(hid_device_info));
|
||||
if (usagePage == hdev.usage_page && usage == hdev.usage)
|
||||
return hdev.path;
|
||||
else
|
||||
return _getDevicePath(hdev.next, usagePage, usage);
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public static string GetDevicePath(ushort vendorId, ushort productId, ushort usagePage, ushort usage) {
|
||||
return _getDevicePath(hid_enumerate(vendorId, productId), usagePage, usage);
|
||||
}
|
||||
}
|
||||
}
|
||||
733
BetterJoyForCemu/Joycon.cs
Normal file
733
BetterJoyForCemu/Joycon.cs
Normal file
|
|
@ -0,0 +1,733 @@
|
|||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Net.NetworkInformation;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace BetterJoyForCemu {
|
||||
public class Joycon {
|
||||
float timing = 60.0f;
|
||||
|
||||
bool isPro = false;
|
||||
|
||||
public enum DebugType : int {
|
||||
NONE,
|
||||
ALL,
|
||||
COMMS,
|
||||
THREADING,
|
||||
IMU,
|
||||
RUMBLE,
|
||||
};
|
||||
public DebugType debug_type = DebugType.NONE;
|
||||
public bool isLeft;
|
||||
public enum state_ : uint {
|
||||
NOT_ATTACHED,
|
||||
DROPPED,
|
||||
NO_JOYCONS,
|
||||
ATTACHED,
|
||||
INPUT_MODE_0x30,
|
||||
IMU_DATA_OK,
|
||||
};
|
||||
public state_ state;
|
||||
public enum Button : int {
|
||||
DPAD_DOWN = 0,
|
||||
DPAD_RIGHT = 1,
|
||||
DPAD_LEFT = 2,
|
||||
DPAD_UP = 3,
|
||||
SL = 4,
|
||||
SR = 5,
|
||||
MINUS = 6,
|
||||
HOME = 7,
|
||||
PLUS = 8,
|
||||
CAPTURE = 9,
|
||||
STICK = 10,
|
||||
SHOULDER_1 = 11,
|
||||
SHOULDER_2 = 12,
|
||||
|
||||
// For pro controller
|
||||
B = 13,
|
||||
A = 14,
|
||||
Y = 15,
|
||||
X = 16,
|
||||
STICK2 = 17,
|
||||
SHOULDER2_1 = 18,
|
||||
SHOULDER2_2 = 19,
|
||||
};
|
||||
private bool[] buttons_down = new bool[20];
|
||||
private bool[] buttons_up = new bool[20];
|
||||
private bool[] buttons = new bool[20];
|
||||
private bool[] down_ = new bool[20];
|
||||
|
||||
private float[] stick = { 0, 0 };
|
||||
private float[] stick2 = { 0, 0 };
|
||||
|
||||
private
|
||||
IntPtr handle;
|
||||
|
||||
byte[] default_buf = { 0x0, 0x1, 0x40, 0x40, 0x0, 0x1, 0x40, 0x40 };
|
||||
|
||||
private byte[] stick_raw = { 0, 0, 0 };
|
||||
private UInt16[] stick_cal = { 0, 0, 0, 0, 0, 0 };
|
||||
private UInt16 deadzone;
|
||||
private UInt16[] stick_precal = { 0, 0 };
|
||||
|
||||
private byte[] stick2_raw = { 0, 0, 0 };
|
||||
private UInt16[] stick2_cal = { 0, 0, 0, 0, 0, 0 };
|
||||
private UInt16 deadzone2;
|
||||
private UInt16[] stick2_precal = { 0, 0 };
|
||||
|
||||
private bool stop_polling = false;
|
||||
private int timestamp;
|
||||
private bool first_imu_packet = true;
|
||||
private bool imu_enabled = false;
|
||||
private Int16[] acc_r = { 0, 0, 0 };
|
||||
private Int16[] acc_neutral = { 0, 0, 0 };
|
||||
private Int16[] acc_sensiti = { 0, 0, 0 };
|
||||
private Vector3 acc_g;
|
||||
|
||||
private Int16[] gyr_r = { 0, 0, 0 };
|
||||
private Int16[] gyr_neutral = { 0, 0, 0 };
|
||||
private Int16[] gyr_sensiti = { 0, 0, 0 };
|
||||
private Vector3 gyr_g;
|
||||
private bool do_localize;
|
||||
private float filterweight;
|
||||
private const uint report_len = 49;
|
||||
private struct Report {
|
||||
byte[] r;
|
||||
System.DateTime t;
|
||||
public ulong ts;
|
||||
public Report(byte[] report, System.DateTime time, ulong timestamp) {
|
||||
r = report;
|
||||
t = time;
|
||||
ts = timestamp;
|
||||
}
|
||||
public System.DateTime GetTime() {
|
||||
return t;
|
||||
}
|
||||
public void CopyBuffer(byte[] b) {
|
||||
for (int i = 0; i < report_len; ++i) {
|
||||
b[i] = r[i];
|
||||
}
|
||||
}
|
||||
};
|
||||
private struct Rumble {
|
||||
private float h_f, amp, l_f;
|
||||
public float t;
|
||||
public bool timed_rumble;
|
||||
|
||||
public void set_vals(float low_freq, float high_freq, float amplitude, int time = 0) {
|
||||
h_f = high_freq;
|
||||
amp = amplitude;
|
||||
l_f = low_freq;
|
||||
timed_rumble = false;
|
||||
t = 0;
|
||||
if (time != 0) {
|
||||
t = time / 1000f;
|
||||
timed_rumble = true;
|
||||
}
|
||||
}
|
||||
public Rumble(float low_freq, float high_freq, float amplitude, int time = 0) {
|
||||
h_f = high_freq;
|
||||
amp = amplitude;
|
||||
l_f = low_freq;
|
||||
timed_rumble = false;
|
||||
t = 0;
|
||||
if (time != 0) {
|
||||
t = time / 1000f;
|
||||
timed_rumble = true;
|
||||
}
|
||||
}
|
||||
private float clamp(float x, float min, float max) {
|
||||
if (x < min) return min;
|
||||
if (x > max) return max;
|
||||
return x;
|
||||
}
|
||||
public byte[] GetData() {
|
||||
byte[] rumble_data = new byte[8];
|
||||
if (amp == 0.0f) {
|
||||
rumble_data[0] = 0x0;
|
||||
rumble_data[1] = 0x1;
|
||||
rumble_data[2] = 0x40;
|
||||
rumble_data[3] = 0x40;
|
||||
} else {
|
||||
l_f = clamp(l_f, 40.875885f, 626.286133f);
|
||||
amp = clamp(amp, 0.0f, 1.0f);
|
||||
h_f = clamp(h_f, 81.75177f, 1252.572266f);
|
||||
UInt16 hf = (UInt16)((Math.Round(32f * Math.Log(h_f * 0.1f, 2)) - 0x60) * 4);
|
||||
byte lf = (byte)(Math.Round(32f * Math.Log(l_f * 0.1f, 2)) - 0x40);
|
||||
byte hf_amp;
|
||||
if (amp == 0) hf_amp = 0;
|
||||
else if (amp < 0.117) hf_amp = (byte)(((Math.Log(amp * 1000, 2) * 32) - 0x60) / (5 - Math.Pow(amp, 2)) - 1);
|
||||
else if (amp < 0.23) hf_amp = (byte)(((Math.Log(amp * 1000, 2) * 32) - 0x60) - 0x5c);
|
||||
else hf_amp = (byte)((((Math.Log(amp * 1000, 2) * 32) - 0x60) * 2) - 0xf6);
|
||||
|
||||
UInt16 lf_amp = (UInt16)(Math.Round((double)hf_amp) * .5);
|
||||
byte parity = (byte)(lf_amp % 2);
|
||||
if (parity > 0) {
|
||||
--lf_amp;
|
||||
}
|
||||
|
||||
lf_amp = (UInt16)(lf_amp >> 1);
|
||||
lf_amp += 0x40;
|
||||
if (parity > 0) lf_amp |= 0x8000;
|
||||
rumble_data = new byte[8];
|
||||
rumble_data[0] = (byte)(hf & 0xff);
|
||||
rumble_data[1] = (byte)((hf >> 8) & 0xff);
|
||||
rumble_data[2] = lf;
|
||||
rumble_data[1] += hf_amp;
|
||||
rumble_data[2] += (byte)((lf_amp >> 8) & 0xff);
|
||||
rumble_data[3] += (byte)(lf_amp & 0xff);
|
||||
}
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
rumble_data[4 + i] = rumble_data[i];
|
||||
}
|
||||
//Debug.Log(string.Format("Encoded hex freq: {0:X2}", encoded_hex_freq));
|
||||
//Debug.Log(string.Format("lf_amp: {0:X4}", lf_amp));
|
||||
//Debug.Log(string.Format("hf_amp: {0:X2}", hf_amp));
|
||||
//Debug.Log(string.Format("l_f: {0:F}", l_f));
|
||||
//Debug.Log(string.Format("hf: {0:X4}", hf));
|
||||
//Debug.Log(string.Format("lf: {0:X2}", lf));
|
||||
return rumble_data;
|
||||
}
|
||||
}
|
||||
private Queue<Report> reports = new Queue<Report>();
|
||||
private Rumble rumble_obj;
|
||||
|
||||
private byte global_count = 0;
|
||||
private string debug_str;
|
||||
|
||||
// For UdpServer
|
||||
public int PadId = 0;
|
||||
public int battery = 2;
|
||||
public int model = 2;
|
||||
public int constate = 2;
|
||||
public int connection = 3;
|
||||
|
||||
public PhysicalAddress PadMacAddress = new PhysicalAddress(new byte[] { 01, 02, 03, 04, 05, 06 });
|
||||
public ulong Timestamp = (ulong)Stopwatch.GetTimestamp();
|
||||
public int packetCounter = 0;
|
||||
//
|
||||
|
||||
public Joycon(IntPtr handle_, bool imu, bool localize, float alpha, bool left, int id = 0, bool isPro=false) {
|
||||
handle = handle_;
|
||||
imu_enabled = imu;
|
||||
do_localize = localize;
|
||||
rumble_obj = new Rumble(160, 320, 0);
|
||||
filterweight = alpha;
|
||||
isLeft = left;
|
||||
|
||||
PadId = id;
|
||||
this.isPro = isPro;
|
||||
}
|
||||
public void DebugPrint(String s, DebugType d) {
|
||||
if (debug_type == DebugType.NONE) return;
|
||||
if (d == DebugType.ALL || d == debug_type || debug_type == DebugType.ALL) {
|
||||
Console.WriteLine(s);
|
||||
}
|
||||
}
|
||||
public bool GetButtonDown(Button b) {
|
||||
return buttons_down[(int)b];
|
||||
}
|
||||
public bool GetButton(Button b) {
|
||||
return buttons[(int)b];
|
||||
}
|
||||
public bool GetButtonUp(Button b) {
|
||||
return buttons_up[(int)b];
|
||||
}
|
||||
public float[] GetStick() {
|
||||
return stick;
|
||||
}
|
||||
public float[] GetStick2() {
|
||||
return stick2;
|
||||
}
|
||||
public Vector3 GetGyro() {
|
||||
return gyr_g;
|
||||
}
|
||||
public Vector3 GetAccel() {
|
||||
return acc_g;
|
||||
}
|
||||
public Quaternion GetVector() {
|
||||
Vector3 v1 = new Vector3(j_b.X, i_b.X, k_b.X);
|
||||
Vector3 v2 = -(new Vector3(j_b.Z, i_b.Z, k_b.Z));
|
||||
if (v2 != Vector3.Zero) {
|
||||
MyQuaternion temp = MyQuaternion.LookRotation(v1, v2);
|
||||
return new Quaternion(temp.eulerAngles, temp.Length);
|
||||
} else {
|
||||
return Quaternion.Identity;
|
||||
}
|
||||
}
|
||||
public int Attach(byte leds_ = 0x0) {
|
||||
state = state_.ATTACHED;
|
||||
byte[] a = { 0x0 };
|
||||
// Input report mode
|
||||
Subcommand(0x3, new byte[] { 0x3f }, 1, false);
|
||||
a[0] = 0x1;
|
||||
dump_calibration_data();
|
||||
// Connect
|
||||
a[0] = 0x01;
|
||||
Subcommand(0x1, a, 1);
|
||||
a[0] = 0x02;
|
||||
Subcommand(0x1, a, 1);
|
||||
a[0] = 0x03;
|
||||
Subcommand(0x1, a, 1);
|
||||
a[0] = leds_;
|
||||
Subcommand(0x30, a, 1);
|
||||
Subcommand(0x40, new byte[] { (imu_enabled ? (byte)0x1 : (byte)0x0) }, 1, true);
|
||||
Subcommand(0x3, new byte[] { 0x30 }, 1, true);
|
||||
Subcommand(0x48, new byte[] { 0x1 }, 1, true);
|
||||
DebugPrint("Done with init.", DebugType.COMMS);
|
||||
return 0;
|
||||
}
|
||||
public void SetFilterCoeff(float a) {
|
||||
filterweight = a;
|
||||
}
|
||||
public void Detach() {
|
||||
stop_polling = true;
|
||||
PrintArray(max, format: "Max {0:S}", d: DebugType.IMU);
|
||||
PrintArray(sum, format: "Sum {0:S}", d: DebugType.IMU);
|
||||
if (state > state_.NO_JOYCONS) {
|
||||
Subcommand(0x30, new byte[] { 0x0 }, 1);
|
||||
Subcommand(0x40, new byte[] { 0x0 }, 1);
|
||||
Subcommand(0x48, new byte[] { 0x0 }, 1);
|
||||
Subcommand(0x3, new byte[] { 0x3f }, 1);
|
||||
}
|
||||
if (state > state_.DROPPED) {
|
||||
HIDapi.hid_close(handle);
|
||||
}
|
||||
state = state_.NOT_ATTACHED;
|
||||
}
|
||||
private byte ts_en;
|
||||
private byte ts_de;
|
||||
private System.DateTime ts_prev;
|
||||
private int ReceiveRaw() {
|
||||
if (handle == IntPtr.Zero) return -2;
|
||||
HIDapi.hid_set_nonblocking(handle, 0);
|
||||
byte[] raw_buf = new byte[report_len];
|
||||
int ret = HIDapi.hid_read(handle, raw_buf, new UIntPtr(report_len));
|
||||
if (ret > 0) {
|
||||
lock (reports) {
|
||||
reports.Enqueue(new Report(raw_buf, System.DateTime.Now, (ulong)Stopwatch.GetTimestamp()));
|
||||
}
|
||||
if (ts_en == raw_buf[1]) {
|
||||
DebugPrint(string.Format("Duplicate timestamp enqueued. TS: {0:X2}", ts_en), DebugType.THREADING);
|
||||
}
|
||||
ts_en = raw_buf[1];
|
||||
DebugPrint(string.Format("Enqueue. Bytes read: {0:D}. Timestamp: {1:X2}", ret, raw_buf[1]), DebugType.THREADING);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
private Thread PollThreadObj;
|
||||
private void Poll() {
|
||||
int attempts = 0;
|
||||
while (!stop_polling & state > state_.NO_JOYCONS) {
|
||||
SendRumble(rumble_obj.GetData());
|
||||
int a = ReceiveRaw();
|
||||
//a = ReceiveRaw();
|
||||
if (a > 0) {
|
||||
state = state_.IMU_DATA_OK;
|
||||
attempts = 0;
|
||||
} else if (attempts > 1000) {
|
||||
state = state_.DROPPED;
|
||||
DebugPrint("Connection lost. Is the Joy-Con connected?", DebugType.ALL);
|
||||
break;
|
||||
} else {
|
||||
DebugPrint("Pause 5ms", DebugType.THREADING);
|
||||
Thread.Sleep((Int32)5);
|
||||
}
|
||||
++attempts;
|
||||
}
|
||||
DebugPrint("End poll loop.", DebugType.THREADING);
|
||||
}
|
||||
float[] max = { 0, 0, 0 };
|
||||
float[] sum = { 0, 0, 0 };
|
||||
public void Update() {
|
||||
if (state > state_.NO_JOYCONS) {
|
||||
byte[] report_buf = new byte[report_len];
|
||||
while (reports.Count > 0) {
|
||||
Report rep;
|
||||
lock (reports) {
|
||||
rep = reports.Dequeue();
|
||||
rep.CopyBuffer(report_buf);
|
||||
}
|
||||
if (imu_enabled) {
|
||||
if (do_localize) {
|
||||
ProcessIMU(report_buf);
|
||||
} else {
|
||||
ExtractIMUValues(report_buf, 0);
|
||||
}
|
||||
}
|
||||
if (ts_de == report_buf[1]) {
|
||||
DebugPrint(string.Format("Duplicate timestamp dequeued. TS: {0:X2}", ts_de), DebugType.THREADING);
|
||||
}
|
||||
ts_de = report_buf[1];
|
||||
DebugPrint(String.Format("Dequeue. Queue length: {0}. Packet ID: {1}. Timestamp: {2}. Lag to dequeue: {3}. Lag between packets (expect 15ms): {4}", reports.Count, report_buf[0], report_buf[1], System.DateTime.Now.Subtract(rep.GetTime()), rep.GetTime().Subtract(ts_prev)), DebugType.THREADING);
|
||||
ts_prev = rep.GetTime();
|
||||
|
||||
// set timestamp and packet count for server
|
||||
Timestamp = rep.ts;
|
||||
}
|
||||
ProcessButtonsAndStick(report_buf);
|
||||
if (rumble_obj.timed_rumble) {
|
||||
if (rumble_obj.t < 0) {
|
||||
rumble_obj.set_vals(160, 320, 0, 0);
|
||||
} else {
|
||||
rumble_obj.t -= (1 / timing);
|
||||
}
|
||||
}
|
||||
|
||||
packetCounter++;
|
||||
Program.server.NewReportIncoming(this);
|
||||
}
|
||||
}
|
||||
private int ProcessButtonsAndStick(byte[] report_buf) {
|
||||
if (report_buf[0] == 0x00) return -1;
|
||||
|
||||
stick_raw[0] = report_buf[6 + (isLeft ? 0 : 3)];
|
||||
stick_raw[1] = report_buf[7 + (isLeft ? 0 : 3)];
|
||||
stick_raw[2] = report_buf[8 + (isLeft ? 0 : 3)];
|
||||
|
||||
if (isPro) {
|
||||
stick2_raw[0] = report_buf[6 + (!isLeft ? 0 : 3)];
|
||||
stick2_raw[1] = report_buf[7 + (!isLeft ? 0 : 3)];
|
||||
stick2_raw[2] = report_buf[8 + (!isLeft ? 0 : 3)];
|
||||
}
|
||||
|
||||
stick_precal[0] = (UInt16)(stick_raw[0] | ((stick_raw[1] & 0xf) << 8));
|
||||
stick_precal[1] = (UInt16)((stick_raw[1] >> 4) | (stick_raw[2] << 4));
|
||||
stick = CenterSticks(stick_precal);
|
||||
|
||||
if (isPro) {
|
||||
stick2_precal[0] = (UInt16)(stick2_raw[0] | ((stick2_raw[1] & 0xf) << 8));
|
||||
stick2_precal[1] = (UInt16)((stick2_raw[1] >> 4) | (stick2_raw[2] << 4));
|
||||
stick2 = CenterSticks(stick2_precal, true);
|
||||
}
|
||||
|
||||
lock (buttons) {
|
||||
lock (down_) {
|
||||
for (int i = 0; i < buttons.Length; ++i) {
|
||||
down_[i] = buttons[i];
|
||||
}
|
||||
}
|
||||
buttons[(int)Button.DPAD_DOWN] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x01 : 0x04)) != 0;
|
||||
buttons[(int)Button.DPAD_RIGHT] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x04 : 0x08)) != 0;
|
||||
buttons[(int)Button.DPAD_UP] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x02 : 0x02)) != 0;
|
||||
buttons[(int)Button.DPAD_LEFT] = (report_buf[3 + (isLeft ? 2 : 0)] & (isLeft ? 0x08 : 0x01)) != 0;
|
||||
buttons[(int)Button.HOME] = ((report_buf[4] & 0x10) != 0);
|
||||
buttons[(int)Button.MINUS] = ((report_buf[4] & 0x01) != 0);
|
||||
buttons[(int)Button.PLUS] = ((report_buf[4] & 0x02) != 0);
|
||||
buttons[(int)Button.STICK] = ((report_buf[4] & (isLeft ? 0x08 : 0x04)) != 0);
|
||||
buttons[(int)Button.SHOULDER_1] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x40) != 0;
|
||||
buttons[(int)Button.SHOULDER_2] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x80) != 0;
|
||||
buttons[(int)Button.SR] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x10) != 0;
|
||||
buttons[(int)Button.SL] = (report_buf[3 + (isLeft ? 2 : 0)] & 0x20) != 0;
|
||||
|
||||
if (isPro) {
|
||||
buttons[(int)Button.B] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x01 : 0x04)) != 0;
|
||||
buttons[(int)Button.A] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x04 : 0x08)) != 0;
|
||||
buttons[(int)Button.X] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x02 : 0x02)) != 0;
|
||||
buttons[(int)Button.Y] = (report_buf[3 + (!isLeft ? 2 : 0)] & (!isLeft ? 0x08 : 0x01)) != 0;
|
||||
|
||||
buttons[(int)Button.STICK2] = ((report_buf[4] & (!isLeft ? 0x08 : 0x04)) != 0);
|
||||
buttons[(int)Button.SHOULDER2_1] = (report_buf[3 + (!isLeft ? 2 : 0)] & 0x40) != 0;
|
||||
buttons[(int)Button.SHOULDER2_2] = (report_buf[3 + (!isLeft ? 2 : 0)] & 0x80) != 0;
|
||||
}
|
||||
|
||||
lock (buttons_up) {
|
||||
lock (buttons_down) {
|
||||
for (int i = 0; i < buttons.Length; ++i) {
|
||||
buttons_up[i] = (down_[i] & !buttons[i]);
|
||||
buttons_down[i] = (!down_[i] & buttons[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
private void ExtractIMUValues(byte[] report_buf, int n = 0) {
|
||||
gyr_r[0] = (Int16)(report_buf[19 + n * 12] | ((report_buf[20 + n * 12] << 8) & 0xff00));
|
||||
gyr_r[1] = (Int16)(report_buf[21 + n * 12] | ((report_buf[22 + n * 12] << 8) & 0xff00));
|
||||
gyr_r[2] = (Int16)(report_buf[23 + n * 12] | ((report_buf[24 + n * 12] << 8) & 0xff00));
|
||||
acc_r[0] = (Int16)(report_buf[13 + n * 12] | ((report_buf[14 + n * 12] << 8) & 0xff00));
|
||||
acc_r[1] = (Int16)(report_buf[15 + n * 12] | ((report_buf[16 + n * 12] << 8) & 0xff00));
|
||||
acc_r[2] = (Int16)(report_buf[17 + n * 12] | ((report_buf[18 + n * 12] << 8) & 0xff00));
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
switch (i) {
|
||||
case 0:
|
||||
acc_g.X = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
|
||||
//gyr_g.X = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
|
||||
gyr_g.X = gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f; // Neutrals may be read wrong
|
||||
if (Math.Abs(acc_g.X) > Math.Abs(max[i]))
|
||||
max[i] = acc_g.X;
|
||||
break;
|
||||
case 1:
|
||||
acc_g.Y = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
|
||||
//gyr_g.Y = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
|
||||
gyr_g.Y = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f;
|
||||
if (Math.Abs(acc_g.Y) > Math.Abs(max[i]))
|
||||
max[i] = acc_g.Y;
|
||||
break;
|
||||
case 2:
|
||||
acc_g.Z = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
|
||||
//gyr_g.Z = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
|
||||
gyr_g.Z = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f;
|
||||
if (Math.Abs(acc_g.Z) > Math.Abs(max[i]))
|
||||
max[i] = acc_g.Z;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private float err;
|
||||
public Vector3 i_b, j_b, k_b, k_acc;
|
||||
private Vector3 d_theta;
|
||||
private Vector3 i_b_;
|
||||
private Vector3 w_a, w_g;
|
||||
private Quaternion vec;
|
||||
|
||||
private int ProcessIMU(byte[] report_buf) {
|
||||
|
||||
// Direction Cosine Matrix method
|
||||
// http://www.starlino.com/dcm_tutorial.html
|
||||
|
||||
if (!imu_enabled | state < state_.IMU_DATA_OK)
|
||||
return -1;
|
||||
|
||||
if (report_buf[0] != 0x30) return -1; // no gyro data
|
||||
|
||||
// read raw IMU values
|
||||
int dt = (report_buf[1] - timestamp);
|
||||
if (report_buf[1] < timestamp) dt += 0x100;
|
||||
|
||||
for (int n = 0; n < 3; ++n) {
|
||||
ExtractIMUValues(report_buf, n);
|
||||
|
||||
float dt_sec = 0.005f * dt;
|
||||
sum[0] += gyr_g.X * dt_sec;
|
||||
sum[1] += gyr_g.Y * dt_sec;
|
||||
sum[2] += gyr_g.Z * dt_sec;
|
||||
|
||||
if (isLeft && !isPro) { // not sure about this
|
||||
gyr_g.Y *= -1;
|
||||
gyr_g.Z *= -1;
|
||||
acc_g.Y *= -1;
|
||||
acc_g.Z *= -1;
|
||||
}
|
||||
|
||||
if (first_imu_packet) {
|
||||
i_b = new Vector3(1, 0, 0);
|
||||
j_b = new Vector3(0, 1, 0);
|
||||
k_b = new Vector3(0, 0, 1);
|
||||
first_imu_packet = false;
|
||||
} else {
|
||||
k_acc = -Vector3.Normalize(acc_g);
|
||||
w_a = Vector3.Cross(k_b, k_acc);
|
||||
w_g = -gyr_g * dt_sec;
|
||||
d_theta = (filterweight * w_a + w_g) / (1f + filterweight);
|
||||
k_b += Vector3.Cross(d_theta, k_b);
|
||||
i_b += Vector3.Cross(d_theta, i_b);
|
||||
j_b += Vector3.Cross(d_theta, j_b);
|
||||
//Correction, ensure new axes are orthogonal
|
||||
err = Vector3.Dot(i_b, j_b) * 0.5f;
|
||||
i_b_ = Vector3.Normalize(i_b - err * j_b);
|
||||
j_b = Vector3.Normalize(j_b - err * i_b);
|
||||
i_b = i_b_;
|
||||
k_b = Vector3.Cross(i_b, j_b);
|
||||
}
|
||||
dt = 1;
|
||||
}
|
||||
timestamp = report_buf[1] + 2;
|
||||
return 0;
|
||||
}
|
||||
public void Begin() {
|
||||
if (PollThreadObj == null) {
|
||||
PollThreadObj = new Thread(new ThreadStart(Poll));
|
||||
PollThreadObj.Start();
|
||||
|
||||
Console.WriteLine("Starting poll thread.");
|
||||
}
|
||||
}
|
||||
public void Recenter() {
|
||||
first_imu_packet = true;
|
||||
}
|
||||
private float[] CenterSticks(UInt16[] vals, bool special=false) {
|
||||
ushort[] t = stick_cal;
|
||||
|
||||
if (special)
|
||||
t = stick2_cal;
|
||||
|
||||
float[] s = { 0, 0 };
|
||||
for (uint i = 0; i < 2; ++i) {
|
||||
float diff = vals[i] - t[2 + i];
|
||||
if (Math.Abs(diff) < deadzone) vals[i] = 0;
|
||||
else if (diff > 0) // if axis is above center
|
||||
{
|
||||
s[i] = diff / t[i];
|
||||
} else {
|
||||
s[i] = diff / t[4 + i];
|
||||
}
|
||||
}
|
||||
return s;
|
||||
}
|
||||
public void SetRumble(float low_freq, float high_freq, float amp, int time = 0) {
|
||||
if (state <= Joycon.state_.ATTACHED) return;
|
||||
if (rumble_obj.timed_rumble == false || rumble_obj.t < 0) {
|
||||
rumble_obj = new Rumble(low_freq, high_freq, amp, time);
|
||||
}
|
||||
}
|
||||
private void SendRumble(byte[] buf) {
|
||||
byte[] buf_ = new byte[report_len];
|
||||
buf_[0] = 0x10;
|
||||
buf_[1] = global_count;
|
||||
if (global_count == 0xf) global_count = 0;
|
||||
else ++global_count;
|
||||
Array.Copy(buf, 0, buf_, 2, 8);
|
||||
PrintArray(buf_, DebugType.RUMBLE, format: "Rumble data sent: {0:S}");
|
||||
HIDapi.hid_write(handle, buf_, new UIntPtr(report_len));
|
||||
}
|
||||
private byte[] Subcommand(byte sc, byte[] buf, uint len, bool print = true) {
|
||||
byte[] buf_ = new byte[report_len];
|
||||
byte[] response = new byte[report_len];
|
||||
Array.Copy(default_buf, 0, buf_, 2, 8);
|
||||
Array.Copy(buf, 0, buf_, 11, len);
|
||||
buf_[10] = sc;
|
||||
buf_[1] = global_count;
|
||||
buf_[0] = 0x1;
|
||||
if (global_count == 0xf) global_count = 0;
|
||||
else ++global_count;
|
||||
if (print) { PrintArray(buf_, DebugType.COMMS, len, 11, "Subcommand 0x" + string.Format("{0:X2}", sc) + " sent. Data: 0x{0:S}"); };
|
||||
HIDapi.hid_write(handle, buf_, new UIntPtr(len + 11));
|
||||
int res = HIDapi.hid_read_timeout(handle, response, new UIntPtr(report_len), 50);
|
||||
if (res < 1) DebugPrint("No response.", DebugType.COMMS);
|
||||
else if (print) { PrintArray(response, DebugType.COMMS, report_len - 1, 1, "Response ID 0x" + string.Format("{0:X2}", response[0]) + ". Data: 0x{0:S}"); }
|
||||
return response;
|
||||
}
|
||||
private void dump_calibration_data() {
|
||||
byte[] buf_ = ReadSPI(0x80, (isLeft ? (byte)0x12 : (byte)0x1d), 9); // get user calibration data if possible
|
||||
bool found = false;
|
||||
for (int i = 0; i < 9; ++i) {
|
||||
if (buf_[i] != 0xff) {
|
||||
Console.WriteLine("Using user stick calibration data.");
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
Console.WriteLine("Using factory stick calibration data.");
|
||||
buf_ = ReadSPI(0x60, (isLeft ? (byte)0x3d : (byte)0x46), 9); // get user calibration data if possible
|
||||
}
|
||||
stick_cal[isLeft ? 0 : 2] = (UInt16)((buf_[1] << 8) & 0xF00 | buf_[0]); // X Axis Max above center
|
||||
stick_cal[isLeft ? 1 : 3] = (UInt16)((buf_[2] << 4) | (buf_[1] >> 4)); // Y Axis Max above center
|
||||
stick_cal[isLeft ? 2 : 4] = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); // X Axis Center
|
||||
stick_cal[isLeft ? 3 : 5] = (UInt16)((buf_[5] << 4) | (buf_[4] >> 4)); // Y Axis Center
|
||||
stick_cal[isLeft ? 4 : 0] = (UInt16)((buf_[7] << 8) & 0xF00 | buf_[6]); // X Axis Min below center
|
||||
stick_cal[isLeft ? 5 : 1] = (UInt16)((buf_[8] << 4) | (buf_[7] >> 4)); // Y Axis Min below center
|
||||
|
||||
PrintArray(stick_cal, len: 6, start: 0, format: "Stick calibration data: {0:S}");
|
||||
|
||||
if (isPro) {
|
||||
buf_ = ReadSPI(0x80, (!isLeft ? (byte)0x12 : (byte)0x1d), 9); // get user calibration data if possible
|
||||
found = false;
|
||||
for (int i = 0; i < 9; ++i) {
|
||||
if (buf_[i] != 0xff) {
|
||||
Console.WriteLine("Using user stick calibration data.");
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
Console.WriteLine("Using factory stick calibration data.");
|
||||
buf_ = ReadSPI(0x60, (!isLeft ? (byte)0x3d : (byte)0x46), 9); // get user calibration data if possible
|
||||
}
|
||||
stick2_cal[!isLeft ? 0 : 2] = (UInt16)((buf_[1] << 8) & 0xF00 | buf_[0]); // X Axis Max above center
|
||||
stick2_cal[!isLeft ? 1 : 3] = (UInt16)((buf_[2] << 4) | (buf_[1] >> 4)); // Y Axis Max above center
|
||||
stick2_cal[!isLeft ? 2 : 4] = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]); // X Axis Center
|
||||
stick2_cal[!isLeft ? 3 : 5] = (UInt16)((buf_[5] << 4) | (buf_[4] >> 4)); // Y Axis Center
|
||||
stick2_cal[!isLeft ? 4 : 0] = (UInt16)((buf_[7] << 8) & 0xF00 | buf_[6]); // X Axis Min below center
|
||||
stick2_cal[!isLeft ? 5 : 1] = (UInt16)((buf_[8] << 4) | (buf_[7] >> 4)); // Y Axis Min below center
|
||||
|
||||
PrintArray(stick2_cal, len: 6, start: 0, format: "Stick calibration data: {0:S}");
|
||||
|
||||
buf_ = ReadSPI(0x60, (!isLeft ? (byte)0x86 : (byte)0x98), 16);
|
||||
deadzone2 = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]);
|
||||
}
|
||||
|
||||
buf_ = ReadSPI(0x60, (isLeft ? (byte)0x86 : (byte)0x98), 16);
|
||||
deadzone = (UInt16)((buf_[4] << 8) & 0xF00 | buf_[3]);
|
||||
|
||||
buf_ = ReadSPI(0x80, 0x28, 10);
|
||||
acc_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
acc_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
acc_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x80, 0x2E, 10);
|
||||
acc_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
acc_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
acc_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x80, 0x34, 10);
|
||||
gyr_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
gyr_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
gyr_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x80, 0x3A, 10);
|
||||
gyr_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
gyr_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
gyr_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
PrintArray(gyr_neutral, len: 3, d: DebugType.IMU, format: "User gyro neutral position: {0:S}");
|
||||
|
||||
// This is an extremely messy way of checking to see whether there is user stick calibration data present, but I've seen conflicting user calibration data on blank Joy-Cons. Worth another look eventually.
|
||||
if (gyr_neutral[0] + gyr_neutral[1] + gyr_neutral[2] == -3 || Math.Abs(gyr_neutral[0]) > 100 || Math.Abs(gyr_neutral[1]) > 100 || Math.Abs(gyr_neutral[2]) > 100) {
|
||||
buf_ = ReadSPI(0x60, 0x20, 10);
|
||||
acc_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
acc_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
acc_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x60, 0x26, 10);
|
||||
acc_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
acc_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
acc_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x60, 0x2C, 10);
|
||||
gyr_neutral[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
gyr_neutral[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
gyr_neutral[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
buf_ = ReadSPI(0x60, 0x32, 10);
|
||||
gyr_sensiti[0] = (Int16)(buf_[0] | ((buf_[1] << 8) & 0xff00));
|
||||
gyr_sensiti[1] = (Int16)(buf_[2] | ((buf_[3] << 8) & 0xff00));
|
||||
gyr_sensiti[2] = (Int16)(buf_[4] | ((buf_[5] << 8) & 0xff00));
|
||||
|
||||
PrintArray(gyr_neutral, len: 3, d: DebugType.IMU, format: "Factory gyro neutral position: {0:S}");
|
||||
}
|
||||
}
|
||||
private byte[] ReadSPI(byte addr1, byte addr2, uint len, bool print = false) {
|
||||
byte[] buf = { addr2, addr1, 0x00, 0x00, (byte)len };
|
||||
byte[] read_buf = new byte[len];
|
||||
byte[] buf_ = new byte[len + 20];
|
||||
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
buf_ = Subcommand(0x10, buf, 5, false);
|
||||
if (buf_[15] == addr2 && buf_[16] == addr1) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
Array.Copy(buf_, 20, read_buf, 0, len);
|
||||
if (print) PrintArray(read_buf, DebugType.COMMS, len);
|
||||
return read_buf;
|
||||
}
|
||||
private void PrintArray<T>(T[] arr, DebugType d = DebugType.NONE, uint len = 0, uint start = 0, string format = "{0:S}") {
|
||||
if (d != debug_type && debug_type != DebugType.ALL) return;
|
||||
if (len == 0) len = (uint)arr.Length;
|
||||
string tostr = "";
|
||||
for (int i = 0; i < len; ++i) {
|
||||
tostr += string.Format((arr[0] is byte) ? "{0:X2} " : ((arr[0] is float) ? "{0:F} " : "{0:D} "), arr[i + start]);
|
||||
}
|
||||
DebugPrint(string.Format(format, tostr), d);
|
||||
}
|
||||
}
|
||||
}
|
||||
675
BetterJoyForCemu/MyQuaternion.cs
Normal file
675
BetterJoyForCemu/MyQuaternion.cs
Normal file
|
|
@ -0,0 +1,675 @@
|
|||
using System;
|
||||
using System.Runtime.Serialization;
|
||||
using System.Xml.Serialization;
|
||||
using System.Numerics;
|
||||
|
||||
/// <summary>
|
||||
/// Quaternions are used to represent rotations.
|
||||
/// A custom completely managed implementation of UnityEngine.Quaternion
|
||||
/// Base is decompiled UnityEngine.Quaternion
|
||||
/// Doesn't implement methods marked Obsolete
|
||||
/// Does implicit coversions to and from UnityEngine.Quaternion
|
||||
///
|
||||
/// Uses code from:
|
||||
/// https://raw.githubusercontent.com/mono/opentk/master/Source/OpenTK/Math/Quaternion.cs
|
||||
/// http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
|
||||
/// http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
|
||||
/// http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha
|
||||
///
|
||||
/// Version: aeroson 2017-07-11 (author yyyy-MM-dd)
|
||||
/// License: ODC Public Domain Dedication & License 1.0 (PDDL-1.0) https://tldrlegal.com/license/odc-public-domain-dedication-&-license-1.0-(pddl-1.0)
|
||||
/// </summary>
|
||||
[Serializable]
|
||||
public struct MyQuaternion : IEquatable<MyQuaternion> {
|
||||
const float radToDeg = (float)(180.0 / Math.PI);
|
||||
const float degToRad = (float)(Math.PI / 180.0);
|
||||
|
||||
public const float kEpsilon = 1E-06f; // should probably be used in the 0 tests in LookRotation or Slerp
|
||||
|
||||
[XmlIgnore]
|
||||
public Vector3 xyz {
|
||||
set {
|
||||
x = value.X;
|
||||
y = value.Y;
|
||||
z = value.Z;
|
||||
}
|
||||
get {
|
||||
return new Vector3(x, y, z);
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>X component of the Quaternion. Don't modify this directly unless you know quaternions inside out.</para>
|
||||
/// </summary>
|
||||
public float x;
|
||||
/// <summary>
|
||||
/// <para>Y component of the Quaternion. Don't modify this directly unless you know quaternions inside out.</para>
|
||||
/// </summary>
|
||||
public float y;
|
||||
/// <summary>
|
||||
/// <para>Z component of the Quaternion. Don't modify this directly unless you know quaternions inside out.</para>
|
||||
/// </summary>
|
||||
public float z;
|
||||
/// <summary>
|
||||
/// <para>W component of the Quaternion. Don't modify this directly unless you know quaternions inside out.</para>
|
||||
/// </summary>
|
||||
public float w;
|
||||
|
||||
[XmlIgnore]
|
||||
public float this[int index] {
|
||||
get {
|
||||
switch (index) {
|
||||
case 0:
|
||||
return this.x;
|
||||
case 1:
|
||||
return this.y;
|
||||
case 2:
|
||||
return this.z;
|
||||
case 3:
|
||||
return this.w;
|
||||
default:
|
||||
throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3");
|
||||
}
|
||||
}
|
||||
set {
|
||||
switch (index) {
|
||||
case 0:
|
||||
this.x = value;
|
||||
break;
|
||||
case 1:
|
||||
this.y = value;
|
||||
break;
|
||||
case 2:
|
||||
this.z = value;
|
||||
break;
|
||||
case 3:
|
||||
this.w = value;
|
||||
break;
|
||||
default:
|
||||
throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3");
|
||||
}
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>The identity rotation (RO).</para>
|
||||
/// </summary>
|
||||
[XmlIgnore]
|
||||
public static MyQuaternion identity {
|
||||
get {
|
||||
return new MyQuaternion(0f, 0f, 0f, 1f);
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns the euler angle representation of the rotation.</para>
|
||||
/// </summary>
|
||||
[XmlIgnore]
|
||||
public Vector3 eulerAngles {
|
||||
get {
|
||||
return MyQuaternion.ToEulerRad(this) * radToDeg;
|
||||
}
|
||||
set {
|
||||
this = MyQuaternion.FromEulerRad(value * degToRad);
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// Gets the length (magnitude) of the quaternion.
|
||||
/// </summary>
|
||||
/// <seealso cref="LengthSquared"/>
|
||||
[XmlIgnore]
|
||||
public float Length {
|
||||
get {
|
||||
return (float)System.Math.Sqrt(x * x + y * y + z * z + w * w);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the square of the quaternion length (magnitude).
|
||||
/// </summary>
|
||||
[XmlIgnore]
|
||||
public float LengthSquared {
|
||||
get {
|
||||
return x * x + y * y + z * z + w * w;
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Constructs new MyQuaternion with given x,y,z,w components.</para>
|
||||
/// </summary>
|
||||
/// <param name="x"></param>
|
||||
/// <param name="y"></param>
|
||||
/// <param name="z"></param>
|
||||
/// <param name="w"></param>
|
||||
public MyQuaternion(float x, float y, float z, float w) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.z = z;
|
||||
this.w = w;
|
||||
}
|
||||
/// <summary>
|
||||
/// Construct a new MyQuaternion from vector and w components
|
||||
/// </summary>
|
||||
/// <param name="v">The vector part</param>
|
||||
/// <param name="w">The w part</param>
|
||||
public MyQuaternion(Vector3 v, float w) {
|
||||
this.x = v.X;
|
||||
this.y = v.Y;
|
||||
this.z = v.Z;
|
||||
this.w = w;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Set x, y, z and w components of an existing MyQuaternion.</para>
|
||||
/// </summary>
|
||||
/// <param name="new_x"></param>
|
||||
/// <param name="new_y"></param>
|
||||
/// <param name="new_z"></param>
|
||||
/// <param name="new_w"></param>
|
||||
public void Set(float new_x, float new_y, float new_z, float new_w) {
|
||||
this.x = new_x;
|
||||
this.y = new_y;
|
||||
this.z = new_z;
|
||||
this.w = new_w;
|
||||
}
|
||||
/// <summary>
|
||||
/// Scales the MyQuaternion to unit length.
|
||||
/// </summary>
|
||||
public void Normalize() {
|
||||
float scale = 1.0f / this.Length;
|
||||
xyz *= scale;
|
||||
w *= scale;
|
||||
}
|
||||
/// <summary>
|
||||
/// Scale the given quaternion to unit length
|
||||
/// </summary>
|
||||
/// <param name="q">The quaternion to normalize</param>
|
||||
/// <returns>The normalized quaternion</returns>
|
||||
public static MyQuaternion Normalize(MyQuaternion q) {
|
||||
MyQuaternion result;
|
||||
Normalize(ref q, out result);
|
||||
return result;
|
||||
}
|
||||
/// <summary>
|
||||
/// Scale the given quaternion to unit length
|
||||
/// </summary>
|
||||
/// <param name="q">The quaternion to normalize</param>
|
||||
/// <param name="result">The normalized quaternion</param>
|
||||
public static void Normalize(ref MyQuaternion q, out MyQuaternion result) {
|
||||
float scale = 1.0f / q.Length;
|
||||
result = new MyQuaternion(q.xyz * scale, q.w * scale);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>The dot product between two rotations.</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
public static float Dot(MyQuaternion a, MyQuaternion b) {
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Creates a rotation which rotates /angle/ degrees around /axis/.</para>
|
||||
/// </summary>
|
||||
/// <param name="angle"></param>
|
||||
/// <param name="axis"></param>
|
||||
public static MyQuaternion AngleAxis(float angle, Vector3 axis) {
|
||||
return MyQuaternion.AngleAxis(angle, ref axis);
|
||||
}
|
||||
private static MyQuaternion AngleAxis(float degress, ref Vector3 axis) {
|
||||
if (axis.LengthSquared() == 0.0f)
|
||||
return identity;
|
||||
|
||||
MyQuaternion result = identity;
|
||||
var radians = degress * degToRad;
|
||||
radians *= 0.5f;
|
||||
axis = Vector3.Normalize(axis);
|
||||
axis = axis * (float)System.Math.Sin(radians);
|
||||
result.x = axis.X;
|
||||
result.y = axis.Y;
|
||||
result.z = axis.Z;
|
||||
result.w = (float)System.Math.Cos(radians);
|
||||
|
||||
return Normalize(result);
|
||||
}
|
||||
public void ToAngleAxis(out float angle, out Vector3 axis) {
|
||||
MyQuaternion.ToAxisAngleRad(this, out axis, out angle);
|
||||
angle *= radToDeg;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Creates a rotation which rotates from /fromDirection/ to /toDirection/.</para>
|
||||
/// </summary>
|
||||
/// <param name="fromDirection"></param>
|
||||
/// <param name="toDirection"></param>
|
||||
public static MyQuaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection) {
|
||||
return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), float.MaxValue);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Creates a rotation which rotates from /fromDirection/ to /toDirection/.</para>
|
||||
/// </summary>
|
||||
/// <param name="fromDirection"></param>
|
||||
/// <param name="toDirection"></param>
|
||||
public void SetFromToRotation(Vector3 fromDirection, Vector3 toDirection) {
|
||||
this = MyQuaternion.FromToRotation(fromDirection, toDirection);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Creates a rotation with the specified /forward/ and /upwards/ directions.</para>
|
||||
/// </summary>
|
||||
/// <param name="forward">The direction to look in.</param>
|
||||
/// <param name="upwards">The vector that defines in which direction up is.</param>
|
||||
public static MyQuaternion LookRotation(Vector3 forward, Vector3 upwards) {
|
||||
return MyQuaternion.LookRotation(ref forward, ref upwards);
|
||||
}
|
||||
public static MyQuaternion LookRotation(Vector3 forward) {
|
||||
Vector3 up = new Vector3(0, 0, 1);
|
||||
return MyQuaternion.LookRotation(ref forward, ref up);
|
||||
}
|
||||
// from http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
|
||||
private static MyQuaternion LookRotation(ref Vector3 forward, ref Vector3 up) {
|
||||
|
||||
forward = Vector3.Normalize(forward);
|
||||
Vector3 right = Vector3.Normalize(Vector3.Cross(up, forward));
|
||||
up = Vector3.Cross(forward, right);
|
||||
var m00 = right.X;
|
||||
var m01 = right.Y;
|
||||
var m02 = right.Z;
|
||||
var m10 = up.X;
|
||||
var m11 = up.Y;
|
||||
var m12 = up.Z;
|
||||
var m20 = forward.X;
|
||||
var m21 = forward.Y;
|
||||
var m22 = forward.Z;
|
||||
|
||||
|
||||
float num8 = (m00 + m11) + m22;
|
||||
var quaternion = new MyQuaternion();
|
||||
if (num8 > 0f) {
|
||||
var num = (float)System.Math.Sqrt(num8 + 1f);
|
||||
quaternion.w = num * 0.5f;
|
||||
num = 0.5f / num;
|
||||
quaternion.x = (m12 - m21) * num;
|
||||
quaternion.y = (m20 - m02) * num;
|
||||
quaternion.z = (m01 - m10) * num;
|
||||
return quaternion;
|
||||
}
|
||||
if ((m00 >= m11) && (m00 >= m22)) {
|
||||
var num7 = (float)System.Math.Sqrt(((1f + m00) - m11) - m22);
|
||||
var num4 = 0.5f / num7;
|
||||
quaternion.x = 0.5f * num7;
|
||||
quaternion.y = (m01 + m10) * num4;
|
||||
quaternion.z = (m02 + m20) * num4;
|
||||
quaternion.w = (m12 - m21) * num4;
|
||||
return quaternion;
|
||||
}
|
||||
if (m11 > m22) {
|
||||
var num6 = (float)System.Math.Sqrt(((1f + m11) - m00) - m22);
|
||||
var num3 = 0.5f / num6;
|
||||
quaternion.x = (m10 + m01) * num3;
|
||||
quaternion.y = 0.5f * num6;
|
||||
quaternion.z = (m21 + m12) * num3;
|
||||
quaternion.w = (m20 - m02) * num3;
|
||||
return quaternion;
|
||||
}
|
||||
var num5 = (float)System.Math.Sqrt(((1f + m22) - m00) - m11);
|
||||
var num2 = 0.5f / num5;
|
||||
quaternion.x = (m20 + m02) * num2;
|
||||
quaternion.y = (m21 + m12) * num2;
|
||||
quaternion.z = 0.5f * num5;
|
||||
quaternion.w = (m01 - m10) * num2;
|
||||
return quaternion;
|
||||
}
|
||||
public void SetLookRotation(Vector3 view) {
|
||||
Vector3 up = new Vector3(0, 0, 1);
|
||||
this.SetLookRotation(view, up);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Creates a rotation with the specified /forward/ and /upwards/ directions.</para>
|
||||
/// </summary>
|
||||
/// <param name="view">The direction to look in.</param>
|
||||
/// <param name="up">The vector that defines in which direction up is.</param>
|
||||
public void SetLookRotation(Vector3 view, Vector3 up) {
|
||||
this = MyQuaternion.LookRotation(view, up);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is clamped to the range [0, 1].</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
/// <param name="t"></param>
|
||||
public static MyQuaternion Slerp(MyQuaternion a, MyQuaternion b, float t) {
|
||||
return MyQuaternion.Slerp(ref a, ref b, t);
|
||||
}
|
||||
private static MyQuaternion Slerp(ref MyQuaternion a, ref MyQuaternion b, float t) {
|
||||
if (t > 1) t = 1;
|
||||
if (t < 0) t = 0;
|
||||
return SlerpUnclamped(ref a, ref b, t);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is not clamped.</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
/// <param name="t"></param>
|
||||
public static MyQuaternion SlerpUnclamped(MyQuaternion a, MyQuaternion b, float t) {
|
||||
return MyQuaternion.SlerpUnclamped(ref a, ref b, t);
|
||||
}
|
||||
private static MyQuaternion SlerpUnclamped(ref MyQuaternion a, ref MyQuaternion b, float t) {
|
||||
// if either input is zero, return the other.
|
||||
if (a.LengthSquared == 0.0f) {
|
||||
if (b.LengthSquared == 0.0f) {
|
||||
return identity;
|
||||
}
|
||||
return b;
|
||||
} else if (b.LengthSquared == 0.0f) {
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
float cosHalfAngle = a.w * b.w + Vector3.Dot(a.xyz, b.xyz);
|
||||
|
||||
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
|
||||
// angle = 0.0f, so just return one input.
|
||||
return a;
|
||||
} else if (cosHalfAngle < 0.0f) {
|
||||
b.xyz = -b.xyz;
|
||||
b.w = -b.w;
|
||||
cosHalfAngle = -cosHalfAngle;
|
||||
}
|
||||
|
||||
float blendA;
|
||||
float blendB;
|
||||
if (cosHalfAngle < 0.99f) {
|
||||
// do proper slerp for big angles
|
||||
float halfAngle = (float)System.Math.Acos(cosHalfAngle);
|
||||
float sinHalfAngle = (float)System.Math.Sin(halfAngle);
|
||||
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
|
||||
blendA = (float)System.Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
|
||||
blendB = (float)System.Math.Sin(halfAngle * t) * oneOverSinHalfAngle;
|
||||
} else {
|
||||
// do lerp if angle is really small.
|
||||
blendA = 1.0f - t;
|
||||
blendB = t;
|
||||
}
|
||||
|
||||
MyQuaternion result = new MyQuaternion(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w);
|
||||
if (result.LengthSquared > 0.0f)
|
||||
return Normalize(result);
|
||||
else
|
||||
return identity;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is clamped to the range [0, 1].</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
/// <param name="t"></param>
|
||||
public static MyQuaternion Lerp(MyQuaternion a, MyQuaternion b, float t) {
|
||||
if (t > 1) t = 1;
|
||||
if (t < 0) t = 0;
|
||||
return Slerp(ref a, ref b, t); // TODO: use lerp not slerp, "Because quaternion works in 4D. Rotation in 4D are linear" ???
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is not clamped.</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
/// <param name="t"></param>
|
||||
public static MyQuaternion LerpUnclamped(MyQuaternion a, MyQuaternion b, float t) {
|
||||
return Slerp(ref a, ref b, t);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Rotates a rotation /from/ towards /to/.</para>
|
||||
/// </summary>
|
||||
/// <param name="from"></param>
|
||||
/// <param name="to"></param>
|
||||
/// <param name="maxDegreesDelta"></param>
|
||||
public static MyQuaternion RotateTowards(MyQuaternion from, MyQuaternion to, float maxDegreesDelta) {
|
||||
float num = MyQuaternion.Angle(from, to);
|
||||
if (num == 0f) {
|
||||
return to;
|
||||
}
|
||||
float t = Math.Min(1f, maxDegreesDelta / num);
|
||||
return MyQuaternion.SlerpUnclamped(from, to, t);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns the Inverse of /rotation/.</para>
|
||||
/// </summary>
|
||||
/// <param name="rotation"></param>
|
||||
public static MyQuaternion Inverse(MyQuaternion rotation) {
|
||||
float lengthSq = rotation.LengthSquared;
|
||||
if (lengthSq != 0.0) {
|
||||
float i = 1.0f / lengthSq;
|
||||
return new MyQuaternion(rotation.xyz * -i, rotation.w * i);
|
||||
}
|
||||
return rotation;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns a nicely formatted string of the MyQuaternion.</para>
|
||||
/// </summary>
|
||||
/// <param name="format"></param>
|
||||
public override string ToString() {
|
||||
return string.Format("({0:F1}, {1:F1}, {2:F1}, {3:F1})", this.x, this.y, this.z, this.w);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns a nicely formatted string of the MyQuaternion.</para>
|
||||
/// </summary>
|
||||
/// <param name="format"></param>
|
||||
public string ToString(string format) {
|
||||
return string.Format("({0}, {1}, {2}, {3})", this.x.ToString(format), this.y.ToString(format), this.z.ToString(format), this.w.ToString(format));
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns the angle in degrees between two rotations /a/ and /b/.</para>
|
||||
/// </summary>
|
||||
/// <param name="a"></param>
|
||||
/// <param name="b"></param>
|
||||
public static float Angle(MyQuaternion a, MyQuaternion b) {
|
||||
float f = MyQuaternion.Dot(a, b);
|
||||
return (float)Math.Acos(Math.Min( Math.Abs( f ), 1f) ) * 2f * radToDeg;
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order).</para>
|
||||
/// </summary>
|
||||
/// <param name="x"></param>
|
||||
/// <param name="y"></param>
|
||||
/// <param name="z"></param>
|
||||
public static MyQuaternion Euler(float x, float y, float z) {
|
||||
return MyQuaternion.FromEulerRad(new Vector3((float)x, (float)y, (float)z) * degToRad);
|
||||
}
|
||||
/// <summary>
|
||||
/// <para>Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order).</para>
|
||||
/// </summary>
|
||||
/// <param name="euler"></param>
|
||||
public static MyQuaternion Euler(Vector3 euler) {
|
||||
return MyQuaternion.FromEulerRad(euler * degToRad);
|
||||
}
|
||||
// from http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
|
||||
private static Vector3 ToEulerRad(MyQuaternion rotation) {
|
||||
float sqw = rotation.w * rotation.w;
|
||||
float sqx = rotation.x * rotation.x;
|
||||
float sqy = rotation.y * rotation.y;
|
||||
float sqz = rotation.z * rotation.z;
|
||||
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
||||
float test = rotation.x * rotation.w - rotation.y * rotation.z;
|
||||
Vector3 v;
|
||||
|
||||
if (test > 0.4995f * unit) { // singularity at north pole
|
||||
v.Y = 2.0f * (float)Math.Atan2(rotation.y, rotation.x);
|
||||
v.X = (float) Math.PI / 2;
|
||||
v.Z = 0;
|
||||
return NormalizeAngles(v);
|
||||
}
|
||||
if (test < -0.4995f * unit) { // singularity at south pole
|
||||
v.Y = -2f * (float) Math.Atan2(rotation.y, rotation.x);
|
||||
v.X = -(float)Math.PI / 2;
|
||||
v.Z = 0;
|
||||
return NormalizeAngles(v);
|
||||
}
|
||||
MyQuaternion q = new MyQuaternion(rotation.w, rotation.z, rotation.x, rotation.y);
|
||||
v.Y = (float)System.Math.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
|
||||
v.X = (float)System.Math.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
|
||||
v.Z = (float)System.Math.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
|
||||
return NormalizeAngles(v);
|
||||
}
|
||||
private static Vector3 NormalizeAngles(Vector3 angles) {
|
||||
angles.X = NormalizeAngle(angles.X);
|
||||
angles.Y = NormalizeAngle(angles.Y);
|
||||
angles.Z = NormalizeAngle(angles.Z);
|
||||
return angles;
|
||||
}
|
||||
private static float NormalizeAngle(float angle) {
|
||||
while (angle > 360)
|
||||
angle -= 360;
|
||||
while (angle < 0)
|
||||
angle += 360;
|
||||
return angle;
|
||||
}
|
||||
// from http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha
|
||||
private static MyQuaternion FromEulerRad(Vector3 euler) {
|
||||
var yaw = euler.X;
|
||||
var pitch = euler.Y;
|
||||
var roll = euler.Z;
|
||||
float rollOver2 = roll * 0.5f;
|
||||
float sinRollOver2 = (float)System.Math.Sin((float)rollOver2);
|
||||
float cosRollOver2 = (float)System.Math.Cos((float)rollOver2);
|
||||
float pitchOver2 = pitch * 0.5f;
|
||||
float sinPitchOver2 = (float)System.Math.Sin((float)pitchOver2);
|
||||
float cosPitchOver2 = (float)System.Math.Cos((float)pitchOver2);
|
||||
float yawOver2 = yaw * 0.5f;
|
||||
float sinYawOver2 = (float)System.Math.Sin((float)yawOver2);
|
||||
float cosYawOver2 = (float)System.Math.Cos((float)yawOver2);
|
||||
MyQuaternion result;
|
||||
result.x = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
||||
result.z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
||||
result.w = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
||||
return result;
|
||||
|
||||
}
|
||||
private static void ToAxisAngleRad(MyQuaternion q, out Vector3 axis, out float angle) {
|
||||
if (System.Math.Abs(q.w) > 1.0f)
|
||||
q.Normalize();
|
||||
angle = 2.0f * (float)System.Math.Acos(q.w); // angle
|
||||
float den = (float)System.Math.Sqrt(1.0 - q.w * q.w);
|
||||
if (den > 0.0001f) {
|
||||
axis = q.xyz / den;
|
||||
} else {
|
||||
// This occurs when the angle is zero.
|
||||
// Not a problem: just set an arbitrary normalized axis.
|
||||
axis = new Vector3(1, 0, 0);
|
||||
}
|
||||
}
|
||||
#region Obsolete methods
|
||||
/*
|
||||
[Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static MyQuaternion EulerRotation(float x, float y, float z)
|
||||
{
|
||||
return MyQuaternion.Internal_FromEulerRad(new Vector3(x, y, z));
|
||||
}
|
||||
[Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static MyQuaternion EulerRotation(Vector3 euler)
|
||||
{
|
||||
return MyQuaternion.Internal_FromEulerRad(euler);
|
||||
}
|
||||
[Obsolete("Use MyQuaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void SetEulerRotation(float x, float y, float z)
|
||||
{
|
||||
this = Quaternion.Internal_FromEulerRad(new Vector3(x, y, z));
|
||||
}
|
||||
[Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void SetEulerRotation(Vector3 euler)
|
||||
{
|
||||
this = Quaternion.Internal_FromEulerRad(euler);
|
||||
}
|
||||
[Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public Vector3 ToEuler()
|
||||
{
|
||||
return Quaternion.Internal_ToEulerRad(this);
|
||||
}
|
||||
[Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static Quaternion EulerAngles(float x, float y, float z)
|
||||
{
|
||||
return Quaternion.Internal_FromEulerRad(new Vector3(x, y, z));
|
||||
}
|
||||
[Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static Quaternion EulerAngles(Vector3 euler)
|
||||
{
|
||||
return Quaternion.Internal_FromEulerRad(euler);
|
||||
}
|
||||
[Obsolete("Use Quaternion.ToAngleAxis instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void ToAxisAngle(out Vector3 axis, out float angle)
|
||||
{
|
||||
Quaternion.Internal_ToAxisAngleRad(this, out axis, out angle);
|
||||
}
|
||||
[Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void SetEulerAngles(float x, float y, float z)
|
||||
{
|
||||
this.SetEulerRotation(new Vector3(x, y, z));
|
||||
}
|
||||
[Obsolete("Use Quaternion.Euler instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void SetEulerAngles(Vector3 euler)
|
||||
{
|
||||
this = Quaternion.EulerRotation(euler);
|
||||
}
|
||||
[Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static Vector3 ToEulerAngles(Quaternion rotation)
|
||||
{
|
||||
return Quaternion.Internal_ToEulerRad(rotation);
|
||||
}
|
||||
[Obsolete("Use Quaternion.eulerAngles instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public Vector3 ToEulerAngles()
|
||||
{
|
||||
return Quaternion.Internal_ToEulerRad(this);
|
||||
}
|
||||
[Obsolete("Use Quaternion.AngleAxis instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public static Quaternion AxisAngle(Vector3 axis, float angle)
|
||||
{
|
||||
return Quaternion.INTERNAL_CALL_AxisAngle(ref axis, angle);
|
||||
}
|
||||
|
||||
private static Quaternion INTERNAL_CALL_AxisAngle(ref Vector3 axis, float angle)
|
||||
{
|
||||
|
||||
}
|
||||
[Obsolete("Use Quaternion.AngleAxis instead. This function was deprecated because it uses radians instead of degrees")]
|
||||
public void SetAxisAngle(Vector3 axis, float angle)
|
||||
{
|
||||
this = Quaternion.AxisAngle(axis, angle);
|
||||
}
|
||||
*/
|
||||
#endregion
|
||||
public override int GetHashCode() {
|
||||
return this.x.GetHashCode() ^ this.y.GetHashCode() << 2 ^ this.z.GetHashCode() >> 2 ^ this.w.GetHashCode() >> 1;
|
||||
}
|
||||
public override bool Equals(object other) {
|
||||
if (!(other is MyQuaternion)) {
|
||||
return false;
|
||||
}
|
||||
MyQuaternion quaternion = (MyQuaternion)other;
|
||||
return this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w);
|
||||
}
|
||||
public bool Equals(MyQuaternion other) {
|
||||
return this.x.Equals(other.x) && this.y.Equals(other.y) && this.z.Equals(other.z) && this.w.Equals(other.w);
|
||||
}
|
||||
public static MyQuaternion operator *(MyQuaternion lhs, MyQuaternion rhs) {
|
||||
return new MyQuaternion(lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y, lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z, lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x, lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z);
|
||||
}
|
||||
public static Vector3 operator *(MyQuaternion rotation, Vector3 point) {
|
||||
float num = rotation.x * 2f;
|
||||
float num2 = rotation.y * 2f;
|
||||
float num3 = rotation.z * 2f;
|
||||
float num4 = rotation.x * num;
|
||||
float num5 = rotation.y * num2;
|
||||
float num6 = rotation.z * num3;
|
||||
float num7 = rotation.x * num2;
|
||||
float num8 = rotation.x * num3;
|
||||
float num9 = rotation.y * num3;
|
||||
float num10 = rotation.w * num;
|
||||
float num11 = rotation.w * num2;
|
||||
float num12 = rotation.w * num3;
|
||||
Vector3 result;
|
||||
result.X = (1f - (num5 + num6)) * point.X + (num7 - num12) * point.Y + (num8 + num11) * point.Z;
|
||||
result.Y = (num7 + num12) * point.X + (1f - (num4 + num6)) * point.Y + (num9 - num10) * point.Z;
|
||||
result.Z = (num8 - num11) * point.X + (num9 + num10) * point.Y + (1f - (num4 + num5)) * point.Z;
|
||||
return result;
|
||||
}
|
||||
public static bool operator ==(MyQuaternion lhs, MyQuaternion rhs) {
|
||||
return MyQuaternion.Dot(lhs, rhs) > 0.999999f;
|
||||
}
|
||||
public static bool operator !=(MyQuaternion lhs, MyQuaternion rhs) {
|
||||
return MyQuaternion.Dot(lhs, rhs) <= 0.999999f;
|
||||
}
|
||||
}
|
||||
151
BetterJoyForCemu/Program.cs
Normal file
151
BetterJoyForCemu/Program.cs
Normal file
|
|
@ -0,0 +1,151 @@
|
|||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Timers;
|
||||
using static BetterJoyForCemu.HIDapi;
|
||||
using System.Net.NetworkInformation;
|
||||
using System.Diagnostics;
|
||||
|
||||
namespace BetterJoyForCemu {
|
||||
public class JoyconManager {
|
||||
// Settings accessible via Unity
|
||||
public bool EnableIMU = true;
|
||||
public bool EnableLocalize = true;
|
||||
|
||||
// Different operating systems either do or don't like the trailing zero
|
||||
private const ushort vendor_id = 0x57e;
|
||||
private const ushort vendor_id_ = 0x057e;
|
||||
private const ushort product_l = 0x2006;
|
||||
private const ushort product_r = 0x2007;
|
||||
private const ushort product_pro = 0x2009;
|
||||
|
||||
public List<Joycon> j; // Array of all connected Joy-Cons
|
||||
static JoyconManager instance;
|
||||
|
||||
public static JoyconManager Instance {
|
||||
get { return instance; }
|
||||
}
|
||||
|
||||
public void Awake() {
|
||||
instance = this;
|
||||
int i = 0;
|
||||
|
||||
j = new List<Joycon>();
|
||||
bool isLeft = false;
|
||||
HIDapi.hid_init();
|
||||
|
||||
IntPtr ptr = HIDapi.hid_enumerate(vendor_id, 0x0);
|
||||
IntPtr top_ptr = ptr;
|
||||
|
||||
if (ptr == IntPtr.Zero) {
|
||||
ptr = HIDapi.hid_enumerate(vendor_id_, 0x0);
|
||||
if (ptr == IntPtr.Zero) {
|
||||
HIDapi.hid_free_enumeration(ptr);
|
||||
Console.WriteLine("No Joy-Cons found!");
|
||||
}
|
||||
}
|
||||
|
||||
string path = "";
|
||||
hid_device_info enumerate;
|
||||
while (ptr != IntPtr.Zero) {
|
||||
enumerate = (hid_device_info)Marshal.PtrToStructure(ptr, typeof(hid_device_info));
|
||||
|
||||
if (enumerate.product_id == product_l || enumerate.product_id == product_r || enumerate.product_id == product_pro) {
|
||||
if (enumerate.product_id == product_l) {
|
||||
isLeft = true;
|
||||
Console.WriteLine("Left Joy-Con connected.");
|
||||
} else if (enumerate.product_id == product_r) {
|
||||
isLeft = false;
|
||||
Console.WriteLine("Right Joy-Con connected.");
|
||||
} else if (enumerate.product_id == product_pro) {
|
||||
isLeft = true;
|
||||
Console.WriteLine("Pro controller connected.");
|
||||
} else {
|
||||
Console.WriteLine("Non Joy-Con input device skipped.");
|
||||
}
|
||||
|
||||
IntPtr handle = HIDapi.hid_open_path(enumerate.path);
|
||||
HIDapi.hid_set_nonblocking(handle, 1);
|
||||
j.Add(new Joycon(handle, EnableIMU, EnableLocalize & EnableIMU, 0.05f, isLeft, j.Count, enumerate.product_id == product_pro));
|
||||
++i;
|
||||
}
|
||||
ptr = enumerate.next;
|
||||
}
|
||||
|
||||
HIDapi.hid_free_enumeration(top_ptr);
|
||||
}
|
||||
|
||||
public void Start() {
|
||||
for (int i = 0; i < j.Count; ++i) {
|
||||
Joycon jc = j[i];
|
||||
byte LEDs = 0x0;
|
||||
LEDs |= (byte)(0x1 << i);
|
||||
jc.Attach(leds_: LEDs);
|
||||
jc.Begin();
|
||||
}
|
||||
}
|
||||
|
||||
public bool shouldUpdate = true;
|
||||
public void Update(object sender, ElapsedEventArgs e) {
|
||||
//while (shouldUpdate) {
|
||||
for (int i = 0; i < j.Count; ++i) {
|
||||
j[i].Update();
|
||||
|
||||
/*if (j.Count > 0) {
|
||||
Joycon jj = j[i];
|
||||
|
||||
if (jj.GetButtonDown(Joycon.Button.DPAD_DOWN))
|
||||
jj.SetRumble(160, 320, 0.6f, 200);
|
||||
}*/
|
||||
}
|
||||
//}
|
||||
}
|
||||
|
||||
public void OnApplicationQuit() {
|
||||
for (int i = 0; i < j.Count; ++i) {
|
||||
j[i].Detach();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
class Program {
|
||||
public static UdpServer server;
|
||||
static float pollsPerSecond = 60.0f;
|
||||
|
||||
static void Main(string[] args) {
|
||||
JoyconManager mgr = new JoyconManager();
|
||||
mgr.Awake();
|
||||
mgr.Start();
|
||||
|
||||
server = new UdpServer(mgr.j);
|
||||
|
||||
//updateThread = new Thread(new ThreadStart(mgr.Update));
|
||||
//updateThread.Start();
|
||||
|
||||
server.Start(26760);
|
||||
System.Timers.Timer timer = new System.Timers.Timer((int)(1000 / pollsPerSecond));
|
||||
timer.Elapsed += mgr.Update;
|
||||
timer.Elapsed += printt;
|
||||
timer.Start();
|
||||
|
||||
Console.Write("Press enter to quit.");
|
||||
Console.ReadLine();
|
||||
|
||||
server.Stop();
|
||||
mgr.shouldUpdate = false;
|
||||
timer.Stop();
|
||||
timer.Dispose();
|
||||
mgr.OnApplicationQuit();
|
||||
}
|
||||
|
||||
static void printt(object sender, ElapsedEventArgs e) {
|
||||
//Console.Write('.');
|
||||
}
|
||||
}
|
||||
}
|
||||
36
BetterJoyForCemu/Properties/AssemblyInfo.cs
Normal file
36
BetterJoyForCemu/Properties/AssemblyInfo.cs
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
using System.Reflection;
|
||||
using System.Runtime.CompilerServices;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
// General Information about an assembly is controlled through the following
|
||||
// set of attributes. Change these attribute values to modify the information
|
||||
// associated with an assembly.
|
||||
[assembly: AssemblyTitle("BetterJoyForCemu")]
|
||||
[assembly: AssemblyDescription("")]
|
||||
[assembly: AssemblyConfiguration("")]
|
||||
[assembly: AssemblyCompany("")]
|
||||
[assembly: AssemblyProduct("BetterJoyForCemu")]
|
||||
[assembly: AssemblyCopyright("Copyright © 2018")]
|
||||
[assembly: AssemblyTrademark("")]
|
||||
[assembly: AssemblyCulture("")]
|
||||
|
||||
// Setting ComVisible to false makes the types in this assembly not visible
|
||||
// to COM components. If you need to access a type in this assembly from
|
||||
// COM, set the ComVisible attribute to true on that type.
|
||||
[assembly: ComVisible(false)]
|
||||
|
||||
// The following GUID is for the ID of the typelib if this project is exposed to COM
|
||||
[assembly: Guid("1bf709e9-c133-41df-933a-c9ff3f664c7b")]
|
||||
|
||||
// Version information for an assembly consists of the following four values:
|
||||
//
|
||||
// Major Version
|
||||
// Minor Version
|
||||
// Build Number
|
||||
// Revision
|
||||
//
|
||||
// You can specify all the values or you can default the Build and Revision Numbers
|
||||
// by using the '*' as shown below:
|
||||
// [assembly: AssemblyVersion("1.0.*")]
|
||||
[assembly: AssemblyVersion("1.0.0.0")]
|
||||
[assembly: AssemblyFileVersion("1.0.0.0")]
|
||||
515
BetterJoyForCemu/UpdServer.cs
Normal file
515
BetterJoyForCemu/UpdServer.cs
Normal file
|
|
@ -0,0 +1,515 @@
|
|||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
using Force.Crc32;
|
||||
using System.Net;
|
||||
using System.Net.NetworkInformation;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace BetterJoyForCemu {
|
||||
class UdpServer {
|
||||
private Socket udpSock;
|
||||
private uint serverId;
|
||||
private bool running;
|
||||
private byte[] recvBuffer = new byte[1024];
|
||||
|
||||
List<Joycon> controllers;
|
||||
|
||||
public UdpServer(List<Joycon> p) {
|
||||
controllers = p;
|
||||
}
|
||||
|
||||
enum MessageType {
|
||||
DSUC_VersionReq = 0x100000,
|
||||
DSUS_VersionRsp = 0x100000,
|
||||
DSUC_ListPorts = 0x100001,
|
||||
DSUS_PortInfo = 0x100001,
|
||||
DSUC_PadDataReq = 0x100002,
|
||||
DSUS_PadDataRsp = 0x100002,
|
||||
};
|
||||
|
||||
private const ushort MaxProtocolVersion = 1001;
|
||||
|
||||
class ClientRequestTimes {
|
||||
DateTime allPads;
|
||||
DateTime[] padIds;
|
||||
Dictionary<PhysicalAddress, DateTime> padMacs;
|
||||
|
||||
public DateTime AllPadsTime { get { return allPads; } }
|
||||
public DateTime[] PadIdsTime { get { return padIds; } }
|
||||
public Dictionary<PhysicalAddress, DateTime> PadMacsTime { get { return padMacs; } }
|
||||
|
||||
public ClientRequestTimes() {
|
||||
allPads = DateTime.MinValue;
|
||||
padIds = new DateTime[4];
|
||||
|
||||
for (int i = 0; i < padIds.Length; i++)
|
||||
padIds[i] = DateTime.MinValue;
|
||||
|
||||
padMacs = new Dictionary<PhysicalAddress, DateTime>();
|
||||
}
|
||||
|
||||
public void RequestPadInfo(byte regFlags, byte idToReg, PhysicalAddress macToReg) {
|
||||
if (regFlags == 0)
|
||||
allPads = DateTime.UtcNow;
|
||||
else {
|
||||
if ((regFlags & 0x01) != 0) //id valid
|
||||
{
|
||||
if (idToReg < padIds.Length)
|
||||
padIds[idToReg] = DateTime.UtcNow;
|
||||
}
|
||||
if ((regFlags & 0x02) != 0) //mac valid
|
||||
{
|
||||
padMacs[macToReg] = DateTime.UtcNow;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private Dictionary<IPEndPoint, ClientRequestTimes> clients = new Dictionary<IPEndPoint, ClientRequestTimes>();
|
||||
|
||||
private int BeginPacket(byte[] packetBuf, ushort reqProtocolVersion = MaxProtocolVersion) {
|
||||
int currIdx = 0;
|
||||
packetBuf[currIdx++] = (byte)'D';
|
||||
packetBuf[currIdx++] = (byte)'S';
|
||||
packetBuf[currIdx++] = (byte)'U';
|
||||
packetBuf[currIdx++] = (byte)'S';
|
||||
|
||||
Array.Copy(BitConverter.GetBytes((ushort)reqProtocolVersion), 0, packetBuf, currIdx, 2);
|
||||
currIdx += 2;
|
||||
|
||||
Array.Copy(BitConverter.GetBytes((ushort)packetBuf.Length - 16), 0, packetBuf, currIdx, 2);
|
||||
currIdx += 2;
|
||||
|
||||
Array.Clear(packetBuf, currIdx, 4); //place for crc
|
||||
currIdx += 4;
|
||||
|
||||
Array.Copy(BitConverter.GetBytes((uint)serverId), 0, packetBuf, currIdx, 4);
|
||||
currIdx += 4;
|
||||
|
||||
return currIdx;
|
||||
}
|
||||
|
||||
private void FinishPacket(byte[] packetBuf) {
|
||||
Array.Clear(packetBuf, 8, 4);
|
||||
|
||||
uint crcCalc = Crc32Algorithm.Compute(packetBuf);
|
||||
Array.Copy(BitConverter.GetBytes((uint)crcCalc), 0, packetBuf, 8, 4);
|
||||
}
|
||||
|
||||
private void SendPacket(IPEndPoint clientEP, byte[] usefulData, ushort reqProtocolVersion = MaxProtocolVersion) {
|
||||
byte[] packetData = new byte[usefulData.Length + 16];
|
||||
int currIdx = BeginPacket(packetData, reqProtocolVersion);
|
||||
Array.Copy(usefulData, 0, packetData, currIdx, usefulData.Length);
|
||||
FinishPacket(packetData);
|
||||
|
||||
try { udpSock.SendTo(packetData, clientEP); } catch (Exception e) { }
|
||||
}
|
||||
|
||||
private void ProcessIncoming(byte[] localMsg, IPEndPoint clientEP) {
|
||||
try {
|
||||
int currIdx = 0;
|
||||
if (localMsg[0] != 'D' || localMsg[1] != 'S' || localMsg[2] != 'U' || localMsg[3] != 'C')
|
||||
return;
|
||||
else
|
||||
currIdx += 4;
|
||||
|
||||
uint protocolVer = BitConverter.ToUInt16(localMsg, currIdx);
|
||||
currIdx += 2;
|
||||
|
||||
if (protocolVer > MaxProtocolVersion)
|
||||
return;
|
||||
|
||||
uint packetSize = BitConverter.ToUInt16(localMsg, currIdx);
|
||||
currIdx += 2;
|
||||
|
||||
if (packetSize < 0)
|
||||
return;
|
||||
|
||||
packetSize += 16; //size of header
|
||||
if (packetSize > localMsg.Length)
|
||||
return;
|
||||
else if (packetSize < localMsg.Length) {
|
||||
byte[] newMsg = new byte[packetSize];
|
||||
Array.Copy(localMsg, newMsg, packetSize);
|
||||
localMsg = newMsg;
|
||||
}
|
||||
|
||||
uint crcValue = BitConverter.ToUInt32(localMsg, currIdx);
|
||||
//zero out the crc32 in the packet once we got it since that's whats needed for calculation
|
||||
localMsg[currIdx++] = 0;
|
||||
localMsg[currIdx++] = 0;
|
||||
localMsg[currIdx++] = 0;
|
||||
localMsg[currIdx++] = 0;
|
||||
|
||||
uint crcCalc = Crc32Algorithm.Compute(localMsg);
|
||||
if (crcValue != crcCalc)
|
||||
return;
|
||||
|
||||
uint clientId = BitConverter.ToUInt32(localMsg, currIdx);
|
||||
currIdx += 4;
|
||||
|
||||
uint messageType = BitConverter.ToUInt32(localMsg, currIdx);
|
||||
currIdx += 4;
|
||||
|
||||
if (messageType == (uint)MessageType.DSUC_VersionReq) {
|
||||
byte[] outputData = new byte[8];
|
||||
int outIdx = 0;
|
||||
Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_VersionRsp), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
Array.Copy(BitConverter.GetBytes((ushort)MaxProtocolVersion), 0, outputData, outIdx, 2);
|
||||
outIdx += 2;
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
|
||||
SendPacket(clientEP, outputData, 1001);
|
||||
} else if (messageType == (uint)MessageType.DSUC_ListPorts) {
|
||||
// Requested information on gamepads - return MAC address
|
||||
int numPadRequests = BitConverter.ToInt32(localMsg, currIdx);
|
||||
currIdx += 4;
|
||||
if (numPadRequests < 0 || numPadRequests > 4)
|
||||
return;
|
||||
|
||||
int requestsIdx = currIdx;
|
||||
for (int i = 0; i < numPadRequests; i++) {
|
||||
byte currRequest = localMsg[requestsIdx + i];
|
||||
if (currRequest < 0 || currRequest > 4)
|
||||
return;
|
||||
}
|
||||
|
||||
byte[] outputData = new byte[16];
|
||||
for (byte i = 0; i < numPadRequests; i++) {
|
||||
byte currRequest = localMsg[requestsIdx + i];
|
||||
var padData = controllers[i];//controllers[currRequest];
|
||||
|
||||
int outIdx = 0;
|
||||
Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PortInfo), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
|
||||
outputData[outIdx++] = (byte)padData.PadId;
|
||||
outputData[outIdx++] = (byte)padData.constate;
|
||||
outputData[outIdx++] = (byte)padData.model;
|
||||
outputData[outIdx++] = (byte)padData.connection;
|
||||
|
||||
var addressBytes = padData.PadMacAddress.GetAddressBytes();
|
||||
if (addressBytes.Length == 6) {
|
||||
outputData[outIdx++] = addressBytes[0];
|
||||
outputData[outIdx++] = addressBytes[1];
|
||||
outputData[outIdx++] = addressBytes[2];
|
||||
outputData[outIdx++] = addressBytes[3];
|
||||
outputData[outIdx++] = addressBytes[4];
|
||||
outputData[outIdx++] = addressBytes[5];
|
||||
} else {
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
outputData[outIdx++] = 0;
|
||||
}
|
||||
|
||||
outputData[outIdx++] = (byte)padData.battery;//(byte)padData.BatteryStatus;
|
||||
outputData[outIdx++] = 0;
|
||||
|
||||
SendPacket(clientEP, outputData, 1001);
|
||||
}
|
||||
} else if (messageType == (uint)MessageType.DSUC_PadDataReq) {
|
||||
byte regFlags = localMsg[currIdx++];
|
||||
byte idToReg = localMsg[currIdx++];
|
||||
PhysicalAddress macToReg = null;
|
||||
{
|
||||
byte[] macBytes = new byte[6];
|
||||
Array.Copy(localMsg, currIdx, macBytes, 0, macBytes.Length);
|
||||
currIdx += macBytes.Length;
|
||||
macToReg = new PhysicalAddress(macBytes);
|
||||
}
|
||||
|
||||
lock (clients) {
|
||||
if (clients.ContainsKey(clientEP))
|
||||
clients[clientEP].RequestPadInfo(regFlags, idToReg, macToReg);
|
||||
else {
|
||||
var clientTimes = new ClientRequestTimes();
|
||||
clientTimes.RequestPadInfo(regFlags, idToReg, macToReg);
|
||||
clients[clientEP] = clientTimes;
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (Exception e) { }
|
||||
}
|
||||
|
||||
private void ReceiveCallback(IAsyncResult iar) {
|
||||
byte[] localMsg = null;
|
||||
EndPoint clientEP = new IPEndPoint(IPAddress.Any, 0);
|
||||
|
||||
try {
|
||||
//Get the received message.
|
||||
Socket recvSock = (Socket)iar.AsyncState;
|
||||
int msgLen = recvSock.EndReceiveFrom(iar, ref clientEP);
|
||||
|
||||
localMsg = new byte[msgLen];
|
||||
Array.Copy(recvBuffer, localMsg, msgLen);
|
||||
} catch (Exception e) { }
|
||||
|
||||
//Start another receive as soon as we copied the data
|
||||
StartReceive();
|
||||
|
||||
//Process the data if its valid
|
||||
if (localMsg != null) {
|
||||
ProcessIncoming(localMsg, (IPEndPoint)clientEP);
|
||||
}
|
||||
}
|
||||
private void StartReceive() {
|
||||
try {
|
||||
if (running) {
|
||||
//Start listening for a new message.
|
||||
EndPoint newClientEP = new IPEndPoint(IPAddress.Any, 0);
|
||||
udpSock.BeginReceiveFrom(recvBuffer, 0, recvBuffer.Length, SocketFlags.None, ref newClientEP, ReceiveCallback, udpSock);
|
||||
}
|
||||
} catch (SocketException ex) {
|
||||
uint IOC_IN = 0x80000000;
|
||||
uint IOC_VENDOR = 0x18000000;
|
||||
uint SIO_UDP_CONNRESET = IOC_IN | IOC_VENDOR | 12;
|
||||
udpSock.IOControl((int)SIO_UDP_CONNRESET, new byte[] { Convert.ToByte(false) }, null);
|
||||
|
||||
StartReceive();
|
||||
}
|
||||
}
|
||||
|
||||
public void Start(int port=26760) {
|
||||
if (running) {
|
||||
if (udpSock != null) {
|
||||
udpSock.Close();
|
||||
udpSock = null;
|
||||
}
|
||||
running = false;
|
||||
}
|
||||
|
||||
udpSock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
|
||||
try { udpSock.Bind(new IPEndPoint(IPAddress.Loopback, port)); } catch (SocketException ex) {
|
||||
udpSock.Close();
|
||||
udpSock = null;
|
||||
|
||||
throw ex;
|
||||
}
|
||||
|
||||
byte[] randomBuf = new byte[4];
|
||||
new Random().NextBytes(randomBuf);
|
||||
serverId = BitConverter.ToUInt32(randomBuf, 0);
|
||||
|
||||
running = true;
|
||||
Console.WriteLine("Starting server.");
|
||||
StartReceive();
|
||||
}
|
||||
|
||||
public void Stop() {
|
||||
running = false;
|
||||
if (udpSock != null) {
|
||||
udpSock.Close();
|
||||
udpSock = null;
|
||||
}
|
||||
}
|
||||
|
||||
private bool ReportToBuffer(Joycon hidReport, byte[] outputData, ref int outIdx) {
|
||||
outputData[outIdx] = 0;
|
||||
|
||||
if (hidReport.GetButton(Joycon.Button.DPAD_LEFT)) outputData[outIdx] |= 0x80;
|
||||
if (hidReport.GetButton(Joycon.Button.DPAD_DOWN)) outputData[outIdx] |= 0x40;
|
||||
if (hidReport.GetButton(Joycon.Button.DPAD_RIGHT)) outputData[outIdx] |= 0x20;
|
||||
if (hidReport.GetButton(Joycon.Button.DPAD_UP)) outputData[outIdx] |= 0x10;
|
||||
|
||||
if (hidReport.GetButton(Joycon.Button.PLUS)) outputData[outIdx] |= 0x08;
|
||||
if (hidReport.GetButton(Joycon.Button.STICK2)) outputData[outIdx] |= 0x04;
|
||||
if (hidReport.GetButton(Joycon.Button.STICK)) outputData[outIdx] |= 0x02;
|
||||
if (hidReport.GetButton(Joycon.Button.MINUS)) outputData[outIdx] |= 0x01;
|
||||
|
||||
outputData[++outIdx] = 0;
|
||||
|
||||
if (hidReport.GetButton(Joycon.Button.Y)) outputData[outIdx] |= 0x80;
|
||||
if (hidReport.GetButton(Joycon.Button.B)) outputData[outIdx] |= 0x40;
|
||||
if (hidReport.GetButton(Joycon.Button.A)) outputData[outIdx] |= 0x20;
|
||||
if (hidReport.GetButton(Joycon.Button.X)) outputData[outIdx] |= 0x10;
|
||||
|
||||
if (hidReport.GetButton(Joycon.Button.SHOULDER2_1)) outputData[outIdx] |= 0x08;
|
||||
if (hidReport.GetButton(Joycon.Button.SHOULDER_1)) outputData[outIdx] |= 0x04;
|
||||
if (hidReport.GetButton(Joycon.Button.SHOULDER2_2)) outputData[outIdx] |= 0x02;
|
||||
if (hidReport.GetButton(Joycon.Button.SHOULDER_2)) outputData[outIdx] |= 0x01;
|
||||
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.HOME)) ? (byte)1 : (byte)0;
|
||||
outputData[++outIdx] = 0; // no touch pad
|
||||
|
||||
float[] leftStick = hidReport.GetStick(); // 127 is 0
|
||||
outputData[++outIdx] = (byte) (Math.Max(0, Math.Min(255, 127 + leftStick[0] * 127)));
|
||||
outputData[++outIdx] = (byte) (Math.Max(0, Math.Min(255, 127 + leftStick[1] * 127)));
|
||||
|
||||
float[] rightStick = hidReport.GetStick2(); // 127 is 0
|
||||
outputData[++outIdx] = (byte)(Math.Max(0, Math.Min(255, 127 + rightStick[0] * 127)));
|
||||
outputData[++outIdx] = (byte)(Math.Max(0, Math.Min(255, 127 + rightStick[1] * 127)));
|
||||
|
||||
//we don't have analog buttons so just use the Button enums (which give either 0 or 0xFF)
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_LEFT)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_DOWN)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_RIGHT)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.DPAD_UP)) ? (byte)0xFF : (byte)0;
|
||||
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.Y)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.B)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.A)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.X)) ? (byte)0xFF : (byte)0;
|
||||
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER2_1)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER_1)) ? (byte)0xFF : (byte)0;
|
||||
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER2_2)) ? (byte)0xFF : (byte)0;
|
||||
outputData[++outIdx] = (hidReport.GetButton(Joycon.Button.SHOULDER_2)) ? (byte)0xFF : (byte)0;
|
||||
|
||||
outIdx++;
|
||||
|
||||
//DS4 only: touchpad points
|
||||
for (int i = 0; i < 2; i++) {
|
||||
outIdx += 6;
|
||||
}
|
||||
|
||||
//motion timestamp
|
||||
Array.Copy(BitConverter.GetBytes(hidReport.Timestamp), 0, outputData, outIdx, 8);
|
||||
outIdx += 8;
|
||||
|
||||
//accelerometer
|
||||
{
|
||||
var accel = hidReport.GetAccel();
|
||||
if (accel != null) {
|
||||
Array.Copy(BitConverter.GetBytes(accel.Y), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
Array.Copy(BitConverter.GetBytes(-accel.Z), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
Array.Copy(BitConverter.GetBytes(accel.X), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
} else {
|
||||
outIdx += 12;
|
||||
Console.WriteLine("No accelerometer reported.");
|
||||
}
|
||||
}
|
||||
|
||||
//gyroscope
|
||||
{
|
||||
var gyro = hidReport.GetGyro();
|
||||
if (gyro != null) {
|
||||
Array.Copy(BitConverter.GetBytes(gyro.Y), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
Array.Copy(BitConverter.GetBytes(gyro.Z), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
Array.Copy(BitConverter.GetBytes(gyro.X), 0, outputData, outIdx, 4);
|
||||
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
} else {
|
||||
outIdx += 12;
|
||||
Console.WriteLine("No gyroscope reported.");
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void NewReportIncoming(Joycon hidReport) {
|
||||
if (!running)
|
||||
return;
|
||||
|
||||
//Console.WriteLine("New report!");
|
||||
|
||||
var clientsList = new List<IPEndPoint>();
|
||||
var now = DateTime.UtcNow;
|
||||
lock (clients) {
|
||||
var clientsToDelete = new List<IPEndPoint>();
|
||||
|
||||
foreach (var cl in clients) {
|
||||
const double TimeoutLimit = 5;
|
||||
|
||||
if ((now - cl.Value.AllPadsTime).TotalSeconds < TimeoutLimit)
|
||||
clientsList.Add(cl.Key);
|
||||
else if ((hidReport.PadId >= 0 && hidReport.PadId <= 3) &&
|
||||
(now - cl.Value.PadIdsTime[(byte)hidReport.PadId]).TotalSeconds < TimeoutLimit)
|
||||
clientsList.Add(cl.Key);
|
||||
else if (cl.Value.PadMacsTime.ContainsKey(hidReport.PadMacAddress) &&
|
||||
(now - cl.Value.PadMacsTime[hidReport.PadMacAddress]).TotalSeconds < TimeoutLimit)
|
||||
clientsList.Add(cl.Key);
|
||||
else //check if this client is totally dead, and remove it if so
|
||||
{
|
||||
bool clientOk = false;
|
||||
for (int i = 0; i < cl.Value.PadIdsTime.Length; i++) {
|
||||
var dur = (now - cl.Value.PadIdsTime[i]).TotalSeconds;
|
||||
if (dur < TimeoutLimit) {
|
||||
clientOk = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!clientOk) {
|
||||
foreach (var dict in cl.Value.PadMacsTime) {
|
||||
var dur = (now - dict.Value).TotalSeconds;
|
||||
if (dur < TimeoutLimit) {
|
||||
clientOk = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!clientOk)
|
||||
clientsToDelete.Add(cl.Key);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var delCl in clientsToDelete) {
|
||||
clients.Remove(delCl);
|
||||
}
|
||||
clientsToDelete.Clear();
|
||||
clientsToDelete = null;
|
||||
}
|
||||
|
||||
if (clientsList.Count <= 0)
|
||||
return;
|
||||
|
||||
byte[] outputData = new byte[100];
|
||||
int outIdx = BeginPacket(outputData, 1001);
|
||||
Array.Copy(BitConverter.GetBytes((uint)MessageType.DSUS_PadDataRsp), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
|
||||
outputData[outIdx++] = (byte)hidReport.PadId; //(byte)hidReport.PadId;
|
||||
outputData[outIdx++] = (byte)hidReport.constate; //(byte)hidReport.PadState;
|
||||
outputData[outIdx++] = (byte)hidReport.model; //(byte)hidReport.Model;
|
||||
outputData[outIdx++] = (byte)hidReport.connection; //(byte)hidReport.ConnectionType;
|
||||
{
|
||||
byte[] padMac = hidReport.PadMacAddress.GetAddressBytes();
|
||||
outputData[outIdx++] = padMac[0];
|
||||
outputData[outIdx++] = padMac[1];
|
||||
outputData[outIdx++] = padMac[2];
|
||||
outputData[outIdx++] = padMac[3];
|
||||
outputData[outIdx++] = padMac[4];
|
||||
outputData[outIdx++] = padMac[5];
|
||||
}
|
||||
|
||||
outputData[outIdx++] = (byte)hidReport.battery;//(byte)hidReport.BatteryStatus;
|
||||
outputData[outIdx++] = 1;// hidReport.IsPadActive ? (byte)1 : (byte)0;
|
||||
|
||||
// (uint)hidReport.PacketCounter
|
||||
Array.Copy(BitConverter.GetBytes(hidReport.packetCounter), 0, outputData, outIdx, 4);
|
||||
outIdx += 4;
|
||||
|
||||
if (!ReportToBuffer(hidReport, outputData, ref outIdx))
|
||||
return;
|
||||
else
|
||||
FinishPacket(outputData);
|
||||
|
||||
foreach (var cl in clientsList) {
|
||||
try { udpSock.SendTo(outputData, cl); } catch (SocketException ex) { }
|
||||
}
|
||||
clientsList.Clear();
|
||||
clientsList = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
BetterJoyForCemu/hidapi.dll
Normal file
BIN
BetterJoyForCemu/hidapi.dll
Normal file
Binary file not shown.
4
BetterJoyForCemu/packages.config
Normal file
4
BetterJoyForCemu/packages.config
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<packages>
|
||||
<package id="Crc32.NET" version="1.2.0" targetFramework="net461" />
|
||||
</packages>
|
||||
Loading…
Add table
Reference in a new issue