diff --git a/src/ipa/raspberrypi/controller/af_algorithm.h b/src/ipa/raspberrypi/controller/af_algorithm.h new file mode 100644 index 00000000..e88683a0 --- /dev/null +++ b/src/ipa/raspberrypi/controller/af_algorithm.h @@ -0,0 +1,33 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022, Raspberry Pi (Trading) Limited + * + * af_algorithm.h - autofocus control algorithm interface + */ +#pragma once + +#include + +#include "algorithm.h" + +namespace RPiController { + +class AfAlgorithm : public Algorithm +{ +public: + AfAlgorithm(Controller *controller) : Algorithm(controller) {} + /* An af algorithm must provide the following: */ + virtual void setMode(const uint32_t &mode) = 0; + /* start a cycle (in auto mode) */ + virtual void trigger() = 0; + /* cancel a cycle (in auto mode) */ + virtual void cancel() = 0; + /* set AF windows */ + virtual void setWindows(const libcamera::Rectangle &afWindows) = 0; + /* set AF range */ + virtual void setRange(const uint32_t &range) = 0; + /* set AF speed */ + virtual void setSpeed(const uint32_t &speed) = 0; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/raspberrypi/controller/af_status.h b/src/ipa/raspberrypi/controller/af_status.h new file mode 100644 index 00000000..f8cb1301 --- /dev/null +++ b/src/ipa/raspberrypi/controller/af_status.h @@ -0,0 +1,21 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022, Raspberry Pi (Trading) Limited + * Copyright (C) 2022, Ideas On Board + * + * af_status.h - autofocus measurement status + */ +#pragma once + +#include + +/* + * The focus algorithm should post the following structure into the image's + * "af.status" metadata. + */ + +struct AfStatus { + uint32_t lensPosition; + uint32_t state; + libcamera::Rectangle windows; +}; diff --git a/src/ipa/raspberrypi/controller/iob/af.cpp b/src/ipa/raspberrypi/controller/iob/af.cpp new file mode 100644 index 00000000..0100bbaf --- /dev/null +++ b/src/ipa/raspberrypi/controller/iob/af.cpp @@ -0,0 +1,212 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * + * af.cpp - automatic contrast-based focus algorithm + */ + +#include "af.h" + +#include +#include + +#include + +using namespace RPiController; +using namespace libcamera; + +LOG_DEFINE_CATEGORY(IoBAf) + +#define NAME "iob.af" + +/* + * 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; + +Af::Af(Controller *controller) + : AfAlgorithm(controller), focus_(0), bestFocus_(0), + currentContrast_(0.0), previousContrast_(0.0), maxContrast_(0.0), + maxStep_(0), coarseCompleted_(false), fineCompleted_(false), + mode_(0) +{ +} + +char const *Af::name() const +{ + return NAME; +} + +void Af::setMode([[maybe_unused]] const uint32_t &mode) +{ + mode_ = mode; +} + +void Af::trigger() +{ +} + +void Af::cancel() +{ +} + +void Af::setWindows([[maybe_unused]] const libcamera::Rectangle &afWindows) +{ +} + +void Af::setRange([[maybe_unused]] const uint32_t &range) +{ +} + +void Af::setSpeed([[maybe_unused]] const uint32_t &speed) +{ +} + +void Af::initialise() +{ + status_.lensPosition = 0.0; + maxContrast_ = 0.0; + status_.state = 1; +} + +void Af::prepare(Metadata *imageMetadata) +{ + imageMetadata->set("af.status", status_); +} + +void Af::afCoarseScan() +{ + if (coarseCompleted_) + return; + + if (afScan(kCoarseSearchStep)) { + coarseCompleted_ = true; + maxContrast_ = 0; + focus_ = status_.lensPosition - (status_.lensPosition * kFineRange); + status_.lensPosition = focus_; + previousContrast_ = 0; + maxStep_ = std::clamp(focus_ + static_cast((focus_ * kFineRange)), + 0U, kMaxFocusSteps); + } +} + +void Af::afFineScan() +{ + if (!coarseCompleted_) + return; + + if (afScan(kFineSearchStep)) { + LOG(IoBAf, Debug) << "AF found the best focus position !"; + status_.state = 2; + fineCompleted_ = true; + } +} + +bool Af::afScan(uint32_t minSteps) +{ + if (focus_ > maxStep_) { + /* If the max step is reached, move lens to the position. */ + status_.lensPosition = bestFocus_; + 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. + */ + bestFocus_ = focus_; + focus_ += minSteps; + maxContrast_ = currentContrast_; + status_.lensPosition = focus_; + } 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. + */ + status_.lensPosition = bestFocus_; + return true; + } + } + + previousContrast_ = currentContrast_; + LOG(IoBAf, Debug) << " Previous step is " + << bestFocus_ + << " Current step is " + << focus_; + return false; +} + +void Af::afReset() +{ + LOG(IoBAf, Debug) << "Reset AF parameters"; + status_.lensPosition = 0; + focus_ = 0; + status_.state = 0; + previousContrast_ = 0.0; + coarseCompleted_ = false; + fineCompleted_ = false; + maxStep_ = kMaxFocusSteps; + maxContrast_ = 0.0; +} + +bool Af::afIsOutOfFocus() +{ + const uint32_t diff_var = std::abs(currentContrast_ - + maxContrast_); + const double var_ratio = diff_var / maxContrast_; + LOG(IoBAf, Debug) << "Variance change rate: " + << var_ratio + << " Current VCM step: " + << status_.lensPosition; + if (var_ratio > kMaxChange) + return true; + else + return false; +} + +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) +{ + unsigned int i; + currentContrast_ = 0.0; + + /* Use the second filter results only, and cache those. */ + for (i = 0; i < FOCUS_REGIONS; i++) + currentContrast_ += stats->focus_stats[i].contrast_val[1][1] + / stats->focus_stats[i].contrast_val_num[1][1]; + + if (status_.state != 2) { + afCoarseScan(); + afFineScan(); + } else { + if (afIsOutOfFocus()) + afReset(); + } +} + +/* Register algorithm with the system. */ +static Algorithm *Create(Controller *controller) +{ + return new Af(controller); +} +static RegisterAlgorithm reg(NAME, &Create); diff --git a/src/ipa/raspberrypi/controller/iob/af.h b/src/ipa/raspberrypi/controller/iob/af.h new file mode 100644 index 00000000..c7ed6109 --- /dev/null +++ b/src/ipa/raspberrypi/controller/iob/af.h @@ -0,0 +1,62 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2021, Red Hat + * Copyright (C) 2022, Ideas On Board + * + * af.h - automatic contrast-based focus algorithm + */ +#pragma once + +#include + +#include "../af_algorithm.h" +#include "../af_status.h" +#include "../metadata.h" + +namespace RPiController { + +class Af : public AfAlgorithm +{ +public: + Af(Controller *controller); + char const *name() const override; + void initialise() override; + void prepare(Metadata *image_metadata) override; + void process(StatisticsPtr &stats, Metadata *image_metadata) override; + void setMode(const uint32_t &mode) override; + void trigger() override; + void cancel() override; + void setWindows(const libcamera::Rectangle &afWindows) override; + void setRange(const uint32_t &range) override; + void setSpeed(const uint32_t &speed) override; +private: + bool afNeedIgnoreFrame(); + void afCoarseScan(); + void afFineScan(); + bool afScan(uint32_t minSteps); + void afReset(); + bool afIsOutOfFocus(); + + AfStatus status_; + + /* VCM step configuration. It is the current setting of the VCM step. */ + uint32_t focus_; + /* The best VCM step. It is a local optimum VCM step during scanning. */ + uint32_t bestFocus_; + + /* 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 mode_; +}; + +} /* namespace RPiController */ diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build index 517d815b..10ba9608 100644 --- a/src/ipa/raspberrypi/meson.build +++ b/src/ipa/raspberrypi/meson.build @@ -27,6 +27,7 @@ rpi_ipa_sources = files([ 'controller/controller.cpp', 'controller/histogram.cpp', 'controller/algorithm.cpp', + 'controller/iob/af.cpp', 'controller/rpi/alsc.cpp', 'controller/rpi/awb.cpp', 'controller/rpi/sharpen.cpp',