From 5504e573f27d3d4aba6f903e01075fcc27700d3e Mon Sep 17 00:00:00 2001 From: Daniel Semkowicz via libcamera-devel Date: Wed, 13 Jul 2022 10:43:08 +0200 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 | 89 +++++++++ src/ipa/libipa/algorithms/af_hill_climbing.h | 251 +++++++++++++++++++++++++ src/ipa/libipa/algorithms/meson.build | 2 + 3 files changed, 342 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..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 + +#include "af_algorithm.h" + +namespace libcamera { + +LOG_DECLARE_CATEGORY(Af) + +namespace ipa::common::algorithms { + +template +class AfHillClimbing : public AfAlgorithm +{ +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 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((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', ]) -- cgit v1.2.1