private void OnNavigationPacketAcquired(NavigationPacket packet)
{
if (_packetRecorderWorker.IsAlive)
_packetRecorderWorker.EnqueuePacket(packet);
_navigationPacket = packet;
}
public NavigationPacket ReadNavigationPacket()
{
var packet = new NavigationPacket();
packet.Timestamp = ReadInt64();
int dataSize = ReadInt32();
packet.Data = ReadBytes(dataSize);
return packet;
}
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
};
swNavdataTimeout.Restart();
_isAcquiring = true;
_onAcquisitionStarted();
_packetAcquired(packet);
}
if (swKeepAlive.ElapsedMilliseconds > KeepAliveTimeout)
{
SendKeepAliveSignal(udpClient);
swKeepAlive.Restart();
}
}
}
catch (Exception e)
{
}
finally
{
if (_isAcquiring)
{
_isAcquiring = false;
_onAcquisitionStopped();
}
}
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
OnNavigationDataAcquired(navigationData);
_navigationData = navigationData;
ProcessStateTransitions(navigationData.State);
}
}
private void OnNavdataPacketAcquired(NavigationPacket packet)
{
if (NavigationPacketAcquired != null)
NavigationPacketAcquired(packet);
UpdateNavigationData(packet);
}
public void EnqueuePacket(NavigationPacket packet)
{
_packetQueue.Enqueue(packet);
}
private void UpdateNavigationData(NavigationPacket packet)
{
NavigationData navigationData;
if (NavigationPacketParser.TryParse(ref packet, out navigationData))
{
_navigationData = navigationData;
ProcessTransition();
if (NavigationDataUpdated != null)
NavigationDataUpdated(_navigationData);
}
}
public void Write(NavigationPacket packet)
{
Write(packet.Timestamp);
Write(packet.Data.Length);
Write(packet.Data);
}
private void OnNavigationPacketAcquired(NavigationPacket obj)
{
// do nothing
}
private void OnNavigationPacketAcquired(NavigationPacket packet)
{
if (packetRecorderWorker != null && packetRecorderWorker.IsAlive)
packetRecorderWorker.EnqueuePacket(packet);
navigationPacket = packet;
}
private static void OnNavigationPacketAcquired(NavigationPacket packet)
{
_navigationPacket = packet;
}
