From cc010b0c35bfa0eee0bafba6a5803d1405542456 Mon Sep 17 00:00:00 2001 From: Nick Hollinghurst Date: Mon, 23 Jan 2023 15:49:31 +0000 Subject: ipa: raspberrypi: First version of autofocus algorithm using PDAF 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 Signed-off-by: Naushir Patuck Reviewed-by: Naushir Patuck Reviewed-by: David Plowman Signed-off-by: Kieran Bingham --- src/ipa/raspberrypi/controller/rpi/af.cpp | 795 ++++++++++++++++++++++++++++++ src/ipa/raspberrypi/controller/rpi/af.h | 156 ++++++ src/ipa/raspberrypi/meson.build | 1 + 3 files changed, 952 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 00000000..2e72f239 --- /dev/null +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp @@ -0,0 +1,795 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ +/* + * Copyright (C) 2022-2023, Raspberry Pi Ltd + * + * af.cpp - Autofocus control algorithm + */ + +#include "af.h" + +#include +#include +#include + +#include + +#include + +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 +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name) +{ + auto value = params[name].get(); + if (value) + dest = *value; + else + LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\""; +} + +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms) +{ + + readNumber(focusMin, params, "min"); + readNumber(focusMax, params, "max"); + readNumber(focusDefault, params, "default"); +} + +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) +{ + readNumber(stepCoarse, params, "step_coarse"); + readNumber(stepFine, params, "step_fine"); + readNumber(contrastRatio, params, "contrast_ratio"); + readNumber(pdafGain, params, "pdaf_gain"); + readNumber(pdafSquelch, params, "pdaf_squelch"); + readNumber(maxSlew, params, "max_slew"); + readNumber(pdafFrames, params, "pdaf_frames"); + readNumber(dropoutFrames, params, "dropout_frames"); + readNumber(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(confEpsilon, params, "conf_epsilon"); + readNumber(confThresh, params, "conf_thresh"); + readNumber(confClip, params, "conf_clip"); + readNumber(skipFrames, params, "skip_frames"); + + if (params.contains("map")) + map.read(params["map"]); + else + LOG(RPiAf, Warning) << "No map defined"; + + return 0; +} + +void Af::CfgParams::initialise() +{ + if (map.empty()) { + /* Default mapping from dioptres to hardware setting */ + static constexpr double DefaultMapX0 = 0.0; + static constexpr double DefaultMapY0 = 445.0; + static constexpr double DefaultMapX1 = 15.0; + static constexpr double DefaultMapY1 = 925.0; + + map.append(DefaultMapX0, DefaultMapY0); + map.append(DefaultMapX1, DefaultMapY1); + } +} + +/* Af Algorithm class */ + +static constexpr unsigned MaxWindows = 10; + +Af::Af(Controller *controller) + : AfAlgorithm(controller), + cfg_(), + range_(AfRangeNormal), + speed_(AfSpeedNormal), + mode_(AfAlgorithm::AfModeManual), + pauseFlag_(false), + statsRegion_(0, 0, 0, 0), + windows_(), + useWindows_(false), + phaseWeights_{}, + contrastWeights_{}, + sumWeights_(0), + 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; + + /* Assume that PDAF and Focus stats grids cover the visible area */ + statsRegion_.x = (int)cameraMode.cropX; + statsRegion_.y = (int)cameraMode.cropY; + statsRegion_.width = (unsigned)(cameraMode.width * cameraMode.scaleX); + statsRegion_.height = (unsigned)(cameraMode.height * cameraMode.scaleY); + LOG(RPiAf, Debug) << "switchMode: statsRegion: " + << statsRegion_.x << ',' + << statsRegion_.y << ',' + << statsRegion_.width << ',' + << statsRegion_.height; + computeWeights(); + + if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) { + /* + * If a scan was in progress, re-start it, as CDAF statistics + * may have changed. Though if the application is just about + * to take a still picture, this will not help... + */ + startProgrammedScan(); + } + skipCount_ = cfg_.skipFrames; +} + +void Af::computeWeights() +{ + constexpr int MaxCellWeight = 240 / (int)MaxWindows; + + sumWeights_ = 0; + for (int i = 0; i < PDAF_DATA_ROWS; ++i) + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0); + + if (useWindows_ && + statsRegion_.width >= PDAF_DATA_COLS && statsRegion_.height >= PDAF_DATA_ROWS) { + /* + * Here we just merge all of the given windows, weighted by area. + * \todo Perhaps a better approach might be to find the phase in each + * window and choose either the closest or the highest-confidence one? + * + * Using mostly "int" arithmetic, because Rectangle has signed x, y + */ + int cellH = (int)(statsRegion_.height / PDAF_DATA_ROWS); + int cellW = (int)(statsRegion_.width / PDAF_DATA_COLS); + int cellA = cellH * cellW; + + for (auto &w : windows_) { + for (int i = 0; i < PDAF_DATA_ROWS; ++i) { + int y0 = std::max(statsRegion_.y + cellH * i, w.y); + int y1 = std::min(statsRegion_.y + cellH * (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(statsRegion_.x + cellW * j, w.x); + int x1 = std::min(statsRegion_.x + cellW * (j + 1), w.x + (int)(w.width)); + if (x0 >= x1) + continue; + int a = y1 * (x1 - x0); + a = (MaxCellWeight * a + cellA - 1) / cellA; + phaseWeights_[i][j] += a; + sumWeights_ += a; + } + } + } + } + + if (sumWeights_ == 0) { + /* + * Default AF window is the middle 1/2 width of the middle 1/3 height + * since this maps nicely to both PDAF (16x12) and Focus (4x3) grids. + */ + for (int i = PDAF_DATA_ROWS / 3; i < 2 * PDAF_DATA_ROWS / 3; ++i) { + for (int j = PDAF_DATA_COLS / 4; j < 3 * PDAF_DATA_COLS / 4; ++j) { + phaseWeights_[i][j] = MaxCellWeight; + sumWeights_ += MaxCellWeight; + } + } + } + + /* Scale from PDAF to Focus Statistics grid (which has fixed size 4x3) */ + constexpr int FocusStatsRows = 3; + constexpr int FocusStatsCols = 4; + static_assert(FOCUS_REGIONS == FocusStatsRows * FocusStatsCols); + static_assert(PDAF_DATA_ROWS % FocusStatsRows == 0); + static_assert(PDAF_DATA_COLS % FocusStatsCols == 0); + constexpr int YFactor = PDAF_DATA_ROWS / FocusStatsRows; + constexpr int XFactor = PDAF_DATA_COLS / FocusStatsCols; + + LOG(RPiAf, Debug) << "Recomputed weights:"; + for (int i = 0; i < FocusStatsRows; ++i) { + for (int j = 0; j < FocusStatsCols; ++j) { + unsigned w = 0; + for (int y = 0; y < YFactor; ++y) + for (int x = 0; x < XFactor; ++x) + w += phaseWeights_[YFactor * i + y][XFactor * j + x]; + contrastWeights_[FocusStatsCols * i + j] = w; + } + LOG(RPiAf, Debug) << " " + << contrastWeights_[FocusStatsCols * i + 0] << " " + << contrastWeights_[FocusStatsCols * i + 1] << " " + << contrastWeights_[FocusStatsCols * i + 2] << " " + << contrastWeights_[FocusStatsCols * i + 3]; + } +} + +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const +{ + uint32_t sumWc = 0; + int64_t sumWcp = 0; + + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) { + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) { + if (phaseWeights_[i][j]) { + uint32_t c = data.conf[i][j]; + if (c >= cfg_.confThresh) { + if (c > cfg_.confClip) + c = cfg_.confClip; + c -= (cfg_.confThresh >> 2); + sumWc += phaseWeights_[i][j] * c; + c -= (cfg_.confThresh >> 2); + sumWcp += phaseWeights_[i][j] * data.phase[i][j] * (int64_t)c; + } + } + } + } + + if (0 < sumWeights_ && sumWeights_ <= sumWc) { + phase = (double)sumWcp / (double)sumWc; + conf = (double)sumWc / (double)sumWeights_; + return true; + } else { + phase = 0.0; + conf = 0.0; + return false; + } +} + +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const +{ + uint32_t sumWc = 0; + + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) { + unsigned w = contrastWeights_[i]; + sumWc += w * (focus_stats[i].contrast_val[1][1] >> 10); + } + + return (sumWeights_ == 0) ? 0.0 : (double)sumWc / (double)sumWeights_; +} + +void Af::doPDAF(double phase, double conf) +{ + /* Apply loop gain */ + phase *= cfg_.speeds[speed_].pdafGain; + + if (mode_ == AfModeContinuous) { + /* + * PDAF in Continuous mode. Scale down lens movement when + * delta is small or confidence is low, to suppress wobble. + */ + phase *= conf / (conf + cfg_.confEpsilon); + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) { + double a = phase / cfg_.speeds[speed_].pdafSquelch; + phase *= a * a; + } + } else { + /* + * PDAF in triggered-auto mode. Allow early termination when + * phase delta is small; scale down lens movements towards + * the end of the sequence, to ensure a stable image. + */ + if (stepCount_ >= cfg_.speeds[speed_].stepFrames) { + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) + stepCount_ = cfg_.speeds[speed_].stepFrames; + } else + phase *= stepCount_ / cfg_.speeds[speed_].stepFrames; + } + + /* Apply slew rate limit. Report failure if out of bounds. */ + if (phase < -cfg_.speeds[speed_].maxSlew) { + phase = -cfg_.speeds[speed_].maxSlew; + reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed + : AfState::Scanning; + } else if (phase > cfg_.speeds[speed_].maxSlew) { + phase = cfg_.speeds[speed_].maxSlew; + reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed + : AfState::Scanning; + } else + reportState_ = AfState::Focused; + + ftarget_ = fsmooth_ + phase; +} + +bool Af::earlyTerminationByPhase(double phase) +{ + if (scanData_.size() > 0 && + scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) { + double oldFocus = scanData_[scanData_.size() - 1].focus; + double oldPhase = scanData_[scanData_.size() - 1].phase; + + /* + * Check that the gradient is finite and has the expected sign; + * Interpolate/extrapolate the lens position for zero phase. + * Check that the extrapolation is well-conditioned. + */ + if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) { + double param = phase / (phase - oldPhase); + if (-3.0 <= param && param <= 3.5) { + ftarget_ += param * (oldFocus - ftarget_); + LOG(RPiAf, Debug) << "ETBP: param=" << param; + return true; + } + } + } + + return false; +} + +double Af::findPeak(unsigned i) const +{ + double f = scanData_[i].focus; + + if (i > 0 && i + 1 < scanData_.size()) { + double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast; + double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast; + if (0.0 <= dropLo && dropLo < dropHi) { + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); + f += param * (scanData_[i - 1].focus - f); + } else if (0.0 <= dropHi && dropHi < dropLo) { + double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo); + f += param * (scanData_[i + 1].focus - f); + } + } + + LOG(RPiAf, Debug) << "FindPeak: " << f; + return f; +} + +void Af::doScan(double contrast, double phase, double conf) +{ + /* Record lens position, contrast and phase values for the current scan */ + if (scanData_.empty() || contrast > scanMaxContrast_) { + scanMaxContrast_ = contrast; + scanMaxIndex_ = scanData_.size(); + } + if (contrast < scanMinContrast_) + scanMinContrast_ = contrast; + scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf }); + + if (scanState_ == ScanState::Coarse) { + if (ftarget_ >= cfg_.ranges[range_].focusMax || + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { + /* + * Finished course scan, or termination based on contrast. + * Jump to just after max contrast and start fine scan. + */ + ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) + + 2.0 * cfg_.speeds[speed_].stepFine); + scanState_ = ScanState::Fine; + scanData_.clear(); + } else + ftarget_ += cfg_.speeds[speed_].stepCoarse; + } else { /* ScanState::Fine */ + if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 || + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { + /* + * Finished fine scan, or termination based on contrast. + * Use quadratic peak-finding to find best contrast position. + */ + ftarget_ = findPeak(scanMaxIndex_); + scanState_ = ScanState::Settle; + } else + ftarget_ -= cfg_.speeds[speed_].stepFine; + } + + stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames; +} + +void Af::doAF(double contrast, double phase, double conf) +{ + /* Skip frames at startup and after sensor mode change */ + if (skipCount_ > 0) { + LOG(RPiAf, Debug) << "SKIP"; + skipCount_--; + return; + } + + if (scanState_ == ScanState::Pdaf) { + /* + * Use PDAF closed-loop control whenever available, in both CAF + * mode and (for a limited number of iterations) when triggered. + * If PDAF fails (due to poor contrast, noise or large defocus), + * fall back to a CDAF-based scan. To avoid "nuisance" scans, + * scan only after a number of frames with low PDAF confidence. + */ + if (conf > (dropCount_ ? 1.0 : 0.25) * cfg_.confEpsilon) { + doPDAF(phase, conf); + if (stepCount_ > 0) + stepCount_--; + else if (mode_ != AfModeContinuous) + scanState_ = ScanState::Idle; + dropCount_ = 0; + } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames) + startProgrammedScan(); + } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) { + /* + * Scanning sequence. This means PDAF has become unavailable. + * Allow a delay between steps for CDAF FoM statistics to be + * updated, and a "settling time" at the end of the sequence. + * [A coarse or fine scan can be abandoned if two PDAF samples + * allow direct interpolation of the zero-phase lens position.] + */ + if (stepCount_ > 0) + stepCount_--; + else if (scanState_ == ScanState::Settle) { + if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ && + scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) + reportState_ = AfState::Focused; + else + reportState_ = AfState::Failed; + if (mode_ == AfModeContinuous && !pauseFlag_ && + cfg_.speeds[speed_].dropoutFrames > 0) + scanState_ = ScanState::Pdaf; + else + scanState_ = ScanState::Idle; + scanData_.clear(); + } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) { + scanState_ = ScanState::Settle; + stepCount_ = (mode_ == AfModeContinuous) ? 0 + : cfg_.speeds[speed_].stepFrames; + } else + doScan(contrast, phase, conf); + } +} + +void Af::updateLensPosition() +{ + if (scanState_ >= ScanState::Pdaf) { + ftarget_ = std::clamp(ftarget_, + cfg_.ranges[range_].focusMin, + cfg_.ranges[range_].focusMax); + } + + if (initted_) { + /* from a known lens position: apply slew rate limit */ + fsmooth_ = std::clamp(ftarget_, + fsmooth_ - cfg_.speeds[speed_].maxSlew, + fsmooth_ + cfg_.speeds[speed_].maxSlew); + } else { + /* from an unknown position: go straight to target, but add delay */ + fsmooth_ = ftarget_; + initted_ = true; + skipCount_ = cfg_.skipFrames; + } +} + +void Af::startAF() +{ + /* Use PDAF if the tuning file allows it; else CDAF. */ + if (cfg_.speeds[speed_].dropoutFrames > 0 && + (mode_ == AfModeContinuous || cfg_.speeds[speed_].pdafFrames > 0)) { + if (!initted_) { + ftarget_ = cfg_.ranges[range_].focusDefault; + updateLensPosition(); + } + stepCount_ = (mode_ == AfModeContinuous) ? 0 : cfg_.speeds[speed_].pdafFrames; + scanState_ = ScanState::Pdaf; + scanData_.clear(); + dropCount_ = 0; + reportState_ = AfState::Scanning; + } else + startProgrammedScan(); +} + +void Af::startProgrammedScan() +{ + ftarget_ = cfg_.ranges[range_].focusMin; + updateLensPosition(); + scanState_ = ScanState::Coarse; + scanMaxContrast_ = 0.0; + scanMinContrast_ = 1.0e9; + scanMaxIndex_ = 0; + scanData_.clear(); + stepCount_ = cfg_.speeds[speed_].stepFrames; + reportState_ = AfState::Scanning; +} + +void Af::goIdle() +{ + scanState_ = ScanState::Idle; + reportState_ = AfState::Idle; + scanData_.clear(); +} + +/* + * PDAF phase data are available in prepare(), but CDAF statistics are not + * available until process(). We are gambling on the availability of PDAF. + * To expedite feedback control using PDAF, issue the V4L2 lens control from + * prepare(). Conversely, during scans, we must allow an extra frame delay + * between steps, to retrieve CDAF statistics from the previous process() + * so we can terminate the scan early without having to change our minds. + */ + +void Af::prepare(Metadata *imageMetadata) +{ + /* Initialize for triggered scan or start of CAF mode */ + if (scanState_ == ScanState::Trigger) + startAF(); + + if (initted_) { + /* Get PDAF from the embedded metadata, and run AF algorithm core */ + 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(reportState_) + << " sst" << static_cast(oldSs) + << "->" << static_cast(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(cfg_.map.eval(fsmooth_)) + : std::nullopt; + imageMetadata->set("af.status", status); +} + +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) +{ + (void)imageMetadata; + prevContrast_ = getContrast(stats->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) +{ + if (useWindows_ != mode) { + useWindows_ = mode; + computeWeights(); + } +} + +void Af::setWindows(libcamera::Span const &wins) +{ + windows_.clear(); + for (auto &w : wins) { + LOG(RPiAf, Debug) << "Window: " + << w.x << ", " + << w.y << ", " + << w.width << ", " + << w.height; + windows_.push_back(w); + if (windows_.size() >= MaxWindows) + break; + } + computeWeights(); +} + +bool Af::setLensPosition(double dioptres, int *hwpos) +{ + bool changed = false; + + if (mode_ == AfModeManual) { + LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; + ftarget_ = cfg_.map.domain().clip(dioptres); + changed = !(initted_ && fsmooth_ == ftarget_); + updateLensPosition(); + } + + if (hwpos) + *hwpos = cfg_.map.eval(fsmooth_); + + return changed; +} + +std::optional Af::getLensPosition() const +{ + /* + * \todo We ought to perform some precise timing here to determine + * the current lens position. + */ + return initted_ ? std::optional(fsmooth_) : std::nullopt; +} + +void Af::cancelScan() +{ + LOG(RPiAf, Debug) << "cancelScan"; + if (mode_ == AfModeAuto) + goIdle(); +} + +void Af::triggerScan() +{ + LOG(RPiAf, Debug) << "triggerScan"; + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) + scanState_ = ScanState::Trigger; +} + +void Af::setMode(AfAlgorithm::AfMode mode) +{ + LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode; + if (mode_ != mode) { + mode_ = mode; + pauseFlag_ = false; + if (mode == AfModeContinuous) + scanState_ = ScanState::Trigger; + else if (mode != AfModeAuto || scanState_ < ScanState::Coarse) + goIdle(); + } +} + +AfAlgorithm::AfMode Af::getMode() const +{ + return mode_; +} + +void Af::pause(AfAlgorithm::AfPause pause) +{ + LOG(RPiAf, Debug) << "pause: " << (unsigned)pause; + if (mode_ == AfModeContinuous) { + if (pause == AfPauseResume && pauseFlag_) { + pauseFlag_ = false; + if (scanState_ < ScanState::Coarse) + scanState_ = ScanState::Trigger; + } else if (pause != AfPauseResume && !pauseFlag_) { + pauseFlag_ = true; + if (pause == AfPauseImmediate || scanState_ < ScanState::Coarse) + goIdle(); + } + } +} + +// Register algorithm with the system. +static Algorithm *create(Controller *controller) +{ + return (Algorithm *)new Af(controller); +} +static RegisterAlgorithm reg(NAME, &create); diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h new file mode 100644 index 00000000..f7baf897 --- /dev/null +++ b/src/ipa/raspberrypi/controller/rpi/af.h @@ -0,0 +1,156 @@ +/* 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 const &wins) override; + void setMode(AfMode mode) override; + AfMode getMode() const override; + bool setLensPosition(double dioptres, int32_t *hwpos) override; + std::optional getLensPosition() const override; + void triggerScan() override; + void cancelScan() override; + void pause(AfPause pause) override; + +private: + enum class ScanState { + Idle = 0, + Trigger, + 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; + }; + + void computeWeights(); + 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); + double findPeak(unsigned index) const; + void doScan(double contrast, double phase, double conf); + void doAF(double contrast, double phase, double conf); + void updateLensPosition(); + void startAF(); + void startProgrammedScan(); + void goIdle(); + + /* Configuration and settings */ + CfgParams cfg_; + AfRange range_; + AfSpeed speed_; + AfMode mode_; + bool pauseFlag_; + libcamera::Rectangle statsRegion_; + std::vector windows_; + bool useWindows_; + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS]; + uint16_t contrastWeights_[FOCUS_REGIONS]; + uint32_t sumWeights_; + + /* Working state. */ + ScanState scanState_; + bool initted_; + double ftarget_, fsmooth_; + double prevContrast_; + unsigned skipCount_, stepCount_, dropCount_; + unsigned scanMaxIndex_; + double scanMaxContrast_, scanMinContrast_; + std::vector scanData_; + AfState reportState_; +}; + +} // namespace RPiController diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build index 517d815b..4e268953 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', -- cgit v1.2.1