public AxisOrangeUnityData(AxisOrangeData raw)
{
timestamp = raw.timestamp;
acc = new Vector3(raw.acc[0], raw.acc[2], raw.acc[1]);
gyro = new Vector3(raw.gyro[0], raw.gyro[2], raw.gyro[1]);
quaternion = new Quaternion(raw.quaternion[1], raw.quaternion[3], raw.quaternion[2], -raw.quaternion[0]).normalized;
}
void SerialRecieveLoop()
{
SerialHeader header = default;
AxisOrangeData data = default;
AxisOrangeButton button = default;
while (serialDevice.IsNotNullAndOpened())
{
// serial read
if (serialDevice.BytesToRead >= SerialHeaderDef.HeaderLength &&
serialReader.TryReadSerialHeader(serialDevice, ref header))
{
if (header.dataId == SerialHeaderDef.ImuDataId)
{
if (serialReader.TryReadImuData(serialDevice, header.dataLength, ref data))
{
OnSensorDataUpdate.Invoke(data);
}
}
else if (header.dataId == SerialHeaderDef.ButtonDataId)
{
if (serialReader.TryReadButtonData(serialDevice, header.dataLength, ref button))
{
OnSensorButtonUpdate.Invoke(button);
}
}
else
{
// Do Nothing
}
}
// finish loop?
if (isListening)
{
System.Threading.Thread.Sleep(1);
}
else
{
break;
}
}
}
public bool TryReadImuData(SerialPort serial, int dataLength, ref AxisOrangeData data)
{
if (!serial.IsNotNullAndOpened())
{
return(false);
}
var buf = new byte[dataLength];
if (serial.Read(buf, 0, dataLength) != dataLength)
{
return(false);
}
var t = buf.ToUInt(0);
var acc = buf.ToVector3(4);
var gyro = buf.ToVector3(16);
var quat = buf.ToQuaternion(28);
data = new AxisOrangeData(t, acc, gyro, quat);
return(true);
}