new file mode 100644
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi (Trading) Limited
+ *
+ * af_algorithm.h - autofocus control algorithm interface
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+#include "algorithm.h"
+
+namespace RPiController {
+
+class AfAlgorithm : public Algorithm
+{
+public:
+ AfAlgorithm(Controller *controller) : Algorithm(controller) {}
+ /* An af algorithm must provide the following: */
+ virtual void setMode(const uint32_t &mode) = 0;
+ /* start a cycle (in auto mode) */
+ virtual void trigger() = 0;
+ /* cancel a cycle (in auto mode) */
+ virtual void cancel() = 0;
+ /* set AF windows */
+ virtual void setWindows(const libcamera::Rectangle &afWindows) = 0;
+ /* set AF range */
+ virtual void setRange(const uint32_t &range) = 0;
+ /* set AF speed */
+ virtual void setSpeed(const uint32_t &speed) = 0;
+};
+
+} /* namespace RPiController */
new file mode 100644
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022, Raspberry Pi (Trading) Limited
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af_status.h - autofocus measurement status
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+/*
+ * The focus algorithm should post the following structure into the image's
+ * "af.status" metadata.
+ */
+
+struct AfStatus {
+ uint32_t lensPosition;
+ uint32_t state;
+ libcamera::Rectangle windows;
+};
new file mode 100644
@@ -0,0 +1,212 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af.cpp - automatic contrast-based focus algorithm
+ */
+
+#include "af.h"
+
+#include <cmath>
+#include <stdint.h>
+
+#include <libcamera/base/log.h>
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(IoBAf)
+
+#define NAME "iob.af"
+
+/*
+ * Maximum focus steps of the VCM control
+ * \todo should be obtained from the VCM driver
+ */
+static constexpr uint32_t kMaxFocusSteps = 1023;
+
+/* Minimum focus step for searching appropriate focus */
+static constexpr uint32_t kCoarseSearchStep = 30;
+static constexpr uint32_t kFineSearchStep = 1;
+
+/* Max ratio of variance change, 0.0 < kMaxChange < 1.0 */
+static constexpr double kMaxChange = 0.5;
+
+/* Fine scan range 0 < kFineRange < 1 */
+static constexpr double kFineRange = 0.05;
+
+Af::Af(Controller *controller)
+ : AfAlgorithm(controller), focus_(0), bestFocus_(0),
+ currentContrast_(0.0), previousContrast_(0.0), maxContrast_(0.0),
+ maxStep_(0), coarseCompleted_(false), fineCompleted_(false),
+ mode_(0)
+{
+}
+
+char const *Af::name() const
+{
+ return NAME;
+}
+
+void Af::setMode([[maybe_unused]] const uint32_t &mode)
+{
+ mode_ = mode;
+}
+
+void Af::trigger()
+{
+}
+
+void Af::cancel()
+{
+}
+
+void Af::setWindows([[maybe_unused]] const libcamera::Rectangle &afWindows)
+{
+}
+
+void Af::setRange([[maybe_unused]] const uint32_t &range)
+{
+}
+
+void Af::setSpeed([[maybe_unused]] const uint32_t &speed)
+{
+}
+
+void Af::initialise()
+{
+ status_.lensPosition = 0.0;
+ maxContrast_ = 0.0;
+ status_.state = 1;
+}
+
+void Af::prepare(Metadata *imageMetadata)
+{
+ imageMetadata->set("af.status", status_);
+}
+
+void Af::afCoarseScan()
+{
+ if (coarseCompleted_)
+ return;
+
+ if (afScan(kCoarseSearchStep)) {
+ coarseCompleted_ = true;
+ maxContrast_ = 0;
+ focus_ = status_.lensPosition - (status_.lensPosition * kFineRange);
+ status_.lensPosition = focus_;
+ previousContrast_ = 0;
+ maxStep_ = std::clamp(focus_ + static_cast<uint32_t>((focus_ * kFineRange)),
+ 0U, kMaxFocusSteps);
+ }
+}
+
+void Af::afFineScan()
+{
+ if (!coarseCompleted_)
+ return;
+
+ if (afScan(kFineSearchStep)) {
+ LOG(IoBAf, Debug) << "AF found the best focus position !";
+ status_.state = 2;
+ fineCompleted_ = true;
+ }
+}
+
+bool Af::afScan(uint32_t minSteps)
+{
+ if (focus_ > maxStep_) {
+ /* If the max step is reached, move lens to the position. */
+ status_.lensPosition = bestFocus_;
+ return true;
+ } else {
+ /*
+ * Find the maximum of the variance by estimating its
+ * derivative. If the direction changes, it means we have passed
+ * a maximum one step before.
+ */
+ if ((currentContrast_ - maxContrast_) >= -(maxContrast_ * 0.1)) {
+ /*
+ * Positive and zero derivative:
+ * The variance is still increasing. The focus could be
+ * increased for the next comparison. Also, the max
+ * variance and previous focus value are updated.
+ */
+ bestFocus_ = focus_;
+ focus_ += minSteps;
+ maxContrast_ = currentContrast_;
+ status_.lensPosition = focus_;
+ } else {
+ /*
+ * Negative derivative:
+ * The variance starts to decrease which means the maximum
+ * variance is found. Set focus step to previous good one
+ * then return immediately.
+ */
+ status_.lensPosition = bestFocus_;
+ return true;
+ }
+ }
+
+ previousContrast_ = currentContrast_;
+ LOG(IoBAf, Debug) << " Previous step is "
+ << bestFocus_
+ << " Current step is "
+ << focus_;
+ return false;
+}
+
+void Af::afReset()
+{
+ LOG(IoBAf, Debug) << "Reset AF parameters";
+ status_.lensPosition = 0;
+ focus_ = 0;
+ status_.state = 0;
+ previousContrast_ = 0.0;
+ coarseCompleted_ = false;
+ fineCompleted_ = false;
+ maxStep_ = kMaxFocusSteps;
+ maxContrast_ = 0.0;
+}
+
+bool Af::afIsOutOfFocus()
+{
+ const uint32_t diff_var = std::abs(currentContrast_ -
+ maxContrast_);
+ const double var_ratio = diff_var / maxContrast_;
+ LOG(IoBAf, Debug) << "Variance change rate: "
+ << var_ratio
+ << " Current VCM step: "
+ << status_.lensPosition;
+ if (var_ratio > kMaxChange)
+ return true;
+ else
+ return false;
+}
+
+void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
+{
+ unsigned int i;
+ currentContrast_ = 0.0;
+
+ /* Use the second filter results only, and cache those. */
+ for (i = 0; i < FOCUS_REGIONS; i++)
+ currentContrast_ += stats->focus_stats[i].contrast_val[1][1]
+ / stats->focus_stats[i].contrast_val_num[1][1];
+
+ if (status_.state != 2) {
+ afCoarseScan();
+ afFineScan();
+ } else {
+ if (afIsOutOfFocus())
+ afReset();
+ }
+}
+
+/* Register algorithm with the system. */
+static Algorithm *Create(Controller *controller)
+{
+ return new Af(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
new file mode 100644
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ *
+ * af.h - automatic contrast-based focus algorithm
+ */
+#pragma once
+
+#include <libcamera/geometry.h>
+
+#include "../af_algorithm.h"
+#include "../af_status.h"
+#include "../metadata.h"
+
+namespace RPiController {
+
+class Af : public AfAlgorithm
+{
+public:
+ Af(Controller *controller);
+ char const *name() const override;
+ void initialise() override;
+ void prepare(Metadata *image_metadata) override;
+ void process(StatisticsPtr &stats, Metadata *image_metadata) override;
+ void setMode(const uint32_t &mode) override;
+ void trigger() override;
+ void cancel() override;
+ void setWindows(const libcamera::Rectangle &afWindows) override;
+ void setRange(const uint32_t &range) override;
+ void setSpeed(const uint32_t &speed) override;
+private:
+ bool afNeedIgnoreFrame();
+ void afCoarseScan();
+ void afFineScan();
+ bool afScan(uint32_t minSteps);
+ void afReset();
+ bool afIsOutOfFocus();
+
+ AfStatus status_;
+
+ /* VCM step configuration. It is the current setting of the VCM step. */
+ uint32_t focus_;
+ /* The best VCM step. It is a local optimum VCM step during scanning. */
+ uint32_t bestFocus_;
+
+ /* Current AF statistic contrast. */
+ double currentContrast_;
+ /* It is used to determine the derivative during scanning */
+ double previousContrast_;
+ double maxContrast_;
+ /* The designated maximum range of focus scanning. */
+ uint32_t maxStep_;
+ /* If the coarse scan completes, it is set to true. */
+ bool coarseCompleted_;
+ /* If the fine scan completes, it is set to true. */
+ bool fineCompleted_;
+
+ uint32_t mode_;
+};
+
+} /* namespace RPiController */
@@ -27,6 +27,7 @@ rpi_ipa_sources = files([
'controller/controller.cpp',
'controller/histogram.cpp',
'controller/algorithm.cpp',
+ 'controller/iob/af.cpp',
'controller/rpi/alsc.cpp',
'controller/rpi/awb.cpp',
'controller/rpi/sharpen.cpp',