summaryrefslogtreecommitdiff
path: root/src/ipa/rpi/controller/rpi/af.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/ipa/rpi/controller/rpi/af.cpp')
-rw-r--r--src/ipa/rpi/controller/rpi/af.cpp797
1 files changed, 797 insertions, 0 deletions
diff --git a/src/ipa/rpi/controller/rpi/af.cpp b/src/ipa/rpi/controller/rpi/af.cpp
new file mode 100644
index 00000000..ed0c8a94
--- /dev/null
+++ b/src/ipa/rpi/controller/rpi/af.cpp
@@ -0,0 +1,797 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022-2023, Raspberry Pi Ltd
+ *
+ * af.cpp - Autofocus control algorithm
+ */
+
+#include "af.h"
+
+#include <iomanip>
+#include <math.h>
+#include <stdlib.h>
+
+#include <libcamera/base/log.h>
+
+#include <libcamera/control_ids.h>
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiAf)
+
+#define NAME "rpi.af"
+
+/*
+ * Default values for parameters. All may be overridden in the tuning file.
+ * Many of these values are sensor- or module-dependent; the defaults here
+ * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
+ *
+ * Here all focus values are in dioptres (1/m). They are converted to hardware
+ * units when written to status.lensSetting or returned from setLensPosition().
+ *
+ * Gain and delay values are relative to the update rate, since much (not all)
+ * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
+ * but note that algorithms are updated at no more than 30 Hz.
+ */
+
+Af::RangeDependentParams::RangeDependentParams()
+ : focusMin(0.0),
+ focusMax(12.0),
+ focusDefault(1.0)
+{
+}
+
+Af::SpeedDependentParams::SpeedDependentParams()
+ : stepCoarse(1.0),
+ stepFine(0.25),
+ contrastRatio(0.75),
+ pdafGain(-0.02),
+ pdafSquelch(0.125),
+ maxSlew(2.0),
+ pdafFrames(20),
+ dropoutFrames(6),
+ stepFrames(4)
+{
+}
+
+Af::CfgParams::CfgParams()
+ : confEpsilon(8),
+ confThresh(16),
+ confClip(512),
+ skipFrames(5),
+ map()
+{
+}
+
+template<typename T>
+static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
+{
+ auto value = params[name].get<T>();
+ if (value)
+ dest = *value;
+ else
+ LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
+}
+
+void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
+{
+
+ readNumber<double>(focusMin, params, "min");
+ readNumber<double>(focusMax, params, "max");
+ readNumber<double>(focusDefault, params, "default");
+}
+
+void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
+{
+ readNumber<double>(stepCoarse, params, "step_coarse");
+ readNumber<double>(stepFine, params, "step_fine");
+ readNumber<double>(contrastRatio, params, "contrast_ratio");
+ readNumber<double>(pdafGain, params, "pdaf_gain");
+ readNumber<double>(pdafSquelch, params, "pdaf_squelch");
+ readNumber<double>(maxSlew, params, "max_slew");
+ readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
+ readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
+ readNumber<uint32_t>(stepFrames, params, "step_frames");
+}
+
+int Af::CfgParams::read(const libcamera::YamlObject &params)
+{
+ if (params.contains("ranges")) {
+ auto &rr = params["ranges"];
+
+ if (rr.contains("normal"))
+ ranges[AfRangeNormal].read(rr["normal"]);
+ else
+ LOG(RPiAf, Warning) << "Missing range \"normal\"";
+
+ ranges[AfRangeMacro] = ranges[AfRangeNormal];
+ if (rr.contains("macro"))
+ ranges[AfRangeMacro].read(rr["macro"]);
+
+ ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
+ ranges[AfRangeMacro].focusMin);
+ ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
+ ranges[AfRangeMacro].focusMax);
+ ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
+ if (rr.contains("full"))
+ ranges[AfRangeFull].read(rr["full"]);
+ } else
+ LOG(RPiAf, Warning) << "No ranges defined";
+
+ if (params.contains("speeds")) {
+ auto &ss = params["speeds"];
+
+ if (ss.contains("normal"))
+ speeds[AfSpeedNormal].read(ss["normal"]);
+ else
+ LOG(RPiAf, Warning) << "Missing speed \"normal\"";
+
+ speeds[AfSpeedFast] = speeds[AfSpeedNormal];
+ if (ss.contains("fast"))
+ speeds[AfSpeedFast].read(ss["fast"]);
+ } else
+ LOG(RPiAf, Warning) << "No speeds defined";
+
+ readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
+ readNumber<uint32_t>(confThresh, params, "conf_thresh");
+ readNumber<uint32_t>(confClip, params, "conf_clip");
+ readNumber<uint32_t>(skipFrames, params, "skip_frames");
+
+ if (params.contains("map"))
+ map.read(params["map"]);
+ else
+ LOG(RPiAf, Warning) << "No map defined";
+
+ return 0;
+}
+
+void Af::CfgParams::initialise()
+{
+ if (map.empty()) {
+ /* Default mapping from dioptres to hardware setting */
+ static constexpr double DefaultMapX0 = 0.0;
+ static constexpr double DefaultMapY0 = 445.0;
+ static constexpr double DefaultMapX1 = 15.0;
+ static constexpr double DefaultMapY1 = 925.0;
+
+ map.append(DefaultMapX0, DefaultMapY0);
+ map.append(DefaultMapX1, DefaultMapY1);
+ }
+}
+
+/* Af Algorithm class */
+
+static constexpr unsigned MaxWindows = 10;
+
+Af::Af(Controller *controller)
+ : AfAlgorithm(controller),
+ cfg_(),
+ range_(AfRangeNormal),
+ speed_(AfSpeedNormal),
+ mode_(AfAlgorithm::AfModeManual),
+ pauseFlag_(false),
+ statsRegion_(0, 0, 0, 0),
+ windows_(),
+ useWindows_(false),
+ phaseWeights_(),
+ contrastWeights_(),
+ scanState_(ScanState::Idle),
+ initted_(false),
+ ftarget_(-1.0),
+ fsmooth_(-1.0),
+ prevContrast_(0.0),
+ skipCount_(0),
+ stepCount_(0),
+ dropCount_(0),
+ scanMaxContrast_(0.0),
+ scanMinContrast_(1.0e9),
+ scanData_(),
+ reportState_(AfState::Idle)
+{
+ /*
+ * Reserve space for data, to reduce memory fragmentation. It's too early
+ * to query the size of the PDAF (from camera) and Contrast (from ISP)
+ * statistics, but these are plausible upper bounds.
+ */
+ phaseWeights_.w.reserve(16 * 12);
+ contrastWeights_.w.reserve(getHardwareConfig().focusRegions.width *
+ getHardwareConfig().focusRegions.height);
+ scanData_.reserve(32);
+}
+
+Af::~Af()
+{
+}
+
+char const *Af::name() const
+{
+ return NAME;
+}
+
+int Af::read(const libcamera::YamlObject &params)
+{
+ return cfg_.read(params);
+}
+
+void Af::initialise()
+{
+ cfg_.initialise();
+}
+
+void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
+{
+ (void)metadata;
+
+ /* Assume that PDAF and Focus stats grids cover the visible area */
+ statsRegion_.x = (int)cameraMode.cropX;
+ statsRegion_.y = (int)cameraMode.cropY;
+ statsRegion_.width = (unsigned)(cameraMode.width * cameraMode.scaleX);
+ statsRegion_.height = (unsigned)(cameraMode.height * cameraMode.scaleY);
+ LOG(RPiAf, Debug) << "switchMode: statsRegion: "
+ << statsRegion_.x << ','
+ << statsRegion_.y << ','
+ << statsRegion_.width << ','
+ << statsRegion_.height;
+ invalidateWeights();
+
+ if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
+ /*
+ * If a scan was in progress, re-start it, as CDAF statistics
+ * may have changed. Though if the application is just about
+ * to take a still picture, this will not help...
+ */
+ startProgrammedScan();
+ }
+ skipCount_ = cfg_.skipFrames;
+}
+
+void Af::computeWeights(RegionWeights *wgts, unsigned rows, unsigned cols)
+{
+ wgts->rows = rows;
+ wgts->cols = cols;
+ wgts->sum = 0;
+ wgts->w.resize(rows * cols);
+ std::fill(wgts->w.begin(), wgts->w.end(), 0);
+
+ if (rows > 0 && cols > 0 && useWindows_ &&
+ statsRegion_.height >= rows && statsRegion_.width >= cols) {
+ /*
+ * Here we just merge all of the given windows, weighted by area.
+ * \todo Perhaps a better approach might be to find the phase in each
+ * window and choose either the closest or the highest-confidence one?
+ * Ensure weights sum to less than (1<<16). 46080 is a "round number"
+ * below 65536, for better rounding when window size is a simple
+ * fraction of image dimensions.
+ */
+ const unsigned maxCellWeight = 46080u / (MaxWindows * rows * cols);
+ const unsigned cellH = statsRegion_.height / rows;
+ const unsigned cellW = statsRegion_.width / cols;
+ const unsigned cellA = cellH * cellW;
+
+ for (auto &w : windows_) {
+ for (unsigned r = 0; r < rows; ++r) {
+ int y0 = std::max(statsRegion_.y + (int)(cellH * r), w.y);
+ int y1 = std::min(statsRegion_.y + (int)(cellH * (r + 1)),
+ w.y + (int)(w.height));
+ if (y0 >= y1)
+ continue;
+ y1 -= y0;
+ for (unsigned c = 0; c < cols; ++c) {
+ int x0 = std::max(statsRegion_.x + (int)(cellW * c), w.x);
+ int x1 = std::min(statsRegion_.x + (int)(cellW * (c + 1)),
+ w.x + (int)(w.width));
+ if (x0 >= x1)
+ continue;
+ unsigned a = y1 * (x1 - x0);
+ a = (maxCellWeight * a + cellA - 1) / cellA;
+ wgts->w[r * cols + c] += a;
+ wgts->sum += a;
+ }
+ }
+ }
+ }
+
+ if (wgts->sum == 0) {
+ /* Default AF window is the middle 1/2 width of the middle 1/3 height */
+ for (unsigned r = rows / 3; r < rows - rows / 3; ++r) {
+ for (unsigned c = cols / 4; c < cols - cols / 4; ++c) {
+ wgts->w[r * cols + c] = 1;
+ wgts->sum += 1;
+ }
+ }
+ }
+}
+
+void Af::invalidateWeights()
+{
+ phaseWeights_.sum = 0;
+ contrastWeights_.sum = 0;
+}
+
+bool Af::getPhase(PdafRegions const &regions, double &phase, double &conf)
+{
+ libcamera::Size size = regions.size();
+ if (size.height != phaseWeights_.rows || size.width != phaseWeights_.cols ||
+ phaseWeights_.sum == 0) {
+ LOG(RPiAf, Debug) << "Recompute Phase weights " << size.width << 'x' << size.height;
+ computeWeights(&phaseWeights_, size.height, size.width);
+ }
+
+ uint32_t sumWc = 0;
+ int64_t sumWcp = 0;
+ for (unsigned i = 0; i < regions.numRegions(); ++i) {
+ unsigned w = phaseWeights_.w[i];
+ if (w) {
+ const PdafData &data = regions.get(i).val;
+ unsigned c = data.conf;
+ if (c >= cfg_.confThresh) {
+ if (c > cfg_.confClip)
+ c = cfg_.confClip;
+ c -= (cfg_.confThresh >> 2);
+ sumWc += w * c;
+ c -= (cfg_.confThresh >> 2);
+ sumWcp += (int64_t)(w * c) * (int64_t)data.phase;
+ }
+ }
+ }
+
+ if (0 < phaseWeights_.sum && phaseWeights_.sum <= sumWc) {
+ phase = (double)sumWcp / (double)sumWc;
+ conf = (double)sumWc / (double)phaseWeights_.sum;
+ return true;
+ } else {
+ phase = 0.0;
+ conf = 0.0;
+ return false;
+ }
+}
+
+double Af::getContrast(const FocusRegions &focusStats)
+{
+ libcamera::Size size = focusStats.size();
+ if (size.height != contrastWeights_.rows ||
+ size.width != contrastWeights_.cols || contrastWeights_.sum == 0) {
+ LOG(RPiAf, Debug) << "Recompute Contrast weights "
+ << size.width << 'x' << size.height;
+ computeWeights(&contrastWeights_, size.height, size.width);
+ }
+
+ uint64_t sumWc = 0;
+ for (unsigned i = 0; i < focusStats.numRegions(); ++i)
+ sumWc += contrastWeights_.w[i] * focusStats.get(i).val;
+
+ return (contrastWeights_.sum > 0) ? ((double)sumWc / (double)contrastWeights_.sum) : 0.0;
+}
+
+void Af::doPDAF(double phase, double conf)
+{
+ /* Apply loop gain */
+ phase *= cfg_.speeds[speed_].pdafGain;
+
+ if (mode_ == AfModeContinuous) {
+ /*
+ * PDAF in Continuous mode. Scale down lens movement when
+ * delta is small or confidence is low, to suppress wobble.
+ */
+ phase *= conf / (conf + cfg_.confEpsilon);
+ if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
+ double a = phase / cfg_.speeds[speed_].pdafSquelch;
+ phase *= a * a;
+ }
+ } else {
+ /*
+ * PDAF in triggered-auto mode. Allow early termination when
+ * phase delta is small; scale down lens movements towards
+ * the end of the sequence, to ensure a stable image.
+ */
+ if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
+ if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
+ stepCount_ = cfg_.speeds[speed_].stepFrames;
+ } else
+ phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
+ }
+
+ /* Apply slew rate limit. Report failure if out of bounds. */
+ if (phase < -cfg_.speeds[speed_].maxSlew) {
+ phase = -cfg_.speeds[speed_].maxSlew;
+ reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
+ : AfState::Scanning;
+ } else if (phase > cfg_.speeds[speed_].maxSlew) {
+ phase = cfg_.speeds[speed_].maxSlew;
+ reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
+ : AfState::Scanning;
+ } else
+ reportState_ = AfState::Focused;
+
+ ftarget_ = fsmooth_ + phase;
+}
+
+bool Af::earlyTerminationByPhase(double phase)
+{
+ if (scanData_.size() > 0 &&
+ scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
+ double oldFocus = scanData_[scanData_.size() - 1].focus;
+ double oldPhase = scanData_[scanData_.size() - 1].phase;
+
+ /*
+ * Check that the gradient is finite and has the expected sign;
+ * Interpolate/extrapolate the lens position for zero phase.
+ * Check that the extrapolation is well-conditioned.
+ */
+ if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
+ double param = phase / (phase - oldPhase);
+ if (-3.0 <= param && param <= 3.5) {
+ ftarget_ += param * (oldFocus - ftarget_);
+ LOG(RPiAf, Debug) << "ETBP: param=" << param;
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+double Af::findPeak(unsigned i) const
+{
+ double f = scanData_[i].focus;
+
+ if (i > 0 && i + 1 < scanData_.size()) {
+ double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
+ double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
+ if (0.0 <= dropLo && dropLo < dropHi) {
+ double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
+ f += param * (scanData_[i - 1].focus - f);
+ } else if (0.0 <= dropHi && dropHi < dropLo) {
+ double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
+ f += param * (scanData_[i + 1].focus - f);
+ }
+ }
+
+ LOG(RPiAf, Debug) << "FindPeak: " << f;
+ return f;
+}
+
+void Af::doScan(double contrast, double phase, double conf)
+{
+ /* Record lens position, contrast and phase values for the current scan */
+ if (scanData_.empty() || contrast > scanMaxContrast_) {
+ scanMaxContrast_ = contrast;
+ scanMaxIndex_ = scanData_.size();
+ }
+ if (contrast < scanMinContrast_)
+ scanMinContrast_ = contrast;
+ scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
+
+ if (scanState_ == ScanState::Coarse) {
+ if (ftarget_ >= cfg_.ranges[range_].focusMax ||
+ contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
+ /*
+ * Finished course scan, or termination based on contrast.
+ * Jump to just after max contrast and start fine scan.
+ */
+ ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
+ 2.0 * cfg_.speeds[speed_].stepFine);
+ scanState_ = ScanState::Fine;
+ scanData_.clear();
+ } else
+ ftarget_ += cfg_.speeds[speed_].stepCoarse;
+ } else { /* ScanState::Fine */
+ if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
+ contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
+ /*
+ * Finished fine scan, or termination based on contrast.
+ * Use quadratic peak-finding to find best contrast position.
+ */
+ ftarget_ = findPeak(scanMaxIndex_);
+ scanState_ = ScanState::Settle;
+ } else
+ ftarget_ -= cfg_.speeds[speed_].stepFine;
+ }
+
+ stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
+}
+
+void Af::doAF(double contrast, double phase, double conf)
+{
+ /* Skip frames at startup and after sensor mode change */
+ if (skipCount_ > 0) {
+ LOG(RPiAf, Debug) << "SKIP";
+ skipCount_--;
+ return;
+ }
+
+ if (scanState_ == ScanState::Pdaf) {
+ /*
+ * Use PDAF closed-loop control whenever available, in both CAF
+ * mode and (for a limited number of iterations) when triggered.
+ * If PDAF fails (due to poor contrast, noise or large defocus),
+ * fall back to a CDAF-based scan. To avoid "nuisance" scans,
+ * scan only after a number of frames with low PDAF confidence.
+ */
+ if (conf > (dropCount_ ? 1.0 : 0.25) * cfg_.confEpsilon) {
+ doPDAF(phase, conf);
+ if (stepCount_ > 0)
+ stepCount_--;
+ else if (mode_ != AfModeContinuous)
+ scanState_ = ScanState::Idle;
+ dropCount_ = 0;
+ } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
+ startProgrammedScan();
+ } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
+ /*
+ * Scanning sequence. This means PDAF has become unavailable.
+ * Allow a delay between steps for CDAF FoM statistics to be
+ * updated, and a "settling time" at the end of the sequence.
+ * [A coarse or fine scan can be abandoned if two PDAF samples
+ * allow direct interpolation of the zero-phase lens position.]
+ */
+ if (stepCount_ > 0)
+ stepCount_--;
+ else if (scanState_ == ScanState::Settle) {
+ if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
+ scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
+ reportState_ = AfState::Focused;
+ else
+ reportState_ = AfState::Failed;
+ if (mode_ == AfModeContinuous && !pauseFlag_ &&
+ cfg_.speeds[speed_].dropoutFrames > 0)
+ scanState_ = ScanState::Pdaf;
+ else
+ scanState_ = ScanState::Idle;
+ scanData_.clear();
+ } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
+ scanState_ = ScanState::Settle;
+ stepCount_ = (mode_ == AfModeContinuous) ? 0
+ : cfg_.speeds[speed_].stepFrames;
+ } else
+ doScan(contrast, phase, conf);
+ }
+}
+
+void Af::updateLensPosition()
+{
+ if (scanState_ >= ScanState::Pdaf) {
+ ftarget_ = std::clamp(ftarget_,
+ cfg_.ranges[range_].focusMin,
+ cfg_.ranges[range_].focusMax);
+ }
+
+ if (initted_) {
+ /* from a known lens position: apply slew rate limit */
+ fsmooth_ = std::clamp(ftarget_,
+ fsmooth_ - cfg_.speeds[speed_].maxSlew,
+ fsmooth_ + cfg_.speeds[speed_].maxSlew);
+ } else {
+ /* from an unknown position: go straight to target, but add delay */
+ fsmooth_ = ftarget_;
+ initted_ = true;
+ skipCount_ = cfg_.skipFrames;
+ }
+}
+
+void Af::startAF()
+{
+ /* Use PDAF if the tuning file allows it; else CDAF. */
+ if (cfg_.speeds[speed_].dropoutFrames > 0 &&
+ (mode_ == AfModeContinuous || cfg_.speeds[speed_].pdafFrames > 0)) {
+ if (!initted_) {
+ ftarget_ = cfg_.ranges[range_].focusDefault;
+ updateLensPosition();
+ }
+ stepCount_ = (mode_ == AfModeContinuous) ? 0 : cfg_.speeds[speed_].pdafFrames;
+ scanState_ = ScanState::Pdaf;
+ scanData_.clear();
+ dropCount_ = 0;
+ reportState_ = AfState::Scanning;
+ } else
+ startProgrammedScan();
+}
+
+void Af::startProgrammedScan()
+{
+ ftarget_ = cfg_.ranges[range_].focusMin;
+ updateLensPosition();
+ scanState_ = ScanState::Coarse;
+ scanMaxContrast_ = 0.0;
+ scanMinContrast_ = 1.0e9;
+ scanMaxIndex_ = 0;
+ scanData_.clear();
+ stepCount_ = cfg_.speeds[speed_].stepFrames;
+ reportState_ = AfState::Scanning;
+}
+
+void Af::goIdle()
+{
+ scanState_ = ScanState::Idle;
+ reportState_ = AfState::Idle;
+ scanData_.clear();
+}
+
+/*
+ * PDAF phase data are available in prepare(), but CDAF statistics are not
+ * available until process(). We are gambling on the availability of PDAF.
+ * To expedite feedback control using PDAF, issue the V4L2 lens control from
+ * prepare(). Conversely, during scans, we must allow an extra frame delay
+ * between steps, to retrieve CDAF statistics from the previous process()
+ * so we can terminate the scan early without having to change our minds.
+ */
+
+void Af::prepare(Metadata *imageMetadata)
+{
+ /* Initialize for triggered scan or start of CAF mode */
+ if (scanState_ == ScanState::Trigger)
+ startAF();
+
+ if (initted_) {
+ /* Get PDAF from the embedded metadata, and run AF algorithm core */
+ PdafRegions regions;
+ double phase = 0.0, conf = 0.0;
+ double oldFt = ftarget_;
+ double oldFs = fsmooth_;
+ ScanState oldSs = scanState_;
+ uint32_t oldSt = stepCount_;
+ if (imageMetadata->get("pdaf.regions", regions) == 0)
+ getPhase(regions, phase, conf);
+ doAF(prevContrast_, phase, conf);
+ updateLensPosition();
+ LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
+ << static_cast<unsigned int>(reportState_)
+ << " sst" << static_cast<unsigned int>(oldSs)
+ << "->" << static_cast<unsigned int>(scanState_)
+ << " stp" << oldSt << "->" << stepCount_
+ << " ft" << oldFt << "->" << ftarget_
+ << " fs" << oldFs << "->" << fsmooth_
+ << " cont=" << (int)prevContrast_
+ << " phase=" << (int)phase << " conf=" << (int)conf;
+ }
+
+ /* Report status and produce new lens setting */
+ AfStatus status;
+ if (pauseFlag_)
+ status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
+ : AfPauseState::Pausing;
+ else
+ status.pauseState = AfPauseState::Running;
+
+ if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
+ status.state = AfState::Scanning;
+ else
+ status.state = reportState_;
+ status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
+ : std::nullopt;
+ imageMetadata->set("af.status", status);
+}
+
+void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
+{
+ (void)imageMetadata;
+ prevContrast_ = getContrast(stats->focusRegions);
+}
+
+/* Controls */
+
+void Af::setRange(AfRange r)
+{
+ LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
+ if (r < AfAlgorithm::AfRangeMax)
+ range_ = r;
+}
+
+void Af::setSpeed(AfSpeed s)
+{
+ LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
+ if (s < AfAlgorithm::AfSpeedMax) {
+ if (scanState_ == ScanState::Pdaf &&
+ cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
+ stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
+ speed_ = s;
+ }
+}
+
+void Af::setMetering(bool mode)
+{
+ if (useWindows_ != mode) {
+ useWindows_ = mode;
+ invalidateWeights();
+ }
+}
+
+void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
+{
+ windows_.clear();
+ for (auto &w : wins) {
+ LOG(RPiAf, Debug) << "Window: "
+ << w.x << ", "
+ << w.y << ", "
+ << w.width << ", "
+ << w.height;
+ windows_.push_back(w);
+ if (windows_.size() >= MaxWindows)
+ break;
+ }
+
+ if (useWindows_)
+ invalidateWeights();
+}
+
+bool Af::setLensPosition(double dioptres, int *hwpos)
+{
+ bool changed = false;
+
+ if (mode_ == AfModeManual) {
+ LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
+ ftarget_ = cfg_.map.domain().clip(dioptres);
+ changed = !(initted_ && fsmooth_ == ftarget_);
+ updateLensPosition();
+ }
+
+ if (hwpos)
+ *hwpos = cfg_.map.eval(fsmooth_);
+
+ return changed;
+}
+
+std::optional<double> Af::getLensPosition() const
+{
+ /*
+ * \todo We ought to perform some precise timing here to determine
+ * the current lens position.
+ */
+ return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
+}
+
+void Af::cancelScan()
+{
+ LOG(RPiAf, Debug) << "cancelScan";
+ if (mode_ == AfModeAuto)
+ goIdle();
+}
+
+void Af::triggerScan()
+{
+ LOG(RPiAf, Debug) << "triggerScan";
+ if (mode_ == AfModeAuto && scanState_ == ScanState::Idle)
+ scanState_ = ScanState::Trigger;
+}
+
+void Af::setMode(AfAlgorithm::AfMode mode)
+{
+ LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
+ if (mode_ != mode) {
+ mode_ = mode;
+ pauseFlag_ = false;
+ if (mode == AfModeContinuous)
+ scanState_ = ScanState::Trigger;
+ else if (mode != AfModeAuto || scanState_ < ScanState::Coarse)
+ goIdle();
+ }
+}
+
+AfAlgorithm::AfMode Af::getMode() const
+{
+ return mode_;
+}
+
+void Af::pause(AfAlgorithm::AfPause pause)
+{
+ LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
+ if (mode_ == AfModeContinuous) {
+ if (pause == AfPauseResume && pauseFlag_) {
+ pauseFlag_ = false;
+ if (scanState_ < ScanState::Coarse)
+ scanState_ = ScanState::Trigger;
+ } else if (pause != AfPauseResume && !pauseFlag_) {
+ pauseFlag_ = true;
+ if (pause == AfPauseImmediate || scanState_ < ScanState::Coarse)
+ goIdle();
+ }
+ }
+}
+
+// Register algorithm with the system.
+static Algorithm *create(Controller *controller)
+{
+ return (Algorithm *)new Af(controller);
+}
+static RegisterAlgorithm reg(NAME, &create);
generated by cgit v1.2.1 (git 2.18.0) at 2024-05-06 18:14:22 +0000