protected override void Loop(CancellationToken token)
{
_isAcquiring = false;
using (var udpClient = new UdpClient(NavdataPort))
try
{
udpClient.Connect(_configuration.DroneHostname, NavdataPort);
SendKeepAliveSignal(udpClient);
var remoteEp = new IPEndPoint(IPAddress.Any, NavdataPort);
Stopwatch swKeepAlive = Stopwatch.StartNew();
Stopwatch swNavdataTimeout = Stopwatch.StartNew();
while (token.IsCancellationRequested == false && swNavdataTimeout.ElapsedMilliseconds < NavdataTimeout)
{
if (udpClient.Available == 0)
{
Thread.Sleep(1);
}
else
{
byte[] data = udpClient.Receive(ref remoteEp);
var packet = new NavigationPacket
{
Timestamp = DateTime.UtcNow.Ticks,
Data = data
};
_packetAcquired(packet);
_isAcquiring = true;
swNavdataTimeout.Restart();
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Restart();
}
}
}
finally
{
if (_isAcquiring)
{
_isAcquiring = false;
_onAcquisitionStopped();
}
}
}
private void OnNavdataPacketAcquired(NavigationPacket packet)
{
if (NavigationPacketAcquired != null)
NavigationPacketAcquired(packet);
UpdateNavigationData(packet);
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
_navigationData = navigationData;
ProcessTransition();
if (NavigationDataUpdated != null)
NavigationDataUpdated(_navigationData);
}
}
