Skip to content

Commit

Permalink
Merge pull request #3 from rajkosto/master
Browse files Browse the repository at this point in the history
Merge of rajkosto ScpToolkit gyro fixes/improvements
  • Loading branch information
chrizonix authored Mar 6, 2021
2 parents e4127aa + 4e8dc19 commit e9796da
Show file tree
Hide file tree
Showing 21 changed files with 1,867 additions and 58 deletions.
14 changes: 11 additions & 3 deletions Installer/ScpToolkit Setup.aip
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@
<ROW Directory="APPDIR" Directory_Parent="TARGETDIR" DefaultDir="APPDIR:." IsPseudoRoot="1"/>
<ROW Directory="DIFxAPI_1_Dir" Directory_Parent="AI_Bin32_Dir" DefaultDir="DIFxAPI"/>
<ROW Directory="DIFxAPI_Dir" Directory_Parent="APPDIR" DefaultDir="DIFxAPI"/>
<ROW Directory="ds3cal_1_Dir" Directory_Parent="AI_Bin32_Dir" DefaultDir="ds3cal"/>
<ROW Directory="ds3cal_Dir" Directory_Parent="APPDIR" DefaultDir="ds3cal"/>
<ROW Directory="DesktopFolder" Directory_Parent="TARGETDIR" DefaultDir="DESKTO~1|DesktopFolder" IsPseudoRoot="1"/>
<ROW Directory="LilyPad_Dir" Directory_Parent="APPDIR" DefaultDir="LilyPad"/>
<ROW Directory="Logs_Dir" Directory_Parent="APPDIR" DefaultDir="Logs"/>
Expand All @@ -60,6 +62,8 @@
<ROW Directory="amd64_4_Dir" Directory_Parent="XOutput_Dir" DefaultDir="amd64"/>
<ROW Directory="amd64_5_Dir" Directory_Parent="WinUSB_Dir" DefaultDir="amd64"/>
<ROW Directory="amd64_Dir" Directory_Parent="DIFxAPI_Dir" DefaultDir="amd64"/>
<ROW Directory="x64_3_Dir" Directory_Parent="ds3cal_Dir" DefaultDir="x64"/>
<ROW Directory="x86_8_Dir" Directory_Parent="ds3cal_1_Dir" DefaultDir="x86"/>
<ROW Directory="irrKlang_1_Dir" Directory_Parent="AI_Bin32_Dir" DefaultDir="irrKlang"/>
<ROW Directory="irrKlang_Dir" Directory_Parent="APPDIR" DefaultDir="irrKlang"/>
<ROW Directory="libwdi_1_Dir" Directory_Parent="AI_Bin32_Dir" DefaultDir="libwdi"/>
Expand All @@ -81,6 +85,8 @@
<ROW Component="DBreeze.dll" ComponentId="{39FC5E77-200E-422A-B3A5-FF1ED73C69F3}" Directory_="APPDIR" Attributes="0" KeyPath="DBreeze.dll"/>
<ROW Component="DIFxAPI.dll" ComponentId="{3C6052E3-C1BE-4F7F-8471-E876931225FF}" Directory_="amd64_Dir" Attributes="256" Condition="VersionNT64" KeyPath="DIFxAPI.dll"/>
<ROW Component="DIFxAPI.dll_1" ComponentId="{6E89B26B-7691-4879-AA3B-A05041D86EAE}" Directory_="x86_1_Dir" Attributes="0" Condition="NOT VersionNT64" KeyPath="DIFxAPI.dll_1"/>
<ROW Component="ds3cal.dll" ComponentId="{75A6123B-7687-4992-A01A-AB27FE8D22B8}" Directory_="x64_3_Dir" Attributes="256" Condition="VersionNT64" KeyPath="ds3cal.dll"/>
<ROW Component="ds3cal.dll_1" ComponentId="{DAC3E78D-FB27-49F8-AC39-975885A667CE}" Directory_="x86_8_Dir" Attributes="0" Condition="NOT VersionNT64" KeyPath="ds3cal.dll_1"/>
<ROW Component="Hardcodet.Wpf.TaskbarNotification.dll" ComponentId="{D9794138-F802-4EC8-800B-EF767715FE7C}" Directory_="APPDIR" Attributes="0" KeyPath="Hardcodet.Wpf.TaskbarNotification.dll"/>
<ROW Component="Hardcodet.Wpf.TaskbarNotification.pdb" ComponentId="{CAA7AC0E-E01B-4008-B65F-BAAFAD09EB91}" Directory_="APPDIR" Attributes="0" KeyPath="Hardcodet.Wpf.TaskbarNotification.pdb" Type="4"/>
<ROW Component="HidSharp.pdb" ComponentId="{D6B65719-2E80-49F4-9F60-15FEF1BADD6F}" Directory_="APPDIR" Attributes="0" KeyPath="HidSharp.pdb" Type="4"/>
Expand Down Expand Up @@ -175,7 +181,7 @@
<COMPONENT cid="caphyon.advinst.msicomp.MsiFeatsComponent">
<ROW Feature="AI32BitFiles" Title="32-bit" Description="32-bit Executables and Libraries" Display="0" Level="1" Directory_="APPDIR" Attributes="16" Components="AI_ExePath"/>
<ROW Feature="AI64BitFiles" Title="64-bit" Description="64-bit Executables and Libraries" Display="0" Level="1" Directory_="APPDIR" Attributes="16" Components="AI_ExePath"/>
<ROW Feature="AIOtherFiles" Title="ScpToolkit Core Features" Description="Shared Resource and Regular Files" Display="3" Level="1" Directory_="APPDIR" Attributes="16" Components="AI_ExePath CSScriptLibrary.dll CsvHelper.dll DBreeze.dll DIFxAPI.dll DIFxAPI.dll_1 Hardcodet.Wpf.TaskbarNotification.dll LilyPad LilyPadScpr5875.dll Logs Mono.CSharp.dll Newtonsoft.Json.dll Path SHORTCUTDIR ScpControl.dll.config ScpControl.ini ScpDriverInstaller.exe ScpDriverInstaller.exe.config ScpService.exe ScpService.exe.config ScpSettings.exe ScpSettings.exe.config ScpUpdater.exe ScpVBus.inf ScpVBus.inf_1 ScpVBus.sys ScpVBus.sys_1 Trinet.Core.IO.Ntfs.dll WdfCoInstaller01011.dll WdfCoInstaller01011.dll_1 WdfCoinstaller01009.dll WdfCoinstaller01009.dll_1 WindowsInput.dll XInput XInput1_3.dll XInput1_3.dll_1 XInput1_3.exp XInput_log4net.config XOutput1_1.dll XOutput1_1.dll_1 Xceed.Wpf.Toolkit.dll bluetoothhost.cat connect.wav disconnect.flac ikpFlac.dll ikpFlac.dll_1 ikpMP3.dll ikpMP3.dll_1 irrKlang.NET4.dll irrKlang.NET4.dll_1 libwdi.dll libwdi.dll_1 scpvbus.cat scpvbus.cat_1 startup.ogg wdisimple.exe wdisimple.exe_1 winusbcoinstaller2.dll winusbcoinstaller2.dll_1 x64 x86"/>
<ROW Feature="AIOtherFiles" Title="ScpToolkit Core Features" Description="Shared Resource and Regular Files" Display="3" Level="1" Directory_="APPDIR" Attributes="16" Components="AI_ExePath CSScriptLibrary.dll CsvHelper.dll DBreeze.dll DIFxAPI.dll DIFxAPI.dll_1 ds3cal.dll ds3cal.dll_1 Hardcodet.Wpf.TaskbarNotification.dll LilyPad LilyPadScpr5875.dll Logs Mono.CSharp.dll Newtonsoft.Json.dll Path SHORTCUTDIR ScpControl.dll.config ScpControl.ini ScpDriverInstaller.exe ScpDriverInstaller.exe.config ScpService.exe ScpService.exe.config ScpSettings.exe ScpSettings.exe.config ScpUpdater.exe ScpVBus.inf ScpVBus.inf_1 ScpVBus.sys ScpVBus.sys_1 Trinet.Core.IO.Ntfs.dll WdfCoInstaller01011.dll WdfCoInstaller01011.dll_1 WdfCoinstaller01009.dll WdfCoinstaller01009.dll_1 WindowsInput.dll XInput XInput1_3.dll XInput1_3.dll_1 XInput1_3.exp XInput_log4net.config XOutput1_1.dll XOutput1_1.dll_1 Xceed.Wpf.Toolkit.dll bluetoothhost.cat connect.wav disconnect.flac ikpFlac.dll ikpFlac.dll_1 ikpMP3.dll ikpMP3.dll_1 irrKlang.NET4.dll irrKlang.NET4.dll_1 libwdi.dll libwdi.dll_1 scpvbus.cat scpvbus.cat_1 startup.ogg wdisimple.exe wdisimple.exe_1 winusbcoinstaller2.dll winusbcoinstaller2.dll_1 x64 x86"/>
<ROW Feature="B63_41B8_9CB7_8C5E07C0FC18_" Title="Visual C++ 2010 SP1 x64 (MFC Security Update)" Description="Visual C++ 2010 SP1 x64 (MFC Security Update)" Display="0" Level="1" Attributes="16" Components="AI_ExePath"/>
<ROW Feature="CA62D813A4E74FA2AAE86A7D7B7B1493" Title="Visual C++ Redistributable for Visual Studio 2013 x64" Description="Visual C++ Redistributable for Visual Studio 2013 x64" Display="0" Level="1" Attributes="16" Components="AI_ExePath"/>
<ROW Feature="CB585686_DAA3_4384_BDDF_08ABA631E9F3_" Title="Visual C++ 2010 SP1 x86 (MFC Security Update)" Description="Visual C++ 2010 SP1 x86 (MFC Security Update)" Display="0" Level="1" Attributes="16" Components="AI_ExePath"/>
Expand All @@ -202,8 +208,10 @@
<ROW File="CsvHelper.dll" Component_="CsvHelper.dll" FileName="CSVHEL~1.DLL|CsvHelper.dll" Attributes="0" SourcePath="..\..\bin\CsvHelper.dll" SelfReg="false" NextFile="CSScriptLibrary.dll"/>
<ROW File="CsvHelper.pdb" Component_="CsvHelper.pdb" FileName="CSVHEL~1.PDB|CsvHelper.pdb" Attributes="0" SourcePath="..\..\bin\CsvHelper.pdb" SelfReg="false" NextFile="Hardcodet.Wpf.TaskbarNotification.pdb"/>
<ROW File="DBreeze.dll" Component_="DBreeze.dll" FileName="DBreeze.dll" Attributes="0" SourcePath="..\..\bin\DBreeze.dll" SelfReg="false" NextFile="Newtonsoft.Json.dll"/>
<ROW File="DIFxAPI.dll" Component_="DIFxAPI.dll" FileName="DIFxAPI.dll" Attributes="0" SourcePath="..\..\bin\DIFxAPI\amd64\DIFxAPI.dll" SelfReg="false" NextFile="ikpFlac.dll"/>
<ROW File="DIFxAPI.dll_1" Component_="DIFxAPI.dll_1" FileName="DIFxAPI.dll" Attributes="0" SourcePath="..\..\bin\DIFxAPI\x86\DIFxAPI.dll" SelfReg="false" NextFile="ikpFlac.dll_1"/>
<ROW File="DIFxAPI.dll" Component_="DIFxAPI.dll" FileName="DIFxAPI.dll" Attributes="0" SourcePath="..\..\bin\DIFxAPI\amd64\DIFxAPI.dll" SelfReg="false" NextFile="ds3cal.dll"/>
<ROW File="ds3cal.dll" Component_="ds3cal.dll" FileName="ds3cal.dll" Attributes="0" SourcePath="..\..\bin\ds3cal\x64\DIFxAPI.dll" SelfReg="false" NextFile="ikpFlac.dll"/>
<ROW File="DIFxAPI.dll_1" Component_="DIFxAPI.dll_1" FileName="DIFxAPI.dll" Attributes="0" SourcePath="..\..\bin\DIFxAPI\x86\DIFxAPI.dll" SelfReg="false" NextFile="ds3cal.dll_1"/>
<ROW File="ds3cal.dll_1" Component_="ds3cal.dll_1" FileName="ds3cal.dll" Attributes="0" SourcePath="..\..\bin\ds3cal\x86\ds3cal.dll" SelfReg="false" NextFile="ikpFlac.dll_1"/>
<ROW File="Ds3Controller.inf" Component_="bluetoothhost.cat" FileName="DS3CON~1.INF|Ds3Controller.inf" Attributes="0" SourcePath="..\..\bin\WinUSB\Ds3Controller.inf" SelfReg="false" NextFile="Ds4Controller.inf"/>
<ROW File="Ds4Controller.inf" Component_="bluetoothhost.cat" FileName="DS4CON~1.INF|Ds4Controller.inf" Attributes="0" SourcePath="..\..\bin\WinUSB\Ds4Controller.inf" SelfReg="false"/>
<ROW File="Hardcodet.Wpf.TaskbarNotification.dll" Component_="Hardcodet.Wpf.TaskbarNotification.dll" FileName="HARDCO~1.DLL|Hardcodet.Wpf.TaskbarNotification.dll" Attributes="0" SourcePath="..\..\bin\Hardcodet.Wpf.TaskbarNotification.dll" SelfReg="false" NextFile="XInput_log4net.config"/>
Expand Down
12 changes: 6 additions & 6 deletions ScpControl.Shared/Core/DualShockMotion.cs
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,15 @@
{
public class DsAccelerometer
{
public short X { get; set; }
public short Y { get; set; }
public short Z { get; set; }
public float X { get; set; }
public float Y { get; set; }
public float Z { get; set; }
}

public class DsGyroscope
{
public short Roll { get; set; }
public short Yaw { get; set; }
public short Pitch { get; set; }
public float Pitch { get; set; }
public float Yaw { get; set; }
public float Roll { get; set; }
}
}
135 changes: 119 additions & 16 deletions ScpControl.Shared/Core/ScpHidReport.cs
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
using System;
using ScpControl.Shared.Utilities;
using System;
using System.Linq;
using System.Net.NetworkInformation;
using System.Reflection;
Expand Down Expand Up @@ -125,7 +126,7 @@ public PhysicalAddress PadMacAddress
set
{
if (value != null)
Buffer.BlockCopy(value.GetAddressBytes(), 0, RawBytes, 90, 6);
Buffer.BlockCopy(value.GetAddressBytes(), 0, RawBytes, RawBytes.Length - 6, 6);
}
}

Expand Down Expand Up @@ -209,24 +210,110 @@ public bool IsPadActive
}
}

public long Timestamp //in us
{
get
{
int lastUsedIdx = RawBytes.Length - 6 - 2; //model and index use 2 more
int currIdx = lastUsedIdx;
ulong retVal = 0;
switch (Model)
{
case DsModel.DS3: //have enough free space in the 96-6 byte report (only 8+49 used) to just store it as 8 bytes
{
retVal |= ((ulong)RawBytes[--currIdx]) << 56;
retVal |= ((ulong)RawBytes[--currIdx]) << 48;
retVal |= ((ulong)RawBytes[--currIdx]) << 40;
retVal |= ((ulong)RawBytes[--currIdx]) << 32;
retVal |= ((ulong)RawBytes[--currIdx]) << 24;
retVal |= ((ulong)RawBytes[--currIdx]) << 16;
retVal |= ((ulong)RawBytes[--currIdx]) << 8;
retVal |= ((ulong)RawBytes[--currIdx]) << 0;
} break;
case DsModel.DS4: //only have 4 bytes left after pad data (76+8 used) so reuse the 2 original timestamp bytes as LSB
{
retVal |= ((ulong)RawBytes[--currIdx]) << 40;
retVal |= ((ulong)RawBytes[--currIdx]) << 32;
retVal |= ((ulong)RawBytes[--currIdx]) << 24;
retVal |= ((ulong)RawBytes[--currIdx]) << 16;

currIdx = (10 + 8) + 2; //just after the original timestamp
retVal |= ((ulong)RawBytes[--currIdx]) << 8;
retVal |= ((ulong)RawBytes[--currIdx]) << 0;
} break;
}

return (long)retVal;
}
set
{
ulong val = (ulong)value;
int lastUsedIdx = RawBytes.Length - 6 - 2;
int currIdx = lastUsedIdx;
switch (Model)
{
case DsModel.DS3:
{
RawBytes[--currIdx] = (byte)(val >> 56);
RawBytes[--currIdx] = (byte)(val >> 48);
RawBytes[--currIdx] = (byte)(val >> 40);
RawBytes[--currIdx] = (byte)(val >> 32);
RawBytes[--currIdx] = (byte)(val >> 24);
RawBytes[--currIdx] = (byte)(val >> 16);
RawBytes[--currIdx] = (byte)(val >> 8);
RawBytes[--currIdx] = (byte)(val >> 0);
} break;
case DsModel.DS4:
{
RawBytes[--currIdx] = (byte)(val >> 40);
RawBytes[--currIdx] = (byte)(val >> 32);
RawBytes[--currIdx] = (byte)(val >> 24);
RawBytes[--currIdx] = (byte)(val >> 16);

currIdx = (10 + 8) + 2;
RawBytes[--currIdx] = (byte)(val >> 8);
RawBytes[--currIdx] = (byte)(val >> 0);
} break;
}
}
}

/// <summary>
/// Gets the motion data from the DualShock accelerometer sensor.
/// Gets the linear acceleration data from the DualShock accelerometer sensor.
/// </summary>
/// <remarks>https://github.com/ehd/node-ds4/blob/master/index.js</remarks>
public DsAccelerometer Motion
/// <remarks>
/// http://eleccelerator.com/wiki/index.php?title=DualShock_3 (off by one and mistaken endianness)
/// https://github.com/RPCS3/rpcs3/blob/master/rpcs3/ds4_pad_handler.cpp
/// </remarks>
public DsAccelerometer Accelerometer
{
get
{
short intX, intY, intZ;

switch (Model)
{
case DsModel.DS3:
throw new NotImplementedException("DualShock 3 accelerometer readout not implemented yet.");
intX = (short)(1023 - (((ushort)RawBytes[41 + 8] << 8) | (ushort)RawBytes[42 + 8])); //negated
intZ = (short) (((ushort)RawBytes[43 + 8] << 8) | (ushort)RawBytes[44 + 8]);
intY = (short) (((ushort)RawBytes[45 + 8] << 8) | (ushort)RawBytes[46 + 8]); //Y is gravity, which appears here

return new DsAccelerometer
{
X = (float)(intX - 512) / 113.0f,
Y = (float)(intY - 512) / 113.0f,
Z = (float)(intZ - 512) / 113.0f
};
case DsModel.DS4:
intX = (short)-(((ushort)RawBytes[20 + 8] << 8) | (ushort)RawBytes[19 + 8]);
intY = (short)-(((ushort)RawBytes[22 + 8] << 8) | (ushort)RawBytes[21 + 8]);
intZ = (short)-(((ushort)RawBytes[24 + 8] << 8) | (ushort)RawBytes[23 + 8]);

return new DsAccelerometer
{
Y = (short) ((RawBytes[22] << 8) | RawBytes[21]),
X = (short) -((RawBytes[24] << 8) | RawBytes[23]),
Z = (short) -((RawBytes[26] << 8) | RawBytes[25])
X = (float)(intX) / 8192.0f,
Y = (float)(intY) / 8192.0f,
Z = (float)(intZ) / 8192.0f
};
}

Expand All @@ -235,23 +322,39 @@ public DsAccelerometer Motion
}

/// <summary>
/// Gets the orientation data from the DualShock gyroscope sensor.
/// Gets the angular velocity data from the DualShock gyroscope sensor.
/// </summary>
/// <remarks>https://github.com/ehd/node-ds4/blob/master/index.js</remarks>
public DsGyroscope Orientation
/// <remarks>
/// http://eleccelerator.com/wiki/index.php?title=DualShock_3 (off by one and mistaken endianness)
/// https://github.com/RPCS3/rpcs3/blob/master/rpcs3/ds4_pad_handler.cpp
/// </remarks>
public DsGyroscope Gyroscope
{
get
{
short intPitch, intYaw, intRoll;

switch (Model)
{
case DsModel.DS3:
throw new NotImplementedException("DualShock 3 gyroscope readout not implemented yet.");
intYaw = (short)(((ushort)RawBytes[47 + 8] << 8) | (ushort)RawBytes[48 + 8]);

return new DsGyroscope
{
Pitch = Single.NaN,
Yaw = (float)(intYaw - 512) * (90.0f/123.0f),
Roll = Single.NaN
};
case DsModel.DS4:
intPitch = (short) (((ushort)RawBytes[14 + 8] << 8) | (ushort)RawBytes[13 + 8]);
intYaw = (short)-(((ushort)RawBytes[16 + 8] << 8) | (ushort)RawBytes[15 + 8]);
intRoll = (short)-(((ushort)RawBytes[18 + 8] << 8) | (ushort)RawBytes[17 + 8]);

return new DsGyroscope
{
Roll = (short) -((RawBytes[28] << 8) | RawBytes[27]),
Yaw = (short) ((RawBytes[30] << 8) | RawBytes[29]),
Pitch = (short) ((RawBytes[32] << 8) | RawBytes[31])
Pitch = (float)(intPitch) / 16.0f,
Yaw = (float)(intYaw) / 16.0f,
Roll = (float)(intRoll) / 16.0f
};
}

Expand Down
2 changes: 2 additions & 0 deletions ScpControl.Shared/ScpControl.Shared.csproj
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@
<Compile Include="Core\ScpHidReport.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="Properties\CommonInfo.cs" />
<Compile Include="Utilities\AccurateTime.cs" />
<Compile Include="Utilities\Crc32.cs" />
<Compile Include="Utilities\DsMath.cs" />
<Compile Include="Win32\Kernel32Natives.cs" />
<Compile Include="XInput\ScpXInputExtensions.cs" />
Expand Down
58 changes: 58 additions & 0 deletions ScpControl.Shared/Utilities/AccurateTime.cs
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
using System;
using System.Collections.Generic;
using ScpControl.Shared.Win32;
using System.ComponentModel;

namespace ScpControl.Shared.Utilities
{
public class AccurateTime
{
private long value;
static private double scale;

static AccurateTime() //figure out QPF only once
{
long currentQpf = 0;
if (!Kernel32Natives.QueryPerformanceFrequency(out currentQpf))
throw new Win32Exception();

scale = 1.0 / (double)currentQpf;
}

public AccurateTime(long timeSpan)
{
value = timeSpan;
}

static public AccurateTime Now
{
get
{
long currentQpc;
if (!Kernel32Natives.QueryPerformanceCounter(out currentQpc))
throw new Win32Exception();

return new AccurateTime(currentQpc);
}
}

public static AccurateTime operator+(AccurateTime startTime, AccurateTime span)
{
return new AccurateTime(startTime.value + span.value);
}
public static AccurateTime operator-(AccurateTime startTime, AccurateTime span)
{
return new AccurateTime(startTime.value - span.value);
}

public double ToSeconds()
{
return (double)(value) * scale;
}

public override string ToString()
{
return ToSeconds().ToString();
}
}
}
Loading

0 comments on commit e9796da

Please sign in to comment.