Skip to content

Commit 26714e7

Browse files
DavidobotDavidobot
committed
Fixed shaky gyro scope.
 - Implemented correct timestamps (microseconds instead of system ticks) - Cleaned up some code
1 parent c8bfdb2 commit 26714e7

3 files changed

Lines changed: 101 additions & 49 deletions

File tree

‎BetterJoyForCemu/Joycon.cs‎

Lines changed: 50 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -93,17 +93,24 @@ public enum Button : int {
9393
private Int16[] gyr_neutral = { 0, 0, 0 };
9494
private Int16[] gyr_sensiti = { 0, 0, 0 };
9595
private Vector3 gyr_g;
96+
97+
private Int16[] pro_hor_offset = { -710, 0, 0 };
98+
private Int16[] left_hor_offset = { 0, 0, 0 };
99+
private Int16[] right_hor_offset = { 0, 0, 0 };
100+
96101
private bool do_localize;
97102
private float filterweight;
98103
private const uint report_len = 49;
99104
private struct Report {
100105
byte[] r;
101106
System.DateTime t;
102107
public ulong ts;
108+
109+
// To-do: get timestamp from report (0th byte); send to server for every 5ms
103110
public Report(byte[] report, System.DateTime time, ulong timestamp) {
104111
r = report;
105112
t = time;
106-
ts = timestamp;
113+
ts = (ulong) ((timestamp / (double)Stopwatch.Frequency) * 1000000); // meant to be in microseconds
107114
}
108115
public System.DateTime GetTime() {
109116
return t;
@@ -264,7 +271,8 @@ public int Attach(byte leds_ = 0x0) {
264271
state = state_.ATTACHED;
265272
byte[] a = { 0x0 };
266273
// Input report mode
267-
Subcommand(0x3, new byte[] { 0x3f }, 1, false);
274+
Subcommand(0x3, new byte[] { 0x30 }, 1, false);
275+
Subcommand(0x3, new byte[] { 0x03, 0x00, 0x00, 0x01 }, 4, false); // higher gyro performance rate
268276
a[0] = 0x1;
269277
dump_calibration_data();
270278
// Connect
@@ -290,10 +298,10 @@ public void Detach() {
290298
PrintArray(max, format: "Max {0:S}", d: DebugType.IMU);
291299
PrintArray(sum, format: "Sum {0:S}", d: DebugType.IMU);
292300
if (state > state_.NO_JOYCONS) {
293-
Subcommand(0x30, new byte[] { 0x0 }, 1);
301+
//Subcommand(0x30, new byte[] { 0x0 }, 1); // Turn off LEDS after pair
294302
Subcommand(0x40, new byte[] { 0x0 }, 1);
295303
Subcommand(0x48, new byte[] { 0x0 }, 1);
296-
Subcommand(0x3, new byte[] { 0x3f }, 1);
304+
//Subcommand(0x3, new byte[] { 0x3f }, 1); // Turn on basic HID mode - not needed
297305
}
298306
if (state > state_.DROPPED) {
299307
HIDapi.hid_close(handle);
@@ -358,6 +366,18 @@ public void Update() {
358366
ProcessIMU(report_buf);
359367
} else {
360368
ExtractIMUValues(report_buf, 0);
369+
// 3 values for 5ms precision instead of 15ms
370+
/*for (int n = 0; n < 3; n++) {
371+
ExtractIMUValues(report_buf, n);
372+
373+
Timestamp = rep.ts + (ulong) n * 5000; // 5ms difference
374+
375+
if (n == 0)
376+
ProcessButtonsAndStick(report_buf);
377+
378+
packetCounter++;
379+
Program.server.NewReportIncoming(this);
380+
}*/
361381
}
362382
}
363383
if (ts_de == report_buf[1]) {
@@ -367,20 +387,20 @@ public void Update() {
367387
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);
368388
ts_prev = rep.GetTime();
369389

370-
// set timestamp and packet count for server
390+
// Sending values at 5ms is not reliable
371391
Timestamp = rep.ts;
392+
ProcessButtonsAndStick(report_buf);
393+
packetCounter++;
394+
Program.server.NewReportIncoming(this);
372395
}
373-
ProcessButtonsAndStick(report_buf);
396+
374397
if (rumble_obj.timed_rumble) {
375398
if (rumble_obj.t < 0) {
376399
rumble_obj.set_vals(160, 320, 0, 0);
377400
} else {
378401
rumble_obj.t -= (1 / timing);
379402
}
380403
}
381-
382-
packetCounter++;
383-
Program.server.NewReportIncoming(this);
384404
}
385405
}
386406
private int ProcessButtonsAndStick(byte[] report_buf) {
@@ -454,29 +474,34 @@ private void ExtractIMUValues(byte[] report_buf, int n = 0) {
454474
acc_r[0] = (Int16)(report_buf[13 + n * 12] | ((report_buf[14 + n * 12] << 8) & 0xff00));
455475
acc_r[1] = (Int16)(report_buf[15 + n * 12] | ((report_buf[16 + n * 12] << 8) & 0xff00));
456476
acc_r[2] = (Int16)(report_buf[17 + n * 12] | ((report_buf[18 + n * 12] << 8) & 0xff00));
477+
478+
Int16[] offset;
479+
if (isPro)
480+
offset = pro_hor_offset;
481+
else if (isLeft)
482+
offset = left_hor_offset;
483+
else
484+
offset = right_hor_offset;
485+
486+
//Console.WriteLine("{0} {1} {2}", gyr_r[0], gyr_r[1], gyr_r[2]);
487+
457488
for (int i = 0; i < 3; ++i) {
458489
switch (i) {
459490
case 0:
460-
acc_g.X = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
461-
//gyr_g.X = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
462-
gyr_g.X = gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f; // Neutrals may be read wrong
463-
if (Math.Abs(acc_g.X) > Math.Abs(max[i]))
464-
max[i] = acc_g.X;
491+
acc_g.X = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
492+
gyr_g.X = gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i]));
493+
465494
break;
466495
case 1:
467-
acc_g.Y = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
468-
//gyr_g.Y = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
469-
gyr_g.Y = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f;
470-
if (Math.Abs(acc_g.Y) > Math.Abs(max[i]))
471-
max[i] = acc_g.Y;
496+
acc_g.Y = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
497+
gyr_g.Y = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i]));
498+
472499
break;
473500
case 2:
474-
acc_g.Z = acc_r[i] * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
475-
//gyr_g.Z = (gyr_r[i] - gyr_neutral[i]) * 0.00122187695f;
476-
gyr_g.Z = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i])) * 0.5f;
477-
if (Math.Abs(acc_g.Z) > Math.Abs(max[i]))
478-
max[i] = acc_g.Z;
479-
break;
501+
acc_g.Z = (acc_r[i] - offset[i]) * (1.0f / (acc_sensiti[i] - acc_neutral[i])) * 4.0f;
502+
gyr_g.Z = -gyr_r[i] * (816.0f / (gyr_sensiti[i] - gyr_neutral[i]));
503+
504+
break;
480505
}
481506
}
482507
}

‎BetterJoyForCemu/Program.cs‎

Lines changed: 50 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ namespace BetterJoyForCemu {
1616
public class JoyconManager {
1717
// Settings accessible via Unity
1818
public bool EnableIMU = true;
19-
public bool EnableLocalize = true;
19+
public bool EnableLocalize = false;
2020

2121
// Different operating systems either do or don't like the trailing zero
2222
private const ushort vendor_id = 0x57e;
@@ -91,20 +91,17 @@ public void Start() {
9191
}
9292
}
9393

94-
public bool shouldUpdate = true;
95-
public void Update(object sender, ElapsedEventArgs e) {
96-
//while (shouldUpdate) {
97-
for (int i = 0; i < j.Count; ++i) {
98-
j[i].Update();
94+
public void Update() {
95+
for (int i = 0; i < j.Count; ++i) {
96+
j[i].Update();
9997

100-
/*if (j.Count > 0) {
101-
Joycon jj = j[i];
98+
/*if (j.Count > 0) {
99+
Joycon jj = j[i];
102100
103-
if (jj.GetButtonDown(Joycon.Button.DPAD_DOWN))
104-
jj.SetRumble(160, 320, 0.6f, 200);
105-
}*/
106-
}
107-
//}
101+
if (jj.GetButtonDown(Joycon.Button.DPAD_DOWN))
102+
jj.SetRumble(160, 320, 0.6f, 200);
103+
}*/
104+
}
108105
}
109106

110107
public void OnApplicationQuit() {
@@ -114,9 +111,47 @@ public void OnApplicationQuit() {
114111
}
115112
}
116113

114+
// Custom timer class because system timers have a limit of 15.6ms
115+
class HighResTimer {
116+
double interval = 0;
117+
double frequency = 0;
118+
119+
Thread thread;
120+
121+
public delegate void ActionDelegate();
122+
ActionDelegate func;
123+
124+
bool run = false;
125+
126+
public HighResTimer(double f, ActionDelegate a) {
127+
frequency = f;
128+
interval = 1.0 / f;
129+
130+
func = a;
131+
}
132+
133+
public void Start() {
134+
run = true;
135+
thread = new Thread(new ThreadStart(Run));
136+
thread.Start();
137+
}
138+
139+
void Run() {
140+
while (run) {
141+
func();
142+
int timeToSleep = (int)(interval * 1000);
143+
Thread.Sleep(timeToSleep);
144+
}
145+
}
146+
147+
public void Stop() {
148+
run = false;
149+
}
150+
}
151+
117152
class Program {
118153
public static UdpServer server;
119-
static float pollsPerSecond = 60.0f;
154+
static double pollsPerSecond = 120.0;
120155

121156
static void Main(string[] args) {
122157
JoyconManager mgr = new JoyconManager();
@@ -129,23 +164,15 @@ static void Main(string[] args) {
129164
//updateThread.Start();
130165

131166
server.Start(26760);
132-
System.Timers.Timer timer = new System.Timers.Timer((int)(1000 / pollsPerSecond));
133-
timer.Elapsed += mgr.Update;
134-
timer.Elapsed += printt;
167+
HighResTimer timer = new HighResTimer(pollsPerSecond, new HighResTimer.ActionDelegate(mgr.Update));
135168
timer.Start();
136169

137170
Console.Write("Press enter to quit.");
138171
Console.ReadLine();
139172

140173
server.Stop();
141-
mgr.shouldUpdate = false;
142174
timer.Stop();
143-
timer.Dispose();
144175
mgr.OnApplicationQuit();
145176
}
146-
147-
static void printt(object sender, ElapsedEventArgs e) {
148-
//Console.Write('.');
149-
}
150177
}
151178
}

‎BetterJoyForCemu/UpdServer.cs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -384,7 +384,7 @@ private bool ReportToBuffer(Joycon hidReport, byte[] outputData, ref int outIdx)
384384
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
385385
outIdx += 4;
386386
Array.Copy(BitConverter.GetBytes(-accel.Z), 0, outputData, outIdx, 4);
387-
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);
387+
//Array.Copy(BitConverter.GetBytes((float)-1), 0, outputData, outIdx, 4);
388388
outIdx += 4;
389389
Array.Copy(BitConverter.GetBytes(accel.X), 0, outputData, outIdx, 4);
390390
//Array.Copy(BitConverter.GetBytes((float)0), 0, outputData, outIdx, 4);

0 commit comments

Comments
 (0)