예제 #1
0
        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);
        }
예제 #2
0
        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);
        }
예제 #3
0
        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;
            }
        }
예제 #4
0
        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);
                }
            }
        }
예제 #5
0
        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));
        }