Add support for autofocus

.github/workflows/main.yml

@@ -21,11 +21,17 @@ jobs:
         bind_mount_repository: true
         commands: |
             apt-get update
-            apt-get install -y libopencv-dev libegl1-mesa-dev libcamera-dev cmake build-essential libdrm-dev libgbm-dev openjdk-11-jdk
+            apt-get install -y libopencv-dev libegl1-mesa-dev cmake build-essential libdrm-dev libgbm-dev openjdk-11-jdk
+            apt-get remove -y libcamera0 python3-libcamera
+            wget https://github.com/ArduCAM/Arducam-Pivariety-V4L2-Driver/releases/download/libcamera-v0.0.5/libcamera-dev-0.0.12-bullseye-arm64.deb
+            dpkg -i libcamera-dev-0.0.12-bullseye-arm64.deb
+            apt-get -f install
             cmake -B build-pi -DCMAKE_BUILD_TYPE=Release
             cmake --build build-pi -j 4
 
     - run: find .
+
+    - run: find / | grep -e "libcamera.so"
 
     - uses: actions/upload-artifact@master
       with:

blocking_future.h

@@ -1,8 +1,8 @@
 #pragma once
 
-#include <optional>
-#include <mutex>
 #include <condition_variable>
+#include <mutex>
+#include <optional>
 
 template <typename T> class BlockingFuture {
   public:

camera_grabber.cpp

@@ -8,12 +8,13 @@
 
 CameraGrabber::CameraGrabber(std::shared_ptr<libcamera::Camera> camera,
                              int width, int height, int rotation)
-    : m_buf_allocator(camera), m_camera(std::move(camera)), m_cameraExposureProfiles(std::nullopt) {
+    : m_buf_allocator(camera), m_camera(std::move(camera)),
+      m_cameraExposureProfiles(std::nullopt) {
 
     if (m_camera->acquire()) {
         throw std::runtime_error("failed to acquire camera");
     }
- 
+
     // Determine model
     auto &cprp = m_camera->properties();
     auto model = cprp.get(libcamera::properties::Model);
@@ -23,30 +24,33 @@ CameraGrabber::CameraGrabber(std::shared_ptr<libcamera::Camera> camera,
         m_model = Unknown;
     }
 
-    std::cout << "Model " << m_model << std::endl;   
+    std::cout << "Model " << m_model << std::endl;
 
     auto config = m_camera->generateConfiguration(
         {libcamera::StreamRole::VideoRecording});
-    
+
     // print active arrays
-    if (m_camera->properties().contains(libcamera::properties::PIXEL_ARRAY_ACTIVE_AREAS)) {
+    if (m_camera->properties().contains(
+            libcamera::properties::PIXEL_ARRAY_ACTIVE_AREAS)) {
         printf("Active areas:\n");
-        auto rects = m_camera->properties().get(libcamera::properties::PixelArrayActiveAreas);
+        auto rects = m_camera->properties().get(
+            libcamera::properties::PixelArrayActiveAreas);
         if (rects.has_value()) {
-            for(const auto rect : rects.value()) {
+            for (const auto rect : rects.value()) {
                 std::cout << rect.toString() << std::endl;
             }
         }
-    } else 
+    } else
         printf("No active areas\n");
 
     config->at(0).size.width = width;
     config->at(0).size.height = height;
 
-    printf("Rotation = %i\n", rotation);
+    // printf("Rotation = %i\n", rotation);
     if (rotation == 180) {
         using namespace libcamera;
-        config->transform = Transform::HFlip * Transform::VFlip * libcamera::Transform::Identity;
+        config->transform = Transform::HFlip * Transform::VFlip *
+                            libcamera::Transform::Identity;
     } else {
         config->transform = libcamera::Transform::Identity;
     }
@@ -124,32 +128,34 @@ void CameraGrabber::setControls(libcamera::Request *request) {
         controls_.set(controls::AwbEnable, false); // AWB disabled
     }
     controls_.set(controls::AnalogueGain,
-                 m_settings.analogGain); // Analog gain, min 1 max big number?
+                  m_settings.analogGain); // Analog gain, min 1 max big number?
 
     if (m_model != OV9281) {
         controls_.set(controls::ColourGains,
-                    libcamera::Span<const float, 2>{
-                        {m_settings.awbRedGain,
-                        m_settings.awbBlueGain}}); // AWB gains, red and blue,
-                                                    // unknown range
+                      libcamera::Span<const float, 2>{
+                          {m_settings.awbRedGain,
+                           m_settings.awbBlueGain}}); // AWB gains, red and
+                                                      // blue, unknown range
     }
 
-    // Note about brightness: -1 makes everything look deep fried, 0 is probably best for most things
+    // Note about brightness: -1 makes everything look deep fried, 0 is probably
+    // best for most things
     controls_.set(libcamera::controls::Brightness,
-                m_settings.brightness); // -1 to 1, 0 means unchanged
+                  m_settings.brightness); // -1 to 1, 0 means unchanged
     controls_.set(controls::Contrast,
-                m_settings.contrast); // Nominal 1
+                  m_settings.contrast); // Nominal 1
 
     if (m_model != OV9281) {
         controls_.set(controls::Saturation,
-                m_settings.saturation); // Nominal 1, 0 would be greyscale
+                      m_settings.saturation); // Nominal 1, 0 would be greyscale
     }
 
     if (m_settings.doAutoExposure) {
         controls_.set(controls::AeEnable,
-                    true); // Auto exposure disabled
+                      true); // Auto exposure disabled
 
-        controls_.set(controls::AeMeteringMode, controls::MeteringCentreWeighted);
+        controls_.set(controls::AeMeteringMode,
+                      controls::MeteringCentreWeighted);
         if (m_model == OV9281) {
             controls_.set(controls::AeExposureMode, controls::ExposureNormal);
         } else {
@@ -160,30 +166,46 @@ void CameraGrabber::setControls(libcamera::Request *request) {
         // seconds * 1e6 = uS
         constexpr const int MIN_FRAME_TIME = 1e6 / 250;
         constexpr const int MAX_FRAME_TIME = 1e6 / 15;
-        controls_.set(
-            libcamera::controls::FrameDurationLimits,
-            libcamera::Span<const int64_t, 2>{
-                {MIN_FRAME_TIME, MAX_FRAME_TIME}});
+        controls_.set(libcamera::controls::FrameDurationLimits,
+                      libcamera::Span<const int64_t, 2>{
+                          {MIN_FRAME_TIME, MAX_FRAME_TIME}});
     } else {
         controls_.set(controls::AeEnable,
-                    false); // Auto exposure disabled
+                      false); // Auto exposure disabled
         controls_.set(controls::ExposureTime,
-                    m_settings.exposureTimeUs); // in microseconds
+                      m_settings.exposureTimeUs); // in microseconds
         controls_.set(
             libcamera::controls::FrameDurationLimits,
             libcamera::Span<const int64_t, 2>{
                 {m_settings.exposureTimeUs,
-                m_settings.exposureTimeUs}}); // Set default to zero, we have
-                                            // specified the exposure time
+                 m_settings.exposureTimeUs}}); // Set default to zero, we have
+                                               // specified the exposure time
+    }
+
+    if (m_settings.doAutofocus) {
+        printf("Starting autofocus...\n");
+        controls_.set(controls::AfMode,
+                      controls::AfModeAuto); // auto focus enable
+        controls_.set(controls::AfTrigger,
+                      controls::AfTriggerStart); // start a focus scan
+        m_settings.doAutofocus = false;
+    }
+
+    if (auto result = controls_.get(libcamera::controls::AfState)) {
+        afState = *result;
+    } else {
+        afState = controls::AfStateFailed;
     }
 
     controls_.set(controls::ExposureValue, 0);
- 
+
     if (m_model != OV7251 && m_model != OV9281) {
         controls_.set(controls::Sharpness, 1);
     }
 }
 
+int CameraGrabber::getAutofocusStatus() { return afState; }
+
 void CameraGrabber::startAndQueue() {
     running = true;
     if (m_camera->start()) {
@@ -204,7 +226,6 @@ void CameraGrabber::stop() {
     m_camera->stop();
 }
 
-
 void CameraGrabber::setOnData(
     std::function<void(libcamera::Request *)> onData) {
     m_onData = std::move(onData);

camera_grabber.h

@@ -18,7 +18,7 @@ struct CameraSettings {
     float awbBlueGain = 1.5;
     float saturation = 1;
     bool doAutoExposure = false;
-    // float digitalGain = 100;
+    bool doAutofocus = false;
 };
 
 class CameraGrabber {
@@ -35,6 +35,8 @@ class CameraGrabber {
 
     inline CameraSettings &cameraSettings() { return m_settings; }
 
+    int getAutofocusStatus();
+
     // Note: these 3 functions must be protected by mutual exclusion.
     // Failure to do so will result in UB.
     void startAndQueue();
@@ -50,14 +52,16 @@ class CameraGrabber {
     std::vector<std::unique_ptr<libcamera::Request>> m_requests;
     std::shared_ptr<libcamera::Camera> m_camera;
     CameraModel m_model;
-    std::optional<std::array<libcamera::ControlValue, 4>> m_cameraExposureProfiles;
+    std::optional<std::array<libcamera::ControlValue, 4>>
+        m_cameraExposureProfiles;
     std::unique_ptr<libcamera::CameraConfiguration> m_config;
 
     std::optional<std::function<void(libcamera::Request *)>> m_onData;
 
+    int afState;
+
     CameraSettings m_settings{};
     bool running = false;
 
-
     void setControls(libcamera::Request *request);
 };

camera_model.cpp

@@ -1,14 +1,21 @@
 #include "camera_model.h"
 #include <cstring>
 
-CameraModel stringToModel(const std::string& model) {
+CameraModel stringToModel(const std::string &model) {
     printf("Checking model: %s\n", model.c_str());
-    const char* famname = model.c_str();
-    if (!strcmp(famname, "ov5647")) return OV5647;
-    else if (!strcmp(famname, "imx219")) return IMX219;
-    else if (!strcmp(famname, "imx477")) return IMX477;
-    else if (!strcmp(famname, "ov9281")) return OV9281;
-    else if (!strcmp(famname, "ov7251")) return OV7251;
-    else if (!strcmp(famname, "Disconnected")) return Disconnected;
-    else return Unknown;
+    const char *famname = model.c_str();
+    if (!strcmp(famname, "ov5647"))
+        return OV5647;
+    else if (!strcmp(famname, "imx219"))
+        return IMX219;
+    else if (!strcmp(famname, "imx477"))
+        return IMX477;
+    else if (!strcmp(famname, "ov9281"))
+        return OV9281;
+    else if (!strcmp(famname, "ov7251"))
+        return OV7251;
+    else if (!strcmp(famname, "Disconnected"))
+        return Disconnected;
+    else
+        return Unknown;
 }

camera_model.h

@@ -12,4 +12,4 @@ enum CameraModel {
     Unknown
 };
 
-CameraModel stringToModel(const std::string& model);
+CameraModel stringToModel(const std::string &model);

camera_runner.cpp

@@ -16,8 +16,8 @@ using latch = Latch;
 #include <sys/mman.h>
 #include <unistd.h>
 
-#include <libcamera/property_ids.h>
 #include <libcamera/control_ids.h>
+#include <libcamera/property_ids.h>
 
 using namespace std::chrono;
 using namespace std::chrono_literals;
@@ -32,9 +32,8 @@ static double approxRollingAverage(double avg, double new_sample) {
 CameraRunner::CameraRunner(int width, int height, int rotation,
                            std::shared_ptr<libcamera::Camera> cam)
     : m_camera(std::move(cam)), m_width(width), m_height(height),
-      grabber(m_camera, m_width, m_height, rotation), m_thresholder(m_width, m_height),
-      allocer("/dev/dma_heap/linux,cma") {
-
+      grabber(m_camera, m_width, m_height, rotation),
+      m_thresholder(m_width, m_height), allocer("/dev/dma_heap/linux,cma") {
 
     grabber.setOnData(
         [&](libcamera::Request *request) { camera_queue.push(request); });
@@ -50,9 +49,7 @@ CameraRunner::~CameraRunner() {
     }
 }
 
-void CameraRunner::requestShaderIdx(int idx) {
-    m_shaderIdx = idx;
-}
+void CameraRunner::requestShaderIdx(int idx) { m_shaderIdx = idx; }
 
 void CameraRunner::setCopyOptions(bool copyIn, bool copyOut) {
     m_copyInput = copyIn;
@@ -75,7 +72,6 @@ void CameraRunner::start() {
             // printf("Threshold thread!\n");
             auto request = camera_queue.pop();
 
-
             if (!request) {
                 break;
             }
@@ -84,10 +80,13 @@ void CameraRunner::start() {
                               .at(grabber.streamConfiguration().stream())
                               ->planes();
 
-            for (int i = 0; i < 3; i++) {
-                // std::cout << "Plane " << (i + 1) << " has fd " << planes[i].fd.get() << " with offset " << planes[i].offset << std::endl;
-                // std::cout << "Plane " << (i + 1) << " has fd " << planes[i].fd.get() << " with offset " << planes[i].offset << " and pitch " << static_cast<EGLint>(stride / 2) << std::endl;
-            }
+            //        for (int i = 0; i < 3; i++) {
+            // std::cout << "Plane " << (i + 1) << " has fd " <<
+            // planes[i].fd.get() << " with offset " << planes[i].offset <<
+            // std::endl; std::cout << "Plane " << (i + 1) << " has fd " <<
+            // planes[i].fd.get() << " with offset " << planes[i].offset << "
+            // and pitch " << static_cast<EGLint>(stride / 2) << std::endl;
+            //      }
 
             std::array<GlHsvThresholder::DmaBufPlaneData, 3> yuv_data{{
                 {planes[0].fd.get(), static_cast<EGLint>(planes[0].offset),
@@ -102,23 +101,22 @@ void CameraRunner::start() {
 
             auto type = static_cast<ProcessType>(m_shaderIdx.load());
 
-            int out = m_thresholder.testFrame(yuv_data,
-                                    encodingFromColorspace(colorspace),
-                                    rangeFromColorspace(colorspace),
-                                              type);
-
+            int out = m_thresholder.testFrame(
+                yuv_data, encodingFromColorspace(colorspace),
+                rangeFromColorspace(colorspace), type);
 
             if (out != 0) {
                 /*
                 From libcamera docs:
 
-                The timestamp, expressed in nanoseconds, represents a monotonically
-                increasing counter since the system boot time, as defined by the
-                Linux-specific CLOCK_BOOTTIME clock id.
+                The timestamp, expressed in nanoseconds, represents a
+                monotonically increasing counter since the system boot time, as
+                defined by the Linux-specific CLOCK_BOOTTIME clock id.
                 */
-                uint64_t sensorTimestamp = static_cast<uint64_t>(request->metadata()
-                                .get(libcamera::controls::SensorTimestamp)
-                                .value_or(0));
+                uint64_t sensorTimestamp = static_cast<uint64_t>(
+                    request->metadata()
+                        .get(libcamera::controls::SensorTimestamp)
+                        .value_or(0));
 
                 gpu_queue.push({out, type, sensorTimestamp});
             }
@@ -127,8 +125,10 @@ void CameraRunner::start() {
                 steady_clock::now() - begintime;
             if (elapsedMillis > 0.9ms) {
                 // gpuTimeAvgMs =
-                //     approxRollingAverage(gpuTimeAvgMs, elapsedMillis.count());
-                // std::cout << "GLProcess: " << elapsedMillis.count() << std::endl;
+                //     approxRollingAverage(gpuTimeAvgMs,
+                //     elapsedMillis.count());
+                // std::cout << "GLProcess: " << elapsedMillis.count() <<
+                // std::endl;
             }
 
             {
@@ -200,9 +200,9 @@ void CameraRunner::start() {
 
             // auto now = steady_clock::now();
             // std::chrono::duration<double, std::milli> elapsed =
-                // (now - lastTime);
-            // fpsTimeAvgMs = approxRollingAverage(fpsTimeAvgMs, elapsed.count());
-            // printf("Delta %.2f FPS: %.2f\n", fpsTimeAvgMs,
+            // (now - lastTime);
+            // fpsTimeAvgMs = approxRollingAverage(fpsTimeAvgMs,
+            // elapsed.count()); printf("Delta %.2f FPS: %.2f\n", fpsTimeAvgMs,
             //        1000.0 / fpsTimeAvgMs);
             // lastTime = now;
         }