hawkeye camera support(not tested)

2025-08-19 12:45:50 +02:00
parent f80b275ad8
commit 6ef6aced8d
170 changed files with 20190 additions and 72 deletions
--- a/framework/Inspectron.HawkEye/UDPB/UDPBSequence.cs
+++ b/framework/Inspectron.HawkEye/UDPB/UDPBSequence.cs
@@ -0,0 +1,142 @@
+using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+
+namespace Inspectron.HawkEye.UDPB
+{
+    public class UDPBSequence
+    {
+        private readonly uint _sequenceId;
+        private readonly UDPBSocket _socket;
+        private readonly ConcurrentQueue<byte[]> _receiveQueue;
+        private readonly byte[] _reconstructionBuffer = new byte[10 * 1024 * 1024]; //10 mb
+
+
+        public bool IsFinished { get; private set; }
+        private readonly UDPBPacket[] _buffer = new UDPBPacket[10000];
+        private List<uint> _nakPackets;
+
+        public UDPBSequence(uint sequenceId, UDPBSocket socket, ConcurrentQueue<byte[]> receiveQueue)
+        {
+            _sequenceId = sequenceId;
+            _socket = socket;
+            _receiveQueue = receiveQueue;
+        }
+
+        public uint SequenceId => _sequenceId;
+
+
+        public void AddData(UDPBPacket dataPacket)
+        {
+            if (dataPacket.SequenceId != _sequenceId)
+            {
+                //error?
+                Console.WriteLine($"Error! Unexpected packet sequence {dataPacket.SequenceId} expecting {_sequenceId}");
+                return;
+            }
+
+
+            _buffer[dataPacket.MessageId] = dataPacket;
+
+            if (IsReconstructing)
+            {
+                _nakPackets.Remove(dataPacket.MessageId);
+                Console.WriteLine($"packet {dataPacket.MessageId} restored");
+
+            }
+            
+        }
+
+        
+
+        public byte[] LastReconstructedBuffer { get; private set; }
+
+        
+        public bool FinishSequence(uint packets, out List<uint> missingPacketIds, 
+            out int dataSize)
+        {
+            
+            missingPacketIds = new List<uint>();
+            
+            dataSize = 0;
+            var defaultDataSize = UDPBQueue.PACKET_SIZE - UDPBPacket.packetHeaderSize;
+            //validate
+            for (uint i = 1; i < packets; i++)
+                if (_buffer[i]==null)
+                {
+                    missingPacketIds.Add(i);
+                    dataSize += defaultDataSize;
+                    //error
+                }
+                else
+                {
+                    var packetLen = _buffer[i].Payload.Length;
+                    Buffer.BlockCopy(_buffer[i].Payload, 0, _reconstructionBuffer, dataSize, packetLen);
+                    dataSize += packetLen;
+                }
+
+            if (missingPacketIds.Count > 0)
+            {
+                Console.WriteLine($"Error! Missing packets:{missingPacketIds.Count} seq:{_sequenceId}");
+                //send Nak
+                
+                
+                
+                _nakPackets = missingPacketIds;
+                var nakLine = _nakPackets.Select(x => x.ToString()).Aggregate((s1, s2) => s1 + "," + s2);
+                Console.WriteLine($"Nak: {nakLine}");
+                RequestMissingPackets(missingPacketIds);
+                RequestAgainIn(TimeSpan.FromMilliseconds(10), missingPacketIds);
+                IsReconstructing = true;
+                
+                return false;
+            }
+            else
+            {
+                var packet = new UDPBPacket();
+                packet.SequenceId = SequenceId;
+                packet.Type = UDPBPacket.EPacketType.Ok;
+                IsFinished = true;
+                Console.WriteLine($"Finished seq:{_sequenceId}");
+                _socket.SendPacket(packet);
+                LastReconstructedBuffer = new byte[dataSize];
+                Array.Copy(_reconstructionBuffer, 0, LastReconstructedBuffer, 0, dataSize);
+                _receiveQueue.Enqueue(LastReconstructedBuffer);
+                return true;
+            }
+
+            
+        }
+        private bool _shouldRequest=false;
+        private void RequestAgainIn(TimeSpan timeout, List<uint> missingPacketIds)
+        {
+            _shouldRequest = true;
+
+
+
+        }
+
+        private void RequestMissingPackets(List<uint> missingPacketIds)
+        {
+            MemoryStream ms = new MemoryStream();
+            BinaryWriter sw = new BinaryWriter(ms);
+            var waitFor = missingPacketIds.Take(10).ToList();
+            sw.Write(waitFor.Count);
+            foreach (uint id in waitFor)
+            {
+                sw.Write(id);
+            }
+
+            var packet = new UDPBPacket();
+            packet.SequenceId = SequenceId;
+            packet.Payload = ms.ToArray();
+            packet.Length = (uint) packet.Payload.Length;
+            packet.Type = UDPBPacket.EPacketType.Nak;
+            _socket.SendPacket(packet);
+        }
+
+        public bool IsReconstructing { get; private set; }
+    }
+}