empathicqubit/BetterJoy
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

- Significantly improved gyro-to-mouse accuracy/stability by using a DCM filter. (https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/)

Browse source 
- Gyro Analogue slider accuracy improved as well
This commit is contained in:
David Khachaturov 2021-02-14 14:52:43 +01:00
parent 8c591fd2f2
commit b8de5003a4
5 changed files with 189 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
BetterJoyForCemu/App.config
View file

@ -51,9 +51,9 @@
<!--Works on pro controller and joined joycons (pro controller case - triggers combined, joycons case - separate tilt)-->
<!--Default: false -->
<add key="GyroAnalogSliders" value="false" />
<!--Change to -20 to change direction of tilt needed. Positive is ramp up if pointing up-->
<!--Default: 20 -->
<add key="GyroAnalogSensitivity" value="20" />
<!--Change to -400 to change direction of tilt needed. Positive is ramp up if pointing up-->
<!--Default: 400 -->
<add key="GyroAnalogSensitivity" value="400" />
<!-- Determines whether or not the program should purge the affected devices list upon exit -->
<!-- Should prevent any more issues of the controller being unusable after the program (even though this can be fixed if you read the README) -->
@ -89,8 +89,9 @@
<!-- Default: none -->
<add key="GyroToJoyOrMouse" value="none"/>
<!-- Sensitivity of gyro-to-mouse movements -->
<!-- Default: 50 -->
<add key="GyroMouseSensitivity" value="50"/>
<!-- Default: 1200; 800 -->
<add key="GyroMouseSensitivityX" value="1200"/>
<add key="GyroMouseSensitivityY" value="800"/>
<!-- Gyro Hold/Toggle activation -->
<!-- Default: true [hold], false [toggle] -->
<add key="GyroHoldToggle" value="true"/>

5
BetterJoyForCemu/BetterJoy.csproj
View file

@ -102,8 +102,8 @@
<Reference Include="JetBrains.Annotations, Version=2020.1.0.0, Culture=neutral, PublicKeyToken=1010a0d8d6380325, processorArchitecture=MSIL">
<HintPath>..\packages\JetBrains.Annotations.2020.1.0\lib\net20\JetBrains.Annotations.dll</HintPath>
</Reference>
<Reference Include="Nefarius.ViGEm.Client, Version=1.16.150.0, Culture=neutral, processorArchitecture=MSIL">
<HintPath>..\packages\Nefarius.ViGEm.Client.1.16.150\lib\net452\Nefarius.ViGEm.Client.dll</HintPath>
<Reference Include="Nefarius.ViGEm.Client, Version=1.17.175.0, Culture=neutral, processorArchitecture=MSIL">
<HintPath>..\packages\Nefarius.ViGEm.Client.1.17.175\lib\net452\Nefarius.ViGEm.Client.dll</HintPath>
</Reference>
<Reference Include="System" />
<Reference Include="System.Configuration" />
@ -136,6 +136,7 @@
<Compile Include="Controller\OutputControllerXbox360.cs" />
<Compile Include="HIDapi.cs" />
<Compile Include="Joycon.cs" />
<Compile Include="MadgwickAHRS.cs" />
<Compile Include="MainForm.cs">
<SubType>Form</SubType>
</Compile>

29
BetterJoyForCemu/Joycon.cs
View file

@ -123,6 +123,8 @@ namespace BetterJoyForCemu {
private Int16[] gyr_sensiti = { 0, 0, 0 };
private Vector3 gyr_g;
private float[] cur_rotation; // Filtered IMU data
private short[] acc_sen = new short[3]{
16384,
16384,
@ -278,6 +280,7 @@ namespace BetterJoyForCemu {
bool thirdParty = false;
private float[] activeData;
private MadgwickAHRS AHRS = new MadgwickAHRS(0.005f, 0.01f); // for getting filtered Euler angles of rotation; 5ms sampling rate
public Joycon(IntPtr handle_, bool imu, bool localize, float alpha, bool left, string path, string serialNum, int id = 0, bool isPro = false, bool isSnes = false, bool thirdParty = false) {
serial_number = serialNum;
@ -425,8 +428,6 @@ namespace BetterJoyForCemu {
Subcommand(0x40, new byte[] { (imu_enabled ? (byte)0x1 : (byte)0x0) }, 1);
Subcommand(0x48, new byte[] { 0x01 }, 1);
Subcommand(0x41, new byte[] { 0x03, 0x00, 0x00, 0x01 }, 4); // higher gyro performance rate
Subcommand(0x3, new byte[] { 0x30 }, 1);
DebugPrint("Done with init.", DebugType.COMMS);
@ -519,7 +520,7 @@ namespace BetterJoyForCemu {
if (state > state_.NO_JOYCONS) {
HIDapi.hid_set_nonblocking(handle, 0);
Subcommand(0x40, new byte[] { 0x0 }, 1); // disable IMU sensor
// Subcommand(0x40, new byte[] { 0x0 }, 1); // disable IMU sensor
//Subcommand(0x48, new byte[] { 0x0 }, 1); // Would turn off rumble?
if (isUSB) {
@ -679,7 +680,8 @@ namespace BetterJoyForCemu {
long PowerOffInactivityMins = Int32.Parse(ConfigurationManager.AppSettings["PowerOffInactivity"]);
string extraGyroFeature = ConfigurationManager.AppSettings["GyroToJoyOrMouse"];
int GyroMouseSensitivity = Int32.Parse(ConfigurationManager.AppSettings["GyroMouseSensitivity"]);
int GyroMouseSensitivityX = Int32.Parse(ConfigurationManager.AppSettings["GyroMouseSensitivityX"]);
int GyroMouseSensitivityY = Int32.Parse(ConfigurationManager.AppSettings["GyroMouseSensitivityY"]);
bool GyroHoldToggle = Boolean.Parse(ConfigurationManager.AppSettings["GyroHoldToggle"]);
bool GyroAnalogSliders = Boolean.Parse(ConfigurationManager.AppSettings["GyroAnalogSliders"]);
int GyroAnalogSensitivity = Int32.Parse(ConfigurationManager.AppSettings["GyroAnalogSensitivity"]);
@ -743,13 +745,15 @@ namespace BetterJoyForCemu {
SimulateContinous((int)Button.SR, Config.Value("sr_r"));
}
// Filtered IMU data
this.cur_rotation = AHRS.GetEulerAngles();
if (GyroAnalogSliders && (other != null || isPro)) {
Button leftT = isLeft ? Button.SHOULDER_2 : Button.SHOULDER2_2;
Button rightT = isLeft ? Button.SHOULDER2_2 : Button.SHOULDER_2;
float dt = 0.015f; // 15ms
Joycon left = isLeft ? this : (isPro ? this : this.other); Joycon right = !isLeft ? this : (isPro ? this : this.other);
int ldy = (int)(GyroAnalogSensitivity * (left.gyr_g.Y * dt) * (Math.Abs(left.gyr_g.Y) < 1 ? 0 : 1));
int rdy = (int)(GyroAnalogSensitivity * (right.gyr_g.Y * dt) * (Math.Abs(right.gyr_g.Y) < 1 ? 0 : 1));
int ldy = (int)(GyroAnalogSensitivity * (left.cur_rotation[0] - left.cur_rotation[3]));
int rdy = (int)(GyroAnalogSensitivity * (right.cur_rotation[0] - right.cur_rotation[3]));
if (buttons[(int)leftT]) {
sliderVal[0] = (byte)Math.Min(Byte.MaxValue, Math.Max(0, (int)sliderVal[0] + ldy));
@ -782,12 +786,10 @@ namespace BetterJoyForCemu {
}
}
float dt = 0.015f; // 15ms
// gyro data is in degrees/s
if (Config.Value("active_gyro") == "0" || active_gyro) {
int dx = (int)(GyroMouseSensitivity * (gyr_g.Z * dt) * (Math.Abs(gyr_g.Z) < 1 ? 0 : 1));
int dy = (int)-(GyroMouseSensitivity * (gyr_g.Y * dt) * (Math.Abs(gyr_g.Y) < 1 ? 0 : 1));
int dx = (int)(GyroMouseSensitivityX * (cur_rotation[1] - cur_rotation[4])); // yaw
int dy = (int)-(GyroMouseSensitivityY * (cur_rotation[0] - cur_rotation[3])); // pitch
WindowsInput.Simulate.Events().MoveBy(dx, dy).Invoke();
}
@ -1020,7 +1022,6 @@ namespace BetterJoyForCemu {
}
}
if (other == null && !isPro) { // single joycon mode; Z do not swap, rest do
if (isLeft) {
acc_g.X = -acc_g.X;
@ -1038,6 +1039,10 @@ namespace BetterJoyForCemu {
gyr_g.X = gyr_g.Y;
gyr_g.Y = temp;
}
// Update rotation Quaternion
float deg_to_rad = 0.0174533f;
AHRS.Update(gyr_g.X * deg_to_rad, gyr_g.Y * deg_to_rad, gyr_g.Z * deg_to_rad, acc_g.X, acc_g.Y, acc_g.Z);
}
}

162
BetterJoyForCemu/MadgwickAHRS.cs Normal file
View file

@ -0,0 +1,162 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
// source: https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MadgwickAHRS.cs
namespace BetterJoyForCemu {
/// <summary>
/// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms.
/// </summary>
/// <remarks>
/// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
/// </remarks>
public class MadgwickAHRS {
/// <summary>
/// Gets or sets the sample period.
/// </summary>
public float SamplePeriod { get; set; }
/// <summary>
/// Gets or sets the algorithm gain beta.
/// </summary>
public float Beta { get; set; }
/// <summary>
/// Gets or sets the Quaternion output.
/// </summary>
public float[] Quaternion { get; set; }
public float[] old_pitchYawRoll { get; set; }
/// <summary>
/// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
public MadgwickAHRS(float samplePeriod)
: this(samplePeriod, 1f) {
}
/// <summary>
/// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
/// <param name="beta">
/// Algorithm gain beta.
/// </param>
public MadgwickAHRS(float samplePeriod, float beta) {
SamplePeriod = samplePeriod;
Beta = beta;
Quaternion = new float[] { 1f, 0f, 0f, 0f };
old_pitchYawRoll = new float[] { 0f, 0f, 0f };
}
/// <summary>
/// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
/// </summary>
/// <param name="gx">
/// Gyroscope x axis measurement in radians/s.
/// </param>
/// <param name="gy">
/// Gyroscope y axis measurement in radians/s.
/// </param>
/// <param name="gz">
/// Gyroscope z axis measurement in radians/s.
/// </param>
/// <param name="ax">
/// Accelerometer x axis measurement in any calibrated units.
/// </param>
/// <param name="ay">
/// Accelerometer y axis measurement in any calibrated units.
/// </param>
/// <param name="az">
/// Accelerometer z axis measurement in any calibrated units.
/// </param>
/// <remarks>
/// Optimised for minimal arithmetic.
/// Total ±: 45
/// Total *: 85
/// Total /: 3
/// Total sqrt: 3
/// </remarks>
public void Update(float gx, float gy, float gz, float ax, float ay, float az) {
float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3]; // short name local variable for readability
float norm;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1 = 2f * q1;
float _2q2 = 2f * q2;
float _2q3 = 2f * q3;
float _2q4 = 2f * q4;
float _4q1 = 4f * q1;
float _4q2 = 4f * q2;
float _4q3 = 4f * q3;
float _8q2 = 8f * q2;
float _8q3 = 8f * q3;
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0f) return; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;
norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * SamplePeriod;
q2 += qDot2 * SamplePeriod;
q3 += qDot3 * SamplePeriod;
q4 += qDot4 * SamplePeriod;
norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
Quaternion[0] = q1 * norm;
Quaternion[1] = q2 * norm;
Quaternion[2] = q3 * norm;
Quaternion[3] = q4 * norm;
}
public float[] GetEulerAngles() {
float[] pitchYawRoll = new float[3];
float q0 = Quaternion[0], q1 = Quaternion[1], q2 = Quaternion[2], q3 = Quaternion[3];
float sq1 = q1 * q1, sq2 = q2 * q2, sq3 = q3 * q3;
pitchYawRoll[0] = (float)Math.Asin(2f * (q0 * q2 - q3 * q1)); // Pitch
pitchYawRoll[1] = (float)Math.Atan2(2f * (q0 * q3 + q1 * q2), 1 - 2f * (sq2 + sq3)); // Yaw
pitchYawRoll[2] = (float)Math.Atan2(2f * (q0 * q1 + q2 * q3), 1 - 2f * (sq1 + sq2)); // Roll
float[] returnAngles = new float[6];
Array.Copy(pitchYawRoll, returnAngles, 3);
Array.Copy(old_pitchYawRoll, 0, returnAngles, 3, 3);
old_pitchYawRoll = pitchYawRoll;
return returnAngles;
}
}
}

2
BetterJoyForCemu/packages.config
View file

@ -2,6 +2,6 @@
<packages>
<package id="Crc32.NET" version="1.2.0" targetFramework="net461" />
<package id="JetBrains.Annotations" version="2020.1.0" targetFramework="net461" />
<package id="Nefarius.ViGEm.Client" version="1.16.150" targetFramework="net461" />
<package id="Nefarius.ViGEm.Client" version="1.17.175" targetFramework="net461" />
<package id="WindowsInput" version="6.1.0" targetFramework="net461" />
</packages>