282 lines
9.0 KiB
C#
282 lines
9.0 KiB
C#
using System.Net;
|
|
using DotNext;
|
|
using Common;
|
|
|
|
namespace Peripherals.JpegClient;
|
|
|
|
static class JpegAddr
|
|
{
|
|
const UInt32 BASE = 0x0000_0000;
|
|
public const UInt32 ENABLE = BASE + 0x0;
|
|
public const UInt32 FRAME_NUM = BASE + 0x1;
|
|
public const UInt32 FRAME_INFO = BASE + 0x2;
|
|
public const UInt32 FRAME_SAMPLE_RATE = BASE + 0x3;
|
|
public const UInt32 FRAME_DATA_MAX_POINTER = BASE + 0x4;
|
|
|
|
public const UInt32 DDR_FRAME_DATA_ADDR = 0x0000_0000;
|
|
public const UInt32 DDR_FRAME_DATA_MAX_ADDR = 0x8000_0000;
|
|
}
|
|
|
|
public class JpegInfo
|
|
{
|
|
public UInt32 Width { get; set; }
|
|
public UInt32 Height { get; set; }
|
|
public UInt32 Size { get; set; }
|
|
|
|
public JpegInfo(UInt32 width, UInt32 height, UInt32 size)
|
|
{
|
|
Width = width;
|
|
Height = height;
|
|
Size = size;
|
|
}
|
|
|
|
public JpegInfo(byte[] data)
|
|
{
|
|
if (data.Length < 8)
|
|
throw new ArgumentException("Invalid data length", nameof(data));
|
|
|
|
Width = ((UInt32)(data[5] << 8 + data[6] & 0xF0));
|
|
Height = ((UInt32)((data[6] & 0x0F) << 4 + data[7]));
|
|
Size = Number.BytesToUInt32(data, 0, 4).Value;
|
|
}
|
|
}
|
|
|
|
public enum JpegSampleRate : UInt32
|
|
{
|
|
RATE_1_1 = 0b1111_1111_1111_1111_1111_1111_1111_1111,
|
|
RATE_1_2 = 0b1010_1010_1010_1010_1010_1010_1010_1010,
|
|
RATE_1_4 = 0b1000_1000_1000_1000_1000_1000_1000_1000,
|
|
RATE_3_4 = 0b1110_1110_1110_1110_1110_1110_1110_1110,
|
|
RATE_1_8 = 0b1000_0000_1000_0000_1000_0000_1000_0000,
|
|
RATE_3_8 = 0b1001_0010_0100_1001_1001_0010_0100_1001,
|
|
RATE_7_8 = 0b1111_1110_1111_1110_1111_1110_1111_1110,
|
|
RATE_1_16 = 0b1000_0000_0000_0000_1000_0000_0000_0000,
|
|
RATE_3_16 = 0b1000_0100_0010_0000_1000_0100_0010_0000,
|
|
RATE_5_16 = 0b1001_0001_0010_0010_0100_0100_1000_1001,
|
|
RATE_15_16 = 0b1111_1111_1111_1110_1111_1111_1111_1110,
|
|
RATE_1_32 = 0b1000_0000_0000_0000_0000_0000_0000_0000,
|
|
RATE_31_32 = 0b1111_1111_1111_1111_1111_1111_1111_1110,
|
|
}
|
|
|
|
public class Jpeg
|
|
{
|
|
private static readonly NLog.Logger logger = NLog.LogManager.GetCurrentClassLogger();
|
|
|
|
readonly int timeout = 2000;
|
|
readonly int taskID;
|
|
readonly int port;
|
|
readonly string address;
|
|
private IPEndPoint ep;
|
|
|
|
public Jpeg(string address, int port, int taskID, int timeout = 2000)
|
|
{
|
|
if (timeout < 0)
|
|
throw new ArgumentException("Timeout couldn't be negative", nameof(timeout));
|
|
this.address = address;
|
|
this.taskID = taskID;
|
|
this.port = port;
|
|
this.ep = new IPEndPoint(IPAddress.Parse(address), port);
|
|
this.timeout = timeout;
|
|
}
|
|
|
|
public async ValueTask<bool> SetEnable(bool enable)
|
|
{
|
|
var ret = await UDPClientPool.WriteAddr(
|
|
this.ep, this.taskID, JpegAddr.ENABLE, Convert.ToUInt32(enable), this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to set JPEG enable: {ret.Error}");
|
|
return false;
|
|
}
|
|
return ret.Value;
|
|
}
|
|
|
|
public async ValueTask<bool> SetSampleRate(uint rate)
|
|
{
|
|
var ret = await UDPClientPool.WriteAddr(
|
|
this.ep, this.taskID, JpegAddr.FRAME_SAMPLE_RATE, rate, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to set JPEG sample rate: {ret.Error}");
|
|
return false;
|
|
}
|
|
return ret.Value;
|
|
}
|
|
|
|
public async ValueTask<bool> SetSampleRate(JpegSampleRate rate)
|
|
{
|
|
return await SetSampleRate((uint)rate);
|
|
}
|
|
|
|
public async ValueTask<uint> GetFrameNumber()
|
|
{
|
|
var ret = await UDPClientPool.ReadAddrByte(
|
|
this.ep, this.taskID, JpegAddr.FRAME_NUM, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to get JPEG frame number: {ret.Error}");
|
|
return 0;
|
|
}
|
|
return Number.BytesToUInt32(ret.Value.Options.Data ?? Array.Empty<byte>()).Value;
|
|
}
|
|
|
|
public async ValueTask<Optional<List<JpegInfo>>> GetFrameInfo(int num)
|
|
{
|
|
var ret = await UDPClientPool.ReadAddr(this.ep, this.taskID, JpegAddr.FRAME_INFO, num, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to get JPEG frame info: {ret.Error}");
|
|
return new(null);
|
|
}
|
|
|
|
var data = ret.Value.Options.Data;
|
|
if (data == null || data.Length == 0)
|
|
{
|
|
logger.Error($"Data is null or empty");
|
|
return new(null);
|
|
}
|
|
if (data.Length != num * 2)
|
|
{
|
|
logger.Error(
|
|
$"Data length should be {num * 2} bytes, instead of {data.Length} bytes");
|
|
return new(null);
|
|
}
|
|
|
|
var infos = new List<JpegInfo>();
|
|
for (int i = 0; i < num; i++)
|
|
{
|
|
infos.Add(new JpegInfo(data[i..(i + 1)]));
|
|
}
|
|
return new(infos);
|
|
}
|
|
|
|
public async ValueTask<bool> UpdatePointer(uint cnt)
|
|
{
|
|
var ret = await UDPClientPool.WriteAddr(
|
|
this.ep, this.taskID, JpegAddr.FRAME_DATA_MAX_POINTER, cnt, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to update pointer: {ret.Error}");
|
|
return false;
|
|
}
|
|
return ret.Value;
|
|
}
|
|
|
|
public async ValueTask<Result<byte[]?>> GetFrame(uint offset, uint length)
|
|
{
|
|
if (!MsgBus.IsRunning)
|
|
{
|
|
logger.Error("Message bus is not running");
|
|
return new(new Exception("Message bus is not running"));
|
|
}
|
|
MsgBus.UDPServer.ClearUDPData(this.ep.Address.ToString(), this.ep.Port);
|
|
|
|
var firstReadLength = (int)(Math.Min(length, JpegAddr.DDR_FRAME_DATA_MAX_ADDR - offset));
|
|
var secondReadLength = (int)(length - firstReadLength);
|
|
var dataBytes = new byte[length];
|
|
|
|
{
|
|
var ret = await UDPClientPool.ReadAddr4Bytes(
|
|
this.ep, this.taskID, JpegAddr.DDR_FRAME_DATA_ADDR + offset, firstReadLength, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to get JPEG frame data: {ret.Error}");
|
|
return null;
|
|
}
|
|
if (ret.Value.Length != firstReadLength)
|
|
{
|
|
logger.Error($"Data length should be {firstReadLength} bytes, instead of {ret.Value.Length} bytes");
|
|
return null;
|
|
}
|
|
Buffer.BlockCopy(ret.Value, 0, dataBytes, 0, firstReadLength);
|
|
}
|
|
|
|
if (secondReadLength > 0)
|
|
{
|
|
var ret = await UDPClientPool.ReadAddr4Bytes(
|
|
this.ep, this.taskID, JpegAddr.DDR_FRAME_DATA_ADDR, secondReadLength, this.timeout);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to get JPEG frame data: {ret.Error}");
|
|
return null;
|
|
}
|
|
if (ret.Value.Length != secondReadLength)
|
|
{
|
|
logger.Error($"Data length should be {secondReadLength} bytes, instead of {ret.Value.Length} bytes");
|
|
return null;
|
|
}
|
|
Buffer.BlockCopy(ret.Value, 0, dataBytes, firstReadLength, secondReadLength);
|
|
}
|
|
|
|
return dataBytes;
|
|
}
|
|
|
|
public async ValueTask<List<byte[]>> GetMultiFrames(uint offset, uint[] sizes)
|
|
{
|
|
var frames = new List<byte[]>();
|
|
for (int i = 0; i < sizes.Length; i++)
|
|
{
|
|
var ret = await GetFrame(offset, sizes[i]);
|
|
if (!ret.IsSuccessful)
|
|
{
|
|
logger.Error($"Failed to get JPEG frame {i} data: {ret.Error}");
|
|
continue;
|
|
}
|
|
if (ret.Value == null)
|
|
{
|
|
logger.Error($"Frame {i} data is null");
|
|
continue;
|
|
}
|
|
if (ret.Value.Length != sizes[i])
|
|
{
|
|
logger.Error(
|
|
$"Frame {i} data length should be {sizes[i]} bytes, instead of {ret.Value.Length} bytes");
|
|
continue;
|
|
}
|
|
|
|
frames.Add(ret.Value);
|
|
offset += sizes[i];
|
|
}
|
|
|
|
{
|
|
var ret = await UpdatePointer((uint)sizes.Length);
|
|
if (!ret) logger.Error($"Failed to update pointer");
|
|
}
|
|
|
|
return frames;
|
|
}
|
|
|
|
public async ValueTask<Result<List<byte[]>?>> GetMultiFrames(uint offset)
|
|
{
|
|
if (!MsgBus.IsRunning)
|
|
{
|
|
logger.Error("Message bus is not running");
|
|
return new(new Exception("Message bus is not running"));
|
|
}
|
|
MsgBus.UDPServer.ClearUDPData(this.ep.Address.ToString(), this.ep.Port);
|
|
|
|
var frameNum = await GetFrameNumber();
|
|
if (frameNum == 0) return null;
|
|
|
|
List<uint>? frameSizes = null;
|
|
{
|
|
var ret = await GetFrameInfo((int)frameNum);
|
|
if (!ret.HasValue || ret.Value.Count == 0)
|
|
{
|
|
logger.Error($"Failed to get frame info");
|
|
return null;
|
|
}
|
|
frameSizes = ret.Value.Select(x => x.Size).ToList();
|
|
}
|
|
|
|
var frames = await GetMultiFrames(offset, frameSizes.ToArray());
|
|
if (frames.Count == 0)
|
|
{
|
|
logger.Error($"Failed to get frames");
|
|
return null;
|
|
}
|
|
|
|
return frames;
|
|
}
|
|
}
|