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 void ReadAccGyroData(SensorDataRaw sensorRawDataObj)
{
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] = 0x20; //command number
writebuffer[3] = (byte)(0x20 + _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;
}
sensorRawDataObj.AccGyroReadbuffer = readbuffer;
}
}
}
public SensorDataRaw ReadData()
{
SensorDataRaw sensorDataObj = new SensorDataRaw();
ReadRawAccGyroData(sensorDataObj);
//ReadOrientation(sensorDataObj);
return(sensorDataObj);
}
/// <summary>
/// Receives raw JSON data as a string. Converts it to SensorDataRaw (contains variables and lists to store all data acquired from raw JSON) using JsonUtility. Sends converted data to ProcessCurrentData().
/// </summary>
/// <param name="json"></param>
public void JsonToObject(string json)
{
SensorDataRaw d = JsonUtility.FromJson <SensorDataRaw>(json);
//sensorDataRaw.Add(d);
//currentSensorDataRaw = d;
ProcessCurrentData(d);
}
/// <summary>
/// Calls UWBObjectManager to find an object in the scene by address. If the object is found, sends sensor data to it.
/// </summary>
/// <param name="data"></param>
public void SendDataToUWBObject(SensorDataRaw data)
{
UWBObject obj = UWBMan.FindObjectByAddress(data.address);
if (obj != null)
{
obj.ReceiveData(data);
}
}
private void SpaceSensorRecordingThread()
{
while (_spaceRecordingStatus)
{
SensorDataRaw sensor1DataObj = SpaceSensorObject1.ReadData();
SensorDataRaw sensor2DataObj = SpaceSensorObject2.ReadData();
SpaceSensorDataEvent(this, new SpaceSensorRecordingDataReceivedEventArgs(sensor1DataObj, sensor2DataObj));
Thread.Sleep(1);
}
}
/// <summary>
/// Processes SensorDataRaw object data. If scene doesn't have an object with the received data address (address from tag), then it tells UWBObjectManager to create and add a new one to the scene with that address.
/// Otherwise calls SendDataToUWBObject() to send the data to the object in scene. After that, removes that data from the data list so that next ones can be processed.
/// </summary>
/// <param name="currentData"></param>
void ProcessCurrentData(SensorDataRaw currentData)
{
// check if there is an object with address
if (!UWBMan.SceneHasObjectWithAddress(currentData.address))
{
UWBMan.CreateNewUWBObject(currentData.address);
}
else
{
SendDataToUWBObject(currentData);
}
//currentSensorDataRaw = null;
//sensorDataRaw.RemoveAt(0);
}
/// <summary>
/// Processes SensorDataRaw object data. If scene doesn't have an object with the received data address (address from tag), then it tells UWBObjectManager to create and add a new one to the scene with that address.
/// Otherwise calls SendDataToUWBObject() to send the data to the object in scene. After that, removes that data from the data list so that next ones can be processed.
/// </summary>
/// <param name="currentData"></param>
void ProcessCurrentData(SensorDataRaw currentData)
{
// check if there is an object with address
if (!UWBObjectManager.i.SceneHasObjectWithAddress(currentData.address))
{
UnityMainThreadDispatcher.Instance().Enqueue(() => UWBObjectManager.i.CreateNewUWBObject(currentData.address)); // Calling external methods from non-main thread is not allowed, so we need a script that calls them on main thread.
}
else
{
UnityMainThreadDispatcher.Instance().Enqueue(() => SendDataToUWBObject(currentData)); // Calling external methods from non-main thread is not allowed, so we need a script that calls them on main thread.
//SendDataToUWBObject(currentData);
}
//currentSensorDataRaw = null;
//sensorDataRaw.RemoveAt(0);
}
/// <summary>
/// Replays recorded JSON data in real-time by iterating over the recordingList. Calculates time between each iteration based on data provided by JSON data entry in the list.
/// </summary>
/// <returns></returns>
IEnumerator Play()
{
foreach (string s in recordingList)
{
// get the sensor data from current entry in recording list
SensorDataRaw sensorData = jsonWorker.JsonToObjectReturn(s);
// get the timestamp for current entry; we know that the data entry is going to start either with the button press or a position change (where we really just need the 1st data), so we just check for those; get timestamp from that data entry;
foreach (Datastream ds in sensorData.datastreams)
{
var val = string.Concat(ds.current_value.Where(c => !char.IsWhiteSpace(c)));
if (ds.id == "user_button")
{
currentDataTimestamp = ds.at;
break;
}
if (ds.id == "posX")
{
currentDataTimestamp = ds.at;
break;
}
}
// for the first data entry prevDataTimestamp is going to be empty, so to not get errors we set it to currentDataTimestamp
if (String.IsNullOrEmpty(previousDataTimestamp))
{
previousDataTimestamp = currentDataTimestamp;
}
// convert timestamps for current and previous data from JSON to DateTime objects and get the time between them
DateTime curTime = ConvertStringToDateTime(currentDataTimestamp);
DateTime prevTime = ConvertStringToDateTime(previousDataTimestamp);
TimeSpan duration = curTime.Subtract(prevTime);
// convert TimeSpan to floats separately to minutes, seconds and milliseconds and calculate those into seconds to later use in WaitForSeconds
float minutes = duration.Minutes;
float seconds = duration.Seconds;
float milliseconds = duration.Milliseconds;
float timeBetweenData = (minutes * 60f) + (seconds) + (milliseconds / 1000f);
jsonWorker.JsonToObject(s);
yield return(new WaitForSeconds(timeBetweenData));
previousDataTimestamp = currentDataTimestamp;
}
playRecording = false;
}
public void ReceiveData(SensorDataRaw _data)
{
#region Maybe needed
//data = _data;
#endregion
foreach (Datastream ds in _data.datastreams)
{
// Remove any whitespace, we don't need those
var val = string.Concat(ds.current_value.Where(c => !char.IsWhiteSpace(c)));
// If user button is pressed, do something
// if (ds.id == "user_button") { OnButtonPressed(); break; }
// goes un Unity X axis
if (ds.id == "posX")
{
if (!m_OverridePositionState[0])
{
print("setting X pos from tag");
float.TryParse(val, out position.x);
}
else
{
print("setting X pos from override");
position.x = m_OverridePosition.x;
}
}
// this goes on Unity z axis
if (ds.id == "posY")
{
if (!m_OverridePositionState[2])
{
print("setting Z pos from tag");
float.TryParse(val, out position.z);
}
else
{
print("setting Z pos from override");
position.z = m_OverridePosition.z;
}
}
// this is height, but it's weird now and doesn't really work, so just set it to static value
if (ds.id == "posZ")
{
if (!m_OverridePositionState[1])
{
float.TryParse(val, out position.y);
}
else
{
position.y = m_OverridePosition.y;
}
}
UpdateObjectPosition(position);
// Create quaternion from string and update rotation with it
// Need to figure out if sensor quaternions are the same as unity, because rotations seem weird
if (ds.id == "quaternion" && allowRotation)
{
Quaternion rot = ExtensionMethods.StringToQuaternion(val);
Vector3 eulerRot = QuaternionToEuler(rot);
UpdateObjectRotation(eulerRot);
break;
}
else if (ds.id == "quaternion" && !allowRotation)
{
Quaternion rot = Quaternion.identity;
UpdateObjectRotation(rot);
}
}
}
/// <summary>
/// Converts raw JSON data to a SensorDataRaw object and returns it.
/// </summary>
/// <param name="json"></param>
/// <returns></returns>
public SensorDataRaw JsonToObjectReturn(string json)
{
SensorDataRaw d = JsonUtility.FromJson <SensorDataRaw>(json);
return(d);
}
public SpaceSensorRecordingDataReceivedEventArgs(SensorDataRaw sensor1DataPacket, SensorDataRaw sensor2DataPacket)
{
_sensor1DataPacket = sensor1DataPacket;
_sensor2DataPacket = sensor2DataPacket;
}
