admin/HawkeyeVision
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

hawkeye camera support(not tested)

Browse Source
This commit is contained in:
meelstorm
2025-08-19 12:45:50 +02:00
parent f80b275ad8
commit 6ef6aced8d
170 changed files with 20190 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
framework/Inspectron.HawkEye/UDPB/Block.cs Normal file
View File

@@ -0,0 +1,9 @@
namespace Inspectron.HawkEye.UDPB
{
public class Block
{
public byte[] Data;
public uint Length;
public uint MessageNumber;
}
}

91
framework/Inspectron.HawkEye/UDPB/UDPBPacket.cs Normal file
View File

@@ -0,0 +1,91 @@
using System;
using System.IO;
namespace Inspectron.HawkEye.UDPB
{
public class UDPBPacket
{
public enum EPacketType:uint
{
Data=0,
CloseSequence=1,
BeginSequence=2,
Nak=3,
Ok=4
}
public UDPBPacket()
{
}
private const int sequenceIndex = 0;
private const int messageIndex = 1;
private const int typeIndex = 2;
private const int datalengthIndex = 3;
public const int packetHeaderSize = 16;
private uint[] _header=new uint[4];
public uint SequenceId
{
get => _header[sequenceIndex];
set => _header[sequenceIndex] = value;
}
public uint MessageId
{
get => _header[messageIndex];
set => _header[messageIndex] = value;
}
public EPacketType Type
{
get => (EPacketType)_header[typeIndex];
set => _header[typeIndex] = (uint)value;
}
private byte[] _payload;
public byte[] Payload
{
get => _payload??new byte[0];
set
{
_payload = value;
}
}
public uint Length
{
get => _header[datalengthIndex];
set => _header[datalengthIndex] = value;
}
public byte[] Serialize()
{
byte[] bytes = new byte[UDPBQueue.PACKET_SIZE+packetHeaderSize];
Buffer.BlockCopy(_header, 0, bytes, 0, packetHeaderSize);
if(_payload!=null)
Array.Copy(_payload, 0, bytes, packetHeaderSize, _payload.Length);
return bytes;
}
public void Deserialize(byte[] data,int offset,int len)
{
Buffer.BlockCopy(data, offset, _header, 0, packetHeaderSize);
var dataLen = _header[datalengthIndex];
if (dataLen > 0)
{
_payload = new byte[dataLen];
Array.Copy(data, packetHeaderSize+ offset, _payload, 0, (int) dataLen);
}
}
}
}

77
framework/Inspectron.HawkEye/UDPB/UDPBQueue.cs Normal file
View File

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

127
framework/Inspectron.HawkEye/UDPB/UDPBReceiveBuffer.cs Normal file
View File

@@ -0,0 +1,127 @@
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
namespace Inspectron.HawkEye.UDPB
{
public class UDPBReceiveBuffer
{
private readonly UDPBSocket _socket;
private readonly ConcurrentQueue<byte[]> _receiveQueue;
public UDPBReceiveBuffer(UDPBSocket socket, ConcurrentQueue<byte[]> receiveQueue)
{
_socket = socket;
_receiveQueue = receiveQueue;
}
List<UDPBSequence> _sequences = new List<UDPBSequence>();
public byte[] LastReconstructedBuffer { get; private set; }
public bool IsSequenceFinished()
{
var res = _sequences.OrderByDescending(x=>x.SequenceId).FirstOrDefault(x => x.IsFinished);
if (res != null)
{
LastReconstructedBuffer = res.LastReconstructedBuffer;
_sequences.Remove(res);
return true;
}
return false;
}
public UDPBSequence StartSequence(uint sequence)
{
while (_sequences.Count > 5)
{
_sequences.Remove(_sequences.OrderBy(x => x.SequenceId).First());
}
var exists = _sequences.FirstOrDefault(x => x.SequenceId == sequence);
if (exists != null)
{
Console.WriteLine($"Sequence exists {sequence}");
return exists;
}
var res = new UDPBSequence(sequence, _socket, _receiveQueue);
_sequences.Add(res);
return res;
}
public void AddPacketBytes(byte[] data, int offset, int len)
{
var packet = new UDPBPacket();
packet.Deserialize(data, offset, len);
if (packet.Type == UDPBPacket.EPacketType.Data)
{
var seqId = packet.SequenceId;
var sequence = _sequences.FirstOrDefault(x => x.SequenceId == seqId);
if (sequence != null)
{
sequence.AddData(packet);
}
else
{
var s=StartSequence(seqId);
s.AddData(packet);
}
}else if (packet.Type == UDPBPacket.EPacketType.Ok)
{
var nb = new byte[len];
Array.Copy(data,offset,nb,0,len);
_receiveQueue.Enqueue(nb);
}
else
{
AddCommand(packet);
}
}
private void AddCommand(UDPBPacket commandPacket)
{
switch (commandPacket.Type)
{
case UDPBPacket.EPacketType.CloseSequence:
bool finishSuccess;
int dataSize;
var sequence = _sequences.FirstOrDefault(x => x.SequenceId == commandPacket.SequenceId);
if (sequence == null) return;
finishSuccess = sequence.FinishSequence(commandPacket.MessageId, out var missingPacketIds,
out dataSize);
//inform error
break;
case UDPBPacket.EPacketType.BeginSequence:
StartSequence(commandPacket.SequenceId);
break;
}
}
}
}

142
framework/Inspectron.HawkEye/UDPB/UDPBSequence.cs Normal file
View File

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

220
framework/Inspectron.HawkEye/UDPB/UDPBSocket.cs Normal file
View File

@@ -0,0 +1,220 @@
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Threading;
namespace Inspectron.HawkEye.UDPB
{
public class UDPBSocket : IDisposable
{
public int LossSimulation { get; set; }
public EndPoint LastConnection => _lastConnection;
private readonly UDPBQueue _queue;
private readonly UDPBReceiveBuffer _receiveBuffer;
private readonly byte[] _receivePacketBuffer = new byte[10 * 1024 * 1024];
private readonly Random _rnd = new Random();
private Socket _localSocket;
private uint _currentSquenceId = 1;
private EndPoint _lastConnection = new IPEndPoint(IPAddress.Any, 27001);
private bool _isServer;
private readonly ConcurrentQueue<byte[]> _receiveQueue = new ConcurrentQueue<byte[]>();
private bool _isReceiveing = true;
private readonly Thread _receiveThread;
private readonly List<UDPBPacket> _packetCache = new List<UDPBPacket>();
public UDPBSocket()
{
_queue = new UDPBQueue();
_receiveBuffer = new UDPBReceiveBuffer(this, _receiveQueue);
_receiveThread = new Thread(ReceiveLoop);
}
public void Dispose()
{
if (!_isServer) _localSocket.Disconnect(false);
_isReceiveing = false;
}
public static int FindFreePort(IPAddress adapter)
{
var port = 0;
var socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
try
{
var localEP = new IPEndPoint(adapter, 0);
socket.Bind(localEP);
localEP = (IPEndPoint) socket.LocalEndPoint;
port = localEP.Port;
}
finally
{
socket.Close();
}
return port;
}
public void Listen(IPAddress adapter, int port)
{
Console.WriteLine($"Listen on {adapter}");
_isServer = true;
_localSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
_localSocket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.ReuseAddress, true);
//_localSocket.SetSocketOption(SocketOptionLevel.Udp, SocketOptionName.DontFragment, true);
_localSocket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReceiveBuffer, 10 * 1024 * 1024);
_localSocket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.SendBuffer, 10 * 1024 * 1024);
_localSocket.Bind(new IPEndPoint(adapter, port));
_receiveThread.Start();
}
public void Connect(IPEndPoint endpoint, IPAddress adapter)
{
Console.WriteLine($"Connect to {adapter}");
_localSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
//_localSocket.SetSocketOption(SocketOptionLevel.Udp, SocketOptionName.DontFragment, true);
_localSocket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReceiveBuffer, 10 * 1024 * 1024);
_localSocket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.SendBuffer, 10 * 1024 * 1024);
Console.WriteLine($"Binding on {adapter?.MapToIPv4()}");
if (adapter != null)
_localSocket.Bind(new IPEndPoint(adapter.MapToIPv4(), 0));
_localSocket.Connect(endpoint);
_receiveThread.Start();
}
public void SendData(byte[] data)
{
_packetCache.Clear();
IncrementSequence();
var begin = new UDPBPacket {Type = UDPBPacket.EPacketType.BeginSequence, SequenceId = _currentSquenceId};
SendPacket(begin, true);
_queue.Reset();
_queue.AddBuffer(data, 0, data.Length);
var lastId = Flush();
var finish = new UDPBPacket
{Type = UDPBPacket.EPacketType.CloseSequence, MessageId = lastId + 1, SequenceId = _currentSquenceId};
SendPacket(finish, true);
do
{
byte[] dataOK;
dataOK = Receive();
_localSocket.ReceiveTimeout = 0;
var packet = new UDPBPacket();
packet.Deserialize(dataOK, 0, dataOK.Length);
if (packet.Type == UDPBPacket.EPacketType.Ok)
{
Console.WriteLine("OK");
break;
}
if (packet.Type == UDPBPacket.EPacketType.Nak)
{
var ms = new MemoryStream(packet.Payload);
var br = new BinaryReader(ms);
var packets = br.ReadInt32();
Console.WriteLine("NAK " + packets);
for (var i = 0; i < packets; i++)
{
var id = br.ReadUInt32();
var p = _packetCache.First(x => x.MessageId == id);
SendPacket(p, true);
}
SendPacket(finish, true);
}
} while (true);
}
public byte[] Receive()
{
byte[] res;
while (!_receiveQueue.TryDequeue(out res)) Thread.Sleep(1);
return res;
}
internal void SendPacket(UDPBPacket packet, bool noCache = false)
{
if (!noCache) _packetCache.Add(packet);
if (LossSimulation > 0)
if (_rnd.Next(LossSimulation) == LossSimulation - 1)
return;
if (_isServer)
_localSocket.SendTo(packet.Serialize(), _lastConnection);
else
_localSocket.Send(packet.Serialize());
}
private void ReceiveLoop()
{
while (_isReceiveing)
{
var received = ReceiveFrom(_receivePacketBuffer, 0);
_receiveBuffer.AddPacketBytes(_receivePacketBuffer, 0, received);
}
}
private int ReceiveFrom(byte[] buffer, int bufferOffset)
{
return _localSocket.ReceiveFrom(buffer, bufferOffset,
UDPBQueue.PACKET_SIZE + UDPBPacket.packetHeaderSize,
SocketFlags.None, ref _lastConnection);
}
private void IncrementSequence()
{
_currentSquenceId++;
if (_currentSquenceId == uint.MaxValue) _currentSquenceId = 1;
}
private uint Flush()
{
uint lastPacketId = 0;
while (_queue.PacketsToSend() != 0)
{
var packet = new UDPBPacket();
byte[] data = null;
uint msgNo = 0;
var payload = _queue.ReadData(ref data, ref msgNo);
packet.Type = UDPBPacket.EPacketType.Data;
packet.Length = payload;
packet.Payload = data;
packet.MessageId = msgNo;
packet.SequenceId = _currentSquenceId;
SendPacket(packet);
lastPacketId = msgNo;
}
return lastPacketId;
}
}
}