Message ID | 20230119104544.9456-12-naush@raspberrypi.com |
---|---|
State | Superseded |
Headers | show |
Series |
|
Related | show |
Hi Nick, Naush, I was soo close to merging this series in one... but alas there's a minor compiler issue in this one. Quoting Naushir Patuck via libcamera-devel (2023-01-19 10:45:41) > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > Provide the first version of the Raspberry Pi autofocus algorithm. This > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to > having less "hunting" behavior. > > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > Signed-off-by: Naushir Patuck <naush@raspberrypi.com> > Reviewed-by: Naushir Patuck <naush@raspberrypi.com> > Reviewed-by: David Plowman <david.plowman@raspberrypi.com> > --- > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++ > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++ > src/ipa/raspberrypi/meson.build | 1 + > 3 files changed, 909 insertions(+) > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp > new file mode 100644 > index 000000000000..7e2e8961085a > --- /dev/null > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp > @@ -0,0 +1,755 @@ > +/* 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 ¶ms, 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 ¶ms) > +{ > + > + readNumber<double>(focusMin, params, "min"); > + readNumber<double>(focusMax, params, "max"); > + readNumber<double>(focusDefault, params, "default"); > +} > + > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) > +{ > + 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 ¶ms) > +{ > + 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); > + > + LOG(RPiAf, Warning) << "af.map is not defined, "; > + } > +} > + > +/* Af Algorithm class */ > + > +Af::Af(Controller *controller) > + : AfAlgorithm(controller), > + cfg_(), > + range_(AfRangeNormal), > + speed_(AfSpeedNormal), > + mode_(AfAlgorithm::AfModeManual), > + pauseFlag_(false), > + sensorSize_{ 0, 0 }, > + useWeights_(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) > +{ > + scanData_.reserve(24); > +} > + > +Af::~Af() > +{ > +} > + > +char const *Af::name() const > +{ > + return NAME; > +} > + > +int Af::read(const libcamera::YamlObject ¶ms) > +{ > + return cfg_.read(params); > +} > + > +void Af::initialise() > +{ > + cfg_.initialise(); > +} > + > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata) > +{ > + (void)metadata; > + sensorSize_.width = cameraMode.sensorWidth; > + sensorSize_.height = cameraMode.sensorHeight; > + > + 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; > +} > + > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const > +{ > + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = { > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } > + }; > + int32_t sumW = 0; > + int32_t sumWc = 0; > + int32_t sumWcp = 0; > + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights; > + > + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) { > + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) { > + if (wgts[i][j]) { > + uint32_t c = data.conf[i][j]; > + if (c >= cfg_.confThresh) { > + if (c > cfg_.confClip) > + c = cfg_.confClip; > + sumWc += wgts[i][j] * (int32_t)c; > + c -= (cfg_.confThresh >> 1); > + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c; > + } > + sumW += wgts[i][j]; > + } > + } > + } > + > + if (sumWc > 0) { > + phase = (double)sumWcp / (double)sumWc; > + conf = (double)sumWc / (double)sumW; > + return true; > + } else { > + phase = 0.0; > + conf = 0.0; > + return false; > + } > +} > + > +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. > + */ > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { > + double a = phase / cfg_.speeds[speed_].pdafSquelch; > + phase *= a * a; > + } > + phase *= conf / (conf + cfg_.confEpsilon); > + } 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 (dropLo < dropHi) { > + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); > + f += param * (scanData_[i - 1].focus - f); > + } else if (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 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_ ? cfg_.confEpsilon : 0.0)) { > + 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 (mode_ != AfModeManual) { > + ftarget_ = std::clamp(ftarget_, > + cfg_.ranges[range_].focusMin, > + cfg_.ranges[range_].focusMax); > + } > + > + /* \todo Add a clip for manual lens position to be within the PWL limits. */ > + > + 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; > + } > +} > + > +/* > + * 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) > +{ > + if (initted_) { > + /* Get PDAF from the embedded metadata, and run AF algorithm core */ > + PdafData data; > + double phase = 0.0, conf = 0.0; > + double oldFt = ftarget_; > + double oldFs = fsmooth_; > + ScanState oldSs = scanState_; > + uint32_t oldSt = stepCount_; > + if (imageMetadata->get("pdaf.data", data) == 0) > + getPhase(data, 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); > +} > + > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const > +{ > + uint32_t totW = 0, totWc = 0; > + > + if (useWeights_) { > + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) { > + unsigned w = contrastWeights_[i]; > + totW += w; > + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10); > + } > + } > + if (totW == 0) { > + totW = 2; > + totWc = (focus_stats[5].contrast_val[1][1] >> 10) + > + (focus_stats[6].contrast_val[1][1] >> 10); > + } > + > + return (double)totWc / (double)totW; > +} > + > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) > +{ > + (void)imageMetadata; > + prevContrast_ = getContrast(stats->focus_stats); > +} > + > +/* 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) > +{ > + useWeights_ = mode; > +} > + > +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) > +{ > + /* > + * Here we just merge all of the given windows, weighted by area. > + * If there are more than 15 overlapping windows, overflow can occur. > + * TODO: A better approach might be to find the phase in each window > + * and choose either the closest or the highest-confidence one? > + * > + * Using mostly "int" arithmetic, because Rectangle has signed x, y > + */ > + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0); > + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS); > + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS); > + int gridA = gridY * gridX; > + > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) > + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0); > + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0); > + > + for (auto &w : wins) { > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) { > + int y0 = std::max(gridY * i, w.y); > + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height)); > + if (y0 >= y1) > + continue; > + y1 -= y0; > + for (int j = 0; j < PDAF_DATA_COLS; ++j) { > + int x0 = std::max(gridX * j, w.x); > + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width)); > + if (x0 >= x1) > + continue; > + int a = y1 * (x1 - x0); > + a = (16 * a + gridA - 1) / gridA; > + phaseWeights_[i][j] += a; > + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a; > + } > + } > + } > +} > + > +bool Af::setLensPosition(double dioptres, int *hwpos) > +{ > + bool changed = false; > + > + if (mode_ == AfModeManual) { > + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; > + changed = !(initted_ && fsmooth_ == dioptres); > + ftarget_ = dioptres; > + 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::startCAF() > +{ > + /* Try PDAF if the tuning file permits it for CAF; else CDAF */ > + if (cfg_.speeds[speed_].dropoutFrames > 0) { > + if (!initted_) { > + ftarget_ = cfg_.ranges[range_].focusDefault; > + updateLensPosition(); > + } > + 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(); > +} > + > +void Af::cancelScan() > +{ > + LOG(RPiAf, Debug) << "cancelScan"; > + if (mode_ == AfModeAuto) > + goIdle(); > +} > + > +void Af::triggerScan() > +{ > + LOG(RPiAf, Debug) << "triggerScan"; > + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) { > + /* Try PDAF if the tuning file permits it for Auto; else CDAF */ > + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) { > + if (!initted_) { > + ftarget_ = cfg_.ranges[range_].focusDefault; > + updateLensPosition(); > + } > + stepCount_ = cfg_.speeds[speed_].pdafFrames; > + scanState_ = ScanState::Pdaf; > + dropCount_ = 0; > + } else > + startProgrammedScan(); > + reportState_ = AfState::Scanning; > + } > +} > + > +void Af::setMode(AfAlgorithm::AfMode mode) > +{ > + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; > + if (mode_ != mode) { > + mode_ = mode; > + pauseFlag_ = false; > + if (mode == AfModeContinuous) > + startCAF(); > + else if (mode != AfModeAuto) > + 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_) { > + startCAF(); > + pauseFlag_ = false; > + } else if (pause != AfPauseResume && !pauseFlag_) { > + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf) > + goIdle(); > + pauseFlag_ = true; > + } > + } > +} > + > +// Register algorithm with the system. > +static Algorithm *create(Controller *controller) > +{ > + return (Algorithm *)new Af(controller); > +} > +static RegisterAlgorithm reg(NAME, &create); > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h > new file mode 100644 > index 000000000000..0431bd70ae29 > --- /dev/null > +++ b/src/ipa/raspberrypi/controller/rpi/af.h > @@ -0,0 +1,153 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2022-2023, Raspberry Pi Ltd > + * > + * af.h - Autofocus control algorithm > + */ > +#pragma once > + > +#include "../af_algorithm.h" > +#include "../af_status.h" > +#include "../pdaf_data.h" > +#include "../pwl.h" > + > +/* > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF. > + * > + * Whenever PDAF is available, it is used in a continuous feedback loop. > + * When triggered in auto mode, we simply enable AF for a limited number > + * of frames (it may terminate early if the delta becomes small enough). > + * > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus) > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern. > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to > + * estimate the lens position with peak contrast. This is slower due to > + * extra latency in the ISP, and requires a settling time between steps. > + * > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid > + * "nuisance" scans. During each interval where PDAF is not working, only > + * ONE scan will be performed; CAF cannot track objects using CDAF alone. > + * > + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM. > + */ > + > +namespace RPiController { > + > +class Af : public AfAlgorithm > +{ > +public: > + Af(Controller *controller = NULL); > + ~Af(); > + char const *name() const override; > + int read(const libcamera::YamlObject ¶ms) override; > + void initialise() override; > + > + /* IPA calls */ > + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; > + void prepare(Metadata *imageMetadata) override; > + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; > + > + /* controls */ > + void setRange(AfRange range) override; > + void setSpeed(AfSpeed speed) override; > + void setMetering(bool use_windows) override; > + void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override; > + void setMode(AfMode mode) override; > + AfMode getMode() const override; > + bool setLensPosition(double dioptres, int32_t *hwpos) override; > + std::optional<double> getLensPosition() const override; > + void triggerScan() override; > + void cancelScan() override; > + void pause(AfPause pause) override; > + > +private: > + enum class ScanState { > + Idle, > + Pdaf, > + Coarse, > + Fine, > + Settle > + }; > + > + struct RangeDependentParams { > + double focusMin; /* lower (far) limit in dipotres */ > + double focusMax; /* upper (near) limit in dioptres */ > + double focusDefault; /* default setting ("hyperfocal") */ > + > + RangeDependentParams(); > + void read(const libcamera::YamlObject ¶ms); > + }; > + > + struct SpeedDependentParams { > + double stepCoarse; /* used for scans */ > + double stepFine; /* used for scans */ > + double contrastRatio; /* used for scan termination and reporting */ > + double pdafGain; /* coefficient for PDAF feedback loop */ > + double pdafSquelch; /* PDAF stability parameter (device-specific) */ > + double maxSlew; /* limit for lens movement per frame */ > + uint32_t pdafFrames; /* number of iterations when triggered */ > + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */ > + uint32_t stepFrames; /* frames to skip in between steps of a scan */ > + > + SpeedDependentParams(); > + void read(const libcamera::YamlObject ¶ms); > + }; > + > + struct CfgParams { > + RangeDependentParams ranges[AfRangeMax]; > + SpeedDependentParams speeds[AfSpeedMax]; > + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */ > + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */ > + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */ > + uint32_t skipFrames; /* frames to skip at start or modeswitch */ > + Pwl map; /* converts dioptres -> lens driver position */ > + > + CfgParams(); > + int read(const libcamera::YamlObject ¶ms); > + void initialise(); > + }; > + > + struct ScanRecord { > + double focus; > + double contrast; > + double phase; > + double conf; > + }; > + > + bool getPhase(PdafData const &data, double &phase, double &conf) const; > + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const; > + void doPDAF(double phase, double conf); > + bool earlyTerminationByPhase(double phase); > + void doScan(double contrast, double phase, double conf); > + double findPeak(unsigned index) const; > + void doAF(double contrast, double phase, double conf); > + void updateLensPosition(); > + void startProgrammedScan(); > + void startCAF(); > + void goIdle(); > + > + /* Configuration and settings */ > + CfgParams cfg_; > + AfRange range_; > + AfSpeed speed_; > + AfMode mode_; > + bool pauseFlag_; > + libcamera::Size sensorSize_; > + bool useWeights_; > + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS]; > + uint16_t contrastWeights_[FOCUS_REGIONS]; > + > + /* Working state. */ > + ScanState scanState_; > + bool initted_; > + double ftarget_, fsmooth_; > + double prevContrast_; > + bool pdafSeen_; This pdafSeen_ variable is unused and gets noticed by Clang. FAILED: src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o clang++-11 -Isrc/ipa/raspberrypi/ipa_rpi.so.p -Isrc/ipa/raspberrypi -I../../../src/libcamera/src/ipa/raspberrypi -Iinclude -I../../../src/libcamera/include -Isrc/ipa -I../../../src/libcamera/src/ipa -I../../../src/libcamera/src/ipa/raspberrypi/controller -Iinclude/libcamera/ipa -Iinclude/libcamera -fcolor-diagnostics -D_FILE_OFFSET_BITS=64 -Wall -Winvalid-pch -Wnon-virtual-dtor -Wextra -Werror -std=c++17 -O0 -g -Wextra-semi -Wthread-safety -Wshadow -include /home/kbingham/iob/libcamera/ci/integrator/builds/build-matrix/clang-11/config.h -Wno-c99-designator -fPIC -DLIBCAMERA_BASE_PRIVATE -MD -MQ src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o -MF src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o.d -o src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o -c ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.cpp In file included from ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.cpp:8: ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.h:145:7: error: private field 'pdafSeen_' is not used [-Werror,-Wunused-private-field] bool pdafSeen_; ^ 1 error generated. As this is a single line to remove, and I don't think that's controversial - I think that could be fixed while applying if you're ok with that. -- Kieran > + unsigned skipCount_, stepCount_, dropCount_; > + unsigned scanMaxIndex_; > + double scanMaxContrast_, scanMinContrast_; > + std::vector<ScanRecord> scanData_; > + AfState reportState_; > +}; > + > +} // namespace RPiController > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build > index 517d815bb98c..4e2689536257 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/rpi/af.cpp', > 'controller/rpi/alsc.cpp', > 'controller/rpi/awb.cpp', > 'controller/rpi/sharpen.cpp', > -- > 2.25.1 >
Hi all, This patch needs a tweak to handle the case where setWindows is called before configure -- see Af::Af(Controller *controller) constructor below. On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote: > > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > Provide the first version of the Raspberry Pi autofocus algorithm. This > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to > having less "hunting" behavior. > > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > Signed-off-by: Naushir Patuck <naush@raspberrypi.com> > Reviewed-by: Naushir Patuck <naush@raspberrypi.com> > Reviewed-by: David Plowman <david.plowman@raspberrypi.com> > --- > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++ > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++ > src/ipa/raspberrypi/meson.build | 1 + > 3 files changed, 909 insertions(+) > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp > new file mode 100644 > index 000000000000..7e2e8961085a > --- /dev/null > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp > @@ -0,0 +1,755 @@ > +/* 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 ¶ms, 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 ¶ms) > +{ > + > + readNumber<double>(focusMin, params, "min"); > + readNumber<double>(focusMax, params, "max"); > + readNumber<double>(focusDefault, params, "default"); > +} > + > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) > +{ > + 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 ¶ms) > +{ > + 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); > + > + LOG(RPiAf, Warning) << "af.map is not defined, "; > + } > +} > + > +/* Af Algorithm class */ > + > +Af::Af(Controller *controller) > + : AfAlgorithm(controller), > + cfg_(), > + range_(AfRangeNormal), > + speed_(AfSpeedNormal), > + mode_(AfAlgorithm::AfModeManual), > + pauseFlag_(false), > + sensorSize_{ 0, 0 }, sensorSize_ needs to be set before setWindows() is called. For this version it suffices to initialize it to sensorSize_ { 4608, 2502 }, a more general fix may follow later. > + useWeights_(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) > +{ > + scanData_.reserve(24); > +} > + > +Af::~Af() > +{ > +} > + > +char const *Af::name() const > +{ > + return NAME; > +} > + > +int Af::read(const libcamera::YamlObject ¶ms) > +{ > + return cfg_.read(params); > +} > + > +void Af::initialise() > +{ > + cfg_.initialise(); > +} > + > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata) > +{ > + (void)metadata; > + sensorSize_.width = cameraMode.sensorWidth; > + sensorSize_.height = cameraMode.sensorHeight; > + > + 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; > +} > + > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const > +{ > + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = { > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } > + }; > + int32_t sumW = 0; > + int32_t sumWc = 0; > + int32_t sumWcp = 0; > + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights; > + > + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) { > + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) { > + if (wgts[i][j]) { > + uint32_t c = data.conf[i][j]; > + if (c >= cfg_.confThresh) { > + if (c > cfg_.confClip) > + c = cfg_.confClip; > + sumWc += wgts[i][j] * (int32_t)c; > + c -= (cfg_.confThresh >> 1); > + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c; > + } > + sumW += wgts[i][j]; > + } > + } > + } > + > + if (sumWc > 0) { > + phase = (double)sumWcp / (double)sumWc; > + conf = (double)sumWc / (double)sumW; > + return true; > + } else { > + phase = 0.0; > + conf = 0.0; > + return false; > + } > +} > + > +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. > + */ > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { > + double a = phase / cfg_.speeds[speed_].pdafSquelch; > + phase *= a * a; > + } > + phase *= conf / (conf + cfg_.confEpsilon); > + } 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 (dropLo < dropHi) { > + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); > + f += param * (scanData_[i - 1].focus - f); > + } else if (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 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_ ? cfg_.confEpsilon : 0.0)) { > + 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 (mode_ != AfModeManual) { > + ftarget_ = std::clamp(ftarget_, > + cfg_.ranges[range_].focusMin, > + cfg_.ranges[range_].focusMax); > + } > + > + /* \todo Add a clip for manual lens position to be within the PWL limits. */ > + > + 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; > + } > +} > + > +/* > + * 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) > +{ > + if (initted_) { > + /* Get PDAF from the embedded metadata, and run AF algorithm core */ > + PdafData data; > + double phase = 0.0, conf = 0.0; > + double oldFt = ftarget_; > + double oldFs = fsmooth_; > + ScanState oldSs = scanState_; > + uint32_t oldSt = stepCount_; > + if (imageMetadata->get("pdaf.data", data) == 0) > + getPhase(data, 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); > +} > + > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const > +{ > + uint32_t totW = 0, totWc = 0; > + > + if (useWeights_) { > + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) { > + unsigned w = contrastWeights_[i]; > + totW += w; > + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10); > + } > + } > + if (totW == 0) { > + totW = 2; > + totWc = (focus_stats[5].contrast_val[1][1] >> 10) + > + (focus_stats[6].contrast_val[1][1] >> 10); > + } > + > + return (double)totWc / (double)totW; > +} > + > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) > +{ > + (void)imageMetadata; > + prevContrast_ = getContrast(stats->focus_stats); > +} > + > +/* 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) > +{ > + useWeights_ = mode; > +} > + > +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) > +{ > + /* > + * Here we just merge all of the given windows, weighted by area. > + * If there are more than 15 overlapping windows, overflow can occur. > + * TODO: A better approach might be to find the phase in each window > + * and choose either the closest or the highest-confidence one? > + * > + * Using mostly "int" arithmetic, because Rectangle has signed x, y > + */ > + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0); > + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS); > + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS); > + int gridA = gridY * gridX; > + > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) > + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0); > + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0); > + > + for (auto &w : wins) { > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) { > + int y0 = std::max(gridY * i, w.y); > + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height)); > + if (y0 >= y1) > + continue; > + y1 -= y0; > + for (int j = 0; j < PDAF_DATA_COLS; ++j) { > + int x0 = std::max(gridX * j, w.x); > + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width)); > + if (x0 >= x1) > + continue; > + int a = y1 * (x1 - x0); > + a = (16 * a + gridA - 1) / gridA; > + phaseWeights_[i][j] += a; > + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a; > + } > + } > + } > +} > + > +bool Af::setLensPosition(double dioptres, int *hwpos) > +{ > + bool changed = false; > + > + if (mode_ == AfModeManual) { > + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; > + changed = !(initted_ && fsmooth_ == dioptres); > + ftarget_ = dioptres; > + 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::startCAF() > +{ > + /* Try PDAF if the tuning file permits it for CAF; else CDAF */ > + if (cfg_.speeds[speed_].dropoutFrames > 0) { > + if (!initted_) { > + ftarget_ = cfg_.ranges[range_].focusDefault; > + updateLensPosition(); > + } > + 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(); > +} > + > +void Af::cancelScan() > +{ > + LOG(RPiAf, Debug) << "cancelScan"; > + if (mode_ == AfModeAuto) > + goIdle(); > +} > + > +void Af::triggerScan() > +{ > + LOG(RPiAf, Debug) << "triggerScan"; > + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) { > + /* Try PDAF if the tuning file permits it for Auto; else CDAF */ > + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) { > + if (!initted_) { > + ftarget_ = cfg_.ranges[range_].focusDefault; > + updateLensPosition(); > + } > + stepCount_ = cfg_.speeds[speed_].pdafFrames; > + scanState_ = ScanState::Pdaf; > + dropCount_ = 0; > + } else > + startProgrammedScan(); > + reportState_ = AfState::Scanning; > + } > +} > + > +void Af::setMode(AfAlgorithm::AfMode mode) > +{ > + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; > + if (mode_ != mode) { > + mode_ = mode; > + pauseFlag_ = false; > + if (mode == AfModeContinuous) > + startCAF(); > + else if (mode != AfModeAuto) > + 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_) { > + startCAF(); > + pauseFlag_ = false; > + } else if (pause != AfPauseResume && !pauseFlag_) { > + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf) > + goIdle(); > + pauseFlag_ = true; > + } > + } > +} > + > +// Register algorithm with the system. > +static Algorithm *create(Controller *controller) > +{ > + return (Algorithm *)new Af(controller); > +} > +static RegisterAlgorithm reg(NAME, &create); > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h > new file mode 100644 > index 000000000000..0431bd70ae29 > --- /dev/null > +++ b/src/ipa/raspberrypi/controller/rpi/af.h > @@ -0,0 +1,153 @@ > +/* SPDX-License-Identifier: BSD-2-Clause */ > +/* > + * Copyright (C) 2022-2023, Raspberry Pi Ltd > + * > + * af.h - Autofocus control algorithm > + */ > +#pragma once > + > +#include "../af_algorithm.h" > +#include "../af_status.h" > +#include "../pdaf_data.h" > +#include "../pwl.h" > + > +/* > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF. > + * > + * Whenever PDAF is available, it is used in a continuous feedback loop. > + * When triggered in auto mode, we simply enable AF for a limited number > + * of frames (it may terminate early if the delta becomes small enough). > + * > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus) > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern. > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to > + * estimate the lens position with peak contrast. This is slower due to > + * extra latency in the ISP, and requires a settling time between steps. > + * > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid > + * "nuisance" scans. During each interval where PDAF is not working, only > + * ONE scan will be performed; CAF cannot track objects using CDAF alone. > + * > + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM. > + */ > + > +namespace RPiController { > + > +class Af : public AfAlgorithm > +{ > +public: > + Af(Controller *controller = NULL); > + ~Af(); > + char const *name() const override; > + int read(const libcamera::YamlObject ¶ms) override; > + void initialise() override; > + > + /* IPA calls */ > + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; > + void prepare(Metadata *imageMetadata) override; > + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; > + > + /* controls */ > + void setRange(AfRange range) override; > + void setSpeed(AfSpeed speed) override; > + void setMetering(bool use_windows) override; > + void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override; > + void setMode(AfMode mode) override; > + AfMode getMode() const override; > + bool setLensPosition(double dioptres, int32_t *hwpos) override; > + std::optional<double> getLensPosition() const override; > + void triggerScan() override; > + void cancelScan() override; > + void pause(AfPause pause) override; > + > +private: > + enum class ScanState { > + Idle, > + Pdaf, > + Coarse, > + Fine, > + Settle > + }; > + > + struct RangeDependentParams { > + double focusMin; /* lower (far) limit in dipotres */ > + double focusMax; /* upper (near) limit in dioptres */ > + double focusDefault; /* default setting ("hyperfocal") */ > + > + RangeDependentParams(); > + void read(const libcamera::YamlObject ¶ms); > + }; > + > + struct SpeedDependentParams { > + double stepCoarse; /* used for scans */ > + double stepFine; /* used for scans */ > + double contrastRatio; /* used for scan termination and reporting */ > + double pdafGain; /* coefficient for PDAF feedback loop */ > + double pdafSquelch; /* PDAF stability parameter (device-specific) */ > + double maxSlew; /* limit for lens movement per frame */ > + uint32_t pdafFrames; /* number of iterations when triggered */ > + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */ > + uint32_t stepFrames; /* frames to skip in between steps of a scan */ > + > + SpeedDependentParams(); > + void read(const libcamera::YamlObject ¶ms); > + }; > + > + struct CfgParams { > + RangeDependentParams ranges[AfRangeMax]; > + SpeedDependentParams speeds[AfSpeedMax]; > + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */ > + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */ > + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */ > + uint32_t skipFrames; /* frames to skip at start or modeswitch */ > + Pwl map; /* converts dioptres -> lens driver position */ > + > + CfgParams(); > + int read(const libcamera::YamlObject ¶ms); > + void initialise(); > + }; > + > + struct ScanRecord { > + double focus; > + double contrast; > + double phase; > + double conf; > + }; > + > + bool getPhase(PdafData const &data, double &phase, double &conf) const; > + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const; > + void doPDAF(double phase, double conf); > + bool earlyTerminationByPhase(double phase); > + void doScan(double contrast, double phase, double conf); > + double findPeak(unsigned index) const; > + void doAF(double contrast, double phase, double conf); > + void updateLensPosition(); > + void startProgrammedScan(); > + void startCAF(); > + void goIdle(); > + > + /* Configuration and settings */ > + CfgParams cfg_; > + AfRange range_; > + AfSpeed speed_; > + AfMode mode_; > + bool pauseFlag_; > + libcamera::Size sensorSize_; > + bool useWeights_; > + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS]; > + uint16_t contrastWeights_[FOCUS_REGIONS]; > + > + /* Working state. */ > + ScanState scanState_; > + bool initted_; > + double ftarget_, fsmooth_; > + double prevContrast_; > + bool pdafSeen_; > + unsigned skipCount_, stepCount_, dropCount_; > + unsigned scanMaxIndex_; > + double scanMaxContrast_, scanMinContrast_; > + std::vector<ScanRecord> scanData_; > + AfState reportState_; > +}; > + > +} // namespace RPiController > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build > index 517d815bb98c..4e2689536257 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/rpi/af.cpp', > 'controller/rpi/alsc.cpp', > 'controller/rpi/awb.cpp', > 'controller/rpi/sharpen.cpp', > -- > 2.25.1 >
Hi Nick, / Naush, Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44) > Hi all, > > This patch needs a tweak to handle the case where setWindows is called > before configure -- see Af::Af(Controller *controller) constructor > below. > > On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote: > > > > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > > > Provide the first version of the Raspberry Pi autofocus algorithm. This > > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase > > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to > > having less "hunting" behavior. > > > > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > Signed-off-by: Naushir Patuck <naush@raspberrypi.com> > > Reviewed-by: Naushir Patuck <naush@raspberrypi.com> > > Reviewed-by: David Plowman <david.plowman@raspberrypi.com> > > --- > > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++ > > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++ > > src/ipa/raspberrypi/meson.build | 1 + > > 3 files changed, 909 insertions(+) > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h > > > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp > > new file mode 100644 > > index 000000000000..7e2e8961085a > > --- /dev/null > > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp > > @@ -0,0 +1,755 @@ > > +/* 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 ¶ms, 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 ¶ms) > > +{ > > + > > + readNumber<double>(focusMin, params, "min"); > > + readNumber<double>(focusMax, params, "max"); > > + readNumber<double>(focusDefault, params, "default"); > > +} > > + > > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) > > +{ > > + 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 ¶ms) > > +{ > > + 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); > > + > > + LOG(RPiAf, Warning) << "af.map is not defined, "; > > + } > > +} > > + > > +/* Af Algorithm class */ > > + > > +Af::Af(Controller *controller) > > + : AfAlgorithm(controller), > > + cfg_(), > > + range_(AfRangeNormal), > > + speed_(AfSpeedNormal), > > + mode_(AfAlgorithm::AfModeManual), > > + pauseFlag_(false), > > + sensorSize_{ 0, 0 }, > > sensorSize_ needs to be set before setWindows() is called. For this > version it suffices to initialize it to > > sensorSize_ { 4608, 2502 }, > > a more general fix may follow later. Can you send a tested update to this patch please? No need to send the whole series again. If that can include the fix I highlighted, (Remove pdafSeen_) I believe I can merge this whole series. -- Kieran > > > > + useWeights_(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) > > +{ > > + scanData_.reserve(24); > > +} > > + > > +Af::~Af() > > +{ > > +} > > + > > +char const *Af::name() const > > +{ > > + return NAME; > > +} > > + > > +int Af::read(const libcamera::YamlObject ¶ms) > > +{ > > + return cfg_.read(params); > > +} > > + > > +void Af::initialise() > > +{ > > + cfg_.initialise(); > > +} > > + > > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata) > > +{ > > + (void)metadata; > > + sensorSize_.width = cameraMode.sensorWidth; > > + sensorSize_.height = cameraMode.sensorHeight; > > + > > + 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; > > +} > > + > > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const > > +{ > > + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = { > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, > > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } > > + }; > > + int32_t sumW = 0; > > + int32_t sumWc = 0; > > + int32_t sumWcp = 0; > > + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights; > > + > > + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) { > > + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) { > > + if (wgts[i][j]) { > > + uint32_t c = data.conf[i][j]; > > + if (c >= cfg_.confThresh) { > > + if (c > cfg_.confClip) > > + c = cfg_.confClip; > > + sumWc += wgts[i][j] * (int32_t)c; > > + c -= (cfg_.confThresh >> 1); > > + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c; > > + } > > + sumW += wgts[i][j]; > > + } > > + } > > + } > > + > > + if (sumWc > 0) { > > + phase = (double)sumWcp / (double)sumWc; > > + conf = (double)sumWc / (double)sumW; > > + return true; > > + } else { > > + phase = 0.0; > > + conf = 0.0; > > + return false; > > + } > > +} > > + > > +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. > > + */ > > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { > > + double a = phase / cfg_.speeds[speed_].pdafSquelch; > > + phase *= a * a; > > + } > > + phase *= conf / (conf + cfg_.confEpsilon); > > + } 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 (dropLo < dropHi) { > > + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); > > + f += param * (scanData_[i - 1].focus - f); > > + } else if (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 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_ ? cfg_.confEpsilon : 0.0)) { > > + 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 (mode_ != AfModeManual) { > > + ftarget_ = std::clamp(ftarget_, > > + cfg_.ranges[range_].focusMin, > > + cfg_.ranges[range_].focusMax); > > + } > > + > > + /* \todo Add a clip for manual lens position to be within the PWL limits. */ > > + > > + 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; > > + } > > +} > > + > > +/* > > + * 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) > > +{ > > + if (initted_) { > > + /* Get PDAF from the embedded metadata, and run AF algorithm core */ > > + PdafData data; > > + double phase = 0.0, conf = 0.0; > > + double oldFt = ftarget_; > > + double oldFs = fsmooth_; > > + ScanState oldSs = scanState_; > > + uint32_t oldSt = stepCount_; > > + if (imageMetadata->get("pdaf.data", data) == 0) > > + getPhase(data, 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); > > +} > > + > > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const > > +{ > > + uint32_t totW = 0, totWc = 0; > > + > > + if (useWeights_) { > > + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) { > > + unsigned w = contrastWeights_[i]; > > + totW += w; > > + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10); > > + } > > + } > > + if (totW == 0) { > > + totW = 2; > > + totWc = (focus_stats[5].contrast_val[1][1] >> 10) + > > + (focus_stats[6].contrast_val[1][1] >> 10); > > + } > > + > > + return (double)totWc / (double)totW; > > +} > > + > > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) > > +{ > > + (void)imageMetadata; > > + prevContrast_ = getContrast(stats->focus_stats); > > +} > > + > > +/* 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) > > +{ > > + useWeights_ = mode; > > +} > > + > > +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) > > +{ > > + /* > > + * Here we just merge all of the given windows, weighted by area. > > + * If there are more than 15 overlapping windows, overflow can occur. > > + * TODO: A better approach might be to find the phase in each window > > + * and choose either the closest or the highest-confidence one? > > + * > > + * Using mostly "int" arithmetic, because Rectangle has signed x, y > > + */ > > + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0); > > + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS); > > + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS); > > + int gridA = gridY * gridX; > > + > > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) > > + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0); > > + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0); > > + > > + for (auto &w : wins) { > > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) { > > + int y0 = std::max(gridY * i, w.y); > > + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height)); > > + if (y0 >= y1) > > + continue; > > + y1 -= y0; > > + for (int j = 0; j < PDAF_DATA_COLS; ++j) { > > + int x0 = std::max(gridX * j, w.x); > > + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width)); > > + if (x0 >= x1) > > + continue; > > + int a = y1 * (x1 - x0); > > + a = (16 * a + gridA - 1) / gridA; > > + phaseWeights_[i][j] += a; > > + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a; > > + } > > + } > > + } > > +} > > + > > +bool Af::setLensPosition(double dioptres, int *hwpos) > > +{ > > + bool changed = false; > > + > > + if (mode_ == AfModeManual) { > > + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; > > + changed = !(initted_ && fsmooth_ == dioptres); > > + ftarget_ = dioptres; > > + 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::startCAF() > > +{ > > + /* Try PDAF if the tuning file permits it for CAF; else CDAF */ > > + if (cfg_.speeds[speed_].dropoutFrames > 0) { > > + if (!initted_) { > > + ftarget_ = cfg_.ranges[range_].focusDefault; > > + updateLensPosition(); > > + } > > + 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(); > > +} > > + > > +void Af::cancelScan() > > +{ > > + LOG(RPiAf, Debug) << "cancelScan"; > > + if (mode_ == AfModeAuto) > > + goIdle(); > > +} > > + > > +void Af::triggerScan() > > +{ > > + LOG(RPiAf, Debug) << "triggerScan"; > > + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) { > > + /* Try PDAF if the tuning file permits it for Auto; else CDAF */ > > + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) { > > + if (!initted_) { > > + ftarget_ = cfg_.ranges[range_].focusDefault; > > + updateLensPosition(); > > + } > > + stepCount_ = cfg_.speeds[speed_].pdafFrames; > > + scanState_ = ScanState::Pdaf; > > + dropCount_ = 0; > > + } else > > + startProgrammedScan(); > > + reportState_ = AfState::Scanning; > > + } > > +} > > + > > +void Af::setMode(AfAlgorithm::AfMode mode) > > +{ > > + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; > > + if (mode_ != mode) { > > + mode_ = mode; > > + pauseFlag_ = false; > > + if (mode == AfModeContinuous) > > + startCAF(); > > + else if (mode != AfModeAuto) > > + 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_) { > > + startCAF(); > > + pauseFlag_ = false; > > + } else if (pause != AfPauseResume && !pauseFlag_) { > > + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf) > > + goIdle(); > > + pauseFlag_ = true; > > + } > > + } > > +} > > + > > +// Register algorithm with the system. > > +static Algorithm *create(Controller *controller) > > +{ > > + return (Algorithm *)new Af(controller); > > +} > > +static RegisterAlgorithm reg(NAME, &create); > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h > > new file mode 100644 > > index 000000000000..0431bd70ae29 > > --- /dev/null > > +++ b/src/ipa/raspberrypi/controller/rpi/af.h > > @@ -0,0 +1,153 @@ > > +/* SPDX-License-Identifier: BSD-2-Clause */ > > +/* > > + * Copyright (C) 2022-2023, Raspberry Pi Ltd > > + * > > + * af.h - Autofocus control algorithm > > + */ > > +#pragma once > > + > > +#include "../af_algorithm.h" > > +#include "../af_status.h" > > +#include "../pdaf_data.h" > > +#include "../pwl.h" > > + > > +/* > > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF. > > + * > > + * Whenever PDAF is available, it is used in a continuous feedback loop. > > + * When triggered in auto mode, we simply enable AF for a limited number > > + * of frames (it may terminate early if the delta becomes small enough). > > + * > > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus) > > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern. > > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to > > + * estimate the lens position with peak contrast. This is slower due to > > + * extra latency in the ISP, and requires a settling time between steps. > > + * > > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid > > + * "nuisance" scans. During each interval where PDAF is not working, only > > + * ONE scan will be performed; CAF cannot track objects using CDAF alone. > > + * > > + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM. > > + */ > > + > > +namespace RPiController { > > + > > +class Af : public AfAlgorithm > > +{ > > +public: > > + Af(Controller *controller = NULL); > > + ~Af(); > > + char const *name() const override; > > + int read(const libcamera::YamlObject ¶ms) override; > > + void initialise() override; > > + > > + /* IPA calls */ > > + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; > > + void prepare(Metadata *imageMetadata) override; > > + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; > > + > > + /* controls */ > > + void setRange(AfRange range) override; > > + void setSpeed(AfSpeed speed) override; > > + void setMetering(bool use_windows) override; > > + void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override; > > + void setMode(AfMode mode) override; > > + AfMode getMode() const override; > > + bool setLensPosition(double dioptres, int32_t *hwpos) override; > > + std::optional<double> getLensPosition() const override; > > + void triggerScan() override; > > + void cancelScan() override; > > + void pause(AfPause pause) override; > > + > > +private: > > + enum class ScanState { > > + Idle, > > + Pdaf, > > + Coarse, > > + Fine, > > + Settle > > + }; > > + > > + struct RangeDependentParams { > > + double focusMin; /* lower (far) limit in dipotres */ > > + double focusMax; /* upper (near) limit in dioptres */ > > + double focusDefault; /* default setting ("hyperfocal") */ > > + > > + RangeDependentParams(); > > + void read(const libcamera::YamlObject ¶ms); > > + }; > > + > > + struct SpeedDependentParams { > > + double stepCoarse; /* used for scans */ > > + double stepFine; /* used for scans */ > > + double contrastRatio; /* used for scan termination and reporting */ > > + double pdafGain; /* coefficient for PDAF feedback loop */ > > + double pdafSquelch; /* PDAF stability parameter (device-specific) */ > > + double maxSlew; /* limit for lens movement per frame */ > > + uint32_t pdafFrames; /* number of iterations when triggered */ > > + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */ > > + uint32_t stepFrames; /* frames to skip in between steps of a scan */ > > + > > + SpeedDependentParams(); > > + void read(const libcamera::YamlObject ¶ms); > > + }; > > + > > + struct CfgParams { > > + RangeDependentParams ranges[AfRangeMax]; > > + SpeedDependentParams speeds[AfSpeedMax]; > > + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */ > > + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */ > > + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */ > > + uint32_t skipFrames; /* frames to skip at start or modeswitch */ > > + Pwl map; /* converts dioptres -> lens driver position */ > > + > > + CfgParams(); > > + int read(const libcamera::YamlObject ¶ms); > > + void initialise(); > > + }; > > + > > + struct ScanRecord { > > + double focus; > > + double contrast; > > + double phase; > > + double conf; > > + }; > > + > > + bool getPhase(PdafData const &data, double &phase, double &conf) const; > > + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const; > > + void doPDAF(double phase, double conf); > > + bool earlyTerminationByPhase(double phase); > > + void doScan(double contrast, double phase, double conf); > > + double findPeak(unsigned index) const; > > + void doAF(double contrast, double phase, double conf); > > + void updateLensPosition(); > > + void startProgrammedScan(); > > + void startCAF(); > > + void goIdle(); > > + > > + /* Configuration and settings */ > > + CfgParams cfg_; > > + AfRange range_; > > + AfSpeed speed_; > > + AfMode mode_; > > + bool pauseFlag_; > > + libcamera::Size sensorSize_; > > + bool useWeights_; > > + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS]; > > + uint16_t contrastWeights_[FOCUS_REGIONS]; > > + > > + /* Working state. */ > > + ScanState scanState_; > > + bool initted_; > > + double ftarget_, fsmooth_; > > + double prevContrast_; > > + bool pdafSeen_; > > + unsigned skipCount_, stepCount_, dropCount_; > > + unsigned scanMaxIndex_; > > + double scanMaxContrast_, scanMinContrast_; > > + std::vector<ScanRecord> scanData_; > > + AfState reportState_; > > +}; > > + > > +} // namespace RPiController > > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build > > index 517d815bb98c..4e2689536257 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/rpi/af.cpp', > > 'controller/rpi/alsc.cpp', > > 'controller/rpi/awb.cpp', > > 'controller/rpi/sharpen.cpp', > > -- > > 2.25.1 > >
Hi Kieran, On Fri, 20 Jan 2023 at 16:39, Kieran Bingham <kieran.bingham@ideasonboard.com> wrote: > > Hi Nick, / Naush, > > Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44) > > Hi all, > > > > This patch needs a tweak to handle the case where setWindows is called > > before configure -- see Af::Af(Controller *controller) constructor > > below. > > > > On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote: > > > > > > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > > > > > Provide the first version of the Raspberry Pi autofocus algorithm. This > > > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase > > > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to > > > having less "hunting" behavior. > > > > > > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com> > > > Signed-off-by: Naushir Patuck <naush@raspberrypi.com> > > > Reviewed-by: Naushir Patuck <naush@raspberrypi.com> > > > Reviewed-by: David Plowman <david.plowman@raspberrypi.com> > > > --- > > > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++ > > > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++ > > > src/ipa/raspberrypi/meson.build | 1 + > > > 3 files changed, 909 insertions(+) > > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp > > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h > > > > > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp > > > new file mode 100644 > > > index 000000000000..7e2e8961085a > > > --- /dev/null > > > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp > > > @@ -0,0 +1,755 @@ > > > +/* 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 ¶ms, 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 ¶ms) > > > +{ > > > + > > > + readNumber<double>(focusMin, params, "min"); > > > + readNumber<double>(focusMax, params, "max"); > > > + readNumber<double>(focusDefault, params, "default"); > > > +} > > > + > > > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) > > > +{ > > > + 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 ¶ms) > > > +{ > > > + 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); > > > + > > > + LOG(RPiAf, Warning) << "af.map is not defined, "; > > > + } > > > +} > > > + > > > +/* Af Algorithm class */ > > > + > > > +Af::Af(Controller *controller) > > > + : AfAlgorithm(controller), > > > + cfg_(), > > > + range_(AfRangeNormal), > > > + speed_(AfSpeedNormal), > > > + mode_(AfAlgorithm::AfModeManual), > > > + pauseFlag_(false), > > > + sensorSize_{ 0, 0 }, > > > > sensorSize_ needs to be set before setWindows() is called. For this > > version it suffices to initialize it to > > > > sensorSize_ { 4608, 2502 }, > > > > a more general fix may follow later. > > Can you send a tested update to this patch please? No need to send the > whole series again. > > If that can include the fix I highlighted, (Remove pdafSeen_) I believe > I can merge this whole series. > > -- > Kieran I have done a bit of re-work to fix this "properly" and am testing it now. I'll also remove pdafSeen_. The re-work touches "af.cpp", "af.h" only so it affects only this patch. Regards, Nick <snipped remainder>
diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp new file mode 100644 index 000000000000..7e2e8961085a --- /dev/null +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp @@ -0,0 +1,755 @@ +/* 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 ¶ms, 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 ¶ms) +{ + + readNumber<double>(focusMin, params, "min"); + readNumber<double>(focusMax, params, "max"); + readNumber<double>(focusDefault, params, "default"); +} + +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) +{ + 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 ¶ms) +{ + 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); + + LOG(RPiAf, Warning) << "af.map is not defined, "; + } +} + +/* Af Algorithm class */ + +Af::Af(Controller *controller) + : AfAlgorithm(controller), + cfg_(), + range_(AfRangeNormal), + speed_(AfSpeedNormal), + mode_(AfAlgorithm::AfModeManual), + pauseFlag_(false), + sensorSize_{ 0, 0 }, + useWeights_(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) +{ + scanData_.reserve(24); +} + +Af::~Af() +{ +} + +char const *Af::name() const +{ + return NAME; +} + +int Af::read(const libcamera::YamlObject ¶ms) +{ + return cfg_.read(params); +} + +void Af::initialise() +{ + cfg_.initialise(); +} + +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata) +{ + (void)metadata; + sensorSize_.width = cameraMode.sensorWidth; + sensorSize_.height = cameraMode.sensorHeight; + + 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; +} + +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const +{ + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = { + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } + }; + int32_t sumW = 0; + int32_t sumWc = 0; + int32_t sumWcp = 0; + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights; + + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) { + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) { + if (wgts[i][j]) { + uint32_t c = data.conf[i][j]; + if (c >= cfg_.confThresh) { + if (c > cfg_.confClip) + c = cfg_.confClip; + sumWc += wgts[i][j] * (int32_t)c; + c -= (cfg_.confThresh >> 1); + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c; + } + sumW += wgts[i][j]; + } + } + } + + if (sumWc > 0) { + phase = (double)sumWcp / (double)sumWc; + conf = (double)sumWc / (double)sumW; + return true; + } else { + phase = 0.0; + conf = 0.0; + return false; + } +} + +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. + */ + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { + double a = phase / cfg_.speeds[speed_].pdafSquelch; + phase *= a * a; + } + phase *= conf / (conf + cfg_.confEpsilon); + } 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 (dropLo < dropHi) { + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); + f += param * (scanData_[i - 1].focus - f); + } else if (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 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_ ? cfg_.confEpsilon : 0.0)) { + 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 (mode_ != AfModeManual) { + ftarget_ = std::clamp(ftarget_, + cfg_.ranges[range_].focusMin, + cfg_.ranges[range_].focusMax); + } + + /* \todo Add a clip for manual lens position to be within the PWL limits. */ + + 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; + } +} + +/* + * 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) +{ + if (initted_) { + /* Get PDAF from the embedded metadata, and run AF algorithm core */ + PdafData data; + double phase = 0.0, conf = 0.0; + double oldFt = ftarget_; + double oldFs = fsmooth_; + ScanState oldSs = scanState_; + uint32_t oldSt = stepCount_; + if (imageMetadata->get("pdaf.data", data) == 0) + getPhase(data, 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); +} + +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const +{ + uint32_t totW = 0, totWc = 0; + + if (useWeights_) { + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) { + unsigned w = contrastWeights_[i]; + totW += w; + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10); + } + } + if (totW == 0) { + totW = 2; + totWc = (focus_stats[5].contrast_val[1][1] >> 10) + + (focus_stats[6].contrast_val[1][1] >> 10); + } + + return (double)totWc / (double)totW; +} + +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) +{ + (void)imageMetadata; + prevContrast_ = getContrast(stats->focus_stats); +} + +/* 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) +{ + useWeights_ = mode; +} + +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) +{ + /* + * Here we just merge all of the given windows, weighted by area. + * If there are more than 15 overlapping windows, overflow can occur. + * TODO: A better approach might be to find the phase in each window + * and choose either the closest or the highest-confidence one? + * + * Using mostly "int" arithmetic, because Rectangle has signed x, y + */ + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0); + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS); + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS); + int gridA = gridY * gridX; + + for (int i = 0; i < PDAF_DATA_ROWS; ++i) + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0); + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0); + + for (auto &w : wins) { + for (int i = 0; i < PDAF_DATA_ROWS; ++i) { + int y0 = std::max(gridY * i, w.y); + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height)); + if (y0 >= y1) + continue; + y1 -= y0; + for (int j = 0; j < PDAF_DATA_COLS; ++j) { + int x0 = std::max(gridX * j, w.x); + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width)); + if (x0 >= x1) + continue; + int a = y1 * (x1 - x0); + a = (16 * a + gridA - 1) / gridA; + phaseWeights_[i][j] += a; + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a; + } + } + } +} + +bool Af::setLensPosition(double dioptres, int *hwpos) +{ + bool changed = false; + + if (mode_ == AfModeManual) { + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; + changed = !(initted_ && fsmooth_ == dioptres); + ftarget_ = dioptres; + 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::startCAF() +{ + /* Try PDAF if the tuning file permits it for CAF; else CDAF */ + if (cfg_.speeds[speed_].dropoutFrames > 0) { + if (!initted_) { + ftarget_ = cfg_.ranges[range_].focusDefault; + updateLensPosition(); + } + 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(); +} + +void Af::cancelScan() +{ + LOG(RPiAf, Debug) << "cancelScan"; + if (mode_ == AfModeAuto) + goIdle(); +} + +void Af::triggerScan() +{ + LOG(RPiAf, Debug) << "triggerScan"; + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) { + /* Try PDAF if the tuning file permits it for Auto; else CDAF */ + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) { + if (!initted_) { + ftarget_ = cfg_.ranges[range_].focusDefault; + updateLensPosition(); + } + stepCount_ = cfg_.speeds[speed_].pdafFrames; + scanState_ = ScanState::Pdaf; + dropCount_ = 0; + } else + startProgrammedScan(); + reportState_ = AfState::Scanning; + } +} + +void Af::setMode(AfAlgorithm::AfMode mode) +{ + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; + if (mode_ != mode) { + mode_ = mode; + pauseFlag_ = false; + if (mode == AfModeContinuous) + startCAF(); + else if (mode != AfModeAuto) + 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_) { + startCAF(); + pauseFlag_ = false; + } else if (pause != AfPauseResume && !pauseFlag_) { + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf) + goIdle(); + pauseFlag_ = true; + } + } +} + +// Register algorithm with the system. +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Af(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h new file mode 100644 index 000000000000..0431bd70ae29 --- /dev/null +++ b/src/ipa/raspberrypi/controller/rpi/af.h @@ -0,0 +1,153 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022-2023, Raspberry Pi Ltd + * + * af.h - Autofocus control algorithm + */ +#pragma once + +#include "../af_algorithm.h" +#include "../af_status.h" +#include "../pdaf_data.h" +#include "../pwl.h" + +/* + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF. + * + * Whenever PDAF is available, it is used in a continuous feedback loop. + * When triggered in auto mode, we simply enable AF for a limited number + * of frames (it may terminate early if the delta becomes small enough). + * + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus) + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern. + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to + * estimate the lens position with peak contrast. This is slower due to + * extra latency in the ISP, and requires a settling time between steps. + * + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid + * "nuisance" scans. During each interval where PDAF is not working, only + * ONE scan will be performed; CAF cannot track objects using CDAF alone. + * + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM. + */ + +namespace RPiController { + +class Af : public AfAlgorithm +{ +public: + Af(Controller *controller = NULL); + ~Af(); + char const *name() const override; + int read(const libcamera::YamlObject ¶ms) override; + void initialise() override; + + /* IPA calls */ + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override; + void prepare(Metadata *imageMetadata) override; + void process(StatisticsPtr &stats, Metadata *imageMetadata) override; + + /* controls */ + void setRange(AfRange range) override; + void setSpeed(AfSpeed speed) override; + void setMetering(bool use_windows) override; + void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override; + void setMode(AfMode mode) override; + AfMode getMode() const override; + bool setLensPosition(double dioptres, int32_t *hwpos) override; + std::optional<double> getLensPosition() const override; + void triggerScan() override; + void cancelScan() override; + void pause(AfPause pause) override; + +private: + enum class ScanState { + Idle, + Pdaf, + Coarse, + Fine, + Settle + }; + + struct RangeDependentParams { + double focusMin; /* lower (far) limit in dipotres */ + double focusMax; /* upper (near) limit in dioptres */ + double focusDefault; /* default setting ("hyperfocal") */ + + RangeDependentParams(); + void read(const libcamera::YamlObject ¶ms); + }; + + struct SpeedDependentParams { + double stepCoarse; /* used for scans */ + double stepFine; /* used for scans */ + double contrastRatio; /* used for scan termination and reporting */ + double pdafGain; /* coefficient for PDAF feedback loop */ + double pdafSquelch; /* PDAF stability parameter (device-specific) */ + double maxSlew; /* limit for lens movement per frame */ + uint32_t pdafFrames; /* number of iterations when triggered */ + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */ + uint32_t stepFrames; /* frames to skip in between steps of a scan */ + + SpeedDependentParams(); + void read(const libcamera::YamlObject ¶ms); + }; + + struct CfgParams { + RangeDependentParams ranges[AfRangeMax]; + SpeedDependentParams speeds[AfSpeedMax]; + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */ + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */ + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */ + uint32_t skipFrames; /* frames to skip at start or modeswitch */ + Pwl map; /* converts dioptres -> lens driver position */ + + CfgParams(); + int read(const libcamera::YamlObject ¶ms); + void initialise(); + }; + + struct ScanRecord { + double focus; + double contrast; + double phase; + double conf; + }; + + bool getPhase(PdafData const &data, double &phase, double &conf) const; + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const; + void doPDAF(double phase, double conf); + bool earlyTerminationByPhase(double phase); + void doScan(double contrast, double phase, double conf); + double findPeak(unsigned index) const; + void doAF(double contrast, double phase, double conf); + void updateLensPosition(); + void startProgrammedScan(); + void startCAF(); + void goIdle(); + + /* Configuration and settings */ + CfgParams cfg_; + AfRange range_; + AfSpeed speed_; + AfMode mode_; + bool pauseFlag_; + libcamera::Size sensorSize_; + bool useWeights_; + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS]; + uint16_t contrastWeights_[FOCUS_REGIONS]; + + /* Working state. */ + ScanState scanState_; + bool initted_; + double ftarget_, fsmooth_; + double prevContrast_; + bool pdafSeen_; + unsigned skipCount_, stepCount_, dropCount_; + unsigned scanMaxIndex_; + double scanMaxContrast_, scanMinContrast_; + std::vector<ScanRecord> scanData_; + AfState reportState_; +}; + +} // namespace RPiController diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build index 517d815bb98c..4e2689536257 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/rpi/af.cpp', 'controller/rpi/alsc.cpp', 'controller/rpi/awb.cpp', 'controller/rpi/sharpen.cpp',