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, 478 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..e2eaf3d4
--- /dev/null
+++ b/src/ipa/libipa/algorithms/af_hill_climbing.cpp
@@ -0,0 +1,374 @@
+/* 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"
+
+#include "libcamera/internal/yaml_parser.h"
+
+/**
+ * \file af_hill_climbing.h
+ * \brief AF Hill Climbing common algorithm
+ */
+
+namespace libcamera::ipa::common::algorithms {
+
+LOG_DEFINE_CATEGORY(Af)
+
+/**
+ * \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.
+ */
+
+/**
+ * \brief Initialize the Algorithm with tuning data
+ * \param[in] tuningData The tuning data for the algorithm
+ *
+ * This function should be called in the init() function of the derived class.
+ * See alse: libcamera::ipa::Algorithm::init()
+ *
+ * \return 0 if successful, an error code otherwise
+ */
+int AfHillClimbing::initBase(const YamlObject &tuningData)
+{
+	minVcmPosition_ = tuningData["min-vcm-position"].get<uint32_t>(0);
+	maxVcmPosition_ = tuningData["max-vcm-position"].get<uint32_t>(100);
+	coarseSearchStep_ = tuningData["coarse-search-step"].get<uint32_t>(30);
+	fineSearchStep_ = tuningData["fine-search-step"].get<uint32_t>(1);
+	fineRange_ = tuningData["fine-scan-range"].get<double>(0.05);
+	maxChange_ = tuningData["max-variance-change"].get<double>(0.5);
+
+	LOG(Af, Debug) << "minVcmPosition_: " << minVcmPosition_
+		       << ", maxVcmPosition_: " << maxVcmPosition_
+		       << ", coarseSearchStep_: " << coarseSearchStep_
+		       << ", fineSearchStep_: " << fineSearchStep_
+		       << ", fineRange_: " << fineRange_
+		       << ", maxChange_: " << maxChange_;
+
+	return 0;
+}
+
+/**
+ * \brief Provide control values to the algorithm
+ * \param[in] frame The frame number to apply the control values
+ * \param[in] controls The list of user controls
+ *
+ * This function should be called in the queueRequest() function of the derived class.
+ * See alse: libcamera::ipa::Algorithm::queueRequest()
+ */
+void AfHillClimbing::queueRequestBase([[maybe_unused]] const uint32_t frame, const ControlList &controls)
+{
+	for (auto const &[id, value] : controls) {
+		switch (id) {
+		case controls::AF_MODE: {
+			setMode(static_cast<controls::AfModeEnum>(value.get<int32_t>()));
+			break;
+		}
+		case controls::AF_TRIGGER: {
+			setTrigger(static_cast<controls::AfTriggerEnum>(value.get<int32_t>()));
+			break;
+		}
+		case controls::AF_PAUSE: {
+			setPause(static_cast<controls::AfPauseEnum>(value.get<int32_t>()));
+			break;
+		}
+		case controls::LENS_POSITION: {
+			setLensPosition(value.get<float>());
+			break;
+		}
+		default:
+			break;
+		}
+	}
+}
+
+/**
+ * \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
+ */
+uint32_t AfHillClimbing::processAutofocus(double currentContrast)
+{
+	currentContrast_ = currentContrast;
+
+	if (shouldSkipFrame())
+		return lensPosition_;
+
+	switch (mode_) {
+	case controls::AfModeManual:
+		/* Nothing to process. */
+		break;
+	case controls::AfModeAuto:
+		processAutoMode();
+		break;
+	case controls::AfModeContinuous:
+		processContinousMode();
+		break;
+	default:
+		break;
+	}
+
+	return lensPosition_;
+}
+
+void AfHillClimbing::processAutoMode()
+{
+	if (state_ == controls::AfStateScanning) {
+		afCoarseScan();
+		afFineScan();
+	}
+}
+
+void AfHillClimbing::processContinousMode()
+{
+	/* If we are in a paused state, we won't process the stats */
+	if (pauseState_ == controls::AfPauseStatePaused)
+		return;
+
+	if (state_ == controls::AfStateScanning) {
+		afCoarseScan();
+		afFineScan();
+		return;
+	}
+
+	/* We can re-start the scan at any moment in AfModeContinuous */
+	if (afIsOutOfFocus()) {
+		afReset();
+	}
+}
+
+/**
+ * \brief Request AF to skip n frames
+ * \param[in] n Number of frames to be skipped
+ *
+ * Requested number of frames will not be used for AF calculation.
+ */
+void AfHillClimbing::setFramesToSkip(uint32_t n)
+{
+	if (n > framesToSkip_)
+		framesToSkip_ = n;
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setMode
+ */
+void AfHillClimbing::setMode(controls::AfModeEnum mode)
+{
+	if (mode == mode_)
+		return;
+
+	LOG(Af, Debug) << "Switched AF mode from " << mode_ << " to " << mode;
+	mode_ = mode;
+
+	state_ = controls::AfStateIdle;
+	pauseState_ = controls::AfPauseStateRunning;
+
+	if (mode_ == controls::AfModeContinuous)
+		afReset();
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setRange
+ */
+void AfHillClimbing::setRange([[maybe_unused]] controls::AfRangeEnum range)
+{
+	LOG(Af, Error) << __FUNCTION__ << " not implemented!";
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setSpeed
+ */
+void AfHillClimbing::setSpeed([[maybe_unused]] controls::AfSpeedEnum speed)
+{
+	LOG(Af, Error) << __FUNCTION__ << " not implemented!";
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setTrigger
+ */
+void AfHillClimbing::setTrigger(controls::AfTriggerEnum trigger)
+{
+	if (mode_ != controls::AfModeAuto) {
+		LOG(Af, Warning) << __FUNCTION__ << " not possible in mode " << mode_;
+		return;
+	}
+
+	LOG(Af, Debug) << "Trigger called with " << trigger;
+
+	if (trigger == controls::AfTriggerStart)
+		afReset();
+	else
+		state_ = controls::AfStateIdle;
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setPause
+ */
+void AfHillClimbing::setPause(controls::AfPauseEnum pause)
+{
+	if (mode_ != controls::AfModeContinuous) {
+		LOG(Af, Warning) << __FUNCTION__ << " not possible in mode " << mode_;
+		return;
+	}
+
+	switch (pause) {
+	case controls::AfPauseImmediate:
+		pauseState_ = controls::AfPauseStatePaused;
+		break;
+	case controls::AfPauseDeferred:
+		/* \todo: add the AfPauseDeferred mode */
+		LOG(Af, Warning) << "AfPauseDeferred is not supported!";
+		break;
+	case controls::AfPauseResume:
+		pauseState_ = controls::AfPauseStateRunning;
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setLensPosition
+ */
+void AfHillClimbing::setLensPosition(float lensPosition)
+{
+	if (mode_ != controls::AfModeManual) {
+		LOG(Af, Warning) << __FUNCTION__ << " not possible in mode " << mode_;
+		return;
+	}
+
+	lensPosition_ = static_cast<uint32_t>(lensPosition);
+
+	LOG(Af, Debug) << "Requesting lens position " << lensPosition_;
+}
+
+/**
+ * \fn AfHillClimbing::setMeteringMode()
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setMeteringMode
+ */
+
+/**
+ * \fn AfHillClimbing::setWindows()
+ * \copydoc libcamera::ipa::common::algorithms::AfInterface::setWindows
+ */
+
+void AfHillClimbing::afCoarseScan()
+{
+	if (coarseCompleted_)
+		return;
+
+	if (afScan(coarseSearchStep_)) {
+		coarseCompleted_ = true;
+		maxContrast_ = 0;
+		lensPosition_ = lensPosition_ - (lensPosition_ * fineRange_);
+		previousContrast_ = 0;
+		maxStep_ = std::clamp(lensPosition_ + static_cast<uint32_t>((lensPosition_ * fineRange_)),
+				      0U, maxVcmPosition_);
+	}
+}
+
+void AfHillClimbing::afFineScan()
+{
+	if (!coarseCompleted_)
+		return;
+
+	if (afScan(fineSearchStep_)) {
+		LOG(Af, Debug) << "AF found the best focus position!";
+		state_ = controls::AfStateFocused;
+		fineCompleted_ = true;
+	}
+}
+
+bool AfHillClimbing::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 AfHillClimbing::afReset()
+{
+	LOG(Af, Debug) << "Reset AF parameters";
+	lensPosition_ = minVcmPosition_;
+	maxStep_ = maxVcmPosition_;
+	state_ = controls::AfStateScanning;
+	previousContrast_ = 0.0;
+	coarseCompleted_ = false;
+	fineCompleted_ = false;
+	maxContrast_ = 0.0;
+	setFramesToSkip(1);
+}
+
+bool AfHillClimbing::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 > maxChange_)
+		return true;
+	else
+		return false;
+}
+
+bool AfHillClimbing::shouldSkipFrame()
+{
+	if (framesToSkip_ > 0) {
+		framesToSkip_--;
+		return true;
+	}
+
+	return false;
+}
+
+} /* namespace libcamera::ipa::common::algorithms */
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..6ce95884
--- /dev/null
+++ b/src/ipa/libipa/algorithms/af_hill_climbing.h
@@ -0,0 +1,102 @@
+/* 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_interface.h"
+
+namespace libcamera {
+
+class YamlObject;
+
+namespace ipa::common::algorithms {
+
+LOG_DECLARE_CATEGORY(Af)
+
+class AfHillClimbing : public AfInterface
+{
+public:
+	AfHillClimbing() = default;
+	virtual ~AfHillClimbing() {}
+
+	controls::AfStateEnum getState() final { return state_; }
+	controls::AfPauseStateEnum getPauseState() final { return pauseState_; }
+
+	/* These methods should be implemented by the derived class: */
+	virtual void setMeteringMode(controls::AfMeteringEnum metering) = 0;
+	virtual void setWindows(Span<const Rectangle> windows) = 0;
+
+protected:
+	int initBase(const YamlObject &tuningData);
+	void queueRequestBase(const uint32_t frame, const ControlList &controls);
+	uint32_t processAutofocus(double currentContrast);
+	void setFramesToSkip(uint32_t n);
+
+private:
+	void setMode(controls::AfModeEnum mode) final;
+	void setRange(controls::AfRangeEnum range) final;
+	void setSpeed(controls::AfSpeedEnum speed) final;
+	void setTrigger(controls::AfTriggerEnum trigger) final;
+	void setPause(controls::AfPauseEnum pause) final;
+	void setLensPosition(float lensPosition) final;
+
+	void processAutoMode();
+	void processContinousMode();
+	void afCoarseScan();
+	void afFineScan();
+	bool afScan(uint32_t minSteps);
+	void afReset();
+	bool afIsOutOfFocus();
+	bool shouldSkipFrame();
+
+	controls::AfModeEnum mode_ = controls::AfModeManual;
+	controls::AfStateEnum state_ = controls::AfStateIdle;
+	controls::AfPauseStateEnum pauseState_ = controls::AfPauseStateRunning;
+
+	/* VCM step configuration. It is the current setting of the VCM step. */
+	uint32_t lensPosition_ = 0;
+	/* The best VCM step. It is a local optimum VCM step during scanning. */
+	uint32_t bestPosition_ = 0;
+
+	/* Current AF statistic contrast. */
+	double currentContrast_ = 0;
+	/* It is used to determine the derivative during scanning */
+	double previousContrast_ = 0;
+	double maxContrast_ = 0;
+	/* The designated maximum range of focus scanning. */
+	uint32_t maxStep_ = 0;
+	/* If the coarse scan completes, it is set to true. */
+	bool coarseCompleted_ = false;
+	/* If the fine scan completes, it is set to true. */
+	bool fineCompleted_ = false;
+
+	uint32_t framesToSkip_ = 0;
+
+	/*
+	* Focus steps range of the VCM control
+	* \todo should be obtained from the VCM driver
+	*/
+	uint32_t minVcmPosition_;
+	uint32_t maxVcmPosition_;
+
+	/* Minimum focus step for searching appropriate focus */
+	uint32_t coarseSearchStep_;
+	uint32_t fineSearchStep_;
+
+	/* Fine scan range 0 < fineRange_ < 1 */
+	double fineRange_;
+
+	/* Max ratio of variance change, 0.0 < maxChange_ < 1.0 */
+	double maxChange_;
+};
+
+} /* namespace ipa::common::algorithms */
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/algorithms/meson.build b/src/ipa/libipa/algorithms/meson.build
index 0a1f18fa..a8d94056 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_interface.h',
+    'af_hill_climbing.h',
 ])
 
 common_ipa_algorithms_sources = files([
     'af_interface.cpp',
+    'af_hill_climbing.cpp',
 ])