From ced7e294ebe53a6b08428d0814d755ec8d7b7c84 Mon Sep 17 00:00:00 2001 From: Alek-ban Date: Thu, 15 Jan 2026 14:58:11 +0300 Subject: [PATCH] feat: CameraService Add stable optainitng of raw image from camera --- GSS2.Core/Extentions/EnumerableExtentions.cs | 7 + GSS2.Core/GSS2.Core.csproj | 3 +- GSS2.Core/Hardware/CameraService.cs | 340 ++++++++++++++---- .../BGGR_PISP_COMP1_RAW_FrameDecoder.cs | 152 ++++++++ GSS2.FrameDecoders/FrameDecoderHelper.cs | 8 +- GSS2.FrameDecoders/IFrameDecoder.cs | 2 +- GSS2.Test/Worker.cs | 36 +- GSS2.Test/appsettings.json | 9 +- 8 files changed, 467 insertions(+), 90 deletions(-) create mode 100644 GSS2.Core/Extentions/EnumerableExtentions.cs create mode 100644 GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs diff --git a/GSS2.Core/Extentions/EnumerableExtentions.cs b/GSS2.Core/Extentions/EnumerableExtentions.cs new file mode 100644 index 0000000..0a26e6b --- /dev/null +++ b/GSS2.Core/Extentions/EnumerableExtentions.cs @@ -0,0 +1,7 @@ +namespace GSS2.Core.Extentions; + +static class EnumerableExtentions +{ + public static IEnumerable<(int i, T value)> Enumerate(this IEnumerable values) => values.Select((value, i) => (i, value)); +} + diff --git a/GSS2.Core/GSS2.Core.csproj b/GSS2.Core/GSS2.Core.csproj index affbaaa..02da2a2 100644 --- a/GSS2.Core/GSS2.Core.csproj +++ b/GSS2.Core/GSS2.Core.csproj @@ -18,9 +18,10 @@ + - + diff --git a/GSS2.Core/Hardware/CameraService.cs b/GSS2.Core/Hardware/CameraService.cs index 6ff29ed..f5d876c 100644 --- a/GSS2.Core/Hardware/CameraService.cs +++ b/GSS2.Core/Hardware/CameraService.cs @@ -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? _mappedBuffers; + private List? _imageCaptureMappedBuffers; private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource(); public CameraService(ILogger 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,20 +206,42 @@ 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) { _logger.LogCritical("Error while camera configuration validation"); 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(); + _imageCaptureMappedBuffers = new List(); - 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(); + var planesData = new List(); 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) - cts.SetResult(); - }; - + 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>(); + 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(); + foreach (var plane in buffer.Planes) { - effectiveCancellationToken.ThrowIfCancellationRequested(); + 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); + } - var pointer = GetPlanePointer(plane); - var data = new byte[plane.Length]; - Marshal.Copy(pointer, data, 0, checked((int)plane.Length)); - planesData.Add(new Memory(data)); + 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,17 +590,19 @@ public class CameraService : IDisposable { _logger.LogError(e, "Error while image capture"); } + finally + { + 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; } - private IntPtr GetPlanePointer(FrameBuffer.Plane plane) - { - 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)); - } 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; } } diff --git a/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs b/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs new file mode 100644 index 0000000..363c19c --- /dev/null +++ b/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs @@ -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 src, int width, int height, int stride, Span 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 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 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; + } +} diff --git a/GSS2.FrameDecoders/FrameDecoderHelper.cs b/GSS2.FrameDecoders/FrameDecoderHelper.cs index 81d759f..a9437c9 100644 --- a/GSS2.FrameDecoders/FrameDecoderHelper.cs +++ b/GSS2.FrameDecoders/FrameDecoderHelper.cs @@ -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 && diff --git a/GSS2.FrameDecoders/IFrameDecoder.cs b/GSS2.FrameDecoders/IFrameDecoder.cs index 88012b1..f47e1ed 100644 --- a/GSS2.FrameDecoders/IFrameDecoder.cs +++ b/GSS2.FrameDecoders/IFrameDecoder.cs @@ -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> planes); + public Mat Decode(int width, int height, int stride, List planes); } diff --git a/GSS2.Test/Worker.cs b/GSS2.Test/Worker.cs index e2dd286..136d17a 100644 --- a/GSS2.Test/Worker.cs +++ b/GSS2.Test/Worker.cs @@ -6,21 +6,43 @@ namespace GSS2.Test; public class Worker( ILogger logger, - AmineContentCalibrationContext calibrationContext, - LightsService lightsService + IHostApplicationLifetime applicationLifetime, + AmineContentCalibrationContext amineCalibrationContext, + AmineContentResultsContext amineResultsContext, + LightsService lightsService, + IlluminatorService illuminatorService, + CameraService cameraService ) : BackgroundService { private readonly ILogger _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(); } } diff --git a/GSS2.Test/appsettings.json b/GSS2.Test/appsettings.json index 214205d..26fa27e 100644 --- a/GSS2.Test/appsettings.json +++ b/GSS2.Test/appsettings.json @@ -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,