public static unsafe bool TryParse(ref NavigationPacket packet, out NativeNavdata navigationData) { byte[] data = packet.Data; navigationData = new NativeNavdata(); if (data.Length < sizeof (navdata_t)) return false; fixed (byte* pData = &data[0]) { navdata_t navdata_t = *(navdata_t*) pData; if (navdata_t.header == NavdataHeader) { navigationData.ardrone_state = (def_ardrone_state_mask_t) navdata_t.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; }
protected override void Loop(CancellationToken token) { _udpClient.Connect(_config.Hostname, NavdataPort); SendKeepAliveSignal(); 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(); swKeepAlive.Restart(); } Thread.Sleep(5); } }
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; }
private void OnNavigationPacketAcquired(NavigationPacket packet) { if (_navigationData.State == NavigationState.Unknown) _request = Request.Initialize; if (NavigationPacketAcquired != null) NavigationPacketAcquired(packet); NativeNavdata nativeNavdata; if (NativeNavdataParser.TryParse(ref packet, out nativeNavdata)) { NavigationData navigationData = nativeNavdata.ToNavigationData(); OnNavigationDataDecoded(navigationData); } }
public static void WritePacket(this BinaryWriter writer, NavigationPacket packet) { writer.Write(packet.Timestamp); writer.Write(packet.Data.Length); writer.Write(packet.Data); }
public void EnqueuePacket(NavigationPacket packet) { _packetQueue.Enqueue(packet); }