ipa: Add class that implements base AF control algorithm

Move the code that was common for IPU3 and RPi AF algorithms to
a separate class independent of platform specific code.
This way each platform can just implement contrast calculation and
run the AF control loop basing on this class.

Signed-off-by: Daniel Semkowicz <dse@thaumatec.com>

3 files changed, 342 insertions, 0 deletions

diff --git a/src/ipa/libipa/algorithms/af_hill_climbing.cpp b/src/ipa/libipa/algorithms/af_hill_climbing.cpp
new file mode 100644
index 00000000..f666c6c2
--- /dev/null
+++ b/src/ipa/libipa/algorithms/af_hill_climbing.cpp
@@ -0,0 +1,89 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ * Copyright (C) 2022, Theobroma Systems
+ *
+ * af_hill_climbing.cpp - AF Hill Climbing common algorithm
+ */
+
+#include "af_hill_climbing.h"
+
+/**
+ * \file af_hill_climbing.h
+ * \brief AF Hill Climbing common algorithm
+ */
+
+namespace libcamera {
+
+LOG_DEFINE_CATEGORY(Af)
+
+namespace ipa::common::algorithms {
+
+/**
+ * \class AfHillClimbing
+ * \brief The base class implementing hill climbing AF control algorithm
+ * \tparam Module The IPA module type for this class of algorithms
+ *
+ * Control part of auto focus algorithm. It calculates the lens position basing
+ * on contrast measure supplied by the higher level. This way it is independent
+ * from the platform.
+ *
+ * Derived class should call processAutofocus() for each measured contrast value
+ * and set the lens to the calculated position.
+ */
+
+/**
+ * \fn AfHillClimbing::setMode()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setMode
+ */
+
+/**
+ * \fn AfHillClimbing::setRange()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setRange
+ */
+
+/**
+ * \fn AfHillClimbing::setSpeed()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setSpeed
+ */
+
+/**
+ * \fn AfHillClimbing::setMetering()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setMetering
+ */
+
+/**
+ * \fn AfHillClimbing::setWindows()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setWindows
+ */
+
+/**
+ * \fn AfHillClimbing::setTrigger()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setTrigger
+ */
+
+/**
+ * \fn AfHillClimbing::setPause()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setPause
+ */
+
+/**
+ * \fn AfHillClimbing::setLensPosition()
+ * \copydoc libcamera::ipa::common::algorithms::AfAlgorithm::setLensPosition
+ */
+
+/**
+ * \fn AfHillClimbing::processAutofocus()
+ * \brief Run the auto focus algorithm loop
+ * \param[in] currentContrast New value of contrast measured for current frame
+ *
+ * This method should be called for each new contrast value that was measured,
+ * usually in the process() method.
+ *
+ * \return New lens position calculated by AF algorithm
+ */
+
+} /* namespace ipa::common::algorithms */
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/algorithms/af_hill_climbing.h b/src/ipa/libipa/algorithms/af_hill_climbing.h
new file mode 100644
index 00000000..db9fc058
--- /dev/null
+++ b/src/ipa/libipa/algorithms/af_hill_climbing.h
@@ -0,0 +1,251 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+/*
+ * Copyright (C) 2021, Red Hat
+ * Copyright (C) 2022, Ideas On Board
+ * Copyright (C) 2022, Theobroma Systems
+ *
+ * af_hill_climbing.h - AF Hill Climbing common algorithm
+ */
+
+#pragma once
+
+#include <libcamera/base/log.h>
+
+#include "af_algorithm.h"
+
+namespace libcamera {
+
+LOG_DECLARE_CATEGORY(Af)
+
+namespace ipa::common::algorithms {
+
+template<typename Module>
+class AfHillClimbing : public AfAlgorithm<Module>
+{
+public:
+	AfHillClimbing()
+		: mode_(controls::AfModeAuto), state_(controls::AfStateIdle),
+		  pauseState_(controls::AfPauseStateRunning),
+		  lensPosition_(0), bestPosition_(0), currentContrast_(0.0),
+		  previousContrast_(0.0), maxContrast_(0.0), maxStep_(0),
+		  coarseCompleted_(false), fineCompleted_(false),
+		  lowStep_(0), highStep_(kMaxFocusSteps)
+	{
+	}
+
+	virtual ~AfHillClimbing() {}
+
+	void setMode(controls::AfModeEnum mode) final
+	{
+		if (mode != mode_) {
+			LOG(Af, Debug) << "Switched AF mode from " << mode_
+				       << " to " << mode;
+			pauseState_ = libcamera::controls::AfPauseStateRunning;
+			mode_ = mode;
+		}
+	}
+
+	void setRange([[maybe_unused]] controls::AfRangeEnum range) final
+	{
+		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
+	}
+
+	void setSpeed([[maybe_unused]] controls::AfSpeedEnum speed) final
+	{
+		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
+	}
+
+	void setTrigger(controls::AfTriggerEnum trigger) final
+	{
+		LOG(Af, Debug) << "Trigger called in mode " << mode_
+			       << " with " << trigger;
+		if (mode_ == libcamera::controls::AfModeAuto) {
+			if (trigger == libcamera::controls::AfTriggerStart)
+				afReset();
+			else
+				state_ = libcamera::controls::AfStateIdle;
+		}
+	}
+
+	void setPause(controls::AfPauseEnum pause) final
+	{
+		/* \todo: add the AfPauseDeferred mode */
+		if (mode_ == libcamera::controls::AfModeContinuous) {
+			if (pause == libcamera::controls::AfPauseImmediate)
+				pauseState_ = libcamera::controls::AfPauseStatePaused;
+			else if (pause == libcamera::controls::AfPauseResume)
+				pauseState_ = libcamera::controls::AfPauseStateRunning;
+		}
+	}
+
+	void setLensPosition([[maybe_unused]] float lensPosition) final
+	{
+		LOG(Af, Error) << __FUNCTION__ << " not implemented!";
+	}
+
+	/* These methods should be implemented by derived class */
+	virtual void setMetering(controls::AfMeteringEnum metering) = 0;
+	virtual void setWindows(Span<const Rectangle> windows) = 0;
+
+protected:
+	uint32_t processAutofocus(double currentContrast)
+	{
+		currentContrast_ = currentContrast;
+
+		/* If we are in a paused state, we won't process the stats */
+		if (pauseState_ == libcamera::controls::AfPauseStatePaused)
+			return lensPosition_;
+
+		/* Depending on the mode, we may or may not process the stats */
+		if (state_ == libcamera::controls::AfStateIdle)
+			return lensPosition_;
+
+		if (state_ != libcamera::controls::AfStateFocused) {
+			afCoarseScan();
+			afFineScan();
+		} else {
+			/* We can re-start the scan at any moment in AfModeContinuous */
+			if (mode_ == libcamera::controls::AfModeContinuous)
+				if (afIsOutOfFocus())
+					afReset();
+		}
+
+		return lensPosition_;
+	}
+
+private:
+	void afCoarseScan()
+	{
+		if (coarseCompleted_)
+			return;
+
+		if (afScan(kCoarseSearchStep)) {
+			coarseCompleted_ = true;
+			maxContrast_ = 0;
+			lensPosition_ = lensPosition_ - (lensPosition_ * kFineRange);
+			previousContrast_ = 0;
+			maxStep_ = std::clamp(lensPosition_ + static_cast<uint32_t>((lensPosition_ * kFineRange)),
+					      0U, highStep_);
+		}
+	}
+
+	void afFineScan()
+	{
+		if (!coarseCompleted_)
+			return;
+
+		if (afScan(kFineSearchStep)) {
+			LOG(Af, Debug) << "AF found the best focus position !";
+			state_ = libcamera::controls::AfStateFocused;
+			fineCompleted_ = true;
+		}
+	}
+
+	bool afScan(uint32_t minSteps)
+	{
+		if (lensPosition_ + minSteps > maxStep_) {
+			/* If the max step is reached, move lens to the position. */
+			lensPosition_ = bestPosition_;
+			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.
+				*/
+				bestPosition_ = lensPosition_;
+				lensPosition_ += minSteps;
+				maxContrast_ = currentContrast_;
+			} 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.
+				*/
+				lensPosition_ = bestPosition_;
+				return true;
+			}
+		}
+
+		previousContrast_ = currentContrast_;
+		LOG(Af, Debug) << " Previous step is " << bestPosition_
+			       << " Current step is " << lensPosition_;
+		return false;
+	}
+
+	void afReset()
+	{
+		LOG(Af, Debug) << "Reset AF parameters";
+		lensPosition_ = lowStep_;
+		maxStep_ = highStep_;
+		state_ = libcamera::controls::AfStateScanning;
+		previousContrast_ = 0.0;
+		coarseCompleted_ = false;
+		fineCompleted_ = false;
+		maxContrast_ = 0.0;
+	}
+
+	bool afIsOutOfFocus()
+	{
+		const uint32_t diff_var = std::abs(currentContrast_ -
+						   maxContrast_);
+		const double var_ratio = diff_var / maxContrast_;
+		LOG(Af, Debug) << "Variance change rate: " << var_ratio
+			       << " Current VCM step: " << lensPosition_;
+		if (var_ratio > kMaxChange)
+			return true;
+		else
+			return false;
+	}
+
+	controls::AfModeEnum mode_;
+	controls::AfStateEnum state_;
+	controls::AfPauseStateEnum pauseState_;
+
+	/* VCM step configuration. It is the current setting of the VCM step. */
+	uint32_t lensPosition_;
+	/* The best VCM step. It is a local optimum VCM step during scanning. */
+	uint32_t bestPosition_;
+
+	/* 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 lowStep_;
+	uint32_t highStep_;
+
+	/*
+	* 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;
+};
+
+} /* namespace ipa::common::algorithms */
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/algorithms/meson.build b/src/ipa/libipa/algorithms/meson.build
index ab8da13a..860dc199 100644
--- a/src/ipa/libipa/algorithms/meson.build
+++ b/src/ipa/libipa/algorithms/meson.build
@@ -2,8 +2,10 @@
 
 common_ipa_algorithms_headers = files([
     'af_algorithm.h',
+    'af_hill_climbing.h',
 ])
 
 common_ipa_algorithms_sources = files([
     'af_algorithm.cpp',
+    'af_hill_climbing.cpp',
 ])