private int ReceiveRaw() { if (handle == IntPtr.Zero) { return(-2); } HIDapi.hid_set_nonblocking(handle, 0); byte[] raw_buf = new byte[report_len]; int ret = HIDapi.hid_read(handle, raw_buf, new UIntPtr(report_len)); 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); } 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); }
private int ReceiveRaw() { if (handle == IntPtr.Zero) { return(-2); } HIDapi.hid_set_nonblocking(handle, 0); var raw_buf = new byte[report_len]; var ret = HIDapi.hid_read(handle, raw_buf, new UIntPtr(report_len)); if (ret <= 0) { return(ret); } // Process packets as soon as they come for (var n = 0; n < 3; n++) { ExtractIMUValues(raw_buf, n); if (n == 0) { var lag = (byte)Math.Max(0, raw_buf[1] - ts_en - 3); Timestamp += (ulong)lag * 5000; // add lag once ProcessButtonsAndStick(raw_buf); } Timestamp += 5000; // 5ms difference packetCounter++; if (Program.server != null) { Program.server.NewReportIncoming(this); } xin?.SendReport(report); } if (ts_en == raw_buf[1]) { form.AppendTextBox("Duplicate timestamp enqueued.\r\n"); DebugPrint($"Duplicate timestamp enqueued. TS: {ts_en:X2}", DebugType.THREADING); } ts_en = raw_buf[1]; DebugPrint($"Enqueue. Bytes read: {ret:D}. Timestamp: {raw_buf[1]:X2}", DebugType.THREADING); return(ret); }
public int Attach(byte leds_ = 0x0) { state = state_.ATTACHED; // Make sure command is received HIDapi.hid_set_nonblocking(handle, 0); byte[] a = { 0x0 }; // Connect if (!isUSB) { // Input report mode Subcommand(0x03, new byte[] { 0x30 }, 1, false); a[0] = 0x1; dump_calibration_data(); } else { Subcommand(0x03, new byte[] { 0x3f }, 1, false); a = Enumerable.Repeat((byte)0, 64).ToArray(); Console.WriteLine("Using USB."); a[0] = 0x80; a[1] = 0x01; HIDapi.hid_write(handle, a, new UIntPtr(2)); HIDapi.hid_read(handle, a, new UIntPtr(64)); if (a[2] != 0x3) { PadMacAddress = new PhysicalAddress(new byte[] { a[9], a[8], a[7], a[6], a[5], a[4] }); } // USB Pairing a = Enumerable.Repeat((byte)0, 64).ToArray(); a[0] = 0x80; a[1] = 0x02; // Handshake HIDapi.hid_write(handle, a, new UIntPtr(2)); a[0] = 0x80; a[1] = 0x03; // 3Mbit baud rate HIDapi.hid_write(handle, a, new UIntPtr(2)); a[0] = 0x80; a[1] = 0x02; // Handshake at new baud rate HIDapi.hid_write(handle, a, new UIntPtr(2)); a[0] = 0x80; a[1] = 0x04; // Prevent HID timeout HIDapi.hid_write(handle, a, new UIntPtr(2)); dump_calibration_data(); } a[0] = leds_; Subcommand(0x30, a, 1); Subcommand(0x40, new byte[] { (imu_enabled ? (byte)0x1 : (byte)0x0) }, 1, true); Subcommand(0x3, new byte[] { 0x30 }, 1, true); Subcommand(0x48, new byte[] { 0x1 }, 1, true); Subcommand(0x41, new byte[] { 0x03, 0x00, 0x00, 0x01 }, 4, false); // higher gyro performance rate DebugPrint("Done with init.", DebugType.COMMS); HIDapi.hid_set_nonblocking(handle, 1); return(0); }