/// <summary>
/// Deserialize a scan head status packet from a raw byte stream.
/// </summary>
/// <exception cref="VersionCompatibilityException">Thrown if the scan head version
/// is incompatible with the client API</exception>"
/// <param name="buf"></param>
internal ScanHeadStatus(byte[] buf)
{
int idx = 0;
// The header and version data should never change to maintain backwards compatibility
Header = new PacketHeader
{
Magic = NetworkByteUnpacker.ExtractUShortFromNetworkBuffer(buf, ref idx),
Size = NetworkByteUnpacker.ExtractByteFromNetworkBuffer(buf, ref idx),
Type = (ScanPacketType)NetworkByteUnpacker.ExtractByteFromNetworkBuffer(buf, ref idx)
};
FirmwareVersion = new ScanHeadVersionInformation
{
Major = NetworkByteUnpacker.ExtractUIntFromNetworkBuffer(buf, ref idx),
Minor = NetworkByteUnpacker.ExtractUIntFromNetworkBuffer(buf, ref idx),
Patch = NetworkByteUnpacker.ExtractUIntFromNetworkBuffer(buf, ref idx),
Commit = NetworkByteUnpacker.ExtractUIntFromNetworkBuffer(buf, ref idx),
HardwareId =
(ScanHeadVersionHardwareId)NetworkByteUnpacker.ExtractUShortFromNetworkBuffer(buf, ref idx),
Flags = NetworkByteUnpacker.ExtractUShortFromNetworkBuffer(buf, ref idx)
};
// Minor versions can be differ, but differences in
// major versions are considered to be incompatible
if (FirmwareVersion.Major != VersionInformation.Major)
{
throw new VersionCompatibilityException(FirmwareVersion);
}
// Static Data
ScanHeadSerialNumber = NetworkByteUnpacker.ExtractIntFromNetworkBuffer(buf, ref idx);
MaxScanRate = NetworkByteUnpacker.ExtractIntFromNetworkBuffer(buf, ref idx);
ScanHeadIPAddress = NetworkByteUnpacker.ExtractIPAddressFromNetworkBuffer(buf, ref idx);
ClientIPAddress = NetworkByteUnpacker.ExtractIPAddressFromNetworkBuffer(buf, ref idx);
ClientUdpPort = NetworkByteUnpacker.ExtractUShortFromNetworkBuffer(buf, ref idx);
ScanSyncID = NetworkByteUnpacker.ExtractUShortFromNetworkBuffer(buf, ref idx);
GlobalTime = NetworkByteUnpacker.ExtractLongFromNetworkBuffer(buf, ref idx);
NumPacketsSent = NetworkByteUnpacker.ExtractIntFromNetworkBuffer(buf, ref idx);
ProfilesSentCount = NetworkByteUnpacker.ExtractIntFromNetworkBuffer(buf, ref idx);
NumValidEncoders = NetworkByteUnpacker.ExtractByteFromNetworkBuffer(buf, ref idx);
NumValidCameras = NetworkByteUnpacker.ExtractByteFromNetworkBuffer(buf, ref idx);
// Variable Length Data
EncoderValues = new Dictionary <Encoder, long>(NumValidEncoders);
foreach (int i in Enumerable.Range(0, NumValidEncoders))
{
Encoder encoder = (Encoder)Enum.ToObject(typeof(Encoder), i);
var encoderValue = NetworkByteUnpacker.ExtractLongFromNetworkBuffer(buf, ref idx);
EncoderValues.Add(encoder, encoderValue);
}
PixelsInWindow = new Dictionary <Camera, int>(NumValidCameras);
foreach (int i in Enumerable.Range(0, NumValidCameras))
{
Camera camera = (Camera)Enum.ToObject(typeof(Camera), i);
var pixels = NetworkByteUnpacker.ExtractIntFromNetworkBuffer(buf, ref idx);
PixelsInWindow.Add(camera, pixels);
}
Temperatures = new Dictionary <TemperatureSensor, float>(NumValidCameras);
foreach (int i in Enumerable.Range(0, NumValidCameras))
{
var temperatureSensor = (TemperatureSensor)Enum.ToObject(typeof(TemperatureSensor), i);
var temperature = NetworkByteUnpacker.ExtractFloatFromNetworkBuffer(buf, ref idx);
Temperatures.Add(temperatureSensor, temperature);
}
}
private void ReceiveMain()
{
IPEndPoint ep = new IPEndPoint(IPAddress.Any, 0);
bytesReceived = 0;
// this callback will kill the socket when the
// token was canceled, which is the only way to get out
// of the blocking udpClient.Receive()
var demuxerDict = new Dictionary <int, ProfileFragments>();
for (;;)
{
try
{
token.ThrowIfCancellationRequested();
// next call blocks!
var raw = receiveUdpClient.Receive(ref ep);
if (!isRunning)
{
continue; // we ignore everything
}
var from = ep.Address;
var header = new PacketHeader(raw);
if (header.Magic == 0xFACD) // Data packet
{
lastReceivedPacketTime = timeBase.ElapsedMilliseconds;
goodPackets++;
bytesReceived += raw.Length;
var p = new DataPacket(raw, timeBase.ElapsedMilliseconds);
// handle the de-multiplexing
if (p.NumParts == 1) // one part-datagram
{
// hand straight over to workers
profileAssembler.AssembleProfiles(new ProfileFragments(p, timeBase.ElapsedMilliseconds));
continue;
}
// source is a composite of scan head, camera and laser, we use it to identify packets from the same head/camera/laser combo
var id = p.Source;
if (!demuxerDict.ContainsKey(id))
{
// first time we see a packet from this source
demuxerDict[id] = new ProfileFragments(p, timeBase.ElapsedMilliseconds);
}
else
{
if (demuxerDict[id].Timestamp == p.Timestamp)
{
// the timestamp on this packet is the same as is the dict, so it must belong to the same profile
demuxerDict[id].Add(p);
if (demuxerDict[id].Complete)
{
CompleteProfilesReceivedCount++;
// hand it off to a processor thread, we're done with it.
profileAssembler.AssembleProfiles(demuxerDict[id]);
demuxerDict.Remove(id);
// but also record the Id in a fixed size queue, so that stragglers and
// duplicates don't create a mess
// TODO: add a done queue
}
}
else
{
// the timestamp on the current packet is newer than on the existing
// set of packets in the dictionary, which means we either received out of order or
// a packet got lost
// If we got the next in sequence, we now consider the previous profile done
// hand it off to a processor thread, since we're done with it.
profileAssembler.AssembleProfiles(demuxerDict[id]);
demuxerDict.Remove(id);
evictedForNextSeq++;
demuxerDict[id] = new ProfileFragments(p, timeBase.ElapsedMilliseconds);
}
// here we would check for timeouts, but that is tricky since the Receive call blocks.
// We may need to de-couple receiving from assembling.
}
}
else if (header.Magic == 0xFACE) // Non-data packets
{
switch (header.Type)
{
case ScanPacketType.Status:
try
{
scanHead.Status = new StatusPacket(raw, from).ScanHeadStatus;
}
catch (VersionCompatibilityException e)
{
IsVersionMismatched = true;
VersionMismatchReason = e.Message;
// Versions are not compatible, try to send a disconnect before bailing
CreateIPEndPoint(from);
Disconnect();
Stop();
break;
}
if (scanHead.SerialNumber == scanHead.Status.ScanHeadSerialNumber &&
scanHeadDataIpEndPoint == null)
{
CreateIPEndPoint(from);
}
if (scanHeadDataIpEndPoint != null)
{
lastReceivedPacketTime = timeBase.ElapsedMilliseconds;
}
break;
default:
// Unknown command
BadPacketsCount++;
break;
}
}
else
{
// Wrong signature for received packet: expected 0xFACE or 0xFACD
BadPacketsCount++;
}
}
catch (ObjectDisposedException)
{
// Time to break out of receive loop
break;
}
catch (SocketException)
{
// We get here if we call Close() on the UdpClient
// while it is in the Receive(0 call. Apparently
// the only way to abort a Receive call is to
// close the underlying Socket.
break;
}
catch (OperationCanceledException)
{
// Time to break out of receive loop
break;
}
#pragma warning disable CA1031 // Do not catch general exception types
catch (Exception)
#pragma warning restore CA1031 // Do not catch general exception types
{
BadPacketsCount++;
}
}
}
