internal static ImuEventData Parse(long timestamp, Tuple <double, double, double, double, double, double> item, bool hasCamera)
{
ImuEventData obj = new ImuEventData();
obj._Time = (double)timestamp / (1000 * 1000 * 1000);
obj._GyroX = item.Item1;
obj._GyroY = item.Item2;
obj._GyroZ = item.Item3;
obj._AccelX = item.Item4;
obj._AccelY = item.Item5;
obj._AccelZ = item.Item6;
//TODO:
obj._Temperatur = 0;
obj._HasCameraImage = hasCamera;
return(obj);
}
internal static ImuEventData Parse(byte[] data, int offset, double gyroScale, double accScale, double tempScale, double tempOffset)
{
ImuEventData obj = new ImuEventData();
UInt32 time_ms = BitConverter.ToUInt32(data, 0);
UInt32 time_us = BitConverter.ToUInt32(data, 4);
int multi = (int)(time_ms / ((Math.Pow(2, 32)) / 1000));
obj._Time = (time_us + multi * Math.Pow(2, 32)) / (1000 * 1000);
obj._GyroX = ((double)BitConverter.ToInt16(data, 8)) / gyroScale;
obj._GyroY = ((double)BitConverter.ToInt16(data, 10)) / gyroScale;
obj._GyroZ = ((double)BitConverter.ToInt16(data, 12)) / gyroScale;
obj._AccelX = ((double)BitConverter.ToInt16(data, 14)) / accScale;
obj._AccelY = ((double)BitConverter.ToInt16(data, 16)) / accScale;
obj._AccelZ = ((double)BitConverter.ToInt16(data, 18)) / accScale;
obj._Temperatur = ((double)BitConverter.ToInt16(data, 20)) / tempScale + tempOffset;
obj._HasCameraImage = BitConverter.ToBoolean(data, 22);
return(obj);
}
private void _CameraEventLabel_Fired(LinkUpEventLabel label, byte[] data)
{
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberImu = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCamImu = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCam = null;
lock (_Subscriptions)
{
subscriberCam = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraEvent).ToList();
subscriberImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.ImuEvent).ToList();
subscriberCamImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraImuEvent).ToList();
}
if (label == _CameraEventLabel && subscriberCam != null && subscriberCam.Count() > 0)
{
CameraEventData eventData = CameraEventData.Parse(data, 0, true);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCam)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
else if (label == _ImuEventLabel && subscriberImu != null && subscriberImu.Count() > 0)
{
ImuEventData eventData = ImuEventData.Parse(data, 0, _SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberImu)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
else if (label == _CameraImuEventLabel)
{
ImuEventData imuEventData = ImuEventData.Parse(data, 0, _SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
CameraEventData cameraEventData = null;
if (imuEventData.HasCameraImage)
{
cameraEventData = CameraEventData.Parse(data, 23, true);
}
if (subscriberCamImu != null && subscriberCamImu.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCamImu)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData, imuEventData
});
}
else
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
}
if (subscriberCam != null && subscriberCam.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCam)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData
});
}
}
}
if (subscriberImu != null && subscriberImu.Count() > 0)
{
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberImu)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
lastTimestamp = imuEventData.TimeNanoSeconds;
}
}
private void DoWork()
{
while (_Running)
{
Tuple <ImuEventData, CameraEventData> next;
if (_BackgroundQueue.TryTake(out next, 100))
{
ImuEventData imuEventData = next.Item1;
CameraEventData cameraEventData = next.Item2;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCamImu = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberCam = null;
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriberImu = null;
lock (_Subscriptions)
{
subscriberCamImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraImuEvent).ToList();
subscriberCam = _Subscriptions.Where(c => c.Item2 == ProxyEventType.CameraEvent).ToList();
subscriberImu = _Subscriptions.Where(c => c.Item2 == ProxyEventType.ImuEvent).ToList();
}
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCamImu)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData, imuEventData
});
}
else
{
if (imuEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
}
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberCam)
{
if (cameraEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
cameraEventData
});
}
}
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriberImu)
{
if (imuEventData != null)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
imuEventData
});
}
}
}
else
{
Thread.Sleep(1);
}
}
}
public void ReplayOffline(RawDataReader reader, Action <TimeSpan> updateTime, Action onClose, Func <bool> isPaused, Func <bool> isStopped)
{
_Tasks.Add(Task.Factory.StartNew(() =>
{
ProxyMode = IOProxyMode.Offline;
Stopwatch watch = new Stopwatch();
watch.Start();
int nextTimeUpdate = 1000;
long startTime = -1;
int currentTime = 0;
while (reader.HasNext())
{
while (isPaused())
{
watch.Stop();
Thread.Sleep(500);
watch.Start();
}
if (isStopped())
{
break;
}
Tuple <long, List <Tuple <RawReaderMode, object> > > res = reader.Next();
if (startTime == -1)
{
startTime = res.Item1;
}
ImuEventData imuEventData = null;
CameraEventData cameraEventData = null;
int rawSize = 0;
byte[] rawImage = null;
byte[] rawImu = null;
double exposureTime = 0.0;
foreach (Tuple <RawReaderMode, object> val in res.Item2)
{
if (val.Item1 == RawReaderMode.Imu0)
{
imuEventData = ImuEventData.Parse(res.Item1, (Tuple <double, double, double, double, double, double>)val.Item2, res.Item2.Any(c => c.Item1 == RawReaderMode.Camera0));
rawSize += imuEventData.RawSize;
rawImu = imuEventData.GetRaw(_SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
}
if (val.Item1 == RawReaderMode.Camera0)
{
cameraEventData = CameraEventData.Parse(((Tuple <double, byte[]>)val.Item2).Item2, 0, false, ((Tuple <double, byte[]>)val.Item2).Item1);
rawSize += cameraEventData.RawSize;
rawImage = ((Tuple <double, byte[]>)val.Item2).Item2;
exposureTime = ((Tuple <double, byte[]>)val.Item2).Item1;
}
}
if (ConnectivityState == LinkUpConnectivityState.Connected)
{
if (rawSize > 0)
{
byte[] data = new byte[rawSize];
Array.Copy(rawImu, data, rawImu.Length);
if (rawImage != null)
{
Array.Copy(BitConverter.GetBytes(exposureTime), 0, data, imuEventData.RawSize, sizeof(double));
Array.Copy(rawImage, 0, data, imuEventData.RawSize + sizeof(double), rawImage.Length);
}
_ReplayDataSend.AsyncCall(data);
}
}
else
{
_BackgroundQueue.Add(new Tuple <ImuEventData, CameraEventData>(imuEventData, cameraEventData));
}
currentTime += reader.DeltaTimeMs;
int sleep = (int)(currentTime - watch.ElapsedMilliseconds);
if (sleep > reader.DeltaTimeMs)
{
Thread.Sleep(sleep);
}
if (res.Item1 / 1000 > nextTimeUpdate)
{
nextTimeUpdate += 1000;
updateTime(reader.Length - TimeSpan.FromMilliseconds((res.Item1 - startTime) / (1000 * 1000)));
}
}
reader.Close();
ProxyMode = IOProxyMode.Live;
onClose();
}, TaskCreationOptions.LongRunning));
}