diff options
Diffstat (limited to 'src/ipa/libipa/algorithms/af_hill_climbing.cpp')
| -rw-r--r-- | src/ipa/libipa/algorithms/af_hill_climbing.cpp | 374 |
1 files changed, 374 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 */ |