diff --git a/BetterJoyForCemu/App.config b/BetterJoyForCemu/App.config index 2546c10..bf1a530 100644 --- a/BetterJoyForCemu/App.config +++ b/BetterJoyForCemu/App.config @@ -51,9 +51,9 @@ - - - + + + @@ -89,8 +89,9 @@ - - + + + diff --git a/BetterJoyForCemu/BetterJoy.csproj b/BetterJoyForCemu/BetterJoy.csproj index 5840bd7..e65e77f 100644 --- a/BetterJoyForCemu/BetterJoy.csproj +++ b/BetterJoyForCemu/BetterJoy.csproj @@ -102,8 +102,8 @@ ..\packages\JetBrains.Annotations.2020.1.0\lib\net20\JetBrains.Annotations.dll - - ..\packages\Nefarius.ViGEm.Client.1.16.150\lib\net452\Nefarius.ViGEm.Client.dll + + ..\packages\Nefarius.ViGEm.Client.1.17.175\lib\net452\Nefarius.ViGEm.Client.dll @@ -136,6 +136,7 @@ + Form diff --git a/BetterJoyForCemu/Joycon.cs b/BetterJoyForCemu/Joycon.cs index fe6d054..a6d53b7 100644 --- a/BetterJoyForCemu/Joycon.cs +++ b/BetterJoyForCemu/Joycon.cs @@ -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); } } diff --git a/BetterJoyForCemu/MadgwickAHRS.cs b/BetterJoyForCemu/MadgwickAHRS.cs new file mode 100644 index 0000000..7eb47f5 --- /dev/null +++ b/BetterJoyForCemu/MadgwickAHRS.cs @@ -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 { + /// + /// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms. + /// + /// + /// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + /// + public class MadgwickAHRS { + /// + /// Gets or sets the sample period. + /// + public float SamplePeriod { get; set; } + + /// + /// Gets or sets the algorithm gain beta. + /// + public float Beta { get; set; } + + /// + /// Gets or sets the Quaternion output. + /// + public float[] Quaternion { get; set; } + + public float[] old_pitchYawRoll { get; set; } + + /// + /// Initializes a new instance of the class. + /// + /// + /// Sample period. + /// + public MadgwickAHRS(float samplePeriod) + : this(samplePeriod, 1f) { + } + + /// + /// Initializes a new instance of the class. + /// + /// + /// Sample period. + /// + /// + /// Algorithm gain beta. + /// + public MadgwickAHRS(float samplePeriod, float beta) { + SamplePeriod = samplePeriod; + Beta = beta; + Quaternion = new float[] { 1f, 0f, 0f, 0f }; + old_pitchYawRoll = new float[] { 0f, 0f, 0f }; + } + + /// + /// Algorithm IMU update method. Requires only gyroscope and accelerometer data. + /// + /// + /// Gyroscope x axis measurement in radians/s. + /// + /// + /// Gyroscope y axis measurement in radians/s. + /// + /// + /// Gyroscope z axis measurement in radians/s. + /// + /// + /// Accelerometer x axis measurement in any calibrated units. + /// + /// + /// Accelerometer y axis measurement in any calibrated units. + /// + /// + /// Accelerometer z axis measurement in any calibrated units. + /// + /// + /// Optimised for minimal arithmetic. + /// Total ±: 45 + /// Total *: 85 + /// Total /: 3 + /// Total sqrt: 3 + /// + 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; + } + } +} diff --git a/BetterJoyForCemu/packages.config b/BetterJoyForCemu/packages.config index c46fb29..af913b6 100644 --- a/BetterJoyForCemu/packages.config +++ b/BetterJoyForCemu/packages.config @@ -2,6 +2,6 @@ - + \ No newline at end of file