Tracker2MessageType Decode(const UByte* buffer, int size)
{
if (size < 64)
return Tracker2Message_SizeError;
LastCommandID = DecodeUInt16(buffer + 1);
NumSamples = buffer[3];
RunningSampleCount = DecodeUInt16(buffer + 4);
Temperature = DecodeSInt16(buffer + 6);
SampleTimestamp = DecodeUInt32(buffer + 8);
// Only unpack as many samples as there actually are.
UByte iterationCount = (NumSamples > 1) ? 2 : NumSamples;
for (UByte i = 0; i < iterationCount; i++)
{
UnpackSensor(buffer + 12 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
UnpackSensor(buffer + 20 + 16 * i, &Samples[i].GyroX, &Samples[i].GyroY, &Samples[i].GyroZ);
}
MagX = DecodeSInt16(buffer + 44);
MagY = DecodeSInt16(buffer + 46);
MagZ = DecodeSInt16(buffer + 48);
FrameCount = DecodeUInt16(buffer + 50);
FrameTimestamp = DecodeUInt32(buffer + 52);
FrameID = buffer[56];
CameraPattern = buffer[57];
CameraFrameCount = DecodeUInt16(buffer + 58);
CameraTimestamp = DecodeUInt32(buffer + 60);
return Tracker2Message_Sensors;
}
/////////////////////////////////////////////////////////////////////////////////////////////
// Sensor Display Info
// HID Type: Get Feature
// HID Packet Length: 56
/////////////////////////////////////////////////////////////////////////////////////////////
BOOLEAN getSensorInfo( Device *dev )
{
UInt8 Buffer[56];
//UInt16 CommandId;
int res;
Buffer[0] = 0x9; // DisplayInfo
res = hid_get_feature_report(dev->hidapi_dev, Buffer, 56 );
if (res < 0)
{
return FALSE;
}
else
{
dev->sensorInfo.DistortionType = Buffer[3];
dev->sensorInfo.HResolution = DecodeUInt16(Buffer+4);
dev->sensorInfo.VResolution = DecodeUInt16(Buffer+6);
dev->sensorInfo.HScreenSize = DecodeUInt32(Buffer+8) * (1/1000000.f);
dev->sensorInfo.VScreenSize = DecodeUInt32(Buffer+12) * (1/1000000.f);
dev->sensorInfo.VCenter = DecodeUInt32(Buffer+16) * (1/1000000.f);
dev->sensorInfo.LensSeparation = DecodeUInt32(Buffer+20) * (1/1000000.f);
dev->sensorInfo.EyeToScreenDistance[0] = DecodeUInt32(Buffer+24) * (1/1000000.f);
dev->sensorInfo.EyeToScreenDistance[1] = DecodeUInt32(Buffer+28) * (1/1000000.f);
dev->sensorInfo.DistortionK[0] = DecodeFloat(Buffer+32);
dev->sensorInfo.DistortionK[1] = DecodeFloat(Buffer+36);
dev->sensorInfo.DistortionK[2] = DecodeFloat(Buffer+40);
dev->sensorInfo.DistortionK[3] = DecodeFloat(Buffer+44);
dev->sensorInfo.DistortionK[4] = DecodeFloat(Buffer+48);
dev->sensorInfo.DistortionK[5] = DecodeFloat(Buffer+52);
#if 0
{
int i;
printf ("\nSensor Info:\n");
for (i = 0; i < res; i++)
printf("%hhx ", Buffer[i]);
puts("\n");
printf ("\nR: %d x %d", dev->sensorInfo.HResolution, dev->sensorInfo.VResolution );
printf ("\tS: %f x %f\n", dev->sensorInfo.HScreenSize, dev->sensorInfo.VScreenSize );
}
#endif
}
return TRUE;
}
