forked from amkovkov/GranuSightSoftware2
feat: CameraService
Add stable optainitng of raw image from camera
This commit is contained in:
@@ -0,0 +1,7 @@
|
||||
namespace GSS2.Core.Extentions;
|
||||
|
||||
static class EnumerableExtentions
|
||||
{
|
||||
public static IEnumerable<(int i, T value)> Enumerate<T>(this IEnumerable<T> values) => values.Select((value, i) => (i, value));
|
||||
}
|
||||
|
||||
@@ -18,9 +18,10 @@
|
||||
<PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="10.0.0" />
|
||||
<PackageReference Include="Microsoft.Extensions.DependencyInjection" Version="10.0.0" />
|
||||
<PackageReference Include="OpenCvSharp4" Version="4.11.0.20250507" />
|
||||
<PackageReference Include="OpenCvSharp4.runtime.linux-arm" Version="4.11.0.20250506" />
|
||||
<PackageReference Include="System.Device.Gpio" Version="4.0.1" />
|
||||
<PackageReference Include="Iot.Device.Bindings" Version="4.0.1" />
|
||||
<PackageReference Include="LibCameraSharp" Version="0.5.2-20251214" />
|
||||
<PackageReference Include="LibCameraSharp" Version="0.5.2-20260114" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
|
||||
@@ -2,6 +2,8 @@ using System.Runtime.InteropServices;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using LibCameraSharp;
|
||||
using Stream = LibCameraSharp.Stream;
|
||||
using System.Buffers;
|
||||
using GSS2.Core.Extentions;
|
||||
|
||||
namespace GSS2.Core.Hardware;
|
||||
|
||||
@@ -28,7 +30,7 @@ public class CameraService : IDisposable
|
||||
public static CameraServiceConfiguration Default => new CameraServiceConfiguration
|
||||
{
|
||||
CameraId = "",
|
||||
FrameBufferCount = 5,
|
||||
ViewFinderFrameBufferCount = 5,
|
||||
ViewFinderWidth = 1920,
|
||||
ViewFinderHeight = 1080,
|
||||
ViewFinderFps = 30,
|
||||
@@ -37,10 +39,11 @@ public class CameraService : IDisposable
|
||||
};
|
||||
|
||||
public string CameraId { get; set; } = "";
|
||||
public int FrameBufferCount { get; set; }
|
||||
public int ViewFinderFrameBufferCount { get; set; }
|
||||
public int ViewFinderWidth { get; set; }
|
||||
public int ViewFinderHeight { get; set; }
|
||||
public int ViewFinderFps { get; set; }
|
||||
public int ImageCaptureFrameBufferCount { get; set; }
|
||||
public int ImageCaptureWidth { get; set; }
|
||||
public int ImageCaptureHeight { get; set; }
|
||||
}
|
||||
@@ -64,10 +67,13 @@ public class CameraService : IDisposable
|
||||
private CameraManager? _cameraManager;
|
||||
private Camera? _camera;
|
||||
private CameraConfiguration? _cameraConfiguration;
|
||||
private FrameBufferAllocator? _frameBufferAllocator;
|
||||
private StreamConfiguration? _viewFinderStreamConfiguration;
|
||||
private StreamConfiguration? _imageCaptureStreamConfiguration;
|
||||
private FrameBufferAllocator? _viewFinderFrameBufferAllocator;
|
||||
private FrameBufferAllocator? _imageCaptureFrameBufferAllocator;
|
||||
private Stream? _viewFinderStream;
|
||||
private Stream? _imageCaptureStream;
|
||||
private List<MappedBuffer>? _mappedBuffers;
|
||||
private List<MappedBuffer>? _imageCaptureMappedBuffers;
|
||||
private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource();
|
||||
|
||||
public CameraService(ILogger<CameraService> logger, CameraServiceConfiguration? configuration = null, bool autoInitialize = false)
|
||||
@@ -87,6 +93,14 @@ public class CameraService : IDisposable
|
||||
}
|
||||
|
||||
_logger.LogDebug(@$"CameraService use following configuration:
|
||||
CameraId: {_configuration.CameraId}
|
||||
ViewFinderFrameBufferCount: {_configuration.ViewFinderFrameBufferCount}
|
||||
ViewFinderWidth: {_configuration.ViewFinderWidth}
|
||||
ViewFinderHeight: {_configuration.ViewFinderHeight}
|
||||
ViewFinderFps: {_configuration.ViewFinderFps}
|
||||
ImageCaptureFrameBufferCount: {_configuration.ImageCaptureFrameBufferCount}
|
||||
ImageCaptureWidth: {_configuration.ImageCaptureWidth}
|
||||
ImageCaptureHeight: {_configuration.ImageCaptureHeight}
|
||||
");
|
||||
|
||||
if (autoInitialize)
|
||||
@@ -115,9 +129,12 @@ public class CameraService : IDisposable
|
||||
CameraSelect(effectiveCancellationToken);
|
||||
CameraAcquire(effectiveCancellationToken);
|
||||
CameraConfigure(effectiveCancellationToken);
|
||||
FrameBuffersAllocate(effectiveCancellationToken);
|
||||
ImageCaptureFrameBuffersMap(effectiveCancellationToken);
|
||||
// ViewFinderFrameBuffersAllocate(effectiveCancellationToken);
|
||||
ImageCaptureFrameBuffersAllocate(effectiveCancellationToken);
|
||||
CameraStart(effectiveCancellationToken);
|
||||
// ViewFinderFrameBuffersInitialize(effectiveCancellationToken);
|
||||
ImageCaptureFrameBuffersInitialize(effectiveCancellationToken);
|
||||
ImageCaptureFrameBuffersMap(effectiveCancellationToken);
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
@@ -189,13 +206,21 @@ public class CameraService : IDisposable
|
||||
|
||||
_cameraConfiguration = _camera.GenerateConfiguration(
|
||||
StreamRoleEnum.Viewfinder,
|
||||
StreamRoleEnum.StillCapture);
|
||||
StreamRoleEnum.Raw);
|
||||
|
||||
var viewFinderConfig = _cameraConfiguration.First();
|
||||
viewFinderConfig.BufferCount = (uint)_configuration.FrameBufferCount;
|
||||
viewFinderConfig.Size = new Size(
|
||||
(uint)_configuration.ViewFinderWidth,
|
||||
(uint)_configuration.ViewFinderHeight);
|
||||
_viewFinderStreamConfiguration = _cameraConfiguration.First();
|
||||
_viewFinderStreamConfiguration.BufferCount = checked((uint)_configuration.ViewFinderFrameBufferCount);
|
||||
_viewFinderStreamConfiguration.Size = new Size(
|
||||
checked((uint)_configuration.ViewFinderWidth),
|
||||
checked((uint)_configuration.ViewFinderHeight)
|
||||
);
|
||||
|
||||
_imageCaptureStreamConfiguration = _cameraConfiguration.Skip(1).First();
|
||||
_imageCaptureStreamConfiguration.BufferCount = checked((uint)_configuration.ImageCaptureFrameBufferCount);
|
||||
_imageCaptureStreamConfiguration.Size = new Size(
|
||||
checked((uint)_configuration.ImageCaptureWidth),
|
||||
checked((uint)_configuration.ImageCaptureHeight)
|
||||
);
|
||||
|
||||
if (_cameraConfiguration.Validate() == CameraConfiguration.Status.Invalid)
|
||||
{
|
||||
@@ -203,6 +228,20 @@ public class CameraService : IDisposable
|
||||
throw new Exception("Error while camera configuration validation");
|
||||
}
|
||||
|
||||
_logger.LogDebug(@$"ViewFinder stream configuration:
|
||||
Size: {_viewFinderStreamConfiguration.Size.ToString(true)}
|
||||
FrameSize: {_viewFinderStreamConfiguration.FrameSize}
|
||||
Stride: {_viewFinderStreamConfiguration.Stride}
|
||||
ColorSpace: {_viewFinderStreamConfiguration.ColorSpace?.ToString(true)}
|
||||
PixelFormat: {_viewFinderStreamConfiguration.PixelFormat?.ToString(true)}");
|
||||
|
||||
_logger.LogDebug(@$"ImageCapture stream configuration:
|
||||
Size: {_imageCaptureStreamConfiguration.Size.ToString(true)}
|
||||
FrameSize: {_imageCaptureStreamConfiguration.FrameSize}
|
||||
Stride: {_imageCaptureStreamConfiguration.Stride}
|
||||
ColorSpace: {_imageCaptureStreamConfiguration.ColorSpace?.ToString(true)}
|
||||
PixelFormat: {_imageCaptureStreamConfiguration.PixelFormat?.ToString(true)}");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
int result = _camera.Configure(_cameraConfiguration);
|
||||
@@ -212,8 +251,8 @@ public class CameraService : IDisposable
|
||||
throw new Exception($"Error while camera configuration {result}");
|
||||
}
|
||||
|
||||
_viewFinderStream = _cameraConfiguration.First().Stream();
|
||||
_imageCaptureStream = _cameraConfiguration.Skip(1).First().Stream();
|
||||
_viewFinderStream = _viewFinderStreamConfiguration.Stream();
|
||||
_imageCaptureStream = _imageCaptureStreamConfiguration.Stream();
|
||||
}
|
||||
private void CameraStart(CancellationToken cancellationToken)
|
||||
{
|
||||
@@ -231,22 +270,20 @@ public class CameraService : IDisposable
|
||||
|
||||
_state |= State.CameraStarted;
|
||||
}
|
||||
private void FrameBuffersAllocate(CancellationToken cancellationToken)
|
||||
private void ViewFinderFrameBuffersAllocate(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_camera is null)
|
||||
throw new InvalidOperationException("Camera not initialized");
|
||||
if (_viewFinderStream is null)
|
||||
throw new InvalidOperationException("ViewFinderStream not initialized");
|
||||
if (_imageCaptureStream is null)
|
||||
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
_frameBufferAllocator = new FrameBufferAllocator(_camera);
|
||||
_viewFinderFrameBufferAllocator = new FrameBufferAllocator(_camera);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
int result = _frameBufferAllocator.Allocate(_viewFinderStream);
|
||||
int result = _viewFinderFrameBufferAllocator.Allocate(_viewFinderStream);
|
||||
if (result < 0)
|
||||
{
|
||||
_logger.LogCritical("Error while view finder frame buffers allocation {Result}", result);
|
||||
@@ -254,10 +291,21 @@ public class CameraService : IDisposable
|
||||
}
|
||||
|
||||
_state |= State.ViewFinderFrameBufferAllocated;
|
||||
}
|
||||
private void ImageCaptureFrameBuffersAllocate(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_camera is null)
|
||||
throw new InvalidOperationException("Camera not initialized");
|
||||
if (_imageCaptureStream is null)
|
||||
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
result = _frameBufferAllocator.Allocate(_imageCaptureStream);
|
||||
_imageCaptureFrameBufferAllocator = new FrameBufferAllocator(_camera);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
int result = _imageCaptureFrameBufferAllocator.Allocate(_imageCaptureStream);
|
||||
if (result < 0)
|
||||
{
|
||||
_logger.LogCritical("Error while image capture frame buffers allocation {Result}", result);
|
||||
@@ -266,47 +314,137 @@ public class CameraService : IDisposable
|
||||
|
||||
_state |= State.ImageCaptureFrameBufferAllocated;
|
||||
}
|
||||
private void ViewFinderFrameBuffersInitialize(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_camera is null)
|
||||
throw new InvalidOperationException("Camera not initialized");
|
||||
if (_viewFinderStream is null)
|
||||
throw new InvalidOperationException("ViewFinderStream not initialized");
|
||||
if (_viewFinderFrameBufferAllocator is null)
|
||||
throw new InvalidOperationException("ViewFinderFrameBufferAllocator not initialized");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var buffers = _viewFinderFrameBufferAllocator.Buffers(_viewFinderStream);
|
||||
|
||||
_logger.LogDebug("View finder frame buffers initialization");
|
||||
foreach (var buffer in buffers)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
||||
var result = request.AddBuffer(_viewFinderStream, buffer);
|
||||
if (result < 0)
|
||||
throw new Exception("Error while request create");
|
||||
|
||||
var cts = new TaskCompletionSource();
|
||||
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
||||
{
|
||||
if (e.Request.Handle == request.Handle)
|
||||
cts.SetResult();
|
||||
}
|
||||
|
||||
_camera.RequestCompleted += RequestCompleted;
|
||||
result = _camera.QueueRequest(request);
|
||||
if (result < 0)
|
||||
{
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
throw new Exception("Error while request queue");
|
||||
}
|
||||
cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
|
||||
_logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
|
||||
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
||||
}
|
||||
}
|
||||
private void ImageCaptureFrameBuffersInitialize(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_camera is null)
|
||||
throw new InvalidOperationException("Camera not initialized");
|
||||
if (_imageCaptureStream is null)
|
||||
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
||||
if (_imageCaptureFrameBufferAllocator is null)
|
||||
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
||||
|
||||
_logger.LogDebug("Image capture frame buffers initialization");
|
||||
foreach (var buffer in buffers)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
||||
var result = request.AddBuffer(_imageCaptureStream, buffer);
|
||||
if (result < 0)
|
||||
throw new Exception("Error while request create");
|
||||
|
||||
var cts = new TaskCompletionSource();
|
||||
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
||||
{
|
||||
if (e.Request.Handle == request.Handle)
|
||||
cts.SetResult();
|
||||
}
|
||||
|
||||
_camera.RequestCompleted += RequestCompleted;
|
||||
result = _camera.QueueRequest(request);
|
||||
if (result < 0)
|
||||
{
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
throw new Exception("Error while request queue");
|
||||
}
|
||||
cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
|
||||
_logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
|
||||
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
||||
}
|
||||
}
|
||||
private void ImageCaptureFrameBuffersMap(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_imageCaptureStream is null)
|
||||
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
||||
if (_frameBufferAllocator is null)
|
||||
throw new InvalidOperationException("FrameBufferAllocator not initialized");
|
||||
if (_imageCaptureFrameBufferAllocator is null)
|
||||
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
_mappedBuffers = new List<MappedBuffer>();
|
||||
_imageCaptureMappedBuffers = new List<MappedBuffer>();
|
||||
|
||||
var frameBuffers = _frameBufferAllocator.Buffers(_imageCaptureStream);
|
||||
var frameBuffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
||||
|
||||
foreach (var buffer in frameBuffers)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var planesByFd = buffer.Planes.GroupBy(p => p.Fd);
|
||||
var planesByFd = buffer.Planes.GroupBy(p => p.Fd.Get());
|
||||
|
||||
foreach (var fdGroup in planesByFd)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
int fd = fdGroup.Key.Get();
|
||||
int fd = fdGroup.Key;
|
||||
|
||||
ulong minOffset = fdGroup.Min(p => p.Offset);
|
||||
ulong maxEnd = fdGroup.Max(p => p.Offset + p.Length);
|
||||
ulong maxOffset = fdGroup.Max(p => p.Offset + p.Length);
|
||||
|
||||
ulong pageSize = (ulong)Environment.SystemPageSize;
|
||||
ulong alignedOffset = minOffset & ~(pageSize - 1);
|
||||
ulong alignedLength = maxEnd - alignedOffset;
|
||||
ulong alignedLength = (maxOffset - alignedOffset + pageSize - 1) & ~(pageSize - 1);
|
||||
|
||||
IntPtr address = LibC.mmap(IntPtr.Zero, alignedLength, LibC.PROT_READ, LibC.MAP_SHARED, fd, alignedOffset);
|
||||
|
||||
if (address == new IntPtr(-1))
|
||||
{
|
||||
int error = Marshal.GetLastWin32Error();
|
||||
throw new InvalidOperationException($"Error while buffers mapping, mmap failed for fd={fd}, error={error}");
|
||||
throw new InvalidOperationException($"Error while buffers mapping, mmap failed for fd=0x{fd:X}, error={error}");
|
||||
}
|
||||
|
||||
_mappedBuffers.Add(new MappedBuffer
|
||||
_logger.LogDebug("Mapped buffer fd=0x{:X}, address=0x{:X}, length=0x{:X}", fd, address, alignedLength);
|
||||
|
||||
_imageCaptureMappedBuffers.Add(new MappedBuffer
|
||||
{
|
||||
Fd = fd,
|
||||
Address = address,
|
||||
@@ -325,10 +463,12 @@ public class CameraService : IDisposable
|
||||
throw new InvalidOperationException("Camera not initialized");
|
||||
if (_imageCaptureStream is null)
|
||||
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
||||
if (_frameBufferAllocator is null)
|
||||
throw new InvalidOperationException("FrameBufferAllocator not initialized");
|
||||
if (_mappedBuffers is null)
|
||||
throw new InvalidOperationException("Buffers not mapped");
|
||||
if (_imageCaptureStreamConfiguration is null)
|
||||
throw new InvalidOperationException("ImageCaptureStreamConfiguration not initialized");
|
||||
if (_imageCaptureFrameBufferAllocator is null)
|
||||
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
||||
if (_imageCaptureMappedBuffers is null)
|
||||
throw new InvalidOperationException("ImageCaptureMappedBuffers not mapped");
|
||||
|
||||
var effectiveCancellationToken = CancellationTokenSource
|
||||
.CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
|
||||
@@ -336,15 +476,16 @@ public class CameraService : IDisposable
|
||||
|
||||
OpenCvSharp.Mat? image = null;
|
||||
|
||||
var fdsData = new Dictionary<int, byte[]>();
|
||||
var planesData = new List<byte[]>();
|
||||
try
|
||||
{
|
||||
effectiveCancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var buffers = _frameBufferAllocator.Buffers(_imageCaptureStream);
|
||||
var buffer = buffers.Where(b => b.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup || b.Metadata.Status == FrameMetadata.StatusEnum.FrameSuccess).First();
|
||||
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
||||
var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
||||
var buffer = buffers.First();
|
||||
var result = request.AddBuffer(_imageCaptureStream, buffer);
|
||||
|
||||
if (result < 0)
|
||||
throw new Exception("Error while request create");
|
||||
|
||||
@@ -353,42 +494,85 @@ public class CameraService : IDisposable
|
||||
var cts = new TaskCompletionSource();
|
||||
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
||||
{
|
||||
if (e.Request.Handle == request.Handle)
|
||||
if (e.Request.Handle != request.Handle)
|
||||
return;
|
||||
|
||||
var bufferStatus = request.FindBuffer(_imageCaptureStream).Metadata.Status;
|
||||
_logger.LogDebug("Request buffers status: {}", bufferStatus);
|
||||
if (bufferStatus == FrameMetadata.StatusEnum.FrameError || bufferStatus == FrameMetadata.StatusEnum.FrameStartup)
|
||||
{
|
||||
request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
|
||||
result = _camera.QueueRequest(request);
|
||||
if (result >= 0)
|
||||
return;
|
||||
}
|
||||
cts.SetResult();
|
||||
};
|
||||
}
|
||||
|
||||
_camera.RequestCompleted += RequestCompleted;
|
||||
result = _camera.QueueRequest(request);
|
||||
if (result < 0)
|
||||
{
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
throw new Exception("Error while request queue");
|
||||
}
|
||||
await cts.Task.WaitAsync(effectiveCancellationToken);
|
||||
_camera.RequestCompleted -= RequestCompleted;
|
||||
|
||||
var planesData = new List<Memory<byte>>();
|
||||
foreach (var plane in buffer.Planes)
|
||||
{
|
||||
buffer = request.FindBuffer(_imageCaptureStream);
|
||||
_logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
|
||||
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
||||
_logger.LogDebug("Request buffer planes: ");
|
||||
foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
|
||||
_logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
|
||||
foreach (var (i, plane) in buffer.Planes.Enumerate())
|
||||
_logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
|
||||
|
||||
effectiveCancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var pointer = GetPlanePointer(plane);
|
||||
var data = new byte[plane.Length];
|
||||
Marshal.Copy(pointer, data, 0, checked((int)plane.Length));
|
||||
planesData.Add(new Memory<byte>(data));
|
||||
foreach (var plane in buffer.Planes)
|
||||
{
|
||||
var fd = plane.Fd.Get();
|
||||
if (fdsData.ContainsKey(fd))
|
||||
continue;
|
||||
var mappedBuffer = _imageCaptureMappedBuffers.Find(b => b.Fd == fd);
|
||||
if (mappedBuffer is null)
|
||||
continue;
|
||||
var fdData = new byte[mappedBuffer.Length];
|
||||
Marshal.Copy(mappedBuffer.Address, fdData, 0, checked((int)mappedBuffer.Length));
|
||||
fdsData.Add(fd, fdData);
|
||||
}
|
||||
|
||||
foreach (var plane in buffer.Planes)
|
||||
{
|
||||
var fd = plane.Fd.Get();
|
||||
if (!fdsData.ContainsKey(fd))
|
||||
continue;
|
||||
var fdData = fdsData[fd];
|
||||
var planeData = fdData
|
||||
.Skip(checked((int)plane.Offset))
|
||||
.Take(checked((int)plane.Length))
|
||||
.ToArray();
|
||||
planesData.Add(planeData);
|
||||
}
|
||||
|
||||
effectiveCancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var config = _imageCaptureStream.Configuration();
|
||||
var config = _imageCaptureStreamConfiguration;
|
||||
var size = config.Size;
|
||||
var fourcc = config.PixelFormat.Fourcc;
|
||||
var modifier = config.PixelFormat.Modifier;
|
||||
var primariesEnum = config.ColorSpace!.Primaries;
|
||||
var transferFunctionEnum = config.ColorSpace.TransferFunction;
|
||||
var ycbcrEncodingEnum = config.ColorSpace.YcbcrEncoding;
|
||||
var rangeEnum = config.ColorSpace.Range;
|
||||
var primariesEnum = config.ColorSpace?.Primaries;
|
||||
var transferFunctionEnum = config.ColorSpace?.TransferFunction;
|
||||
var ycbcrEncodingEnum = config.ColorSpace?.YcbcrEncoding;
|
||||
var rangeEnum = config.ColorSpace?.Range;
|
||||
var stride = config.Stride;
|
||||
|
||||
effectiveCancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var decoder = FrameDecoderHelper.FindDecoder(fourcc, modifier, primariesEnum, transferFunctionEnum, ycbcrEncodingEnum, rangeEnum);
|
||||
if (decoder is null)
|
||||
throw new Exception($"Cannot find decoder for fourcc={fourcc} modifier={modifier} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}");
|
||||
throw new Exception($"Cannot find decoder for fourcc=0x{fourcc:X}({new string(BitConverter.GetBytes(fourcc).Select(b => (char)b).ToArray())}) modifier=0x{modifier:X} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}");
|
||||
|
||||
effectiveCancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
@@ -396,9 +580,9 @@ public class CameraService : IDisposable
|
||||
{
|
||||
image = decoder.Decode(checked((int)size.Width), checked((int)size.Height), checked((int)stride), planesData);
|
||||
}
|
||||
catch (Exception e)
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(e, "Error while image decode");
|
||||
_logger.LogError(ex, "Error while image decode");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
@@ -406,16 +590,18 @@ public class CameraService : IDisposable
|
||||
{
|
||||
_logger.LogError(e, "Error while image capture");
|
||||
}
|
||||
return image;
|
||||
}
|
||||
private IntPtr GetPlanePointer(FrameBuffer.Plane plane)
|
||||
finally
|
||||
{
|
||||
if (_mappedBuffers is null)
|
||||
throw new InvalidOperationException("Buffers not mapped");
|
||||
|
||||
var mapped = _mappedBuffers.First(m => m.Fd == plane.Fd.Get());
|
||||
ulong offsetInMapping = plane.Offset - mapped.AlignedOffset;
|
||||
return IntPtr.Add(mapped.Address, checked((int)offsetInMapping));
|
||||
foreach (var key in fdsData.Keys)
|
||||
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
|
||||
fdsData[key] = null;
|
||||
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
|
||||
for (int i = 0; i < planesData.Count(); i++)
|
||||
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
|
||||
planesData[i] = null;
|
||||
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
@@ -435,20 +621,24 @@ public class CameraService : IDisposable
|
||||
Dispose(disposing: true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_state.HasFlag(State.ImageCaptureFrameBuffersMapped) && _mappedBuffers is not null)
|
||||
if (_state.HasFlag(State.ImageCaptureFrameBuffersMapped) && _imageCaptureMappedBuffers is not null)
|
||||
{
|
||||
foreach (var buffer in _mappedBuffers)
|
||||
foreach (var buffer in _imageCaptureMappedBuffers)
|
||||
{
|
||||
_logger.LogDebug("MUNMAP address=0x{:X}, length=0x{:X}", buffer.Address, buffer.Length);
|
||||
LibC.munmap(buffer.Address, buffer.Length);
|
||||
_mappedBuffers.Clear();
|
||||
}
|
||||
_imageCaptureMappedBuffers.Clear();
|
||||
}
|
||||
if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
|
||||
_frameBufferAllocator?.Free(_viewFinderStream);
|
||||
_viewFinderFrameBufferAllocator?.Free(_viewFinderStream);
|
||||
if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
|
||||
_frameBufferAllocator?.Free(_imageCaptureStream);
|
||||
_imageCaptureFrameBufferAllocator?.Free(_imageCaptureStream);
|
||||
if (_state.HasFlag(State.CameraStarted))
|
||||
_camera?.Stop();
|
||||
if (_state.HasFlag(State.CameraAcquired))
|
||||
@@ -456,9 +646,10 @@ public class CameraService : IDisposable
|
||||
if (_state.HasFlag(State.CameraManagerStarted))
|
||||
_cameraManager?.Stop();
|
||||
|
||||
_viewFinderStream?.Dispose();
|
||||
_imageCaptureStream?.Dispose();
|
||||
_frameBufferAllocator?.Dispose();
|
||||
// _viewFinderStream?.Dispose();
|
||||
// _imageCaptureStream?.Dispose();
|
||||
// _viewFinderFrameBufferAllocator?.Dispose();
|
||||
// _imageCaptureFrameBufferAllocator?.Dispose();
|
||||
_cameraManager?.Dispose();
|
||||
}
|
||||
catch (Exception e)
|
||||
@@ -469,10 +660,11 @@ public class CameraService : IDisposable
|
||||
{
|
||||
_cameraManager = null;
|
||||
_camera = null;
|
||||
_frameBufferAllocator = null;
|
||||
_viewFinderFrameBufferAllocator = null;
|
||||
_imageCaptureFrameBufferAllocator = null;
|
||||
_viewFinderStream = null;
|
||||
_imageCaptureStream = null;
|
||||
_mappedBuffers = null;
|
||||
_imageCaptureMappedBuffers = null;
|
||||
_state = State.NotInitialized;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,152 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
using OpenCvSharp;
|
||||
|
||||
using LibCameraSharp;
|
||||
|
||||
namespace GSS2.FrameDecoders;
|
||||
|
||||
public class BGGR_PISP_COMP1_RAW_FrameDecoder : IFrameDecoder // BGGR_PISP_COMP1/RAW
|
||||
{
|
||||
// Не знаю как оно работает, но работает
|
||||
static class PispDecompressor
|
||||
{
|
||||
const int COMPRESS_OFFSET = 2048;
|
||||
|
||||
public static void DecompressPisp(ReadOnlySpan<byte> src, int width, int height, int stride, Span<ushort> dst)
|
||||
{
|
||||
int paddedWidth = (width + 7) & ~7;
|
||||
|
||||
for (int y = 0; y < height; y++)
|
||||
{
|
||||
int srcRow = y * stride;
|
||||
int dstRow = y * paddedWidth;
|
||||
|
||||
int sp = srcRow;
|
||||
int dp = dstRow;
|
||||
|
||||
for (int x = 0; x < paddedWidth; x += 8)
|
||||
{
|
||||
uint w0 = BinaryPrimitives.ReadUInt32LittleEndian(src.Slice(sp, 4));
|
||||
uint w1 = BinaryPrimitives.ReadUInt32LittleEndian(src.Slice(sp + 4, 4));
|
||||
sp += 8;
|
||||
|
||||
SubBlock(dst, dp, w0); // even pixels
|
||||
SubBlock(dst, dp + 1, w1); // odd pixels
|
||||
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
dst[dp + i] = Postprocess(dst[dp + i]);
|
||||
}
|
||||
|
||||
dp += 8;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static ushort Postprocess(ushort a)
|
||||
{
|
||||
int v = a + COMPRESS_OFFSET;
|
||||
return (ushort)Math.Min(0xFFFF, v);
|
||||
}
|
||||
|
||||
static void SubBlock(Span<ushort> d, int baseIndex, uint w)
|
||||
{
|
||||
int qmode = (int)(w & 3);
|
||||
int[] q = new int[4];
|
||||
|
||||
if (qmode < 3)
|
||||
{
|
||||
int field0 = (int)((w >> 2) & 511);
|
||||
int field1 = (int)((w >> 11) & 127);
|
||||
int field2 = (int)((w >> 18) & 127);
|
||||
int field3 = (int)((w >> 25) & 127);
|
||||
|
||||
if (qmode == 2 && field0 >= 384)
|
||||
{
|
||||
q[1] = field0;
|
||||
q[2] = field1 + 384;
|
||||
}
|
||||
else
|
||||
{
|
||||
q[1] = (field1 >= 64) ? field0 : field0 + 64 - field1;
|
||||
q[2] = (field1 >= 64) ? field0 + field1 - 64 : field0;
|
||||
}
|
||||
|
||||
int p1 = Math.Max(0, q[1] - 64);
|
||||
int p2 = Math.Max(0, q[2] - 64);
|
||||
if (qmode == 2)
|
||||
{
|
||||
p1 = Math.Min(384, p1);
|
||||
p2 = Math.Min(384, p2);
|
||||
}
|
||||
|
||||
q[0] = p1 + field2;
|
||||
q[3] = p2 + field3;
|
||||
}
|
||||
else
|
||||
{
|
||||
int pack0 = (int)((w >> 2) & 32767);
|
||||
int pack1 = (int)((w >> 17) & 32767);
|
||||
|
||||
q[0] = (pack0 & 15) + 16 * ((pack0 >> 8) / 11);
|
||||
q[1] = (pack0 >> 4) % 176;
|
||||
q[2] = (pack1 & 15) + 16 * ((pack1 >> 8) / 11);
|
||||
q[3] = (pack1 >> 4) % 176;
|
||||
}
|
||||
|
||||
d[baseIndex + 0] = Dequantize(q[0], qmode);
|
||||
d[baseIndex + 2] = Dequantize(q[1], qmode);
|
||||
d[baseIndex + 4] = Dequantize(q[2], qmode);
|
||||
d[baseIndex + 6] = Dequantize(q[3], qmode);
|
||||
}
|
||||
|
||||
static ushort Dequantize(int q, int qmode)
|
||||
{
|
||||
int v = qmode switch
|
||||
{
|
||||
0 => (q < 320) ? 16 * q : 32 * (q - 160),
|
||||
1 => 64 * q,
|
||||
2 => 128 * q,
|
||||
_ => (q < 94) ? 256 * q : Math.Min(0xFFFF, 512 * (q - 47))
|
||||
};
|
||||
|
||||
return (ushort)Math.Min(0xFFFF, v);
|
||||
}
|
||||
}
|
||||
|
||||
public uint Fourcc => 0x32525942; // BYR2
|
||||
// Модификатор 0xC00000000000001 в соответствии с libcamera является PISP_FORMAT_MOD_COMPRESS_MODE1
|
||||
// https://github.com/raspberrypi/libcamera/blob/f0e40f1c50bd0afe65727d6e407d0dcb42666ada/include/linux/drm_fourcc.h#L1692
|
||||
// Режимы компрессии Raspberry Pi PiSP описаны в документации ядра linux:
|
||||
// 2.6.1.3.1. Raspberry Pi PiSP compressed 8-bit Bayer formats
|
||||
// https://docs.kernel.org/userspace-api/media/v4l/pixfmt-srggb8-pisp-comp.html
|
||||
// Пример реализации декодера приведён в репозитории https://github.com/raspberrypi/rpicam-apps
|
||||
// https://github.com/raspberrypi/rpicam-apps/blob/d821489c2ec89f541915393a823555632653a58f/image/dng.cpp
|
||||
public ulong Modifier => 0xC00000000000001; // PISP_FORMAT_MOD_COMPRESS_MODE1
|
||||
public ColorSpace.PrimariesEnum PrimariesEnum => ColorSpace.PrimariesEnum.Raw;
|
||||
public ColorSpace.TransferFunctionEnum TransferFunctionEnum => ColorSpace.TransferFunctionEnum.Linear;
|
||||
public ColorSpace.YcbcrEncodingEnum YcbcrEncodingEnum => ColorSpace.YcbcrEncodingEnum.None;
|
||||
public ColorSpace.RangeEnum RangeEnum => ColorSpace.RangeEnum.Full;
|
||||
|
||||
public Mat Decode(int width, int height, int stride, List<byte[]> planes)
|
||||
{
|
||||
// Дектоирование этого формата состоит из двух этапов:
|
||||
// 1) Распаковка исходных данных в которых каждые 8 байт представляют 8 пикселей (это не значит что 1 байт представляет 1 пиксель!)
|
||||
// 2) Демозаика полученого Bayer изображения в RGB (BGR)
|
||||
|
||||
if (planes.Count != 1)
|
||||
throw new ArgumentException($"Single plane expected, got {planes.Count}");
|
||||
|
||||
var source = planes[0];
|
||||
|
||||
var unpacked = new ushort[width * height];
|
||||
PispDecompressor.DecompressPisp(source, width, height, stride, unpacked);
|
||||
|
||||
using var bayer = Mat.FromPixelData(height, width, MatType.CV_16UC1, unpacked);
|
||||
var bgr = new Mat();
|
||||
Cv2.CvtColor(bayer, bgr, ColorConversionCodes.BayerRG2BGR);
|
||||
|
||||
return bgr;
|
||||
}
|
||||
}
|
||||
@@ -47,10 +47,10 @@ public static class FrameDecoderHelper
|
||||
public static IFrameDecoder? FindDecoder(
|
||||
uint fourcc,
|
||||
ulong modifier,
|
||||
ColorSpace.PrimariesEnum primariesEnum,
|
||||
ColorSpace.TransferFunctionEnum transferFunctionEnum,
|
||||
ColorSpace.YcbcrEncodingEnum ycbcrEncodingEnum,
|
||||
ColorSpace.RangeEnum rangeEnum)
|
||||
ColorSpace.PrimariesEnum? primariesEnum,
|
||||
ColorSpace.TransferFunctionEnum? transferFunctionEnum,
|
||||
ColorSpace.YcbcrEncodingEnum? ycbcrEncodingEnum,
|
||||
ColorSpace.RangeEnum? rangeEnum)
|
||||
{
|
||||
return _decoders.FirstOrDefault(d =>
|
||||
d.Fourcc == fourcc &&
|
||||
|
||||
@@ -13,5 +13,5 @@ public interface IFrameDecoder
|
||||
public ColorSpace.YcbcrEncodingEnum YcbcrEncodingEnum { get; }
|
||||
public ColorSpace.RangeEnum RangeEnum { get; }
|
||||
|
||||
public Mat Decode(int width, int height, int stride, List<Memory<byte>> planes);
|
||||
public Mat Decode(int width, int height, int stride, List<byte[]> planes);
|
||||
}
|
||||
|
||||
+29
-7
@@ -6,21 +6,43 @@ namespace GSS2.Test;
|
||||
|
||||
public class Worker(
|
||||
ILogger<Worker> logger,
|
||||
AmineContentCalibrationContext calibrationContext,
|
||||
LightsService lightsService
|
||||
IHostApplicationLifetime applicationLifetime,
|
||||
AmineContentCalibrationContext amineCalibrationContext,
|
||||
AmineContentResultsContext amineResultsContext,
|
||||
LightsService lightsService,
|
||||
IlluminatorService illuminatorService,
|
||||
CameraService cameraService
|
||||
) : BackgroundService
|
||||
{
|
||||
private readonly ILogger<Worker> _logger = logger;
|
||||
private readonly AmineContentCalibrationContext _calibrationContext = calibrationContext;
|
||||
private readonly IHostApplicationLifetime _applicationLifetime = applicationLifetime;
|
||||
private readonly AmineContentCalibrationContext _amineCalibrationContext = amineCalibrationContext;
|
||||
private readonly AmineContentResultsContext _amineResultsContext = amineResultsContext;
|
||||
private readonly LightsService _lightsService = lightsService;
|
||||
private readonly IlluminatorService _illuminatorService = illuminatorService;
|
||||
private readonly CameraService _cameraService = cameraService;
|
||||
|
||||
protected override async Task ExecuteAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
await _cameraService.InitializeAsync(cancellationToken);
|
||||
|
||||
_ = _lightsService.Run(cancellationToken);
|
||||
_ = _lightsService.SnowfallAsync(0.3, 1, Color.Aqua, cancellationToken);
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await Task.Delay(5000);
|
||||
}
|
||||
|
||||
_illuminatorService.SetIntensity(0.5, 0, 0);
|
||||
_illuminatorService.TurnOn();
|
||||
|
||||
await Task.Delay(1000);
|
||||
|
||||
var img = await _cameraService.CaptureImage(cancellationToken);
|
||||
img?.SaveImage("IMAGE.png");
|
||||
|
||||
await Task.Delay(1000);
|
||||
|
||||
_illuminatorService.TurnOff();
|
||||
|
||||
await _lightsService.DisconnectAsync(cancellationToken);
|
||||
|
||||
applicationLifetime.StopApplication();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,8 +6,8 @@
|
||||
"Microsoft.EntityFrameworkCore.Migrations": "Warning",
|
||||
"Microsoft.EntityFrameworkCore.Database.Command": "Warning",
|
||||
"GSS2.Core.Hardware.LightsService": "Information",
|
||||
"GSS2.Core.Hardware.IlluminatorService":"Information",
|
||||
"GSS2.Core.Hardware.CameraService":"Information",
|
||||
"GSS2.Core.Hardware.IlluminatorService": "Information",
|
||||
"GSS2.Core.Hardware.CameraService": "Debug",
|
||||
"LibCamera": "Information",
|
||||
"LibCamera-Camera": "Information",
|
||||
"LibCamera-RPI": "Information",
|
||||
@@ -35,11 +35,14 @@
|
||||
"MaxUv365PwmDuty": 1.0
|
||||
},
|
||||
"Camera": {
|
||||
"CameraId": "",
|
||||
"ViewFinderFrameBufferCount": 10,
|
||||
"ViewFinderWidth": 1920,
|
||||
"ViewFinderHeight": 1080,
|
||||
"ViewFinderFps": 30,
|
||||
"ImageCaptureFrameBufferCount": 2,
|
||||
"ImageCaptureWidth": 4056,
|
||||
"ImageCaptureHeight": 3080
|
||||
"ImageCaptureHeight": 3040
|
||||
},
|
||||
"TemperatureHumidity": {
|
||||
"Bus": 6,
|
||||
|
||||
Reference in New Issue
Block a user