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/UDPBQueue.cs
+++ b/framework/Inspectron.HawkEye/UDPB/UDPBQueue.cs
@@ -0,0 +1,77 @@
+using System;
+using System.Collections.Generic;
+
+namespace Inspectron.HawkEye.UDPB
+{
+    public class UDPBQueue
+    {
+        public const int PACKET_SIZE = 1400;
+        private const int QUEUE_SIZE = 20000;
+        Block[] _dataQueue = new Block[QUEUE_SIZE];
+        private int _queueWritePtr = 0;
+        private uint _messageId = 1;
+        private int _queueReadPtr = 0;
+        public UDPBQueue()
+        {
+            for (int i = 0; i < QUEUE_SIZE; i++)
+            {
+                _dataQueue[i]=new Block();
+                _dataQueue[i].Data=new byte[PACKET_SIZE];
+            }
+        }
+
+        public void Reset()
+        {
+            _messageId = 1;
+        }
+        public void AddBuffer(byte[] data, int offset, int len)
+        {
+            int size = len / PACKET_SIZE;
+            if ((len % PACKET_SIZE) != 0)
+                size++;
+            for (int i = 0; i < size; i++)
+            {
+                uint pktlen = (uint)(len - i * PACKET_SIZE);
+                if (pktlen > PACKET_SIZE)
+                    pktlen = PACKET_SIZE;
+                Array.Copy(data, i * PACKET_SIZE + offset, _dataQueue[_queueWritePtr].Data,0, pktlen);
+                _dataQueue[_queueWritePtr].Length = pktlen;
+                _dataQueue[_queueWritePtr].MessageNumber = _messageId;
+
+                IncrementWrite();
+                _messageId++;
+                if (_messageId == UInt32.MaxValue) _messageId = 1;
+            }
+
+            
+        }
+
+        public uint ReadData(ref byte[] data, ref uint msgno)
+        {
+            if (_queueReadPtr == _queueWritePtr)
+                return 0;
+
+            data = _dataQueue[_queueReadPtr].Data;
+            msgno = _dataQueue[_queueReadPtr].MessageNumber;
+            uint readlen = _dataQueue[_queueReadPtr].Length;
+            IncrementRead();
+            return readlen;
+        }
+
+        private void IncrementWrite()
+        {
+            _queueWritePtr++;
+            if (_queueWritePtr == QUEUE_SIZE) _queueWritePtr = 0;
+        }
+        private void IncrementRead()
+        {
+            _queueReadPtr++;
+            if (_queueReadPtr == QUEUE_SIZE) _queueReadPtr = 0;
+        }
+
+        public int PacketsToSend()
+        {
+            return _queueReadPtr - _queueWritePtr;
+        }
+    }
+}