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 int ReceiveRaw()
{
if (handle == IntPtr.Zero)
{
return(-2);
}
HIDapi.hid_set_nonblocking(handle, 1);
byte[] raw_buf = new byte[report_len];
int ret = HIDapi.hid_read_timeout(handle, raw_buf, new UIntPtr(report_len), 5000);
if (ret > 0)
{
// Process packets as soon as they come
for (int n = 0; n < 3; n++)
{
ExtractIMUValues(raw_buf, n);
byte lag = (byte)Math.Max(0, raw_buf[1] - ts_en - 3);
if (n == 0)
{
Timestamp += (ulong)lag * 5000; // add lag once
ProcessButtonsAndStick(raw_buf);
int newbat = battery;
battery = (raw_buf[2] >> 4) / 2;
if (newbat != battery)
{
BatteryChanged();
}
}
Timestamp += 5000; // 5ms difference
packetCounter++;
if (Program.server != null)
{
Program.server.NewReportIncoming(this);
}
//this.SendPackage(); // JC Send Pack
if (xin != null)
{
xin.SendReport(report);
}
}
if (ts_en == raw_buf[1])
{
form.AppendTextBox("Duplicate timestamp enqueued.\r\n");
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);
}