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;
}
