Ejemplo n.º 1
0
        protected override void Loop(CancellationToken token)
        {
            using (var udpClient = new UdpClient(NavdataPort))
            {
                udpClient.Connect(_configuration.DroneHostname, NavdataPort);

                SendKeepAliveSignal(udpClient);

                var droneEp = new IPEndPoint(IPAddress.Any, NavdataPort);
                Stopwatch swKeepAlive = Stopwatch.StartNew();
                Stopwatch swNavdataTimeout = Stopwatch.StartNew();
                while (token.IsCancellationRequested == false &&
                       swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
                {
                    if (udpClient.Available > 0)
                    {
                        byte[] data = udpClient.Receive(ref droneEp);
                        var packet = new NavigationPacket
                            {
                                Timestamp = DateTime.UtcNow.Ticks,
                                Data = data
                            };
                        _navigationPacketAcquired(packet);
                        swNavdataTimeout.Restart();
                    }

                    if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
                    {
                        SendKeepAliveSignal(udpClient);
                        swKeepAlive.Restart();
                    }
                    Thread.Sleep(5);
                }
            }
        }
Ejemplo n.º 2
0
        public static unsafe bool TryParse(ref NavigationPacket packet, out NavdataBag navigationData)
        {
            byte[] data = packet.Data;
            navigationData = new NavdataBag();

            if (data.Length < sizeof (navdata_t))
                return false;

            fixed (byte* pData = &data[0])
            {
                navdata_t navdata = *(navdata_t*) pData;
                if (navdata.header == NavdataHeader)
                {
                    navigationData.ardrone_state = navdata.ardrone_state;

                    int offset = sizeof (navdata_t);
                    while (offset < data.Length)
                    {
                        var option = (navdata_option_t*) (pData + offset);
                        ProcessOption(option, ref navigationData);
                        offset += option->size;
                    }
                    uint dataCheckSum = CalculateChecksum(data);
                    if (navigationData.cks.cks == dataCheckSum)
                    {
                        return true;
                    }
                }
            }
            return false;
        }
Ejemplo n.º 3
0
 public static NavigationPacket ReadNavigationPacket(this BinaryReader reader)
 {
     var packet = new NavigationPacket();
     packet.Timestamp = reader.ReadInt64();
     int dataSize = reader.ReadInt32();
     packet.Data = reader.ReadBytes(dataSize);
     return packet;
 }
Ejemplo n.º 4
0
 public void EnqueuePacket(NavigationPacket packet)
 {
     _packetQueue.Enqueue(packet);
 }
Ejemplo n.º 5
0
 public static void WritePacket(this BinaryWriter writer, NavigationPacket packet)
 {
     writer.Write(packet.Timestamp);
     writer.Write(packet.Data.Length);
     writer.Write(packet.Data);
 }
Ejemplo n.º 6
0
        private void OnNavigationPacketAcquired(NavigationPacket packet)
        {
            if (_packetRecorderWorker.IsAlive) _packetRecorderWorker.EnqueuePacket(packet);

            _navigationPacket = packet;
        }