public void OnRecordingDataReceivedEvent(object sender, RecordingDataReceivedEventArgs args)
{
lock (_dataReceiving)
{
// if (args.SensorDataPacket.LogicalID == 0)
_current1SensorData = args.Sensor1DataPacket;
_current2SensorData = args.Sensor2DataPacket;
}
}
private void ReadOrientation(SensorData sensorData)
{
lock (_dataReadSync)
{
int returnDataLength = 19;
//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] = 0x0;//command number
writebuffer[3] = (byte)(0x0 + _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;
}
//reverse the array bytes because this data is big endian
readbuffer = readbuffer.Reverse().ToArray();
//readbuffer = readbuffer.ToArray();
//Populate the data backwards because we reversed the order of the values
//as well as their internal bytes
Quaternion quatObj = new Quaternion();
//quaternion x
quatObj.X = BitConverter.ToSingle(readbuffer, 12);
//quaternion y
quatObj.Y = BitConverter.ToSingle(readbuffer, 8);
//quaternion z
quatObj.Z = BitConverter.ToSingle(readbuffer, 4);
//quaternion w
quatObj.W = BitConverter.ToSingle(readbuffer, 0);
sensorData.QuaternionObject = quatObj;
}
}
}
public RecordingDataReceivedEventArgs(SensorData sensor1DataPacket, SensorData sensor2DataPacket)
{
_sensor1DataPacket = sensor1DataPacket;
_sensor2DataPacket = sensor2DataPacket;
}
public void ReceivedEventRecordingData(SensorData sensor1Data, SensorData sensor2Data)
{
if(RecordingDataReceivedEvent != null)
RecordingDataReceivedEvent(this, new RecordingDataReceivedEventArgs(sensor1Data, sensor2Data));
}
private void SpaceSensorRecordingThreadSimulation()
{
CSVFileParser l_Parser = new CSVFileParser();
l_Parser.startParsingCSVData(ProjectConstants.SPACE_SENSOR_SIMULATED_DATA_PATH, 0, 14);
string[] row;
while ((row = l_Parser.getNextRow(0)) != null)
{
SensorData sensor1DataObj = new SensorData();
ReadSimulationSensorData(sensor1DataObj,row,2);
SensorData sensor2DataObj = new SensorData();
ReadSimulationSensorData(sensor2DataObj, row, 12);
mParent.ReceivedEventRecordingData(sensor2DataObj, sensor2DataObj);
Thread.Sleep(1);
}
l_Parser.close();
}
private void SpaceSensorProcessingThread()
{
while (true)
{
while(_spaceSensorDataQueue.Count == 0)
{
if(!_spaceProcessingStatus)
return;
Thread.Sleep(10);
}
string[] rowHeader = new string[2];
_sequenceNumber++;
rowHeader[0] = _sequenceNumber.ToString();
//rowHeader[1] = String.Format("{0:hh:mm:ss.fff}", DateTime.Now);
TimeSpan recordTime = DateTime.Now - _startRecordingTime;
rowHeader[1] = recordTime.TotalMilliseconds.ToString();
SpaceSensorRecordingDataReceivedEventArgs args = _spaceSensorDataQueue.Dequeue();
SensorData sensor1DataObj = new SensorData();
ReadAccGyroData(args.Sensor1DataPacket,sensor1DataObj);
SensorData sensor2DataObj = new SensorData();
ReadAccGyroData(args.Sensor2DataPacket,sensor2DataObj);
string[] sensor1Data = ConvertSensorDataToCSV(sensor1DataObj);
string[] sensor2Data = ConvertSensorDataToCSV(sensor2DataObj);
if(m_CSVFileWriter != null)
logSpaceSensorCSVLineItem(new SpaceSensorCSVLineItem(rowHeader, sensor1Data, sensor2Data));
}
}
private void ReadSimulationSensorData(SensorData sensorDataObj,string []row,int startIndex)
{
Vector3 accData = new Vector3();
accData.X = Convert.ToSingle(row[startIndex]);
accData.Y = Convert.ToSingle(row[startIndex+1]);
accData.Z = Convert.ToSingle(row[startIndex+2]);
sensorDataObj.AccDataObject.DataVector = accData;
GyroData gyroObj = sensorDataObj.GyroDataObject;
gyroObj.Pitch = Convert.ToSingle(row[startIndex+3]);
gyroObj.Yaw = Convert.ToSingle(row[startIndex+4]);
gyroObj.Roll = Convert.ToSingle(row[startIndex+5]);
Quaternion quatObj = sensorDataObj.QuaternionObject;
quatObj.X = Convert.ToSingle(row[startIndex+6]);
quatObj.Y = Convert.ToSingle(row[startIndex+7]);
quatObj.Z = Convert.ToSingle(row[startIndex+8]);
quatObj.W = Convert.ToSingle(row[startIndex+9]);
}
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;
}
private string[] ConvertSensorDataToCSV(SensorData sensorDataObj)
{
//Quaternion sensorQuatObj = _spaceSensorMain.SpaceSensorObject2.ReadOrientation();
string[] dataRow = new string[10];
if (sensorDataObj.AccDataObject != null)
{
dataRow[0] = sensorDataObj.AccDataObject.DataVector.X.ToString();
dataRow[1] = sensorDataObj.AccDataObject.DataVector.Y.ToString();
dataRow[2] = sensorDataObj.AccDataObject.DataVector.Z.ToString();
dataRow[3] = sensorDataObj.GyroDataObject.Pitch.ToString();
dataRow[4] = sensorDataObj.GyroDataObject.Yaw.ToString();
dataRow[5] = sensorDataObj.GyroDataObject.Roll.ToString();
}
if (sensorDataObj.QuaternionObject != null)
{
dataRow[6] = sensorDataObj.QuaternionObject.X.ToString();
dataRow[7] = sensorDataObj.QuaternionObject.Y.ToString();
dataRow[8] = sensorDataObj.QuaternionObject.Z.ToString();
dataRow[9] = sensorDataObj.QuaternionObject.W.ToString();
}
return dataRow;
}
private OscMessage CreateOscMessage(SensorData sensorData)
{
OscMessage dataOscMessage = new OscMessage(_sourceEndPoint, "/taprapper/sensordata", sOscClient);
dataOscMessage.Append(sensorData.AccDataObject.DataVector.X);
dataOscMessage.Append(sensorData.AccDataObject.DataVector.Y);
dataOscMessage.Append(sensorData.AccDataObject.DataVector.Z);
dataOscMessage.Append(sensorData.GyroDataObject.Pitch);
dataOscMessage.Append(sensorData.GyroDataObject.Roll);
dataOscMessage.Append(sensorData.GyroDataObject.Yaw);
dataOscMessage.Append(sensorData.QuaternionObject.X);
dataOscMessage.Append(sensorData.QuaternionObject.Y);
dataOscMessage.Append(sensorData.QuaternionObject.Z);
dataOscMessage.Append(sensorData.QuaternionObject.W);
return dataOscMessage;
}
public void SendMessage(SensorData sensor1Data, SensorData sensor2Data)
{
OscBundle sBundle = new OscBundle(_sourceEndPoint);
OscMessage dataOscMessage = CreateOscMessage(sensor1Data);
sBundle.Append(dataOscMessage);
dataOscMessage = CreateOscMessage(sensor2Data);
sBundle.Append(dataOscMessage);
_oscMessageQueue.Enqueue(dataOscMessage);
//MemoryStream memoryStream = new MemoryStream();
//Resources.Image.Save(memoryStream, ImageFormat.Jpeg);
//oscMessage.Append(memoryStream.ToArray());
//sSendMessages = false;
}
