From 0f6172999dafbf9f80c2b5941bf0487a09b18dd5 Mon Sep 17 00:00:00 2001 From: Daniel Semkowicz Date: Thu, 19 Jan 2023 09:41:08 +0100 Subject: 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 --- src/ipa/libipa/algorithms/af_hill_climbing.cpp | 374 +++++++++++++++++++++++++ src/ipa/libipa/algorithms/af_hill_climbing.h | 102 +++++++ src/ipa/libipa/algorithms/meson.build | 2 + 3 files changed, 478 insertions(+) create mode 100644 src/ipa/libipa/algorithms/af_hill_climbing.cpp create mode 100644 src/ipa/libipa/algorithms/af_hill_climbing.h (limited to 'src') 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(0); + maxVcmPosition_ = tuningData["max-vcm-position"].get(100); + coarseSearchStep_ = tuningData["coarse-search-step"].get(30); + fineSearchStep_ = tuningData["fine-search-step"].get(1); + fineRange_ = tuningData["fine-scan-range"].get(0.05); + maxChange_ = tuningData["max-variance-change"].get(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(value.get())); + break; + } + case controls::AF_TRIGGER: { + setTrigger(static_cast(value.get())); + break; + } + case controls::AF_PAUSE: { + setPause(static_cast(value.get())); + break; + } + case controls::LENS_POSITION: { + setLensPosition(value.get()); + 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(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((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 + +#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 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', ]) -- cgit v1.2.1