private static void showTransfer(UsbTransferQueue.Handle handle, int transferIndex)
{
if (mStartTime == DateTime.MinValue)
{
mStartTime = DateTime.Now;
Console.WriteLine("Synchronizing..");
return;
}
mTotalBytes += handle.Transferred;
double bytesSec = mTotalBytes / (DateTime.Now - mStartTime).TotalSeconds;
Console.WriteLine("#{0} complete. {1} bytes/sec ({2} bytes) Data[1]={3:X2}",
transferIndex,
Math.Round(bytesSec, 2),
handle.Transferred,
handle.Data[1]);
}
private static void showTransfer(UsbTransferQueue.Handle handle, int transferIndex)
{
if (mStartTime == DateTime.MinValue)
{
mStartTime = DateTime.Now;
Console.WriteLine("Synchronizing..");
return;
}
mTotalBytes += handle.Transferred;
double bytesSec = mTotalBytes / (DateTime.Now - mStartTime).TotalSeconds;
if (ImuDataUpdate != null)
{
short tmpQ_w = (short)(((handle.Data[1] & 0xFF) << 8) | (handle.Data[0] & 0xFF));
short tmpQ_x = (short)(((handle.Data[3] & 0xFF) << 8) | (handle.Data[2] & 0xFF));
short tmpQ_y = (short)(((handle.Data[5] & 0xFF) << 8) | (handle.Data[4] & 0xFF));
short tmpQ_z = (short)(((handle.Data[7] & 0xFF) << 8) | (handle.Data[6] & 0xFF));
Double w = (((Double)tmpQ_w) / ((Double)(1 << 14)));
Double x = (((Double)tmpQ_x) / ((Double)(1 << 14)));
Double y = (((Double)tmpQ_y) / ((Double)(1 << 14)));
Double z = (((Double)tmpQ_z) / ((Double)(1 << 14)));
Double m = Math.Sqrt((x * x) + (y * y) + (z * z) + (w * w));
if (Math.Abs(1 - m) < 0.005)
{
//Console.WriteLine("ImuDataUpdate x:{0},y:{1},z:{2},w:{3}", x, y, z, w);
ImuDataUpdate(x, y, z, w);
}
}
/*
* Console.WriteLine("#{0} complete. {1} bytes/sec ({2} bytes) Data[1]={3:X2}",
* transferIndex,
* Math.Round(bytesSec, 2),
* handle.Transferred,
* handle.Data[1]);
*/
}
private void showTransfer(UsbTransferQueue.Handle handle, int transferIndex)
{
if (mStartTime == DateTime.MinValue)
{
mStartTime = DateTime.Now;
form.setText("Synchronizing..\r\n");
return;
}
mTotalBytes += handle.Transferred;
double bytesSec = mTotalBytes / (DateTime.Now - mStartTime).TotalSeconds;
form.setText(string.Format("#{0} complete. {1} bytes/sec ({2} bytes)\r\n",
transferIndex,
Math.Round(bytesSec, 2),
handle.Transferred,
handle.Data[1]));
form.setText(string.Format("Data: "));
for (int i = 0; i < handle.Data.Length; i++)
{
form.setText(string.Format("{0:X2} ", handle.Data[i]));
}
form.setText("\r\n");
}
private static void ImuDataPolling()
{
ErrorCode ec = ErrorCode.None;
UsbTransferQueue transferQeue = null;
try
{
UsbRegDeviceList regList = null;
do
{
regList = UsbDevice.AllDevices.FindAll(MyUsbFinder);
if (regList.Count > 0)
{
break;
}
Thread.Sleep(2000);
} while (true);
// Find and open the usb device.
UsbInterfaceInfo usbInterfaceInfo = null;
UsbEndpointInfo usbEndpointInfo = null;
// Look through all conected devices with this vid and pid until
// one is found that has and and endpoint that matches TRANFER_ENDPOINT.
//
foreach (UsbRegistry regDevice in regList)
{
if (regDevice.Open(out MyUsbDevice))
{
if (MyUsbDevice.Configs.Count > 0)
{
// if TRANFER_ENDPOINT is 0x80 or 0x00, LookupEndpointInfo will return the
// first read or write (respectively).
if (UsbEndpointBase.LookupEndpointInfo(MyUsbDevice.Configs[0], TRANFER_ENDPOINT,
out usbInterfaceInfo, out usbEndpointInfo))
{
break;
}
MyUsbDevice.Close();
MyUsbDevice = null;
}
}
}
// If the device is open and ready
if (MyUsbDevice == null)
{
throw new Exception("Device Not Found.");
}
// If this is a "whole" usb device (libusb-win32, linux libusb-1.0)
// it exposes an IUsbDevice interface. If not (WinUSB) the
// 'wholeUsbDevice' variable will be null indicating this is
// an interface of a device; it does not require or support
// configuration and interface selection.
IUsbDevice wholeUsbDevice = MyUsbDevice as IUsbDevice;
if (!ReferenceEquals(wholeUsbDevice, null))
{
// This is a "whole" USB device. Before it can be used,
// the desired configuration and interface must be selected.
// Select config #1
wholeUsbDevice.SetConfiguration(1);
// Claim interface #0.
wholeUsbDevice.ClaimInterface(usbInterfaceInfo.Descriptor.InterfaceID);
}
// open read endpoint.
UsbEndpointReader reader = MyUsbDevice.OpenEndpointReader(
(ReadEndpointID)usbEndpointInfo.Descriptor.EndpointID,
0,
(EndpointType)(usbEndpointInfo.Descriptor.Attributes & 0x3));
if (ReferenceEquals(reader, null))
{
throw new Exception("Failed locating read endpoint.");
}
reader.Reset();
// The benchmark device firmware works with this example but it must be put into PC read mode.
#if IS_BENCHMARK_DEVICE
int transferred;
byte[] ctrlData = new byte[1];
UsbSetupPacket setTestTypePacket =
new UsbSetupPacket((byte)(UsbCtrlFlags.Direction_In | UsbCtrlFlags.Recipient_Device | UsbCtrlFlags.RequestType_Vendor),
0x0E, 0x01, usbInterfaceInfo.Descriptor.InterfaceID, 1);
MyUsbDevice.ControlTransfer(ref setTestTypePacket, ctrlData, 1, out transferred);
#endif
TRANFER_SIZE -= (TRANFER_SIZE % usbEndpointInfo.Descriptor.MaxPacketSize);
transferQeue = new UsbTransferQueue(reader,
TRANFER_MAX_OUTSTANDING_IO,
TRANFER_SIZE,
5000,
usbEndpointInfo.Descriptor.MaxPacketSize);
do
{
UsbTransferQueue.Handle handle = null;
// Begin submitting transfers until TRANFER_MAX_OUTSTANDING_IO has benn reached.
// then wait for the oldest outstanding transfer to complete.
//
ec = transferQeue.Transfer(out handle);
if (ec != ErrorCode.Success)
{
throw new Exception("Failed getting async result");
}
// Show some information on the completed transfer.
showTransfer(handle, mTransferCount++);
} while (true);
}
catch (Exception ex)
{
if (transferQeue != null)
{
transferQeue.Free();
}
Console.WriteLine();
Console.WriteLine((ec != ErrorCode.None ? ec + ":" : String.Empty) + ex.Message);
}
finally
{
if (MyUsbDevice != null)
{
if (MyUsbDevice.IsOpen)
{
// If this is a "whole" usb device (libusb-win32, linux libusb-1.0)
// it exposes an IUsbDevice interface. If not (WinUSB) the
// 'wholeUsbDevice' variable will be null indicating this is
// an interface of a device; it does not require or support
// configuration and interface selection.
if (transferQeue != null)
{
// Cancels any oustanding transfers and free's the transfer queue handles.
// NOTE: A transfer queue can be reused after it's freed.
transferQeue.Free();
Console.WriteLine("\r\nDone!\r\n");
}
IUsbDevice wholeUsbDevice = MyUsbDevice as IUsbDevice;
if (!ReferenceEquals(wholeUsbDevice, null))
{
// Release interface #0.
wholeUsbDevice.ReleaseInterface(0);
}
MyUsbDevice.Close();
}
MyUsbDevice = null;
}
// Wait for user input..
//Console.ReadKey();
// Free usb resources
//UsbDevice.Exit();
ImuDataThreadStart();
}
}
private static void showTransfer(UsbTransferQueue.Handle handle, int transferIndex, TransferParams tParams)
{
double samplesPerSecond = 0;
int numBytesTransferred = handle.Data.Length;
int maxTransferSize = handle.Context.IsoPacketSize;
int x = handle.Context.Transmitted;
int packetCounter = 0;
int[][] jagaPacketData;
int[] potentialErrors;
int cachedOldRemnantLength = 0;
if (old_remnant != null)
{
cachedOldRemnantLength = old_remnant.Length;
}
AlignTransfersIntoJagaPackets(handle.Data, old_remnant, out jagaPacketData, out new_remnant, handle.Transferred, maxTransferSize != 160, out potentialErrors);
old_remnant = new_remnant;
StringBuilder sbCurrentTransfer = new StringBuilder();
int header = FindHeaderBoundary(handle.Transferred, handle.Data);
bool hasErroneousPacket = false;
bool hasCorrectedPacket = false;
sbCurrentTransfer.Append("\r\nTransfer ");
// Show transfer # at beginning of each line
sbCurrentTransfer.Append((transfersReceived++).ToString() + ":\t");
// Show bytes in transfer
for (int j = 0; j < handle.Transferred; j++)
{
sbCurrentTransfer.Append(String.Format(" {0:X2}", handle.Data[j]));
}
sbCurrentTransfer.Append(string.Format("\t:{0} bytes in transfer", handle.Transferred));
if (cachedOldRemnantLength > 0)
{
sbCurrentTransfer.Append(", appended to " + cachedOldRemnantLength.ToString() + " from previous transfer");
}
if (new_remnant != null && new_remnant.Length > 0)
{
sbCurrentTransfer.Append(", with " + new_remnant.Length.ToString() + " left over for next transfer");
}
if (header >= 0 && handle.Data.Length >= header + 13)
{
samplesPerPacket = handle.Data[header + 10];
channels = handle.Data[header + 9];
sample_rate = (handle.Data[header + 11] << 8) + handle.Data[header + 12];
sbCurrentTransfer.Append(" HEADER PACKET");
sb_header.Append(string.Format("Header info: version {0}, channels {1}, samples per packet {2}, sample rate {3}", handle.Data[header + 8], channels, samplesPerPacket, sample_rate));
}
if (jagaPacketData.Length > 0)
{
// Now show packets
sbCurrentTransfer.Append("\r\n");
}
for (int packetNum = 0; packetNum < jagaPacketData.Length; packetNum++)
{
if (jagaPacketData[packetNum] == null)
{
// The # of packets is not deterministic. Once we hit nulls, we are done.
break;
}
packetCounter = 0;
sbCurrentTransfer.Append("\r\nPacket " + mTotalPacketsHandled.ToString() + ":\t");
int numSamples = jagaPacketData[packetNum].Length;
for (int j = 0; j < numSamples; j++)
{
// Calculate packet counter from LSB
int val = jagaPacketData[packetNum][j];
packetCounter = packetCounter * 2 + (val & 1);
if (j % 4 == 0)
{
sbCurrentTransfer.Append(" ");
}
sbCurrentTransfer.Append(String.Format(" {0:X4}", val));
}
mTotalPacketsHandled++;
// Append packet counter
sbCurrentTransfer.Append(String.Format("\t: Counter {0:X4}", packetCounter));
if (prevPacketCounter >= 0)
{
int increment = CalculateIncrement14_bit(prevPacketCounter, packetCounter);
sbCurrentTransfer.Append(String.Format("\tIncrement {0}", increment));
if (increment <= 0)
{
// Check if bit reversal restores correct count
if (potentialErrors[0] >= 0)
{
int bitToFlip = 15 - (potentialErrors[0] / 2);
bitToFlip = 1 << bitToFlip;
if ((packetCounter & bitToFlip) == 0)
{
// Switch 0 to 1
int adjustedCounter = packetCounter + bitToFlip;
if (CalculateIncrement14_bit(prevPacketCounter, adjustedCounter) == 1)
{
// We have fixed packet counter
sbCurrentTransfer.Append(" \tCorrected packet counter from " + packetCounter + " to " + adjustedCounter);
packetCounter = adjustedCounter;
packetsCorrected++;
increment = 1;
hasCorrectedPacket = true;
}
}
if (!hasCorrectedPacket)
{
sbCurrentTransfer.Append(" \tUnable to correct packet counter - error location " + potentialErrors[0].ToString() + ", bitToFlip is " + bitToFlip.ToString());
}
}
}
if (increment > 0)
{
apparentPacketsTransmitted += increment;
}
if (increment != 1)
{
hasErroneousPacket = true;
sbCurrentTransfer.Append(" ERROR IN PACKET COUNTER, EXPECTED " + (packetCounter + 1).ToString());
packetErrors++;
}
}
prevPacketCounter = packetCounter;
}
sbCurrentTransfer.Append(", ");
samplesPerSecond = apparentPacketsTransmitted * samplesPerPacket / channels / 2 / (DateTime.Now - mStartTime).TotalSeconds;
sbCurrentTransfer.Append(String.Format("{0} avg samples/sec\r\n\r\n", Math.Round(samplesPerSecond, 2)));
if (header >= 0)
{
sb_header.Append(String.Format(", Calculated sample rate = {0} samples/sec", Math.Round(samplesPerSecond, 2)));
}
if (hasErroneousPacket || hasCorrectedPacket)
{
sb_error_rate.Append(string.Format("Counter errors {0}, error rate {3:F2}%, corrected packet counters {1}, total packets {2}",
packetErrors, packetsCorrected, mTotalPacketsHandled, packetErrors * 100.0 / mTotalPacketsHandled));
}
bool showThisPacket;
if (tParams.showAll)
{
showThisPacket = true;
}
else
{
showThisPacket = (tParams.showErroneous && hasErroneousPacket) || (tParams.showHeaders && header >= 0) || (tParams.showCorrected && hasCorrectedPacket);
}
if (showThisPacket)
{
if (!tParams.showAll && (tParams.showCorrected || tParams.showErroneous))
{
// For errors, show previous transfers, to help understand error.
sb.Append("\r\n\r\n");
if (handle.Transferred < 50)
{
// If transfers are small, then show previous two, since each transfer doesn't have that much info.
sb.Append(sbPreviousTransfer2);
}
sb.Append(sbPreviousTransfer);
}
// Append to ongoing string that will eventually get sent to screen
sb.Append(sbCurrentTransfer);
}
// Done:
// Save current text so that if we are showing only anomalous packets, then next time we can show both current and previous packets (since anomalous packets usually arise from
// irregularities involving a packet spanning two transfers.
sbPreviousTransfer2.Clear();
sbPreviousTransfer2.Append(sbPreviousTransfer);
sbPreviousTransfer.Clear();
sbPreviousTransfer.Append(sbCurrentTransfer);
}
