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); } } }
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; }
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; }
public void EnqueuePacket(NavigationPacket packet) { _packetQueue.Enqueue(packet); }
public static void WritePacket(this BinaryWriter writer, NavigationPacket packet) { writer.Write(packet.Timestamp); writer.Write(packet.Data.Length); writer.Write(packet.Data); }
private void OnNavigationPacketAcquired(NavigationPacket packet) { if (_packetRecorderWorker.IsAlive) _packetRecorderWorker.EnqueuePacket(packet); _navigationPacket = packet; }