private void ReceiveMain()
{
var ep = new IPEndPoint(IPAddress.Any, 0);
bytesReceived = 0;
int lastID = 0;
long lastTimestamp = 0;
var currentProfile = new ProfileFragments();
// this callback will kill the socket when the
// token was canceled, which is the only way to get out
// of the blocking udpClient.Receive()
for (; ;)
{
try
{
token.ThrowIfCancellationRequested();
// next call blocks!
byte[] raw = receiveUdpClient.Receive(ref ep);
if (!isRunning)
{
continue;
}
var header = new PacketHeader(raw);
if (header.Magic == 0xFACD) // Data packet
{
lastReceivedPacketTime = timeBase.ElapsedMilliseconds;
bytesReceived += raw.Length;
var p = new DataPacket(raw, timeBase.ElapsedMilliseconds);
goodPackets++;
int newID = p.Source;
long newTimestamp = p.Timestamp;
if (newID != lastID || newTimestamp != lastTimestamp)
{
if (currentProfile.Count > 0)
{
profileAssembler.AssembleProfiles(currentProfile);
++IncompleteProfilesReceivedCount;
currentProfile.Clear();
}
}
currentProfile.Add(p);
if (currentProfile.Complete)
{
profileAssembler.AssembleProfiles(currentProfile);
++CompleteProfilesReceivedCount;
currentProfile.Clear();
}
lastID = newID;
lastTimestamp = newTimestamp;
}
else if (header.Magic == 0xFACE) // Non-data packets
{
switch (header.Type)
{
case ScanPacketType.Status:
var from = ep.Address;
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();
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;
}
catch (Exception)
{
BadPacketsCount++;
}
}
}
internal void AssembleProfiles(ProfileFragments fragments)
{
var p = new Profile
{
// copy common data from the first fragment
ScanHeadID = (uint)fragments[0].ScanHead,
Camera = fragments[0].Camera,
Laser = fragments[0].Laser,
Timestamp = fragments[0].Timestamp,
LaserOnTime = fragments[0].LaserOnTime,
ExposureTime = fragments[0].ExposureTime,
AllDataFormat = dataFormat,
EncoderValues = new Dictionary <Encoder, long>(fragments[0].NumEncoderVals)
};
// copy encoder vals
for (var i = 0; i < fragments[0].NumEncoderVals; i++)
{
p.EncoderValues[(Encoder)i] = fragments[0].EncoderVals[i];
}
if (rawPointsArrayIndex >= RawPointsArrayCapacity)
{
rawPointsArray = new Point2D[RawPointsArrayCapacity * Globals.RawProfileDataLength];
rawPointsArrayIndex = 0;
}
p.RawPointsMemory = new Memory <Point2D>(rawPointsArray, rawPointsArrayIndex * Globals.RawProfileDataLength,
Globals.RawProfileDataLength);
var rawPointsSpan = p.RawPointsMemory.Span;
rawPointsSpan.Fill(new Point2D(double.NaN, double.NaN, Globals.ProfileDataInvalidBrightness));
var validPointCount = 0;
Point2D[] cameraCoords = null;
if ((fragments[0].Contents & DataType.IM) > 0)
{
// we have image data
p.Image = new byte[1456 * 1088];
}
// helpers local
var fragmentsCount = fragments.Count();
var tr = alignmentParameters[fragments[0].Camera];
var sinRoll = tr.SinRoll;
var cosRoll = tr.CosRoll;
var cosYaw = tr.CosYaw;
var shiftX = tr.ShiftX;
var shiftY = tr.ShiftY;
var xXCoefficient = cosYaw * cosRoll / 1000;
var xYCoefficient = sinRoll / 1000;
var yXCoefficient = cosYaw * sinRoll / 1000;
var yYCoefficient = cosRoll / 1000;
foreach (DataType dt in dataTypes)
{
for (var fragmentNumber = 0; fragmentNumber < fragmentsCount; fragmentNumber++)
{
if ((fragments[0].Contents & dt) != 0)
{
var currentFragment = fragments[fragmentNumber];
var startCol = fragments[0].StartColumn;
var numVals = currentFragment.FragmentLayouts[dt].numVals;
var step = currentFragment.FragmentLayouts[dt].step;
var sourcePos = currentFragment.FragmentLayouts[dt].offset;
var currentFragmentRaw = currentFragment.Raw;
switch (dt)
{
case DataType.LM:
for (var j = 0; j < numVals; j++)
{
var destPos = startCol + (j * fragmentsCount + fragmentNumber) * step;
rawPointsSpan[destPos].Brightness = currentFragmentRaw[sourcePos];
sourcePos++;
}
break;
case DataType.XY:
for (var j = 0; j < numVals; j++)
{
var destPos =
startCol + (j * fragmentsCount + fragmentNumber) *
step; // for looking up brightness
var xraw = (short)(currentFragmentRaw[sourcePos + 1] |
currentFragmentRaw[sourcePos] << 8);
var yraw = (short)(currentFragmentRaw[sourcePos + 3] |
currentFragmentRaw[sourcePos + 2] << 8);
// check for invalid value for pt here
if (xraw != Globals.ProfileDataInvalidXY && yraw != Globals.ProfileDataInvalidXY)
{
validPointCount++;
rawPointsSpan[destPos].X =
xraw * xXCoefficient - yraw * xYCoefficient + shiftX;
rawPointsSpan[destPos].Y =
xraw * yXCoefficient + yraw * yYCoefficient + shiftY;
}
sourcePos += 4;
}
break;
case DataType.PW:
break;
case DataType.VR:
break;
case DataType.SP:
for (var j = 0; j < numVals; j++)
{
if (cameraCoords == null)
{
cameraCoords = new Point2D[Globals.RawProfileDataLength];
}
var col = startCol + (j * fragmentsCount + fragmentNumber) * step;
var rowPixel =
IPAddress.NetworkToHostOrder(BitConverter.ToInt16(currentFragment.Raw,
sourcePos));
cameraCoords[col] = (new Point2D(rowPixel, col, rawPointsSpan[col].Brightness));
sourcePos += 2;
}
break;
case DataType.IM:
//TODO:: Adapt to use SP type
if (fragmentNumber == fragmentsCount - 1)
{
if (cameraCoords == null)
{
cameraCoords = new Point2D[Globals.RawProfileDataLength];
}
for (int i = 0; i < 1456; i++)
{
int rowPixel = BitConverter.ToInt16(currentFragment.Raw, sourcePos);
sourcePos += 2;
int brightness = BitConverter.ToInt16(currentFragment.Raw, sourcePos);
sourcePos += 2;
if (brightness < 0x8000)
{
brightness = brightness / 7;
}
else
{
rowPixel = Globals.ProfileDataInvalidSubpixel;
brightness = 0;
}
cameraCoords[i] = new Point2D(rowPixel, i, brightness);
}
break;
}
for (var j = 0; j < numVals; j++)
{
var pos = fragmentNumber * 4 * 1456 + j;
p.Image[pos] = currentFragment.Raw[sourcePos++];
}
break;
default:
break;
}
}
}
}
if (cameraCoords != null)
{
p.CameraCoordinates = cameraCoords.ToArray();
}
p.ValidPointCount = validPointCount;
if (!profiles.TryAdd(p))
{
ProfileBufferOverflowed = true;
profiles.TryTake(out _);
profiles.TryAdd(p);
}
rawPointsArrayIndex++;
}
internal void AssembleProfiles(ProfileFragments fragments)
{
var seedPacket = fragments[0];
var p = CreateNewProfile(seedPacket);
if (rawPointsArrayIndex >= RawPointsArrayCapacity)
{
rawPointsArray = new Point2D[RawPointsArrayCapacity * Globals.RawProfileDataLength];
rawPointsArrayIndex = 0;
}
p.RawPointsMemory = new Memory <Point2D>(rawPointsArray, rawPointsArrayIndex * Globals.RawProfileDataLength,
Globals.RawProfileDataLength);
var rawPointsSpan = p.RawPointsMemory.Span;
rawPointsSpan.Fill(new Point2D(double.NaN, double.NaN, Globals.ProfileDataInvalidBrightness));
var tr = alignmentParameters[seedPacket.Camera];
double sinRoll = tr.SinRoll;
double cosRoll = tr.CosRoll;
double cosYaw = tr.CosYaw;
double shiftX = tr.ShiftX;
double shiftY = tr.ShiftY;
double xXCoefficient = cosYaw * cosRoll / 1000;
double xYCoefficient = sinRoll / 1000;
double yXCoefficient = cosYaw * sinRoll / 1000;
double yYCoefficient = cosRoll / 1000;
foreach (var currentFragment in fragments)
{
var dataTypes = currentFragment.Contents;
short startCol = currentFragment.StartColumn;
int partNum = currentFragment.PartNum;
int totalParts = currentFragment.NumParts;
byte[] currentFragmentRaw = currentFragment.Raw;
foreach (var dt in dataTypes.GetFlags())
{
var layout = currentFragment.FragmentLayouts[dt];
int numVals = layout.numVals;
int step = layout.step;
int srcIdx = layout.offset;
int inc = totalParts * step;
int destIdx = startCol + (partNum * step);
switch (dt)
{
case DataType.LM:
for (int j = 0; j < numVals; j++)
{
rawPointsSpan[destIdx].Brightness = currentFragmentRaw[srcIdx];
++srcIdx;
destIdx += inc;
}
break;
case DataType.XY:
for (int j = 0; j < numVals; j++)
{
short xraw = (short)(currentFragmentRaw[srcIdx + 1] | (currentFragmentRaw[srcIdx] << 8));
short yraw = (short)(currentFragmentRaw[srcIdx + 3] | (currentFragmentRaw[srcIdx + 2] << 8));
srcIdx += 4;
if (xraw != Globals.ProfileDataInvalidXY && yraw != Globals.ProfileDataInvalidXY)
{
p.ValidPointCount++;
rawPointsSpan[destIdx].X = (xraw * xXCoefficient) - (yraw * xYCoefficient) + shiftX;
rawPointsSpan[destIdx].Y = (xraw * yXCoefficient) + (yraw * yYCoefficient) + shiftY;
}
destIdx += inc;
}
break;
case DataType.SP:
for (int j = 0; j < numVals; j++)
{
short rowPixel = (short)(currentFragmentRaw[srcIdx + 1] | (currentFragmentRaw[srcIdx] << 8));
srcIdx += 2;
p.CameraCoordinates[destIdx] = new Point2D(rowPixel, destIdx, rawPointsSpan[destIdx].Brightness);
destIdx += inc;
}
break;
case DataType.IM:
// TODO: Adapt to use SP type
// last packet is subpixel data corresponding to laser line
if (partNum == totalParts - 1)
{
for (int i = 0; i < 1456; i++)
{
// TODO: SP data doesn't get sent in network order
int rowPixel = currentFragmentRaw[srcIdx + 1] << 8 | currentFragmentRaw[srcIdx];
int brightness = currentFragmentRaw[srcIdx + 3] << 8 | currentFragmentRaw[srcIdx + 2];
srcIdx += 4;
if (brightness < 0x8000)
{
brightness /= 7;
}
else
{
rowPixel = Globals.ProfileDataInvalidSubpixel;
brightness = Globals.ProfileDataInvalidBrightness;
}
p.CameraCoordinates[i] = new Point2D(rowPixel, i, brightness);
}
break;
}
int pos = partNum * 4 * 1456;
Array.Copy(currentFragmentRaw, srcIdx, (Array)p.Image, pos, numVals);
break;
}
}
}
if (!profiles.TryAdd(p))
{
ProfileBufferOverflowed = true;
profiles.TryTake(out _);
profiles.TryAdd(p);
}
rawPointsArrayIndex++;
}
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++;
}
}
}
