feat: CameraService

Add stable optainitng of raw image from camera
2026-01-15 14:58:11 +03:00
parent 717efe6781
commit ced7e294eb
8 changed files with 467 additions and 90 deletions
@@ -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)
-                    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<Memory<byte>>();
+            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<byte>(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;
        }
    }
@@ -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);
 }
@@ -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,