/// <summary>
/// Sets the packet.
/// </summary>
/// <param name='incomingData'>
/// _incoming data in array
/// </param>
public void setPacket(byte[] incomingData)
{
XsDataPacket dataPacket = new XsQuaternionPacket(incomingData);
XsMvnPose pose = dataPacket.getPose();
if (pose != null)
{
lastPosePositions = pose.positions;
lastPoseOrientations = pose.orientations;
dataUpdated = true;
}
}
/// <summary>
/// Start this instance.
/// The datapacket will be set to one of the supported mode, based on it's type.
/// </summary>
public void start()
{
dataUpdated = false;
while (true)
{
if (newData)
{
if (packetMutex.WaitOne(1000))
{
try
{
newData = false;
//create the proper Data Packet based on the packet's type
int ptype = lastPackets[5];
if ( ptype == 0x31)
datapacket = new XsEulerPacket(lastPackets);
else if ( ptype == 0x32)
datapacket = new XsQuaternionPacket(lastPackets);
else
Debug.LogError("[xsens] Not supported packet type ("+ptype+")!");
}
finally
{
packetMutex.ReleaseMutex();
if (poseMutex.WaitOne(1000))
{
try
{
XsMvnPose pose = datapacket.getPose();
if(pose != null)
{
lastPosePositions = pose.positions;
lastPoseOrientations = pose.orientations;
dataUpdated = true;
}
}
finally
{
poseMutex.ReleaseMutex();
}
}//if poseMutex
}
}//if packetMutex
}
Thread.Sleep(1);
}//while
}
public XsMvnPose pose; // Every packet has a pose
/// <summary>
/// Initializes a new instance of the <see cref="xsens.XsDataPacket"/> class.
/// </summary>
/// <param name='readData'>
/// Create the packed from this byte array.
/// </param>
public XsDataPacket(byte[] readData)
{
memStream = new MemoryStream(readData); // Create a memory stream of the packet data
binReader = new BinaryReader(memStream); // Create a binary reader on that memory stream, so we can easely convert the data it contains
parseHeader();
parsePayload(0); // Calls the correct classes parsePayload by itself (inheritance)
pose = new XsMvnPose(isQuaternionPacket);
pose.setHeaderData(headerData); // TODO: figure out if this is even neccesary
pose.setPayloadData(payloadData); // Sets the payload data in the pose
pose.createPose(0, 0); // Try to create a new pose with the data that was send
}
/// <summary>
/// Initializes a new instance of the <see cref="xsens.XsDataPacket"/> class.
/// </summary>
/// <param name='readData'>
/// Create the packed from this byte array.
/// </param>
public XsDataPacket (byte[] readData)
{
using (BinaryReader br = new BinaryReader(new MemoryStream(readData)))
{
pose = null;
int[] headerData = parseHeader (br);
//we only care about pose data, which is Z up
if (headerData [0] == (int)MessageType.PoseDataZup)
{
double[] payloadData = parsePayload (br); // Calls the correct classes parsePayload by itself (inheritance)
pose = new XsMvnPose ();
pose.createPose (payloadData); // Try to create a new pose with the data that was send
}
}
}
protected XsMvnPose pose; // Every valid packet has a pose (Just like every pose has its thorn)
#endregion Fields
#region Constructors
/// <summary>
/// Initializes a new instance of the <see cref="xsens.XsDataPacket"/> class.
/// </summary>
/// <param name='readData'>
/// Create the packed from this byte array.
/// </param>
public XsDataPacket(byte[] readData)
{
using (BinaryReader br = new BinaryReader(new MemoryStream(readData)))
{
pose = null;
int[] headerData = parseHeader (br);
//we only care about pose data, which is Z up
if (headerData [0] == (int)MessageType.PoseDataZup)
{
double[] payloadData = parsePayload (br); // Calls the correct classes parsePayload by itself (inheritance)
pose = new XsMvnPose ();
pose.createPose (payloadData); // Try to create a new pose with the data that was send
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="xsens.XsDataPacket"/> class.
/// </summary>
/// <param name='readData'>
/// Create the packed from this byte array.
/// </param>
public XsDataPacket (byte[] readData)
{
using (BinaryReader br = new BinaryReader(new MemoryStream(readData)))
{
pose = null;
int[] headerData = parseHeader (br);
//we only care about pose data, which is Z up
if ((headerData [0] == (int)MessageType.PoseDataZup)
|| (headerData [0] == (int)MessageType.PosDataUnity))
{
double[] payloadData = parsePayload (br, headerData[3]); // Calls the correct classes parsePayload by itself (inheritance)
if (headerData[5] == 0) //if we are using an MVN version below 2019
{
pose = new XsMvnPose(headerData[3]);
}
else //If we are using MVN 2019 and above
{
pose = new XsMvnPose(headerData[5], headerData[7], headerData[6]);
}
pose.createPose (payloadData); // Try to create a new pose with the data that was send
}
}
}
/// <summary>
/// Start this instance.
/// The datapacket will be set to one of the supported mode, based on its type.
/// </summary>
public void start()
{
dataUpdated = false;
while (true)
{
if (newData)
{
if (packetMutex.WaitOne(1000))
{
try
{
newData = false;
//create the proper Data Packet based on the packets type
string ptypeString = System.String.Empty;
ptypeString += (char)(lastPackets[4]);
ptypeString += (char)(lastPackets[5]); // last two chars contain packet ID type
int ptype = Convert.ToInt32(ptypeString);
switch ((StreamingProtocol)ptype)
{
case StreamingProtocol.SPPoseEuler:
datapacket = new XsEulerPacket(lastPackets);
break;
case StreamingProtocol.SPPoseQuaternion:
case StreamingProtocol.SPPoseUnity3D:
// technically this one should be handled differently
datapacket = new XsQuaternionPacket(lastPackets);
break;
case StreamingProtocol.SPMetaScalingLegacy:
case StreamingProtocol.SPMetaPropInfoLegacy:
case StreamingProtocol.SPMetaMoreMeta:
case StreamingProtocol.SPMetaScaling:
// ignored packet containing not useful meta-data information for the unity plug-in.
break;
case StreamingProtocol.SPPosePositions:
case StreamingProtocol.SPTagPositionsLegacy:
Debug.LogError("[xsens] Wrong protocol identified. Try to change the protocol type in the Network Streamer preferences panel in MVN Studio.");
break;
default:
Debug.LogError("[xsens] Not supported packet type ("+ ptype +")!");
break;
}
}
finally
{
packetMutex.ReleaseMutex();
if (poseMutex.WaitOne(1000))
{
try
{
XsMvnPose pose = datapacket.getPose();
if(pose != null)
{
lastPosePositions = pose.positions;
lastPoseOrientations = pose.orientations;
dataUpdated = true;
}
}
finally
{
poseMutex.ReleaseMutex();
}
}//if poseMutex
}
}//if packetMutex
}
Thread.Sleep(1);
}//while
}
