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