private void ReadRawAccGyroData(SensorDataRaw sensorDataObj)
{
lock (_dataReadSync)
{
int returnDataLength = 39;
//check to make sure the port is present
if (_wirelessDongle.Port != null)
{
byte[] writebuffer = new byte[4];
writebuffer[0] = 0xf8;//command start byte for wireless commands
writebuffer[1] = _sensorLogicalID;//Logical id for the sensor
writebuffer[2] = 0x40;//command number
writebuffer[3] = (byte)(0x40 + _sensorLogicalID);//checksum(sum of all other packet bytes except start byte and checksum
_wirelessDongle.Port.Write(writebuffer, 0, 4);
//use this bit of code to read back data until we have the required amount
int totalbytes = 0;
byte[] readbuffer = new byte[returnDataLength];
while (totalbytes != returnDataLength)
{
int nbytes = _wirelessDongle.Port.Read(readbuffer, totalbytes, returnDataLength - totalbytes);
totalbytes += nbytes;
}
sensorDataObj.AccGyroReadbuffer = readbuffer;
}
}
}
private void ReadAccGyroData(SensorDataRaw rawData, SensorData sensorDataObj)
{
byte []readbuffer = rawData.AccGyroReadbuffer;
//reverse the array bytes because this data is big endian
readbuffer = readbuffer.Reverse().ToArray();
//Populate the data backwards because we reversed the order of the values
//as well as their internal bytes
sensorDataObj.GyroDataObject.Pitch = BitConverter.ToSingle(readbuffer, 32);
sensorDataObj.GyroDataObject.Yaw = BitConverter.ToSingle(readbuffer, 28);
sensorDataObj.GyroDataObject.Roll = BitConverter.ToSingle(readbuffer, 24);
Vector3 accVectorObject = new Vector3();
accVectorObject.X = BitConverter.ToSingle(readbuffer, 20);
accVectorObject.Y = BitConverter.ToSingle(readbuffer, 16);
accVectorObject.Z = BitConverter.ToSingle(readbuffer, 12);
sensorDataObj.AccDataObject.DataVector = accVectorObject;
Vector3 compassVectorObject = new Vector3();
compassVectorObject.X = BitConverter.ToSingle(readbuffer, 8);
compassVectorObject.Y = BitConverter.ToSingle(readbuffer, 4);
compassVectorObject.Z = BitConverter.ToSingle(readbuffer, 0);
sensorDataObj.CompassDataObject.DataVector = compassVectorObject;
}
public SensorDataRaw ReadData()
{
SensorDataRaw sensorDataObj = new SensorDataRaw();
ReadRawAccGyroData(sensorDataObj);
//ReadOrientation(sensorDataObj);
return sensorDataObj;
}
public SpaceSensorRecordingDataReceivedEventArgs(SensorDataRaw sensor1DataPacket, SensorDataRaw sensor2DataPacket)
{
_sensor1DataPacket = sensor1DataPacket;
_sensor2DataPacket = sensor2DataPacket;
}