[{"id":26260,"web_url":"https://patchwork.libcamera.org/comment/26260/","msgid":"<167413321536.42371.10972820555212386853@Monstersaurus>","date":"2023-01-19T13:00:15","subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","submitter":{"id":4,"url":"https://patchwork.libcamera.org/api/people/4/","name":"Kieran Bingham","email":"kieran.bingham@ideasonboard.com"},"content":"Hi Nick, Naush,\n\nI was soo close to merging this series in one... but alas there's a minor\ncompiler issue in this one.\n\n\nQuoting Naushir Patuck via libcamera-devel (2023-01-19 10:45:41)\n> From: Nick Hollinghurst \n> \n> Provide the first version of the Raspberry Pi autofocus algorithm. This\n> implementation uses a hybrid of contrast detect autofocus (CDAF) and phase\n> detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to\n> having less \"hunting\" behavior.\n> \n> Signed-off-by: Nick Hollinghurst \n> Signed-off-by: Naushir Patuck \n> Reviewed-by: Naushir Patuck \n> Reviewed-by: David Plowman \n> ---\n> src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++\n> src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++\n> src/ipa/raspberrypi/meson.build | 1 +\n> 3 files changed, 909 insertions(+)\n> create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp\n> create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h\n> \n> diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> new file mode 100644\n> index 000000000000..7e2e8961085a\n> --- /dev/null\n> +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> @@ -0,0 +1,755 @@\n> +/* SPDX-License-Identifier: BSD-2-Clause */\n> +/*\n> + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> + *\n> + * af.cpp - Autofocus control algorithm\n> + */\n> +\n> +#include \"af.h\"\n> +\n> +#include \n> +#include \n> +#include \n> +\n> +#include \n> +\n> +#include \n> +\n> +using namespace RPiController;\n> +using namespace libcamera;\n> +\n> +LOG_DEFINE_CATEGORY(RPiAf)\n> +\n> +#define NAME \"rpi.af\"\n> +\n> +/*\n> + * Default values for parameters. All may be overridden in the tuning file.\n> + * Many of these values are sensor- or module-dependent; the defaults here\n> + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.\n> + *\n> + * Here all focus values are in dioptres (1/m). They are converted to hardware\n> + * units when written to status.lensSetting or returned from setLensPosition().\n> + *\n> + * Gain and delay values are relative to the update rate, since much (not all)\n> + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;\n> + * but note that algorithms are updated at no more than 30 Hz.\n> + */\n> +\n> +Af::RangeDependentParams::RangeDependentParams()\n> + : focusMin(0.0),\n> + focusMax(12.0),\n> + focusDefault(1.0)\n> +{\n> +}\n> +\n> +Af::SpeedDependentParams::SpeedDependentParams()\n> + : stepCoarse(1.0),\n> + stepFine(0.25),\n> + contrastRatio(0.75),\n> + pdafGain(-0.02),\n> + pdafSquelch(0.125),\n> + maxSlew(2.0),\n> + pdafFrames(20),\n> + dropoutFrames(6),\n> + stepFrames(4)\n> +{\n> +}\n> +\n> +Af::CfgParams::CfgParams()\n> + : confEpsilon(8),\n> + confThresh(16),\n> + confClip(512),\n> + skipFrames(5),\n> + map()\n> +{\n> +}\n> +\n> +template\n> +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name)\n> +{\n> + auto value = params[name].get();\n> + if (value)\n> + dest = *value;\n> + else\n> + LOG(RPiAf, Warning) << \"Missing parameter \\\"\" << name << \"\\\"\";\n> +}\n> +\n> +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> +\n> + readNumber(focusMin, params, \"min\");\n> + readNumber(focusMax, params, \"max\");\n> + readNumber(focusDefault, params, \"default\");\n> +}\n> +\n> +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> + readNumber(stepCoarse, params, \"step_coarse\");\n> + readNumber(stepFine, params, \"step_fine\");\n> + readNumber(contrastRatio, params, \"contrast_ratio\");\n> + readNumber(pdafGain, params, \"pdaf_gain\");\n> + readNumber(pdafSquelch, params, \"pdaf_squelch\");\n> + readNumber(maxSlew, params, \"max_slew\");\n> + readNumber(pdafFrames, params, \"pdaf_frames\");\n> + readNumber(dropoutFrames, params, \"dropout_frames\");\n> + readNumber(stepFrames, params, \"step_frames\");\n> +}\n> +\n> +int Af::CfgParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> + if (params.contains(\"ranges\")) {\n> + auto &rr = params[\"ranges\"];\n> +\n> + if (rr.contains(\"normal\"))\n> + ranges[AfRangeNormal].read(rr[\"normal\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"Missing range \\\"normal\\\"\";\n> +\n> + ranges[AfRangeMacro] = ranges[AfRangeNormal];\n> + if (rr.contains(\"macro\"))\n> + ranges[AfRangeMacro].read(rr[\"macro\"]);\n> +\n> + ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,\n> + ranges[AfRangeMacro].focusMin);\n> + ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,\n> + ranges[AfRangeMacro].focusMax);\n> + ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;\n> + if (rr.contains(\"full\"))\n> + ranges[AfRangeFull].read(rr[\"full\"]);\n> + } else\n> + LOG(RPiAf, Warning) << \"No ranges defined\";\n> +\n> + if (params.contains(\"speeds\")) {\n> + auto &ss = params[\"speeds\"];\n> +\n> + if (ss.contains(\"normal\"))\n> + speeds[AfSpeedNormal].read(ss[\"normal\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"Missing speed \\\"normal\\\"\";\n> +\n> + speeds[AfSpeedFast] = speeds[AfSpeedNormal];\n> + if (ss.contains(\"fast\"))\n> + speeds[AfSpeedFast].read(ss[\"fast\"]);\n> + } else\n> + LOG(RPiAf, Warning) << \"No speeds defined\";\n> +\n> + readNumber(confEpsilon, params, \"conf_epsilon\");\n> + readNumber(confThresh, params, \"conf_thresh\");\n> + readNumber(confClip, params, \"conf_clip\");\n> + readNumber(skipFrames, params, \"skip_frames\");\n> +\n> + if (params.contains(\"map\"))\n> + map.read(params[\"map\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"No map defined\";\n> +\n> + return 0;\n> +}\n> +\n> +void Af::CfgParams::initialise()\n> +{\n> + if (map.empty()) {\n> + /* Default mapping from dioptres to hardware setting */\n> + static constexpr double DefaultMapX0 = 0.0;\n> + static constexpr double DefaultMapY0 = 445.0;\n> + static constexpr double DefaultMapX1 = 15.0;\n> + static constexpr double DefaultMapY1 = 925.0;\n> +\n> + map.append(DefaultMapX0, DefaultMapY0);\n> + map.append(DefaultMapX1, DefaultMapY1);\n> +\n> + LOG(RPiAf, Warning) << \"af.map is not defined, \";\n> + }\n> +}\n> +\n> +/* Af Algorithm class */\n> +\n> +Af::Af(Controller *controller)\n> + : AfAlgorithm(controller),\n> + cfg_(),\n> + range_(AfRangeNormal),\n> + speed_(AfSpeedNormal),\n> + mode_(AfAlgorithm::AfModeManual),\n> + pauseFlag_(false),\n> + sensorSize_{ 0, 0 },\n> + useWeights_(false),\n> + phaseWeights_{},\n> + contrastWeights_{},\n> + scanState_(ScanState::Idle),\n> + initted_(false),\n> + ftarget_(-1.0),\n> + fsmooth_(-1.0),\n> + prevContrast_(0.0),\n> + skipCount_(0),\n> + stepCount_(0),\n> + dropCount_(0),\n> + scanMaxContrast_(0.0),\n> + scanMinContrast_(1.0e9),\n> + scanData_(),\n> + reportState_(AfState::Idle)\n> +{\n> + scanData_.reserve(24);\n> +}\n> +\n> +Af::~Af()\n> +{\n> +}\n> +\n> +char const *Af::name() const\n> +{\n> + return NAME;\n> +}\n> +\n> +int Af::read(const libcamera::YamlObject ¶ms)\n> +{\n> + return cfg_.read(params);\n> +}\n> +\n> +void Af::initialise()\n> +{\n> + cfg_.initialise();\n> +}\n> +\n> +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)\n> +{\n> + (void)metadata;\n> + sensorSize_.width = cameraMode.sensorWidth;\n> + sensorSize_.height = cameraMode.sensorHeight;\n> +\n> + if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {\n> + /*\n> + * If a scan was in progress, re-start it, as CDAF statistics\n> + * may have changed. Though if the application is just about\n> + * to take a still picture, this will not help...\n> + */\n> + startProgrammedScan();\n> + }\n> + skipCount_ = cfg_.skipFrames;\n> +}\n> +\n> +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const\n> +{\n> + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }\n> + };\n> + int32_t sumW = 0;\n> + int32_t sumWc = 0;\n> + int32_t sumWcp = 0;\n> + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;\n> +\n> + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {\n> + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {\n> + if (wgts[i][j]) {\n> + uint32_t c = data.conf[i][j];\n> + if (c >= cfg_.confThresh) {\n> + if (c > cfg_.confClip)\n> + c = cfg_.confClip;\n> + sumWc += wgts[i][j] * (int32_t)c;\n> + c -= (cfg_.confThresh >> 1);\n> + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;\n> + }\n> + sumW += wgts[i][j];\n> + }\n> + }\n> + }\n> +\n> + if (sumWc > 0) {\n> + phase = (double)sumWcp / (double)sumWc;\n> + conf = (double)sumWc / (double)sumW;\n> + return true;\n> + } else {\n> + phase = 0.0;\n> + conf = 0.0;\n> + return false;\n> + }\n> +}\n> +\n> +void Af::doPDAF(double phase, double conf)\n> +{\n> + /* Apply loop gain */\n> + phase *= cfg_.speeds[speed_].pdafGain;\n> +\n> + if (mode_ == AfModeContinuous) {\n> + /*\n> + * PDAF in Continuous mode. Scale down lens movement when\n> + * delta is small or confidence is low, to suppress wobble.\n> + */\n> + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {\n> + double a = phase / cfg_.speeds[speed_].pdafSquelch;\n> + phase *= a * a;\n> + }\n> + phase *= conf / (conf + cfg_.confEpsilon);\n> + } else {\n> + /*\n> + * PDAF in triggered-auto mode. Allow early termination when\n> + * phase delta is small; scale down lens movements towards\n> + * the end of the sequence, to ensure a stable image.\n> + */\n> + if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {\n> + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)\n> + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> + } else\n> + phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;\n> + }\n> +\n> + /* Apply slew rate limit. Report failure if out of bounds. */\n> + if (phase < -cfg_.speeds[speed_].maxSlew) {\n> + phase = -cfg_.speeds[speed_].maxSlew;\n> + reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed\n> + : AfState::Scanning;\n> + } else if (phase > cfg_.speeds[speed_].maxSlew) {\n> + phase = cfg_.speeds[speed_].maxSlew;\n> + reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed\n> + : AfState::Scanning;\n> + } else\n> + reportState_ = AfState::Focused;\n> +\n> + ftarget_ = fsmooth_ + phase;\n> +}\n> +\n> +bool Af::earlyTerminationByPhase(double phase)\n> +{\n> + if (scanData_.size() > 0 &&\n> + scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {\n> + double oldFocus = scanData_[scanData_.size() - 1].focus;\n> + double oldPhase = scanData_[scanData_.size() - 1].phase;\n> +\n> + /*\n> + * Check that the gradient is finite and has the expected sign;\n> + * Interpolate/extrapolate the lens position for zero phase.\n> + * Check that the extrapolation is well-conditioned.\n> + */\n> + if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {\n> + double param = phase / (phase - oldPhase);\n> + if (-3.0 <= param && param <= 3.5) {\n> + ftarget_ += param * (oldFocus - ftarget_);\n> + LOG(RPiAf, Debug) << \"ETBP: param=\" << param;\n> + return true;\n> + }\n> + }\n> + }\n> +\n> + return false;\n> +}\n> +\n> +double Af::findPeak(unsigned i) const\n> +{\n> + double f = scanData_[i].focus;\n> +\n> + if (i > 0 && i + 1 < scanData_.size()) {\n> + double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;\n> + double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;\n> + if (dropLo < dropHi) {\n> + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);\n> + f += param * (scanData_[i - 1].focus - f);\n> + } else if (dropHi < dropLo) {\n> + double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);\n> + f += param * (scanData_[i + 1].focus - f);\n> + }\n> + }\n> +\n> + LOG(RPiAf, Debug) << \"FindPeak: \" << f;\n> + return f;\n> +}\n> +\n> +void Af::doScan(double contrast, double phase, double conf)\n> +{\n> + /* Record lens position, contrast and phase values for the current scan */\n> + if (scanData_.empty() || contrast > scanMaxContrast_) {\n> + scanMaxContrast_ = contrast;\n> + scanMaxIndex_ = scanData_.size();\n> + }\n> + if (contrast < scanMinContrast_)\n> + scanMinContrast_ = contrast;\n> + scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });\n> +\n> + if (scanState_ == ScanState::Coarse) {\n> + if (ftarget_ >= cfg_.ranges[range_].focusMax ||\n> + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> + /*\n> + * Finished course scan, or termination based on contrast.\n> + * Jump to just after max contrast and start fine scan.\n> + */\n> + ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +\n> + 2.0 * cfg_.speeds[speed_].stepFine);\n> + scanState_ = ScanState::Fine;\n> + scanData_.clear();\n> + } else\n> + ftarget_ += cfg_.speeds[speed_].stepCoarse;\n> + } else { /* ScanState::Fine */\n> + if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||\n> + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> + /*\n> + * Finished fine scan, or termination based on contrast.\n> + * Use quadratic peak-finding to find best contrast position.\n> + */\n> + ftarget_ = findPeak(scanMaxIndex_);\n> + scanState_ = ScanState::Settle;\n> + } else\n> + ftarget_ -= cfg_.speeds[speed_].stepFine;\n> + }\n> +\n> + stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;\n> +}\n> +\n> +void Af::doAF(double contrast, double phase, double conf)\n> +{\n> + /* Skip frames at startup and after mode change */\n> + if (skipCount_ > 0) {\n> + LOG(RPiAf, Debug) << \"SKIP\";\n> + skipCount_--;\n> + return;\n> + }\n> +\n> + if (scanState_ == ScanState::Pdaf) {\n> + /*\n> + * Use PDAF closed-loop control whenever available, in both CAF\n> + * mode and (for a limited number of iterations) when triggered.\n> + * If PDAF fails (due to poor contrast, noise or large defocus),\n> + * fall back to a CDAF-based scan. To avoid \"nuisance\" scans,\n> + * scan only after a number of frames with low PDAF confidence.\n> + */\n> + if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {\n> + doPDAF(phase, conf);\n> + if (stepCount_ > 0)\n> + stepCount_--;\n> + else if (mode_ != AfModeContinuous)\n> + scanState_ = ScanState::Idle;\n> + dropCount_ = 0;\n> + } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)\n> + startProgrammedScan();\n> + } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {\n> + /*\n> + * Scanning sequence. This means PDAF has become unavailable.\n> + * Allow a delay between steps for CDAF FoM statistics to be\n> + * updated, and a \"settling time\" at the end of the sequence.\n> + * [A coarse or fine scan can be abandoned if two PDAF samples\n> + * allow direct interpolation of the zero-phase lens position.]\n> + */\n> + if (stepCount_ > 0)\n> + stepCount_--;\n> + else if (scanState_ == ScanState::Settle) {\n> + if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&\n> + scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)\n> + reportState_ = AfState::Focused;\n> + else\n> + reportState_ = AfState::Failed;\n> + if (mode_ == AfModeContinuous && !pauseFlag_ &&\n> + cfg_.speeds[speed_].dropoutFrames > 0)\n> + scanState_ = ScanState::Pdaf;\n> + else\n> + scanState_ = ScanState::Idle;\n> + scanData_.clear();\n> + } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {\n> + scanState_ = ScanState::Settle;\n> + stepCount_ = (mode_ == AfModeContinuous) ? 0\n> + : cfg_.speeds[speed_].stepFrames;\n> + } else\n> + doScan(contrast, phase, conf);\n> + }\n> +}\n> +\n> +void Af::updateLensPosition()\n> +{\n> + if (mode_ != AfModeManual) {\n> + ftarget_ = std::clamp(ftarget_,\n> + cfg_.ranges[range_].focusMin,\n> + cfg_.ranges[range_].focusMax);\n> + }\n> +\n> + /* \\todo Add a clip for manual lens position to be within the PWL limits. */\n> +\n> + if (initted_) {\n> + /* from a known lens position: apply slew rate limit */\n> + fsmooth_ = std::clamp(ftarget_,\n> + fsmooth_ - cfg_.speeds[speed_].maxSlew,\n> + fsmooth_ + cfg_.speeds[speed_].maxSlew);\n> + } else {\n> + /* from an unknown position: go straight to target, but add delay */\n> + fsmooth_ = ftarget_;\n> + initted_ = true;\n> + skipCount_ = cfg_.skipFrames;\n> + }\n> +}\n> +\n> +/*\n> + * PDAF phase data are available in prepare(), but CDAF statistics are not\n> + * available until process(). We are gambling on the availability of PDAF.\n> + * To expedite feedback control using PDAF, issue the V4L2 lens control from\n> + * prepare(). Conversely, during scans, we must allow an extra frame delay\n> + * between steps, to retrieve CDAF statistics from the previous process()\n> + * so we can terminate the scan early without having to change our minds.\n> + */\n> +\n> +void Af::prepare(Metadata *imageMetadata)\n> +{\n> + if (initted_) {\n> + /* Get PDAF from the embedded metadata, and run AF algorithm core */\n> + PdafData data;\n> + double phase = 0.0, conf = 0.0;\n> + double oldFt = ftarget_;\n> + double oldFs = fsmooth_;\n> + ScanState oldSs = scanState_;\n> + uint32_t oldSt = stepCount_;\n> + if (imageMetadata->get(\"pdaf.data\", data) == 0)\n> + getPhase(data, phase, conf);\n> + doAF(prevContrast_, phase, conf);\n> + updateLensPosition();\n> + LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)\n> + << static_cast(reportState_)\n> + << \" sst\" << static_cast(oldSs)\n> + << \"->\" << static_cast(scanState_)\n> + << \" stp\" << oldSt << \"->\" << stepCount_\n> + << \" ft\" << oldFt << \"->\" << ftarget_\n> + << \" fs\" << oldFs << \"->\" << fsmooth_\n> + << \" cont=\" << (int)prevContrast_\n> + << \" phase=\" << (int)phase << \" conf=\" << (int)conf;\n> + }\n> +\n> + /* Report status and produce new lens setting */\n> + AfStatus status;\n> + if (pauseFlag_)\n> + status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused\n> + : AfPauseState::Pausing;\n> + else\n> + status.pauseState = AfPauseState::Running;\n> +\n> + if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)\n> + status.state = AfState::Scanning;\n> + else\n> + status.state = reportState_;\n> + status.lensSetting = initted_ ? std::optional(cfg_.map.eval(fsmooth_))\n> + : std::nullopt;\n> + imageMetadata->set(\"af.status\", status);\n> +}\n> +\n> +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const\n> +{\n> + uint32_t totW = 0, totWc = 0;\n> +\n> + if (useWeights_) {\n> + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {\n> + unsigned w = contrastWeights_[i];\n> + totW += w;\n> + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);\n> + }\n> + }\n> + if (totW == 0) {\n> + totW = 2;\n> + totWc = (focus_stats[5].contrast_val[1][1] >> 10) +\n> + (focus_stats[6].contrast_val[1][1] >> 10);\n> + }\n> +\n> + return (double)totWc / (double)totW;\n> +}\n> +\n> +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)\n> +{\n> + (void)imageMetadata;\n> + prevContrast_ = getContrast(stats->focus_stats);\n> +}\n> +\n> +/* Controls */\n> +\n> +void Af::setRange(AfRange r)\n> +{\n> + LOG(RPiAf, Debug) << \"setRange: \" << (unsigned)r;\n> + if (r < AfAlgorithm::AfRangeMax)\n> + range_ = r;\n> +}\n> +\n> +void Af::setSpeed(AfSpeed s)\n> +{\n> + LOG(RPiAf, Debug) << \"setSpeed: \" << (unsigned)s;\n> + if (s < AfAlgorithm::AfSpeedMax) {\n> + if (scanState_ == ScanState::Pdaf &&\n> + cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)\n> + stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;\n> + speed_ = s;\n> + }\n> +}\n> +\n> +void Af::setMetering(bool mode)\n> +{\n> + useWeights_ = mode;\n> +}\n> +\n> +void Af::setWindows(libcamera::Span const &wins)\n> +{\n> + /*\n> + * Here we just merge all of the given windows, weighted by area.\n> + * If there are more than 15 overlapping windows, overflow can occur.\n> + * TODO: A better approach might be to find the phase in each window\n> + * and choose either the closest or the highest-confidence one?\n> + *\n> + * Using mostly \"int\" arithmetic, because Rectangle has signed x, y\n> + */\n> + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);\n> + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);\n> + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);\n> + int gridA = gridY * gridX;\n> +\n> + for (int i = 0; i < PDAF_DATA_ROWS; ++i)\n> + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);\n> + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);\n> +\n> + for (auto &w : wins) {\n> + for (int i = 0; i < PDAF_DATA_ROWS; ++i) {\n> + int y0 = std::max(gridY * i, w.y);\n> + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));\n> + if (y0 >= y1)\n> + continue;\n> + y1 -= y0;\n> + for (int j = 0; j < PDAF_DATA_COLS; ++j) {\n> + int x0 = std::max(gridX * j, w.x);\n> + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));\n> + if (x0 >= x1)\n> + continue;\n> + int a = y1 * (x1 - x0);\n> + a = (16 * a + gridA - 1) / gridA;\n> + phaseWeights_[i][j] += a;\n> + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;\n> + }\n> + }\n> + }\n> +}\n> +\n> +bool Af::setLensPosition(double dioptres, int *hwpos)\n> +{\n> + bool changed = false;\n> +\n> + if (mode_ == AfModeManual) {\n> + LOG(RPiAf, Debug) << \"setLensPosition: \" << dioptres;\n> + changed = !(initted_ && fsmooth_ == dioptres);\n> + ftarget_ = dioptres;\n> + updateLensPosition();\n> + }\n> +\n> + if (hwpos)\n> + *hwpos = cfg_.map.eval(fsmooth_);\n> +\n> + return changed;\n> +}\n> +\n> +std::optional Af::getLensPosition() const\n> +{\n> + /*\n> + * \\todo We ought to perform some precise timing here to determine\n> + * the current lens position.\n> + */\n> + return initted_ ? std::optional(fsmooth_) : std::nullopt;\n> +}\n> +\n> +void Af::startCAF()\n> +{\n> + /* Try PDAF if the tuning file permits it for CAF; else CDAF */\n> + if (cfg_.speeds[speed_].dropoutFrames > 0) {\n> + if (!initted_) {\n> + ftarget_ = cfg_.ranges[range_].focusDefault;\n> + updateLensPosition();\n> + }\n> + scanState_ = ScanState::Pdaf;\n> + scanData_.clear();\n> + dropCount_ = 0;\n> + reportState_ = AfState::Scanning;\n> + } else {\n> + startProgrammedScan();\n> + }\n> +}\n> +\n> +void Af::startProgrammedScan()\n> +{\n> + ftarget_ = cfg_.ranges[range_].focusMin;\n> + updateLensPosition();\n> + scanState_ = ScanState::Coarse;\n> + scanMaxContrast_ = 0.0;\n> + scanMinContrast_ = 1.0e9;\n> + scanMaxIndex_ = 0;\n> + scanData_.clear();\n> + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> + reportState_ = AfState::Scanning;\n> +}\n> +\n> +void Af::goIdle()\n> +{\n> + scanState_ = ScanState::Idle;\n> + reportState_ = AfState::Idle;\n> + scanData_.clear();\n> +}\n> +\n> +void Af::cancelScan()\n> +{\n> + LOG(RPiAf, Debug) << \"cancelScan\";\n> + if (mode_ == AfModeAuto)\n> + goIdle();\n> +}\n> +\n> +void Af::triggerScan()\n> +{\n> + LOG(RPiAf, Debug) << \"triggerScan\";\n> + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {\n> + /* Try PDAF if the tuning file permits it for Auto; else CDAF */\n> + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {\n> + if (!initted_) {\n> + ftarget_ = cfg_.ranges[range_].focusDefault;\n> + updateLensPosition();\n> + }\n> + stepCount_ = cfg_.speeds[speed_].pdafFrames;\n> + scanState_ = ScanState::Pdaf;\n> + dropCount_ = 0;\n> + } else\n> + startProgrammedScan();\n> + reportState_ = AfState::Scanning;\n> + }\n> +}\n> +\n> +void Af::setMode(AfAlgorithm::AfMode mode)\n> +{\n> + LOG(RPiAf, Debug) << \"setMode: \" << (unsigned)mode;\n> + if (mode_ != mode) {\n> + mode_ = mode;\n> + pauseFlag_ = false;\n> + if (mode == AfModeContinuous)\n> + startCAF();\n> + else if (mode != AfModeAuto)\n> + goIdle();\n> + }\n> +}\n> +\n> +AfAlgorithm::AfMode Af::getMode() const\n> +{\n> + return mode_;\n> +}\n> +\n> +void Af::pause(AfAlgorithm::AfPause pause)\n> +{\n> + LOG(RPiAf, Debug) << \"pause: \" << (unsigned)pause;\n> + if (mode_ == AfModeContinuous) {\n> + if (pause == AfPauseResume && pauseFlag_) {\n> + startCAF();\n> + pauseFlag_ = false;\n> + } else if (pause != AfPauseResume && !pauseFlag_) {\n> + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)\n> + goIdle();\n> + pauseFlag_ = true;\n> + }\n> + }\n> +}\n> +\n> +// Register algorithm with the system.\n> +static Algorithm *create(Controller *controller)\n> +{\n> + return (Algorithm *)new Af(controller);\n> +}\n> +static RegisterAlgorithm reg(NAME, &create);\n> diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h\n> new file mode 100644\n> index 000000000000..0431bd70ae29\n> --- /dev/null\n> +++ b/src/ipa/raspberrypi/controller/rpi/af.h\n> @@ -0,0 +1,153 @@\n> +/* SPDX-License-Identifier: BSD-2-Clause */\n> +/*\n> + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> + *\n> + * af.h - Autofocus control algorithm\n> + */\n> +#pragma once\n> +\n> +#include \"../af_algorithm.h\"\n> +#include \"../af_status.h\"\n> +#include \"../pdaf_data.h\"\n> +#include \"../pwl.h\"\n> +\n> +/*\n> + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.\n> + *\n> + * Whenever PDAF is available, it is used in a continuous feedback loop.\n> + * When triggered in auto mode, we simply enable AF for a limited number\n> + * of frames (it may terminate early if the delta becomes small enough).\n> + *\n> + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)\n> + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.\n> + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to\n> + * estimate the lens position with peak contrast. This is slower due to\n> + * extra latency in the ISP, and requires a settling time between steps.\n> + *\n> + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid\n> + * \"nuisance\" scans. During each interval where PDAF is not working, only\n> + * ONE scan will be performed; CAF cannot track objects using CDAF alone.\n> + *\n> + * This algorithm is unrelated to \"rpi.focus\" which merely reports CDAF FoM.\n> + */\n> +\n> +namespace RPiController {\n> +\n> +class Af : public AfAlgorithm\n> +{\n> +public:\n> + Af(Controller *controller = NULL);\n> + ~Af();\n> + char const *name() const override;\n> + int read(const libcamera::YamlObject ¶ms) override;\n> + void initialise() override;\n> +\n> + /* IPA calls */\n> + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;\n> + void prepare(Metadata *imageMetadata) override;\n> + void process(StatisticsPtr &stats, Metadata *imageMetadata) override;\n> +\n> + /* controls */\n> + void setRange(AfRange range) override;\n> + void setSpeed(AfSpeed speed) override;\n> + void setMetering(bool use_windows) override;\n> + void setWindows(libcamera::Span const &wins) override;\n> + void setMode(AfMode mode) override;\n> + AfMode getMode() const override;\n> + bool setLensPosition(double dioptres, int32_t *hwpos) override;\n> + std::optional getLensPosition() const override;\n> + void triggerScan() override;\n> + void cancelScan() override;\n> + void pause(AfPause pause) override;\n> +\n> +private:\n> + enum class ScanState {\n> + Idle,\n> + Pdaf,\n> + Coarse,\n> + Fine,\n> + Settle\n> + };\n> +\n> + struct RangeDependentParams {\n> + double focusMin; /* lower (far) limit in dipotres */\n> + double focusMax; /* upper (near) limit in dioptres */\n> + double focusDefault; /* default setting (\"hyperfocal\") */\n> +\n> + RangeDependentParams();\n> + void read(const libcamera::YamlObject ¶ms);\n> + };\n> +\n> + struct SpeedDependentParams {\n> + double stepCoarse; /* used for scans */\n> + double stepFine; /* used for scans */\n> + double contrastRatio; /* used for scan termination and reporting */\n> + double pdafGain; /* coefficient for PDAF feedback loop */\n> + double pdafSquelch; /* PDAF stability parameter (device-specific) */\n> + double maxSlew; /* limit for lens movement per frame */\n> + uint32_t pdafFrames; /* number of iterations when triggered */\n> + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */\n> + uint32_t stepFrames; /* frames to skip in between steps of a scan */\n> +\n> + SpeedDependentParams();\n> + void read(const libcamera::YamlObject ¶ms);\n> + };\n> +\n> + struct CfgParams {\n> + RangeDependentParams ranges[AfRangeMax];\n> + SpeedDependentParams speeds[AfSpeedMax];\n> + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */\n> + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */\n> + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */\n> + uint32_t skipFrames; /* frames to skip at start or modeswitch */\n> + Pwl map; /* converts dioptres -> lens driver position */\n> +\n> + CfgParams();\n> + int read(const libcamera::YamlObject ¶ms);\n> + void initialise();\n> + };\n> +\n> + struct ScanRecord {\n> + double focus;\n> + double contrast;\n> + double phase;\n> + double conf;\n> + };\n> +\n> + bool getPhase(PdafData const &data, double &phase, double &conf) const;\n> + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;\n> + void doPDAF(double phase, double conf);\n> + bool earlyTerminationByPhase(double phase);\n> + void doScan(double contrast, double phase, double conf);\n> + double findPeak(unsigned index) const;\n> + void doAF(double contrast, double phase, double conf);\n> + void updateLensPosition();\n> + void startProgrammedScan();\n> + void startCAF();\n> + void goIdle();\n> +\n> + /* Configuration and settings */\n> + CfgParams cfg_;\n> + AfRange range_;\n> + AfSpeed speed_;\n> + AfMode mode_;\n> + bool pauseFlag_;\n> + libcamera::Size sensorSize_;\n> + bool useWeights_;\n> + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];\n> + uint16_t contrastWeights_[FOCUS_REGIONS];\n> +\n> + /* Working state. */\n> + ScanState scanState_;\n> + bool initted_;\n> + double ftarget_, fsmooth_;\n> + double prevContrast_;\n> + bool pdafSeen_;\n\nThis pdafSeen_ variable is unused and gets noticed by Clang.\n\nFAILED: src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o\nclang++-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\nIn file included from ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.cpp:8:\n../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.h:145:7: error: private field 'pdafSeen_' is not used [-Werror,-Wunused-private-field]\n bool pdafSeen_;\n ^\n1 error generated.\n\nAs this is a single line to remove, and I don't think that's\ncontroversial - I think that could be fixed while applying if you're ok\nwith that.\n\n--\nKieran\n\n\n\n> + unsigned skipCount_, stepCount_, dropCount_;\n> + unsigned scanMaxIndex_;\n> + double scanMaxContrast_, scanMinContrast_;\n> + std::vector scanData_;\n> + AfState reportState_;\n> +};\n> +\n> +} // namespace RPiController\n> diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build\n> index 517d815bb98c..4e2689536257 100644\n> --- a/src/ipa/raspberrypi/meson.build\n> +++ b/src/ipa/raspberrypi/meson.build\n> @@ -27,6 +27,7 @@ rpi_ipa_sources = files([\n> 'controller/controller.cpp',\n> 'controller/histogram.cpp',\n> 'controller/algorithm.cpp',\n> + 'controller/rpi/af.cpp',\n> 'controller/rpi/alsc.cpp',\n> 'controller/rpi/awb.cpp',\n> 'controller/rpi/sharpen.cpp',\n> -- \n> 2.25.1\n>","headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id 4A334BD87C\n\tfor ;\n\tThu, 19 Jan 2023 13:00:20 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id AD2BD625D8;\n\tThu, 19 Jan 2023 14:00:19 +0100 (CET)","from perceval.ideasonboard.com (perceval.ideasonboard.com\n\t[213.167.242.64])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 1D1D861EFE\n\tfor ;\n\tThu, 19 Jan 2023 14:00:18 +0100 (CET)","from pendragon.ideasonboard.com\n\t(cpc89244-aztw30-2-0-cust3082.18-1.cable.virginm.net [86.31.172.11])\n\tby perceval.ideasonboard.com (Postfix) with ESMTPSA id 9001A7EC;\n\tThu, 19 Jan 2023 14:00:17 +0100 (CET)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674133219;\n\tbh=G12RDTNg4JY5Ra94uNsol2wAzFya9OWbUltryLOgXAY=;\n\th=In-Reply-To:References:To:Date:Subject:List-Id:List-Unsubscribe:\n\tList-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc:\n\tFrom;\n\tb=D7tsqwLeUZXp+SoxZ30o8WAhKmVO4L6LABlyvOAhRj5OBCbXO2RDMtca8aos3DMwp\n\tPBVVlC7mNv+tkQJbik5OuXyLXa7FBYH1aowU9sNU9yGuVlSqfb43Heb+zBLor+gR3t\n\tNPkTcTkNzonXfNbghOPyr0OEiMNJ17VhQJS3MsmVKu6t4wvlplWvINAI8tZ1Eht+fS\n\t4wKWEGoa3AssKgcbfsV6+dHhxt6lkHuXadW+oJrvimn456ZWbjo8dD+RApJGOx6He1\n\tHnqLL3ufN4lZtYmt8UlXVLrE1p5QBUeEI6EuIt2zdPDOA0MT3+SNXM1uiF3BdEgMDG\n\to2cdcR0w86q/w==","v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com;\n\ts=mail; t=1674133217;\n\tbh=G12RDTNg4JY5Ra94uNsol2wAzFya9OWbUltryLOgXAY=;\n\th=In-Reply-To:References:Subject:From:Cc:To:Date:From;\n\tb=aMz0ZmNIhs4CzpeIJav+cnm5TPjCCfeR8550rPcoG5u9/8ZYAg9LraDnDRLS4msqG\n\tQk5i0lgjBTv1miJCD6M31tIUDAEh+nNMCZUDyXuqJoOZrBMOL0cTkL6Ts0vDMd7Lxj\n\tYHkxXy/DO1z0pBhUzSdq9PfQ2E2QTvYDyeOxlx0c="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (1024-bit key; \n\tunprotected) header.d=ideasonboard.com\n\theader.i=@ideasonboard.com\n\theader.b=\"aMz0ZmNI\"; dkim-atps=neutral","Content-Type":"text/plain; charset=\"utf-8\"","MIME-Version":"1.0","Content-Transfer-Encoding":"quoted-printable","In-Reply-To":"<20230119104544.9456-12-naush@raspberrypi.com>","References":"<20230119104544.9456-1-naush@raspberrypi.com>\n\t<20230119104544.9456-12-naush@raspberrypi.com>","To":"Naushir Patuck ,\n\tlibcamera-devel@lists.libcamera.org","Date":"Thu, 19 Jan 2023 13:00:15 +0000","Message-ID":"<167413321536.42371.10972820555212386853@Monstersaurus>","User-Agent":"alot/0.10","Subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","From":"Kieran Bingham via libcamera-devel\n\t","Reply-To":"Kieran Bingham ","Cc":"Nick Hollinghurst ","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "}},{"id":26262,"web_url":"https://patchwork.libcamera.org/comment/26262/","msgid":"","date":"2023-01-19T13:07:44","subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","submitter":{"id":130,"url":"https://patchwork.libcamera.org/api/people/130/","name":"Nick Hollinghurst","email":"nick.hollinghurst@raspberrypi.com"},"content":"Hi all,\n\nThis patch needs a tweak to handle the case where setWindows is called\nbefore configure -- see Af::Af(Controller *controller) constructor\nbelow.\n\nOn Thu, 19 Jan 2023 at 10:46, Naushir Patuck wrote:\n>\n> From: Nick Hollinghurst \n>\n> Provide the first version of the Raspberry Pi autofocus algorithm. This\n> implementation uses a hybrid of contrast detect autofocus (CDAF) and phase\n> detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to\n> having less \"hunting\" behavior.\n>\n> Signed-off-by: Nick Hollinghurst \n> Signed-off-by: Naushir Patuck \n> Reviewed-by: Naushir Patuck \n> Reviewed-by: David Plowman \n> ---\n> src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++\n> src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++\n> src/ipa/raspberrypi/meson.build | 1 +\n> 3 files changed, 909 insertions(+)\n> create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp\n> create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h\n>\n> diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> new file mode 100644\n> index 000000000000..7e2e8961085a\n> --- /dev/null\n> +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> @@ -0,0 +1,755 @@\n> +/* SPDX-License-Identifier: BSD-2-Clause */\n> +/*\n> + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> + *\n> + * af.cpp - Autofocus control algorithm\n> + */\n> +\n> +#include \"af.h\"\n> +\n> +#include \n> +#include \n> +#include \n> +\n> +#include \n> +\n> +#include \n> +\n> +using namespace RPiController;\n> +using namespace libcamera;\n> +\n> +LOG_DEFINE_CATEGORY(RPiAf)\n> +\n> +#define NAME \"rpi.af\"\n> +\n> +/*\n> + * Default values for parameters. All may be overridden in the tuning file.\n> + * Many of these values are sensor- or module-dependent; the defaults here\n> + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.\n> + *\n> + * Here all focus values are in dioptres (1/m). They are converted to hardware\n> + * units when written to status.lensSetting or returned from setLensPosition().\n> + *\n> + * Gain and delay values are relative to the update rate, since much (not all)\n> + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;\n> + * but note that algorithms are updated at no more than 30 Hz.\n> + */\n> +\n> +Af::RangeDependentParams::RangeDependentParams()\n> + : focusMin(0.0),\n> + focusMax(12.0),\n> + focusDefault(1.0)\n> +{\n> +}\n> +\n> +Af::SpeedDependentParams::SpeedDependentParams()\n> + : stepCoarse(1.0),\n> + stepFine(0.25),\n> + contrastRatio(0.75),\n> + pdafGain(-0.02),\n> + pdafSquelch(0.125),\n> + maxSlew(2.0),\n> + pdafFrames(20),\n> + dropoutFrames(6),\n> + stepFrames(4)\n> +{\n> +}\n> +\n> +Af::CfgParams::CfgParams()\n> + : confEpsilon(8),\n> + confThresh(16),\n> + confClip(512),\n> + skipFrames(5),\n> + map()\n> +{\n> +}\n> +\n> +template\n> +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name)\n> +{\n> + auto value = params[name].get();\n> + if (value)\n> + dest = *value;\n> + else\n> + LOG(RPiAf, Warning) << \"Missing parameter \\\"\" << name << \"\\\"\";\n> +}\n> +\n> +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> +\n> + readNumber(focusMin, params, \"min\");\n> + readNumber(focusMax, params, \"max\");\n> + readNumber(focusDefault, params, \"default\");\n> +}\n> +\n> +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> + readNumber(stepCoarse, params, \"step_coarse\");\n> + readNumber(stepFine, params, \"step_fine\");\n> + readNumber(contrastRatio, params, \"contrast_ratio\");\n> + readNumber(pdafGain, params, \"pdaf_gain\");\n> + readNumber(pdafSquelch, params, \"pdaf_squelch\");\n> + readNumber(maxSlew, params, \"max_slew\");\n> + readNumber(pdafFrames, params, \"pdaf_frames\");\n> + readNumber(dropoutFrames, params, \"dropout_frames\");\n> + readNumber(stepFrames, params, \"step_frames\");\n> +}\n> +\n> +int Af::CfgParams::read(const libcamera::YamlObject ¶ms)\n> +{\n> + if (params.contains(\"ranges\")) {\n> + auto &rr = params[\"ranges\"];\n> +\n> + if (rr.contains(\"normal\"))\n> + ranges[AfRangeNormal].read(rr[\"normal\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"Missing range \\\"normal\\\"\";\n> +\n> + ranges[AfRangeMacro] = ranges[AfRangeNormal];\n> + if (rr.contains(\"macro\"))\n> + ranges[AfRangeMacro].read(rr[\"macro\"]);\n> +\n> + ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,\n> + ranges[AfRangeMacro].focusMin);\n> + ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,\n> + ranges[AfRangeMacro].focusMax);\n> + ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;\n> + if (rr.contains(\"full\"))\n> + ranges[AfRangeFull].read(rr[\"full\"]);\n> + } else\n> + LOG(RPiAf, Warning) << \"No ranges defined\";\n> +\n> + if (params.contains(\"speeds\")) {\n> + auto &ss = params[\"speeds\"];\n> +\n> + if (ss.contains(\"normal\"))\n> + speeds[AfSpeedNormal].read(ss[\"normal\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"Missing speed \\\"normal\\\"\";\n> +\n> + speeds[AfSpeedFast] = speeds[AfSpeedNormal];\n> + if (ss.contains(\"fast\"))\n> + speeds[AfSpeedFast].read(ss[\"fast\"]);\n> + } else\n> + LOG(RPiAf, Warning) << \"No speeds defined\";\n> +\n> + readNumber(confEpsilon, params, \"conf_epsilon\");\n> + readNumber(confThresh, params, \"conf_thresh\");\n> + readNumber(confClip, params, \"conf_clip\");\n> + readNumber(skipFrames, params, \"skip_frames\");\n> +\n> + if (params.contains(\"map\"))\n> + map.read(params[\"map\"]);\n> + else\n> + LOG(RPiAf, Warning) << \"No map defined\";\n> +\n> + return 0;\n> +}\n> +\n> +void Af::CfgParams::initialise()\n> +{\n> + if (map.empty()) {\n> + /* Default mapping from dioptres to hardware setting */\n> + static constexpr double DefaultMapX0 = 0.0;\n> + static constexpr double DefaultMapY0 = 445.0;\n> + static constexpr double DefaultMapX1 = 15.0;\n> + static constexpr double DefaultMapY1 = 925.0;\n> +\n> + map.append(DefaultMapX0, DefaultMapY0);\n> + map.append(DefaultMapX1, DefaultMapY1);\n> +\n> + LOG(RPiAf, Warning) << \"af.map is not defined, \";\n> + }\n> +}\n> +\n> +/* Af Algorithm class */\n> +\n> +Af::Af(Controller *controller)\n> + : AfAlgorithm(controller),\n> + cfg_(),\n> + range_(AfRangeNormal),\n> + speed_(AfSpeedNormal),\n> + mode_(AfAlgorithm::AfModeManual),\n> + pauseFlag_(false),\n> + sensorSize_{ 0, 0 },\n\nsensorSize_ needs to be set before setWindows() is called. For this\nversion it suffices to initialize it to\n\n sensorSize_ { 4608, 2502 },\n\na more general fix may follow later.\n\n\n> + useWeights_(false),\n> + phaseWeights_{},\n> + contrastWeights_{},\n> + scanState_(ScanState::Idle),\n> + initted_(false),\n> + ftarget_(-1.0),\n> + fsmooth_(-1.0),\n> + prevContrast_(0.0),\n> + skipCount_(0),\n> + stepCount_(0),\n> + dropCount_(0),\n> + scanMaxContrast_(0.0),\n> + scanMinContrast_(1.0e9),\n> + scanData_(),\n> + reportState_(AfState::Idle)\n> +{\n> + scanData_.reserve(24);\n> +}\n> +\n> +Af::~Af()\n> +{\n> +}\n> +\n> +char const *Af::name() const\n> +{\n> + return NAME;\n> +}\n> +\n> +int Af::read(const libcamera::YamlObject ¶ms)\n> +{\n> + return cfg_.read(params);\n> +}\n> +\n> +void Af::initialise()\n> +{\n> + cfg_.initialise();\n> +}\n> +\n> +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)\n> +{\n> + (void)metadata;\n> + sensorSize_.width = cameraMode.sensorWidth;\n> + sensorSize_.height = cameraMode.sensorHeight;\n> +\n> + if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {\n> + /*\n> + * If a scan was in progress, re-start it, as CDAF statistics\n> + * may have changed. Though if the application is just about\n> + * to take a still picture, this will not help...\n> + */\n> + startProgrammedScan();\n> + }\n> + skipCount_ = cfg_.skipFrames;\n> +}\n> +\n> +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const\n> +{\n> + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }\n> + };\n> + int32_t sumW = 0;\n> + int32_t sumWc = 0;\n> + int32_t sumWcp = 0;\n> + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;\n> +\n> + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {\n> + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {\n> + if (wgts[i][j]) {\n> + uint32_t c = data.conf[i][j];\n> + if (c >= cfg_.confThresh) {\n> + if (c > cfg_.confClip)\n> + c = cfg_.confClip;\n> + sumWc += wgts[i][j] * (int32_t)c;\n> + c -= (cfg_.confThresh >> 1);\n> + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;\n> + }\n> + sumW += wgts[i][j];\n> + }\n> + }\n> + }\n> +\n> + if (sumWc > 0) {\n> + phase = (double)sumWcp / (double)sumWc;\n> + conf = (double)sumWc / (double)sumW;\n> + return true;\n> + } else {\n> + phase = 0.0;\n> + conf = 0.0;\n> + return false;\n> + }\n> +}\n> +\n> +void Af::doPDAF(double phase, double conf)\n> +{\n> + /* Apply loop gain */\n> + phase *= cfg_.speeds[speed_].pdafGain;\n> +\n> + if (mode_ == AfModeContinuous) {\n> + /*\n> + * PDAF in Continuous mode. Scale down lens movement when\n> + * delta is small or confidence is low, to suppress wobble.\n> + */\n> + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {\n> + double a = phase / cfg_.speeds[speed_].pdafSquelch;\n> + phase *= a * a;\n> + }\n> + phase *= conf / (conf + cfg_.confEpsilon);\n> + } else {\n> + /*\n> + * PDAF in triggered-auto mode. Allow early termination when\n> + * phase delta is small; scale down lens movements towards\n> + * the end of the sequence, to ensure a stable image.\n> + */\n> + if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {\n> + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)\n> + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> + } else\n> + phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;\n> + }\n> +\n> + /* Apply slew rate limit. Report failure if out of bounds. */\n> + if (phase < -cfg_.speeds[speed_].maxSlew) {\n> + phase = -cfg_.speeds[speed_].maxSlew;\n> + reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed\n> + : AfState::Scanning;\n> + } else if (phase > cfg_.speeds[speed_].maxSlew) {\n> + phase = cfg_.speeds[speed_].maxSlew;\n> + reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed\n> + : AfState::Scanning;\n> + } else\n> + reportState_ = AfState::Focused;\n> +\n> + ftarget_ = fsmooth_ + phase;\n> +}\n> +\n> +bool Af::earlyTerminationByPhase(double phase)\n> +{\n> + if (scanData_.size() > 0 &&\n> + scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {\n> + double oldFocus = scanData_[scanData_.size() - 1].focus;\n> + double oldPhase = scanData_[scanData_.size() - 1].phase;\n> +\n> + /*\n> + * Check that the gradient is finite and has the expected sign;\n> + * Interpolate/extrapolate the lens position for zero phase.\n> + * Check that the extrapolation is well-conditioned.\n> + */\n> + if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {\n> + double param = phase / (phase - oldPhase);\n> + if (-3.0 <= param && param <= 3.5) {\n> + ftarget_ += param * (oldFocus - ftarget_);\n> + LOG(RPiAf, Debug) << \"ETBP: param=\" << param;\n> + return true;\n> + }\n> + }\n> + }\n> +\n> + return false;\n> +}\n> +\n> +double Af::findPeak(unsigned i) const\n> +{\n> + double f = scanData_[i].focus;\n> +\n> + if (i > 0 && i + 1 < scanData_.size()) {\n> + double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;\n> + double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;\n> + if (dropLo < dropHi) {\n> + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);\n> + f += param * (scanData_[i - 1].focus - f);\n> + } else if (dropHi < dropLo) {\n> + double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);\n> + f += param * (scanData_[i + 1].focus - f);\n> + }\n> + }\n> +\n> + LOG(RPiAf, Debug) << \"FindPeak: \" << f;\n> + return f;\n> +}\n> +\n> +void Af::doScan(double contrast, double phase, double conf)\n> +{\n> + /* Record lens position, contrast and phase values for the current scan */\n> + if (scanData_.empty() || contrast > scanMaxContrast_) {\n> + scanMaxContrast_ = contrast;\n> + scanMaxIndex_ = scanData_.size();\n> + }\n> + if (contrast < scanMinContrast_)\n> + scanMinContrast_ = contrast;\n> + scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });\n> +\n> + if (scanState_ == ScanState::Coarse) {\n> + if (ftarget_ >= cfg_.ranges[range_].focusMax ||\n> + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> + /*\n> + * Finished course scan, or termination based on contrast.\n> + * Jump to just after max contrast and start fine scan.\n> + */\n> + ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +\n> + 2.0 * cfg_.speeds[speed_].stepFine);\n> + scanState_ = ScanState::Fine;\n> + scanData_.clear();\n> + } else\n> + ftarget_ += cfg_.speeds[speed_].stepCoarse;\n> + } else { /* ScanState::Fine */\n> + if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||\n> + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> + /*\n> + * Finished fine scan, or termination based on contrast.\n> + * Use quadratic peak-finding to find best contrast position.\n> + */\n> + ftarget_ = findPeak(scanMaxIndex_);\n> + scanState_ = ScanState::Settle;\n> + } else\n> + ftarget_ -= cfg_.speeds[speed_].stepFine;\n> + }\n> +\n> + stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;\n> +}\n> +\n> +void Af::doAF(double contrast, double phase, double conf)\n> +{\n> + /* Skip frames at startup and after mode change */\n> + if (skipCount_ > 0) {\n> + LOG(RPiAf, Debug) << \"SKIP\";\n> + skipCount_--;\n> + return;\n> + }\n> +\n> + if (scanState_ == ScanState::Pdaf) {\n> + /*\n> + * Use PDAF closed-loop control whenever available, in both CAF\n> + * mode and (for a limited number of iterations) when triggered.\n> + * If PDAF fails (due to poor contrast, noise or large defocus),\n> + * fall back to a CDAF-based scan. To avoid \"nuisance\" scans,\n> + * scan only after a number of frames with low PDAF confidence.\n> + */\n> + if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {\n> + doPDAF(phase, conf);\n> + if (stepCount_ > 0)\n> + stepCount_--;\n> + else if (mode_ != AfModeContinuous)\n> + scanState_ = ScanState::Idle;\n> + dropCount_ = 0;\n> + } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)\n> + startProgrammedScan();\n> + } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {\n> + /*\n> + * Scanning sequence. This means PDAF has become unavailable.\n> + * Allow a delay between steps for CDAF FoM statistics to be\n> + * updated, and a \"settling time\" at the end of the sequence.\n> + * [A coarse or fine scan can be abandoned if two PDAF samples\n> + * allow direct interpolation of the zero-phase lens position.]\n> + */\n> + if (stepCount_ > 0)\n> + stepCount_--;\n> + else if (scanState_ == ScanState::Settle) {\n> + if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&\n> + scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)\n> + reportState_ = AfState::Focused;\n> + else\n> + reportState_ = AfState::Failed;\n> + if (mode_ == AfModeContinuous && !pauseFlag_ &&\n> + cfg_.speeds[speed_].dropoutFrames > 0)\n> + scanState_ = ScanState::Pdaf;\n> + else\n> + scanState_ = ScanState::Idle;\n> + scanData_.clear();\n> + } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {\n> + scanState_ = ScanState::Settle;\n> + stepCount_ = (mode_ == AfModeContinuous) ? 0\n> + : cfg_.speeds[speed_].stepFrames;\n> + } else\n> + doScan(contrast, phase, conf);\n> + }\n> +}\n> +\n> +void Af::updateLensPosition()\n> +{\n> + if (mode_ != AfModeManual) {\n> + ftarget_ = std::clamp(ftarget_,\n> + cfg_.ranges[range_].focusMin,\n> + cfg_.ranges[range_].focusMax);\n> + }\n> +\n> + /* \\todo Add a clip for manual lens position to be within the PWL limits. */\n> +\n> + if (initted_) {\n> + /* from a known lens position: apply slew rate limit */\n> + fsmooth_ = std::clamp(ftarget_,\n> + fsmooth_ - cfg_.speeds[speed_].maxSlew,\n> + fsmooth_ + cfg_.speeds[speed_].maxSlew);\n> + } else {\n> + /* from an unknown position: go straight to target, but add delay */\n> + fsmooth_ = ftarget_;\n> + initted_ = true;\n> + skipCount_ = cfg_.skipFrames;\n> + }\n> +}\n> +\n> +/*\n> + * PDAF phase data are available in prepare(), but CDAF statistics are not\n> + * available until process(). We are gambling on the availability of PDAF.\n> + * To expedite feedback control using PDAF, issue the V4L2 lens control from\n> + * prepare(). Conversely, during scans, we must allow an extra frame delay\n> + * between steps, to retrieve CDAF statistics from the previous process()\n> + * so we can terminate the scan early without having to change our minds.\n> + */\n> +\n> +void Af::prepare(Metadata *imageMetadata)\n> +{\n> + if (initted_) {\n> + /* Get PDAF from the embedded metadata, and run AF algorithm core */\n> + PdafData data;\n> + double phase = 0.0, conf = 0.0;\n> + double oldFt = ftarget_;\n> + double oldFs = fsmooth_;\n> + ScanState oldSs = scanState_;\n> + uint32_t oldSt = stepCount_;\n> + if (imageMetadata->get(\"pdaf.data\", data) == 0)\n> + getPhase(data, phase, conf);\n> + doAF(prevContrast_, phase, conf);\n> + updateLensPosition();\n> + LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)\n> + << static_cast(reportState_)\n> + << \" sst\" << static_cast(oldSs)\n> + << \"->\" << static_cast(scanState_)\n> + << \" stp\" << oldSt << \"->\" << stepCount_\n> + << \" ft\" << oldFt << \"->\" << ftarget_\n> + << \" fs\" << oldFs << \"->\" << fsmooth_\n> + << \" cont=\" << (int)prevContrast_\n> + << \" phase=\" << (int)phase << \" conf=\" << (int)conf;\n> + }\n> +\n> + /* Report status and produce new lens setting */\n> + AfStatus status;\n> + if (pauseFlag_)\n> + status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused\n> + : AfPauseState::Pausing;\n> + else\n> + status.pauseState = AfPauseState::Running;\n> +\n> + if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)\n> + status.state = AfState::Scanning;\n> + else\n> + status.state = reportState_;\n> + status.lensSetting = initted_ ? std::optional(cfg_.map.eval(fsmooth_))\n> + : std::nullopt;\n> + imageMetadata->set(\"af.status\", status);\n> +}\n> +\n> +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const\n> +{\n> + uint32_t totW = 0, totWc = 0;\n> +\n> + if (useWeights_) {\n> + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {\n> + unsigned w = contrastWeights_[i];\n> + totW += w;\n> + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);\n> + }\n> + }\n> + if (totW == 0) {\n> + totW = 2;\n> + totWc = (focus_stats[5].contrast_val[1][1] >> 10) +\n> + (focus_stats[6].contrast_val[1][1] >> 10);\n> + }\n> +\n> + return (double)totWc / (double)totW;\n> +}\n> +\n> +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)\n> +{\n> + (void)imageMetadata;\n> + prevContrast_ = getContrast(stats->focus_stats);\n> +}\n> +\n> +/* Controls */\n> +\n> +void Af::setRange(AfRange r)\n> +{\n> + LOG(RPiAf, Debug) << \"setRange: \" << (unsigned)r;\n> + if (r < AfAlgorithm::AfRangeMax)\n> + range_ = r;\n> +}\n> +\n> +void Af::setSpeed(AfSpeed s)\n> +{\n> + LOG(RPiAf, Debug) << \"setSpeed: \" << (unsigned)s;\n> + if (s < AfAlgorithm::AfSpeedMax) {\n> + if (scanState_ == ScanState::Pdaf &&\n> + cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)\n> + stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;\n> + speed_ = s;\n> + }\n> +}\n> +\n> +void Af::setMetering(bool mode)\n> +{\n> + useWeights_ = mode;\n> +}\n> +\n> +void Af::setWindows(libcamera::Span const &wins)\n> +{\n> + /*\n> + * Here we just merge all of the given windows, weighted by area.\n> + * If there are more than 15 overlapping windows, overflow can occur.\n> + * TODO: A better approach might be to find the phase in each window\n> + * and choose either the closest or the highest-confidence one?\n> + *\n> + * Using mostly \"int\" arithmetic, because Rectangle has signed x, y\n> + */\n> + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);\n> + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);\n> + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);\n> + int gridA = gridY * gridX;\n> +\n> + for (int i = 0; i < PDAF_DATA_ROWS; ++i)\n> + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);\n> + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);\n> +\n> + for (auto &w : wins) {\n> + for (int i = 0; i < PDAF_DATA_ROWS; ++i) {\n> + int y0 = std::max(gridY * i, w.y);\n> + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));\n> + if (y0 >= y1)\n> + continue;\n> + y1 -= y0;\n> + for (int j = 0; j < PDAF_DATA_COLS; ++j) {\n> + int x0 = std::max(gridX * j, w.x);\n> + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));\n> + if (x0 >= x1)\n> + continue;\n> + int a = y1 * (x1 - x0);\n> + a = (16 * a + gridA - 1) / gridA;\n> + phaseWeights_[i][j] += a;\n> + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;\n> + }\n> + }\n> + }\n> +}\n> +\n> +bool Af::setLensPosition(double dioptres, int *hwpos)\n> +{\n> + bool changed = false;\n> +\n> + if (mode_ == AfModeManual) {\n> + LOG(RPiAf, Debug) << \"setLensPosition: \" << dioptres;\n> + changed = !(initted_ && fsmooth_ == dioptres);\n> + ftarget_ = dioptres;\n> + updateLensPosition();\n> + }\n> +\n> + if (hwpos)\n> + *hwpos = cfg_.map.eval(fsmooth_);\n> +\n> + return changed;\n> +}\n> +\n> +std::optional Af::getLensPosition() const\n> +{\n> + /*\n> + * \\todo We ought to perform some precise timing here to determine\n> + * the current lens position.\n> + */\n> + return initted_ ? std::optional(fsmooth_) : std::nullopt;\n> +}\n> +\n> +void Af::startCAF()\n> +{\n> + /* Try PDAF if the tuning file permits it for CAF; else CDAF */\n> + if (cfg_.speeds[speed_].dropoutFrames > 0) {\n> + if (!initted_) {\n> + ftarget_ = cfg_.ranges[range_].focusDefault;\n> + updateLensPosition();\n> + }\n> + scanState_ = ScanState::Pdaf;\n> + scanData_.clear();\n> + dropCount_ = 0;\n> + reportState_ = AfState::Scanning;\n> + } else {\n> + startProgrammedScan();\n> + }\n> +}\n> +\n> +void Af::startProgrammedScan()\n> +{\n> + ftarget_ = cfg_.ranges[range_].focusMin;\n> + updateLensPosition();\n> + scanState_ = ScanState::Coarse;\n> + scanMaxContrast_ = 0.0;\n> + scanMinContrast_ = 1.0e9;\n> + scanMaxIndex_ = 0;\n> + scanData_.clear();\n> + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> + reportState_ = AfState::Scanning;\n> +}\n> +\n> +void Af::goIdle()\n> +{\n> + scanState_ = ScanState::Idle;\n> + reportState_ = AfState::Idle;\n> + scanData_.clear();\n> +}\n> +\n> +void Af::cancelScan()\n> +{\n> + LOG(RPiAf, Debug) << \"cancelScan\";\n> + if (mode_ == AfModeAuto)\n> + goIdle();\n> +}\n> +\n> +void Af::triggerScan()\n> +{\n> + LOG(RPiAf, Debug) << \"triggerScan\";\n> + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {\n> + /* Try PDAF if the tuning file permits it for Auto; else CDAF */\n> + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {\n> + if (!initted_) {\n> + ftarget_ = cfg_.ranges[range_].focusDefault;\n> + updateLensPosition();\n> + }\n> + stepCount_ = cfg_.speeds[speed_].pdafFrames;\n> + scanState_ = ScanState::Pdaf;\n> + dropCount_ = 0;\n> + } else\n> + startProgrammedScan();\n> + reportState_ = AfState::Scanning;\n> + }\n> +}\n> +\n> +void Af::setMode(AfAlgorithm::AfMode mode)\n> +{\n> + LOG(RPiAf, Debug) << \"setMode: \" << (unsigned)mode;\n> + if (mode_ != mode) {\n> + mode_ = mode;\n> + pauseFlag_ = false;\n> + if (mode == AfModeContinuous)\n> + startCAF();\n> + else if (mode != AfModeAuto)\n> + goIdle();\n> + }\n> +}\n> +\n> +AfAlgorithm::AfMode Af::getMode() const\n> +{\n> + return mode_;\n> +}\n> +\n> +void Af::pause(AfAlgorithm::AfPause pause)\n> +{\n> + LOG(RPiAf, Debug) << \"pause: \" << (unsigned)pause;\n> + if (mode_ == AfModeContinuous) {\n> + if (pause == AfPauseResume && pauseFlag_) {\n> + startCAF();\n> + pauseFlag_ = false;\n> + } else if (pause != AfPauseResume && !pauseFlag_) {\n> + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)\n> + goIdle();\n> + pauseFlag_ = true;\n> + }\n> + }\n> +}\n> +\n> +// Register algorithm with the system.\n> +static Algorithm *create(Controller *controller)\n> +{\n> + return (Algorithm *)new Af(controller);\n> +}\n> +static RegisterAlgorithm reg(NAME, &create);\n> diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h\n> new file mode 100644\n> index 000000000000..0431bd70ae29\n> --- /dev/null\n> +++ b/src/ipa/raspberrypi/controller/rpi/af.h\n> @@ -0,0 +1,153 @@\n> +/* SPDX-License-Identifier: BSD-2-Clause */\n> +/*\n> + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> + *\n> + * af.h - Autofocus control algorithm\n> + */\n> +#pragma once\n> +\n> +#include \"../af_algorithm.h\"\n> +#include \"../af_status.h\"\n> +#include \"../pdaf_data.h\"\n> +#include \"../pwl.h\"\n> +\n> +/*\n> + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.\n> + *\n> + * Whenever PDAF is available, it is used in a continuous feedback loop.\n> + * When triggered in auto mode, we simply enable AF for a limited number\n> + * of frames (it may terminate early if the delta becomes small enough).\n> + *\n> + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)\n> + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.\n> + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to\n> + * estimate the lens position with peak contrast. This is slower due to\n> + * extra latency in the ISP, and requires a settling time between steps.\n> + *\n> + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid\n> + * \"nuisance\" scans. During each interval where PDAF is not working, only\n> + * ONE scan will be performed; CAF cannot track objects using CDAF alone.\n> + *\n> + * This algorithm is unrelated to \"rpi.focus\" which merely reports CDAF FoM.\n> + */\n> +\n> +namespace RPiController {\n> +\n> +class Af : public AfAlgorithm\n> +{\n> +public:\n> + Af(Controller *controller = NULL);\n> + ~Af();\n> + char const *name() const override;\n> + int read(const libcamera::YamlObject ¶ms) override;\n> + void initialise() override;\n> +\n> + /* IPA calls */\n> + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;\n> + void prepare(Metadata *imageMetadata) override;\n> + void process(StatisticsPtr &stats, Metadata *imageMetadata) override;\n> +\n> + /* controls */\n> + void setRange(AfRange range) override;\n> + void setSpeed(AfSpeed speed) override;\n> + void setMetering(bool use_windows) override;\n> + void setWindows(libcamera::Span const &wins) override;\n> + void setMode(AfMode mode) override;\n> + AfMode getMode() const override;\n> + bool setLensPosition(double dioptres, int32_t *hwpos) override;\n> + std::optional getLensPosition() const override;\n> + void triggerScan() override;\n> + void cancelScan() override;\n> + void pause(AfPause pause) override;\n> +\n> +private:\n> + enum class ScanState {\n> + Idle,\n> + Pdaf,\n> + Coarse,\n> + Fine,\n> + Settle\n> + };\n> +\n> + struct RangeDependentParams {\n> + double focusMin; /* lower (far) limit in dipotres */\n> + double focusMax; /* upper (near) limit in dioptres */\n> + double focusDefault; /* default setting (\"hyperfocal\") */\n> +\n> + RangeDependentParams();\n> + void read(const libcamera::YamlObject ¶ms);\n> + };\n> +\n> + struct SpeedDependentParams {\n> + double stepCoarse; /* used for scans */\n> + double stepFine; /* used for scans */\n> + double contrastRatio; /* used for scan termination and reporting */\n> + double pdafGain; /* coefficient for PDAF feedback loop */\n> + double pdafSquelch; /* PDAF stability parameter (device-specific) */\n> + double maxSlew; /* limit for lens movement per frame */\n> + uint32_t pdafFrames; /* number of iterations when triggered */\n> + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */\n> + uint32_t stepFrames; /* frames to skip in between steps of a scan */\n> +\n> + SpeedDependentParams();\n> + void read(const libcamera::YamlObject ¶ms);\n> + };\n> +\n> + struct CfgParams {\n> + RangeDependentParams ranges[AfRangeMax];\n> + SpeedDependentParams speeds[AfSpeedMax];\n> + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */\n> + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */\n> + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */\n> + uint32_t skipFrames; /* frames to skip at start or modeswitch */\n> + Pwl map; /* converts dioptres -> lens driver position */\n> +\n> + CfgParams();\n> + int read(const libcamera::YamlObject ¶ms);\n> + void initialise();\n> + };\n> +\n> + struct ScanRecord {\n> + double focus;\n> + double contrast;\n> + double phase;\n> + double conf;\n> + };\n> +\n> + bool getPhase(PdafData const &data, double &phase, double &conf) const;\n> + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;\n> + void doPDAF(double phase, double conf);\n> + bool earlyTerminationByPhase(double phase);\n> + void doScan(double contrast, double phase, double conf);\n> + double findPeak(unsigned index) const;\n> + void doAF(double contrast, double phase, double conf);\n> + void updateLensPosition();\n> + void startProgrammedScan();\n> + void startCAF();\n> + void goIdle();\n> +\n> + /* Configuration and settings */\n> + CfgParams cfg_;\n> + AfRange range_;\n> + AfSpeed speed_;\n> + AfMode mode_;\n> + bool pauseFlag_;\n> + libcamera::Size sensorSize_;\n> + bool useWeights_;\n> + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];\n> + uint16_t contrastWeights_[FOCUS_REGIONS];\n> +\n> + /* Working state. */\n> + ScanState scanState_;\n> + bool initted_;\n> + double ftarget_, fsmooth_;\n> + double prevContrast_;\n> + bool pdafSeen_;\n> + unsigned skipCount_, stepCount_, dropCount_;\n> + unsigned scanMaxIndex_;\n> + double scanMaxContrast_, scanMinContrast_;\n> + std::vector scanData_;\n> + AfState reportState_;\n> +};\n> +\n> +} // namespace RPiController\n> diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build\n> index 517d815bb98c..4e2689536257 100644\n> --- a/src/ipa/raspberrypi/meson.build\n> +++ b/src/ipa/raspberrypi/meson.build\n> @@ -27,6 +27,7 @@ rpi_ipa_sources = files([\n> 'controller/controller.cpp',\n> 'controller/histogram.cpp',\n> 'controller/algorithm.cpp',\n> + 'controller/rpi/af.cpp',\n> 'controller/rpi/alsc.cpp',\n> 'controller/rpi/awb.cpp',\n> 'controller/rpi/sharpen.cpp',\n> --\n> 2.25.1\n>","headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id 0D370BD87C\n\tfor ;\n\tThu, 19 Jan 2023 13:07:59 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 6B929625D8;\n\tThu, 19 Jan 2023 14:07:58 +0100 (CET)","from mail-ej1-x629.google.com (mail-ej1-x629.google.com\n\t[IPv6:2a00:1450:4864:20::629])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 719A061EFE\n\tfor ;\n\tThu, 19 Jan 2023 14:07:56 +0100 (CET)","by mail-ej1-x629.google.com with SMTP id mp20so5489592ejc.7\n\tfor ;\n\tThu, 19 Jan 2023 05:07:56 -0800 (PST)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674133678;\n\tbh=KvjY07whgR1Lm3s0KHzx5qvNdvpWCzK0eF/3Qas8h68=;\n\th=References:In-Reply-To:Date:To:Subject:List-Id:List-Unsubscribe:\n\tList-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc:\n\tFrom;\n\tb=pqkh+PJXIDuGnfQRokaUuExH3Q3PmQ0bC3yba4+HZDCnjqusOkuc/s2zxOL3IQ5iD\n\txU4izSu/L9s5rvxVNkXyo2ioXtvpUeJ1RxdnQ+N0ToI7yespRj9L7BQNjqfxtmNCpO\n\tUx34tQUoBF10yHYGXrLFRPtY3k4y5kr9Db/bwtEHErggHmHvnD4NfB9DIHaXQNaqef\n\tHNTKApFOeAUFz7isAcZGDwIQK0FGoTziKfP+LLWpwcEppCNIEFnjOLNTR+5ow1ZyEN\n\t41TZHsY5XL5NULpL82cbk3H94u/e7/1fjRtlXp+s4rvsssnGIQI2/2WZ9B4EH2v7ZB\n\tRbYu4zs6YlHoQ==","v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=raspberrypi.com; s=google;\n\th=cc:to:subject:message-id:date:from:in-reply-to:references\n\t:mime-version:from:to:cc:subject:date:message-id:reply-to;\n\tbh=gCYModEf8eJQOKhr9AG0LxkH1ZqJy3vlJGENMGXEC0A=;\n\tb=fjpMVJ0c/kx8e3V5dsXNzzBQ3SiHS1mjQ2ZfLk7MY53wLeiuDjsg47M7rOyq3P1PvP\n\tz9BCCSgY7KjAcaGAnYU5EuPv4pOImgQdOOSlRKoGQcsZp30fnLfZp2nz/pOD7aJ9/G24\n\tJ3jESXljIY6HJohtbmfG8OhNcWKAF9/3wWX/g0Kku4N27ULLkupDB8TbjKs57pckm8su\n\tuJtyvYODb7BUdoYVc7gSth5o7d61RzXM9UtOWhr8LsjdXFqoWR+xDGkMZjvCb1EHqO4K\n\tsvYQLQhkdzh2RO7ZSKYuaFbpIpolRTLzRAPdAtYTJyy9xhUhVL/YVmShYwwQCsEptqmP\n\t3MSg=="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key; \n\tunprotected) header.d=raspberrypi.com\n\theader.i=@raspberrypi.com\n\theader.b=\"fjpMVJ0c\"; dkim-atps=neutral","X-Google-DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=1e100.net; s=20210112;\n\th=cc:to:subject:message-id:date:from:in-reply-to:references\n\t:mime-version:x-gm-message-state:from:to:cc:subject:date:message-id\n\t:reply-to;\n\tbh=gCYModEf8eJQOKhr9AG0LxkH1ZqJy3vlJGENMGXEC0A=;\n\tb=7CXPKrzlZzTo4cMWKyqF/3jZIaLC3npdF/7p38sikHWKR1UkY9NVn1WIDFgSScymBV\n\tObie6zw96/jTM9sGqOSyNPOiv2gWgKEcp3q0hPl6uZ/agdK4V/cJBYYZxwBmIevwawjM\n\tiaMgZQ7g9Jlpyfl4MqkEHUJE30z7DmrwiE0dTF/S/3JEO0olYP1CVAs0/VgP1Hufc3hi\n\t/G8ZAsrBXlPIVpYV8iG1n9MkoHJT03zkqIS9MZHDZkPkehUZNnbonA67x6t+6efnz7n9\n\tnYbmcz0MRf9a1sXtLLsKlP7u2YEpr0Mm0tNJgCtT0SaOTyeFCscsElz8vLMybOE1oELU\n\t1z3w==","X-Gm-Message-State":"AFqh2kp3ctSMhWEsHM1scGJ5QPCJg3lqQWNZIiGLJ3mDbZO2pTqW/x04\n\tiNqEEllK6QIhWaf+9rt963aQckI4q81EN8vqes8R7A==","X-Google-Smtp-Source":"AMrXdXspJj1uAn6/kAZuE3J2YJhCWD0TSTvbevJUU5KnRmr1yQN2FJFDNlkp0ztFQu0EjLjjEzqpRif6jVf1jRAYRVs=","X-Received":"by 2002:a17:906:2552:b0:86d:cc09:d301 with SMTP id\n\tj18-20020a170906255200b0086dcc09d301mr915116ejb.663.1674133675758;\n\tThu, 19 Jan 2023 05:07:55 -0800 (PST)","MIME-Version":"1.0","References":"<20230119104544.9456-1-naush@raspberrypi.com>\n\t<20230119104544.9456-12-naush@raspberrypi.com>","In-Reply-To":"<20230119104544.9456-12-naush@raspberrypi.com>","Date":"Thu, 19 Jan 2023 13:07:44 +0000","Message-ID":"","To":"Naushir Patuck ","Content-Type":"text/plain; charset=\"UTF-8\"","Subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","From":"Nick Hollinghurst via libcamera-devel\n\t","Reply-To":"Nick Hollinghurst ","Cc":"libcamera-devel@lists.libcamera.org","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "}},{"id":26302,"web_url":"https://patchwork.libcamera.org/comment/26302/","msgid":"<167423275289.42371.7103945458596783257@Monstersaurus>","date":"2023-01-20T16:39:12","subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","submitter":{"id":4,"url":"https://patchwork.libcamera.org/api/people/4/","name":"Kieran Bingham","email":"kieran.bingham@ideasonboard.com"},"content":"Hi Nick, / Naush,\n\nQuoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44)\n> Hi all,\n> \n> This patch needs a tweak to handle the case where setWindows is called\n> before configure -- see Af::Af(Controller *controller) constructor\n> below.\n> \n> On Thu, 19 Jan 2023 at 10:46, Naushir Patuck wrote:\n> >\n> > From: Nick Hollinghurst \n> >\n> > Provide the first version of the Raspberry Pi autofocus algorithm. This\n> > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase\n> > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to\n> > having less \"hunting\" behavior.\n> >\n> > Signed-off-by: Nick Hollinghurst \n> > Signed-off-by: Naushir Patuck \n> > Reviewed-by: Naushir Patuck \n> > Reviewed-by: David Plowman \n> > ---\n> > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++\n> > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++\n> > src/ipa/raspberrypi/meson.build | 1 +\n> > 3 files changed, 909 insertions(+)\n> > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp\n> > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h\n> >\n> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> > new file mode 100644\n> > index 000000000000..7e2e8961085a\n> > --- /dev/null\n> > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> > @@ -0,0 +1,755 @@\n> > +/* SPDX-License-Identifier: BSD-2-Clause */\n> > +/*\n> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> > + *\n> > + * af.cpp - Autofocus control algorithm\n> > + */\n> > +\n> > +#include \"af.h\"\n> > +\n> > +#include \n> > +#include \n> > +#include \n> > +\n> > +#include \n> > +\n> > +#include \n> > +\n> > +using namespace RPiController;\n> > +using namespace libcamera;\n> > +\n> > +LOG_DEFINE_CATEGORY(RPiAf)\n> > +\n> > +#define NAME \"rpi.af\"\n> > +\n> > +/*\n> > + * Default values for parameters. All may be overridden in the tuning file.\n> > + * Many of these values are sensor- or module-dependent; the defaults here\n> > + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.\n> > + *\n> > + * Here all focus values are in dioptres (1/m). They are converted to hardware\n> > + * units when written to status.lensSetting or returned from setLensPosition().\n> > + *\n> > + * Gain and delay values are relative to the update rate, since much (not all)\n> > + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;\n> > + * but note that algorithms are updated at no more than 30 Hz.\n> > + */\n> > +\n> > +Af::RangeDependentParams::RangeDependentParams()\n> > + : focusMin(0.0),\n> > + focusMax(12.0),\n> > + focusDefault(1.0)\n> > +{\n> > +}\n> > +\n> > +Af::SpeedDependentParams::SpeedDependentParams()\n> > + : stepCoarse(1.0),\n> > + stepFine(0.25),\n> > + contrastRatio(0.75),\n> > + pdafGain(-0.02),\n> > + pdafSquelch(0.125),\n> > + maxSlew(2.0),\n> > + pdafFrames(20),\n> > + dropoutFrames(6),\n> > + stepFrames(4)\n> > +{\n> > +}\n> > +\n> > +Af::CfgParams::CfgParams()\n> > + : confEpsilon(8),\n> > + confThresh(16),\n> > + confClip(512),\n> > + skipFrames(5),\n> > + map()\n> > +{\n> > +}\n> > +\n> > +template\n> > +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name)\n> > +{\n> > + auto value = params[name].get();\n> > + if (value)\n> > + dest = *value;\n> > + else\n> > + LOG(RPiAf, Warning) << \"Missing parameter \\\"\" << name << \"\\\"\";\n> > +}\n> > +\n> > +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms)\n> > +{\n> > +\n> > + readNumber(focusMin, params, \"min\");\n> > + readNumber(focusMax, params, \"max\");\n> > + readNumber(focusDefault, params, \"default\");\n> > +}\n> > +\n> > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms)\n> > +{\n> > + readNumber(stepCoarse, params, \"step_coarse\");\n> > + readNumber(stepFine, params, \"step_fine\");\n> > + readNumber(contrastRatio, params, \"contrast_ratio\");\n> > + readNumber(pdafGain, params, \"pdaf_gain\");\n> > + readNumber(pdafSquelch, params, \"pdaf_squelch\");\n> > + readNumber(maxSlew, params, \"max_slew\");\n> > + readNumber(pdafFrames, params, \"pdaf_frames\");\n> > + readNumber(dropoutFrames, params, \"dropout_frames\");\n> > + readNumber(stepFrames, params, \"step_frames\");\n> > +}\n> > +\n> > +int Af::CfgParams::read(const libcamera::YamlObject ¶ms)\n> > +{\n> > + if (params.contains(\"ranges\")) {\n> > + auto &rr = params[\"ranges\"];\n> > +\n> > + if (rr.contains(\"normal\"))\n> > + ranges[AfRangeNormal].read(rr[\"normal\"]);\n> > + else\n> > + LOG(RPiAf, Warning) << \"Missing range \\\"normal\\\"\";\n> > +\n> > + ranges[AfRangeMacro] = ranges[AfRangeNormal];\n> > + if (rr.contains(\"macro\"))\n> > + ranges[AfRangeMacro].read(rr[\"macro\"]);\n> > +\n> > + ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,\n> > + ranges[AfRangeMacro].focusMin);\n> > + ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,\n> > + ranges[AfRangeMacro].focusMax);\n> > + ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;\n> > + if (rr.contains(\"full\"))\n> > + ranges[AfRangeFull].read(rr[\"full\"]);\n> > + } else\n> > + LOG(RPiAf, Warning) << \"No ranges defined\";\n> > +\n> > + if (params.contains(\"speeds\")) {\n> > + auto &ss = params[\"speeds\"];\n> > +\n> > + if (ss.contains(\"normal\"))\n> > + speeds[AfSpeedNormal].read(ss[\"normal\"]);\n> > + else\n> > + LOG(RPiAf, Warning) << \"Missing speed \\\"normal\\\"\";\n> > +\n> > + speeds[AfSpeedFast] = speeds[AfSpeedNormal];\n> > + if (ss.contains(\"fast\"))\n> > + speeds[AfSpeedFast].read(ss[\"fast\"]);\n> > + } else\n> > + LOG(RPiAf, Warning) << \"No speeds defined\";\n> > +\n> > + readNumber(confEpsilon, params, \"conf_epsilon\");\n> > + readNumber(confThresh, params, \"conf_thresh\");\n> > + readNumber(confClip, params, \"conf_clip\");\n> > + readNumber(skipFrames, params, \"skip_frames\");\n> > +\n> > + if (params.contains(\"map\"))\n> > + map.read(params[\"map\"]);\n> > + else\n> > + LOG(RPiAf, Warning) << \"No map defined\";\n> > +\n> > + return 0;\n> > +}\n> > +\n> > +void Af::CfgParams::initialise()\n> > +{\n> > + if (map.empty()) {\n> > + /* Default mapping from dioptres to hardware setting */\n> > + static constexpr double DefaultMapX0 = 0.0;\n> > + static constexpr double DefaultMapY0 = 445.0;\n> > + static constexpr double DefaultMapX1 = 15.0;\n> > + static constexpr double DefaultMapY1 = 925.0;\n> > +\n> > + map.append(DefaultMapX0, DefaultMapY0);\n> > + map.append(DefaultMapX1, DefaultMapY1);\n> > +\n> > + LOG(RPiAf, Warning) << \"af.map is not defined, \";\n> > + }\n> > +}\n> > +\n> > +/* Af Algorithm class */\n> > +\n> > +Af::Af(Controller *controller)\n> > + : AfAlgorithm(controller),\n> > + cfg_(),\n> > + range_(AfRangeNormal),\n> > + speed_(AfSpeedNormal),\n> > + mode_(AfAlgorithm::AfModeManual),\n> > + pauseFlag_(false),\n> > + sensorSize_{ 0, 0 },\n> \n> sensorSize_ needs to be set before setWindows() is called. For this\n> version it suffices to initialize it to\n> \n> sensorSize_ { 4608, 2502 },\n> \n> a more general fix may follow later.\n\nCan you send a tested update to this patch please? No need to send the\nwhole series again.\n\nIf that can include the fix I highlighted, (Remove pdafSeen_) I believe\nI can merge this whole series.\n\n--\nKieran\n\n\n> \n> \n> > + useWeights_(false),\n> > + phaseWeights_{},\n> > + contrastWeights_{},\n> > + scanState_(ScanState::Idle),\n> > + initted_(false),\n> > + ftarget_(-1.0),\n> > + fsmooth_(-1.0),\n> > + prevContrast_(0.0),\n> > + skipCount_(0),\n> > + stepCount_(0),\n> > + dropCount_(0),\n> > + scanMaxContrast_(0.0),\n> > + scanMinContrast_(1.0e9),\n> > + scanData_(),\n> > + reportState_(AfState::Idle)\n> > +{\n> > + scanData_.reserve(24);\n> > +}\n> > +\n> > +Af::~Af()\n> > +{\n> > +}\n> > +\n> > +char const *Af::name() const\n> > +{\n> > + return NAME;\n> > +}\n> > +\n> > +int Af::read(const libcamera::YamlObject ¶ms)\n> > +{\n> > + return cfg_.read(params);\n> > +}\n> > +\n> > +void Af::initialise()\n> > +{\n> > + cfg_.initialise();\n> > +}\n> > +\n> > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)\n> > +{\n> > + (void)metadata;\n> > + sensorSize_.width = cameraMode.sensorWidth;\n> > + sensorSize_.height = cameraMode.sensorHeight;\n> > +\n> > + if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {\n> > + /*\n> > + * If a scan was in progress, re-start it, as CDAF statistics\n> > + * may have changed. Though if the application is just about\n> > + * to take a still picture, this will not help...\n> > + */\n> > + startProgrammedScan();\n> > + }\n> > + skipCount_ = cfg_.skipFrames;\n> > +}\n> > +\n> > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const\n> > +{\n> > + static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n> > + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }\n> > + };\n> > + int32_t sumW = 0;\n> > + int32_t sumWc = 0;\n> > + int32_t sumWcp = 0;\n> > + auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;\n> > +\n> > + for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {\n> > + for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {\n> > + if (wgts[i][j]) {\n> > + uint32_t c = data.conf[i][j];\n> > + if (c >= cfg_.confThresh) {\n> > + if (c > cfg_.confClip)\n> > + c = cfg_.confClip;\n> > + sumWc += wgts[i][j] * (int32_t)c;\n> > + c -= (cfg_.confThresh >> 1);\n> > + sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;\n> > + }\n> > + sumW += wgts[i][j];\n> > + }\n> > + }\n> > + }\n> > +\n> > + if (sumWc > 0) {\n> > + phase = (double)sumWcp / (double)sumWc;\n> > + conf = (double)sumWc / (double)sumW;\n> > + return true;\n> > + } else {\n> > + phase = 0.0;\n> > + conf = 0.0;\n> > + return false;\n> > + }\n> > +}\n> > +\n> > +void Af::doPDAF(double phase, double conf)\n> > +{\n> > + /* Apply loop gain */\n> > + phase *= cfg_.speeds[speed_].pdafGain;\n> > +\n> > + if (mode_ == AfModeContinuous) {\n> > + /*\n> > + * PDAF in Continuous mode. Scale down lens movement when\n> > + * delta is small or confidence is low, to suppress wobble.\n> > + */\n> > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {\n> > + double a = phase / cfg_.speeds[speed_].pdafSquelch;\n> > + phase *= a * a;\n> > + }\n> > + phase *= conf / (conf + cfg_.confEpsilon);\n> > + } else {\n> > + /*\n> > + * PDAF in triggered-auto mode. Allow early termination when\n> > + * phase delta is small; scale down lens movements towards\n> > + * the end of the sequence, to ensure a stable image.\n> > + */\n> > + if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {\n> > + if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)\n> > + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> > + } else\n> > + phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;\n> > + }\n> > +\n> > + /* Apply slew rate limit. Report failure if out of bounds. */\n> > + if (phase < -cfg_.speeds[speed_].maxSlew) {\n> > + phase = -cfg_.speeds[speed_].maxSlew;\n> > + reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed\n> > + : AfState::Scanning;\n> > + } else if (phase > cfg_.speeds[speed_].maxSlew) {\n> > + phase = cfg_.speeds[speed_].maxSlew;\n> > + reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed\n> > + : AfState::Scanning;\n> > + } else\n> > + reportState_ = AfState::Focused;\n> > +\n> > + ftarget_ = fsmooth_ + phase;\n> > +}\n> > +\n> > +bool Af::earlyTerminationByPhase(double phase)\n> > +{\n> > + if (scanData_.size() > 0 &&\n> > + scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {\n> > + double oldFocus = scanData_[scanData_.size() - 1].focus;\n> > + double oldPhase = scanData_[scanData_.size() - 1].phase;\n> > +\n> > + /*\n> > + * Check that the gradient is finite and has the expected sign;\n> > + * Interpolate/extrapolate the lens position for zero phase.\n> > + * Check that the extrapolation is well-conditioned.\n> > + */\n> > + if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {\n> > + double param = phase / (phase - oldPhase);\n> > + if (-3.0 <= param && param <= 3.5) {\n> > + ftarget_ += param * (oldFocus - ftarget_);\n> > + LOG(RPiAf, Debug) << \"ETBP: param=\" << param;\n> > + return true;\n> > + }\n> > + }\n> > + }\n> > +\n> > + return false;\n> > +}\n> > +\n> > +double Af::findPeak(unsigned i) const\n> > +{\n> > + double f = scanData_[i].focus;\n> > +\n> > + if (i > 0 && i + 1 < scanData_.size()) {\n> > + double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;\n> > + double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;\n> > + if (dropLo < dropHi) {\n> > + double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);\n> > + f += param * (scanData_[i - 1].focus - f);\n> > + } else if (dropHi < dropLo) {\n> > + double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);\n> > + f += param * (scanData_[i + 1].focus - f);\n> > + }\n> > + }\n> > +\n> > + LOG(RPiAf, Debug) << \"FindPeak: \" << f;\n> > + return f;\n> > +}\n> > +\n> > +void Af::doScan(double contrast, double phase, double conf)\n> > +{\n> > + /* Record lens position, contrast and phase values for the current scan */\n> > + if (scanData_.empty() || contrast > scanMaxContrast_) {\n> > + scanMaxContrast_ = contrast;\n> > + scanMaxIndex_ = scanData_.size();\n> > + }\n> > + if (contrast < scanMinContrast_)\n> > + scanMinContrast_ = contrast;\n> > + scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });\n> > +\n> > + if (scanState_ == ScanState::Coarse) {\n> > + if (ftarget_ >= cfg_.ranges[range_].focusMax ||\n> > + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> > + /*\n> > + * Finished course scan, or termination based on contrast.\n> > + * Jump to just after max contrast and start fine scan.\n> > + */\n> > + ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +\n> > + 2.0 * cfg_.speeds[speed_].stepFine);\n> > + scanState_ = ScanState::Fine;\n> > + scanData_.clear();\n> > + } else\n> > + ftarget_ += cfg_.speeds[speed_].stepCoarse;\n> > + } else { /* ScanState::Fine */\n> > + if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||\n> > + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n> > + /*\n> > + * Finished fine scan, or termination based on contrast.\n> > + * Use quadratic peak-finding to find best contrast position.\n> > + */\n> > + ftarget_ = findPeak(scanMaxIndex_);\n> > + scanState_ = ScanState::Settle;\n> > + } else\n> > + ftarget_ -= cfg_.speeds[speed_].stepFine;\n> > + }\n> > +\n> > + stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;\n> > +}\n> > +\n> > +void Af::doAF(double contrast, double phase, double conf)\n> > +{\n> > + /* Skip frames at startup and after mode change */\n> > + if (skipCount_ > 0) {\n> > + LOG(RPiAf, Debug) << \"SKIP\";\n> > + skipCount_--;\n> > + return;\n> > + }\n> > +\n> > + if (scanState_ == ScanState::Pdaf) {\n> > + /*\n> > + * Use PDAF closed-loop control whenever available, in both CAF\n> > + * mode and (for a limited number of iterations) when triggered.\n> > + * If PDAF fails (due to poor contrast, noise or large defocus),\n> > + * fall back to a CDAF-based scan. To avoid \"nuisance\" scans,\n> > + * scan only after a number of frames with low PDAF confidence.\n> > + */\n> > + if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {\n> > + doPDAF(phase, conf);\n> > + if (stepCount_ > 0)\n> > + stepCount_--;\n> > + else if (mode_ != AfModeContinuous)\n> > + scanState_ = ScanState::Idle;\n> > + dropCount_ = 0;\n> > + } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)\n> > + startProgrammedScan();\n> > + } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {\n> > + /*\n> > + * Scanning sequence. This means PDAF has become unavailable.\n> > + * Allow a delay between steps for CDAF FoM statistics to be\n> > + * updated, and a \"settling time\" at the end of the sequence.\n> > + * [A coarse or fine scan can be abandoned if two PDAF samples\n> > + * allow direct interpolation of the zero-phase lens position.]\n> > + */\n> > + if (stepCount_ > 0)\n> > + stepCount_--;\n> > + else if (scanState_ == ScanState::Settle) {\n> > + if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&\n> > + scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)\n> > + reportState_ = AfState::Focused;\n> > + else\n> > + reportState_ = AfState::Failed;\n> > + if (mode_ == AfModeContinuous && !pauseFlag_ &&\n> > + cfg_.speeds[speed_].dropoutFrames > 0)\n> > + scanState_ = ScanState::Pdaf;\n> > + else\n> > + scanState_ = ScanState::Idle;\n> > + scanData_.clear();\n> > + } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {\n> > + scanState_ = ScanState::Settle;\n> > + stepCount_ = (mode_ == AfModeContinuous) ? 0\n> > + : cfg_.speeds[speed_].stepFrames;\n> > + } else\n> > + doScan(contrast, phase, conf);\n> > + }\n> > +}\n> > +\n> > +void Af::updateLensPosition()\n> > +{\n> > + if (mode_ != AfModeManual) {\n> > + ftarget_ = std::clamp(ftarget_,\n> > + cfg_.ranges[range_].focusMin,\n> > + cfg_.ranges[range_].focusMax);\n> > + }\n> > +\n> > + /* \\todo Add a clip for manual lens position to be within the PWL limits. */\n> > +\n> > + if (initted_) {\n> > + /* from a known lens position: apply slew rate limit */\n> > + fsmooth_ = std::clamp(ftarget_,\n> > + fsmooth_ - cfg_.speeds[speed_].maxSlew,\n> > + fsmooth_ + cfg_.speeds[speed_].maxSlew);\n> > + } else {\n> > + /* from an unknown position: go straight to target, but add delay */\n> > + fsmooth_ = ftarget_;\n> > + initted_ = true;\n> > + skipCount_ = cfg_.skipFrames;\n> > + }\n> > +}\n> > +\n> > +/*\n> > + * PDAF phase data are available in prepare(), but CDAF statistics are not\n> > + * available until process(). We are gambling on the availability of PDAF.\n> > + * To expedite feedback control using PDAF, issue the V4L2 lens control from\n> > + * prepare(). Conversely, during scans, we must allow an extra frame delay\n> > + * between steps, to retrieve CDAF statistics from the previous process()\n> > + * so we can terminate the scan early without having to change our minds.\n> > + */\n> > +\n> > +void Af::prepare(Metadata *imageMetadata)\n> > +{\n> > + if (initted_) {\n> > + /* Get PDAF from the embedded metadata, and run AF algorithm core */\n> > + PdafData data;\n> > + double phase = 0.0, conf = 0.0;\n> > + double oldFt = ftarget_;\n> > + double oldFs = fsmooth_;\n> > + ScanState oldSs = scanState_;\n> > + uint32_t oldSt = stepCount_;\n> > + if (imageMetadata->get(\"pdaf.data\", data) == 0)\n> > + getPhase(data, phase, conf);\n> > + doAF(prevContrast_, phase, conf);\n> > + updateLensPosition();\n> > + LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)\n> > + << static_cast(reportState_)\n> > + << \" sst\" << static_cast(oldSs)\n> > + << \"->\" << static_cast(scanState_)\n> > + << \" stp\" << oldSt << \"->\" << stepCount_\n> > + << \" ft\" << oldFt << \"->\" << ftarget_\n> > + << \" fs\" << oldFs << \"->\" << fsmooth_\n> > + << \" cont=\" << (int)prevContrast_\n> > + << \" phase=\" << (int)phase << \" conf=\" << (int)conf;\n> > + }\n> > +\n> > + /* Report status and produce new lens setting */\n> > + AfStatus status;\n> > + if (pauseFlag_)\n> > + status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused\n> > + : AfPauseState::Pausing;\n> > + else\n> > + status.pauseState = AfPauseState::Running;\n> > +\n> > + if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)\n> > + status.state = AfState::Scanning;\n> > + else\n> > + status.state = reportState_;\n> > + status.lensSetting = initted_ ? std::optional(cfg_.map.eval(fsmooth_))\n> > + : std::nullopt;\n> > + imageMetadata->set(\"af.status\", status);\n> > +}\n> > +\n> > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const\n> > +{\n> > + uint32_t totW = 0, totWc = 0;\n> > +\n> > + if (useWeights_) {\n> > + for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {\n> > + unsigned w = contrastWeights_[i];\n> > + totW += w;\n> > + totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);\n> > + }\n> > + }\n> > + if (totW == 0) {\n> > + totW = 2;\n> > + totWc = (focus_stats[5].contrast_val[1][1] >> 10) +\n> > + (focus_stats[6].contrast_val[1][1] >> 10);\n> > + }\n> > +\n> > + return (double)totWc / (double)totW;\n> > +}\n> > +\n> > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)\n> > +{\n> > + (void)imageMetadata;\n> > + prevContrast_ = getContrast(stats->focus_stats);\n> > +}\n> > +\n> > +/* Controls */\n> > +\n> > +void Af::setRange(AfRange r)\n> > +{\n> > + LOG(RPiAf, Debug) << \"setRange: \" << (unsigned)r;\n> > + if (r < AfAlgorithm::AfRangeMax)\n> > + range_ = r;\n> > +}\n> > +\n> > +void Af::setSpeed(AfSpeed s)\n> > +{\n> > + LOG(RPiAf, Debug) << \"setSpeed: \" << (unsigned)s;\n> > + if (s < AfAlgorithm::AfSpeedMax) {\n> > + if (scanState_ == ScanState::Pdaf &&\n> > + cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)\n> > + stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;\n> > + speed_ = s;\n> > + }\n> > +}\n> > +\n> > +void Af::setMetering(bool mode)\n> > +{\n> > + useWeights_ = mode;\n> > +}\n> > +\n> > +void Af::setWindows(libcamera::Span const &wins)\n> > +{\n> > + /*\n> > + * Here we just merge all of the given windows, weighted by area.\n> > + * If there are more than 15 overlapping windows, overflow can occur.\n> > + * TODO: A better approach might be to find the phase in each window\n> > + * and choose either the closest or the highest-confidence one?\n> > + *\n> > + * Using mostly \"int\" arithmetic, because Rectangle has signed x, y\n> > + */\n> > + ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);\n> > + int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);\n> > + int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);\n> > + int gridA = gridY * gridX;\n> > +\n> > + for (int i = 0; i < PDAF_DATA_ROWS; ++i)\n> > + std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);\n> > + std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);\n> > +\n> > + for (auto &w : wins) {\n> > + for (int i = 0; i < PDAF_DATA_ROWS; ++i) {\n> > + int y0 = std::max(gridY * i, w.y);\n> > + int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));\n> > + if (y0 >= y1)\n> > + continue;\n> > + y1 -= y0;\n> > + for (int j = 0; j < PDAF_DATA_COLS; ++j) {\n> > + int x0 = std::max(gridX * j, w.x);\n> > + int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));\n> > + if (x0 >= x1)\n> > + continue;\n> > + int a = y1 * (x1 - x0);\n> > + a = (16 * a + gridA - 1) / gridA;\n> > + phaseWeights_[i][j] += a;\n> > + contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;\n> > + }\n> > + }\n> > + }\n> > +}\n> > +\n> > +bool Af::setLensPosition(double dioptres, int *hwpos)\n> > +{\n> > + bool changed = false;\n> > +\n> > + if (mode_ == AfModeManual) {\n> > + LOG(RPiAf, Debug) << \"setLensPosition: \" << dioptres;\n> > + changed = !(initted_ && fsmooth_ == dioptres);\n> > + ftarget_ = dioptres;\n> > + updateLensPosition();\n> > + }\n> > +\n> > + if (hwpos)\n> > + *hwpos = cfg_.map.eval(fsmooth_);\n> > +\n> > + return changed;\n> > +}\n> > +\n> > +std::optional Af::getLensPosition() const\n> > +{\n> > + /*\n> > + * \\todo We ought to perform some precise timing here to determine\n> > + * the current lens position.\n> > + */\n> > + return initted_ ? std::optional(fsmooth_) : std::nullopt;\n> > +}\n> > +\n> > +void Af::startCAF()\n> > +{\n> > + /* Try PDAF if the tuning file permits it for CAF; else CDAF */\n> > + if (cfg_.speeds[speed_].dropoutFrames > 0) {\n> > + if (!initted_) {\n> > + ftarget_ = cfg_.ranges[range_].focusDefault;\n> > + updateLensPosition();\n> > + }\n> > + scanState_ = ScanState::Pdaf;\n> > + scanData_.clear();\n> > + dropCount_ = 0;\n> > + reportState_ = AfState::Scanning;\n> > + } else {\n> > + startProgrammedScan();\n> > + }\n> > +}\n> > +\n> > +void Af::startProgrammedScan()\n> > +{\n> > + ftarget_ = cfg_.ranges[range_].focusMin;\n> > + updateLensPosition();\n> > + scanState_ = ScanState::Coarse;\n> > + scanMaxContrast_ = 0.0;\n> > + scanMinContrast_ = 1.0e9;\n> > + scanMaxIndex_ = 0;\n> > + scanData_.clear();\n> > + stepCount_ = cfg_.speeds[speed_].stepFrames;\n> > + reportState_ = AfState::Scanning;\n> > +}\n> > +\n> > +void Af::goIdle()\n> > +{\n> > + scanState_ = ScanState::Idle;\n> > + reportState_ = AfState::Idle;\n> > + scanData_.clear();\n> > +}\n> > +\n> > +void Af::cancelScan()\n> > +{\n> > + LOG(RPiAf, Debug) << \"cancelScan\";\n> > + if (mode_ == AfModeAuto)\n> > + goIdle();\n> > +}\n> > +\n> > +void Af::triggerScan()\n> > +{\n> > + LOG(RPiAf, Debug) << \"triggerScan\";\n> > + if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {\n> > + /* Try PDAF if the tuning file permits it for Auto; else CDAF */\n> > + if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {\n> > + if (!initted_) {\n> > + ftarget_ = cfg_.ranges[range_].focusDefault;\n> > + updateLensPosition();\n> > + }\n> > + stepCount_ = cfg_.speeds[speed_].pdafFrames;\n> > + scanState_ = ScanState::Pdaf;\n> > + dropCount_ = 0;\n> > + } else\n> > + startProgrammedScan();\n> > + reportState_ = AfState::Scanning;\n> > + }\n> > +}\n> > +\n> > +void Af::setMode(AfAlgorithm::AfMode mode)\n> > +{\n> > + LOG(RPiAf, Debug) << \"setMode: \" << (unsigned)mode;\n> > + if (mode_ != mode) {\n> > + mode_ = mode;\n> > + pauseFlag_ = false;\n> > + if (mode == AfModeContinuous)\n> > + startCAF();\n> > + else if (mode != AfModeAuto)\n> > + goIdle();\n> > + }\n> > +}\n> > +\n> > +AfAlgorithm::AfMode Af::getMode() const\n> > +{\n> > + return mode_;\n> > +}\n> > +\n> > +void Af::pause(AfAlgorithm::AfPause pause)\n> > +{\n> > + LOG(RPiAf, Debug) << \"pause: \" << (unsigned)pause;\n> > + if (mode_ == AfModeContinuous) {\n> > + if (pause == AfPauseResume && pauseFlag_) {\n> > + startCAF();\n> > + pauseFlag_ = false;\n> > + } else if (pause != AfPauseResume && !pauseFlag_) {\n> > + if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)\n> > + goIdle();\n> > + pauseFlag_ = true;\n> > + }\n> > + }\n> > +}\n> > +\n> > +// Register algorithm with the system.\n> > +static Algorithm *create(Controller *controller)\n> > +{\n> > + return (Algorithm *)new Af(controller);\n> > +}\n> > +static RegisterAlgorithm reg(NAME, &create);\n> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h\n> > new file mode 100644\n> > index 000000000000..0431bd70ae29\n> > --- /dev/null\n> > +++ b/src/ipa/raspberrypi/controller/rpi/af.h\n> > @@ -0,0 +1,153 @@\n> > +/* SPDX-License-Identifier: BSD-2-Clause */\n> > +/*\n> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> > + *\n> > + * af.h - Autofocus control algorithm\n> > + */\n> > +#pragma once\n> > +\n> > +#include \"../af_algorithm.h\"\n> > +#include \"../af_status.h\"\n> > +#include \"../pdaf_data.h\"\n> > +#include \"../pwl.h\"\n> > +\n> > +/*\n> > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.\n> > + *\n> > + * Whenever PDAF is available, it is used in a continuous feedback loop.\n> > + * When triggered in auto mode, we simply enable AF for a limited number\n> > + * of frames (it may terminate early if the delta becomes small enough).\n> > + *\n> > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)\n> > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.\n> > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to\n> > + * estimate the lens position with peak contrast. This is slower due to\n> > + * extra latency in the ISP, and requires a settling time between steps.\n> > + *\n> > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid\n> > + * \"nuisance\" scans. During each interval where PDAF is not working, only\n> > + * ONE scan will be performed; CAF cannot track objects using CDAF alone.\n> > + *\n> > + * This algorithm is unrelated to \"rpi.focus\" which merely reports CDAF FoM.\n> > + */\n> > +\n> > +namespace RPiController {\n> > +\n> > +class Af : public AfAlgorithm\n> > +{\n> > +public:\n> > + Af(Controller *controller = NULL);\n> > + ~Af();\n> > + char const *name() const override;\n> > + int read(const libcamera::YamlObject ¶ms) override;\n> > + void initialise() override;\n> > +\n> > + /* IPA calls */\n> > + void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;\n> > + void prepare(Metadata *imageMetadata) override;\n> > + void process(StatisticsPtr &stats, Metadata *imageMetadata) override;\n> > +\n> > + /* controls */\n> > + void setRange(AfRange range) override;\n> > + void setSpeed(AfSpeed speed) override;\n> > + void setMetering(bool use_windows) override;\n> > + void setWindows(libcamera::Span const &wins) override;\n> > + void setMode(AfMode mode) override;\n> > + AfMode getMode() const override;\n> > + bool setLensPosition(double dioptres, int32_t *hwpos) override;\n> > + std::optional getLensPosition() const override;\n> > + void triggerScan() override;\n> > + void cancelScan() override;\n> > + void pause(AfPause pause) override;\n> > +\n> > +private:\n> > + enum class ScanState {\n> > + Idle,\n> > + Pdaf,\n> > + Coarse,\n> > + Fine,\n> > + Settle\n> > + };\n> > +\n> > + struct RangeDependentParams {\n> > + double focusMin; /* lower (far) limit in dipotres */\n> > + double focusMax; /* upper (near) limit in dioptres */\n> > + double focusDefault; /* default setting (\"hyperfocal\") */\n> > +\n> > + RangeDependentParams();\n> > + void read(const libcamera::YamlObject ¶ms);\n> > + };\n> > +\n> > + struct SpeedDependentParams {\n> > + double stepCoarse; /* used for scans */\n> > + double stepFine; /* used for scans */\n> > + double contrastRatio; /* used for scan termination and reporting */\n> > + double pdafGain; /* coefficient for PDAF feedback loop */\n> > + double pdafSquelch; /* PDAF stability parameter (device-specific) */\n> > + double maxSlew; /* limit for lens movement per frame */\n> > + uint32_t pdafFrames; /* number of iterations when triggered */\n> > + uint32_t dropoutFrames; /* number of non-PDAF frames to switch to CDAF */\n> > + uint32_t stepFrames; /* frames to skip in between steps of a scan */\n> > +\n> > + SpeedDependentParams();\n> > + void read(const libcamera::YamlObject ¶ms);\n> > + };\n> > +\n> > + struct CfgParams {\n> > + RangeDependentParams ranges[AfRangeMax];\n> > + SpeedDependentParams speeds[AfSpeedMax];\n> > + uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */\n> > + uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */\n> > + uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */\n> > + uint32_t skipFrames; /* frames to skip at start or modeswitch */\n> > + Pwl map; /* converts dioptres -> lens driver position */\n> > +\n> > + CfgParams();\n> > + int read(const libcamera::YamlObject ¶ms);\n> > + void initialise();\n> > + };\n> > +\n> > + struct ScanRecord {\n> > + double focus;\n> > + double contrast;\n> > + double phase;\n> > + double conf;\n> > + };\n> > +\n> > + bool getPhase(PdafData const &data, double &phase, double &conf) const;\n> > + double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;\n> > + void doPDAF(double phase, double conf);\n> > + bool earlyTerminationByPhase(double phase);\n> > + void doScan(double contrast, double phase, double conf);\n> > + double findPeak(unsigned index) const;\n> > + void doAF(double contrast, double phase, double conf);\n> > + void updateLensPosition();\n> > + void startProgrammedScan();\n> > + void startCAF();\n> > + void goIdle();\n> > +\n> > + /* Configuration and settings */\n> > + CfgParams cfg_;\n> > + AfRange range_;\n> > + AfSpeed speed_;\n> > + AfMode mode_;\n> > + bool pauseFlag_;\n> > + libcamera::Size sensorSize_;\n> > + bool useWeights_;\n> > + uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];\n> > + uint16_t contrastWeights_[FOCUS_REGIONS];\n> > +\n> > + /* Working state. */\n> > + ScanState scanState_;\n> > + bool initted_;\n> > + double ftarget_, fsmooth_;\n> > + double prevContrast_;\n> > + bool pdafSeen_;\n> > + unsigned skipCount_, stepCount_, dropCount_;\n> > + unsigned scanMaxIndex_;\n> > + double scanMaxContrast_, scanMinContrast_;\n> > + std::vector scanData_;\n> > + AfState reportState_;\n> > +};\n> > +\n> > +} // namespace RPiController\n> > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build\n> > index 517d815bb98c..4e2689536257 100644\n> > --- a/src/ipa/raspberrypi/meson.build\n> > +++ b/src/ipa/raspberrypi/meson.build\n> > @@ -27,6 +27,7 @@ rpi_ipa_sources = files([\n> > 'controller/controller.cpp',\n> > 'controller/histogram.cpp',\n> > 'controller/algorithm.cpp',\n> > + 'controller/rpi/af.cpp',\n> > 'controller/rpi/alsc.cpp',\n> > 'controller/rpi/awb.cpp',\n> > 'controller/rpi/sharpen.cpp',\n> > --\n> > 2.25.1\n> >","headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id 1E568BE175\n\tfor ;\n\tFri, 20 Jan 2023 16:39:19 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 5F64E625D8;\n\tFri, 20 Jan 2023 17:39:18 +0100 (CET)","from perceval.ideasonboard.com (perceval.ideasonboard.com\n\t[213.167.242.64])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 167AD61EFD\n\tfor ;\n\tFri, 20 Jan 2023 17:39:16 +0100 (CET)","from pendragon.ideasonboard.com\n\t(cpc89244-aztw30-2-0-cust3082.18-1.cable.virginm.net [86.31.172.11])\n\tby perceval.ideasonboard.com (Postfix) with ESMTPSA id 854EE514;\n\tFri, 20 Jan 2023 17:39:15 +0100 (CET)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674232758;\n\tbh=sdqcGaA4N5Z3lPwhnb+vpLplpNbEE/EHBRAHlUFKg74=;\n\th=In-Reply-To:References:To:Date:Subject:List-Id:List-Unsubscribe:\n\tList-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc:\n\tFrom;\n\tb=VurAs7JB4jz5pgI2/IaIBKZPmuxs7y06GgHVpN/UM1EvRydtnUQdXY3uZTTmLs/hq\n\tv1y5k3XPQXha0GksB5WqEdYUjMQcBqtH3askejaqzEC8H07OHLq/nCrtquTuKZHKqv\n\t1hvUAimykT1dCnPG9owufUKVbDQbRsYVYbaU4VdEWmICXQWsejIauv6/xpKYX40a/h\n\tbm/3Rz9VTzWiTYT68pklLaP+DYOjyEEiGtQ7Jx3JLYRsWSqtGBUm5X6EikJA1W83+O\n\tLxpg8jLiqjAoWPa/HUewe3Py1FZWZqNQF8FIWYAxilwSh3gRtH5ws+fkr9SMkz9Rxb\n\tyRXAv0o45ztvw==","v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com;\n\ts=mail; t=1674232755;\n\tbh=sdqcGaA4N5Z3lPwhnb+vpLplpNbEE/EHBRAHlUFKg74=;\n\th=In-Reply-To:References:Subject:From:Cc:To:Date:From;\n\tb=m74r+DVD21aCfSdBSoOOk0YpeEEK3jns2JTJqAwuQYg4ugQi6AhaNvtBCmZEhQau6\n\tcijOrKcoOqoiiZxvm5wf7PJ28/jAt7QGsutDg/ioWccpXuzThbM6A1GvbcJPY/tM3d\n\trvwRFTntqOa4jvd0YgfSnhAs7783glBrWxwdGJek="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (1024-bit key; \n\tunprotected) header.d=ideasonboard.com\n\theader.i=@ideasonboard.com\n\theader.b=\"m74r+DVD\"; dkim-atps=neutral","Content-Type":"text/plain; charset=\"utf-8\"","MIME-Version":"1.0","Content-Transfer-Encoding":"quoted-printable","In-Reply-To":"","References":"<20230119104544.9456-1-naush@raspberrypi.com>\n\t<20230119104544.9456-12-naush@raspberrypi.com>\n\t","To":"Naushir Patuck ,\n\tNick Hollinghurst ,\n\tNick Hollinghurst via libcamera-devel\n\t","Date":"Fri, 20 Jan 2023 16:39:12 +0000","Message-ID":"<167423275289.42371.7103945458596783257@Monstersaurus>","User-Agent":"alot/0.10","Subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","From":"Kieran Bingham via libcamera-devel\n\t","Reply-To":"Kieran Bingham ","Cc":"libcamera-devel@lists.libcamera.org","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "}},{"id":26304,"web_url":"https://patchwork.libcamera.org/comment/26304/","msgid":"","date":"2023-01-20T16:50:41","subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","submitter":{"id":130,"url":"https://patchwork.libcamera.org/api/people/130/","name":"Nick Hollinghurst","email":"nick.hollinghurst@raspberrypi.com"},"content":"Hi Kieran,\n\nOn Fri, 20 Jan 2023 at 16:39, Kieran Bingham\n wrote:\n>\n> Hi Nick, / Naush,\n>\n> Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44)\n> > Hi all,\n> >\n> > This patch needs a tweak to handle the case where setWindows is called\n> > before configure -- see Af::Af(Controller *controller) constructor\n> > below.\n> >\n> > On Thu, 19 Jan 2023 at 10:46, Naushir Patuck wrote:\n> > >\n> > > From: Nick Hollinghurst \n> > >\n> > > Provide the first version of the Raspberry Pi autofocus algorithm. This\n> > > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase\n> > > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to\n> > > having less \"hunting\" behavior.\n> > >\n> > > Signed-off-by: Nick Hollinghurst \n> > > Signed-off-by: Naushir Patuck \n> > > Reviewed-by: Naushir Patuck \n> > > Reviewed-by: David Plowman \n> > > ---\n> > > src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++\n> > > src/ipa/raspberrypi/controller/rpi/af.h | 153 +++++\n> > > src/ipa/raspberrypi/meson.build | 1 +\n> > > 3 files changed, 909 insertions(+)\n> > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp\n> > > create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h\n> > >\n> > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> > > new file mode 100644\n> > > index 000000000000..7e2e8961085a\n> > > --- /dev/null\n> > > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n> > > @@ -0,0 +1,755 @@\n> > > +/* SPDX-License-Identifier: BSD-2-Clause */\n> > > +/*\n> > > + * Copyright (C) 2022-2023, Raspberry Pi Ltd\n> > > + *\n> > > + * af.cpp - Autofocus control algorithm\n> > > + */\n> > > +\n> > > +#include \"af.h\"\n> > > +\n> > > +#include \n> > > +#include \n> > > +#include \n> > > +\n> > > +#include \n> > > +\n> > > +#include \n> > > +\n> > > +using namespace RPiController;\n> > > +using namespace libcamera;\n> > > +\n> > > +LOG_DEFINE_CATEGORY(RPiAf)\n> > > +\n> > > +#define NAME \"rpi.af\"\n> > > +\n> > > +/*\n> > > + * Default values for parameters. All may be overridden in the tuning file.\n> > > + * Many of these values are sensor- or module-dependent; the defaults here\n> > > + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.\n> > > + *\n> > > + * Here all focus values are in dioptres (1/m). They are converted to hardware\n> > > + * units when written to status.lensSetting or returned from setLensPosition().\n> > > + *\n> > > + * Gain and delay values are relative to the update rate, since much (not all)\n> > > + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;\n> > > + * but note that algorithms are updated at no more than 30 Hz.\n> > > + */\n> > > +\n> > > +Af::RangeDependentParams::RangeDependentParams()\n> > > + : focusMin(0.0),\n> > > + focusMax(12.0),\n> > > + focusDefault(1.0)\n> > > +{\n> > > +}\n> > > +\n> > > +Af::SpeedDependentParams::SpeedDependentParams()\n> > > + : stepCoarse(1.0),\n> > > + stepFine(0.25),\n> > > + contrastRatio(0.75),\n> > > + pdafGain(-0.02),\n> > > + pdafSquelch(0.125),\n> > > + maxSlew(2.0),\n> > > + pdafFrames(20),\n> > > + dropoutFrames(6),\n> > > + stepFrames(4)\n> > > +{\n> > > +}\n> > > +\n> > > +Af::CfgParams::CfgParams()\n> > > + : confEpsilon(8),\n> > > + confThresh(16),\n> > > + confClip(512),\n> > > + skipFrames(5),\n> > > + map()\n> > > +{\n> > > +}\n> > > +\n> > > +template\n> > > +static void readNumber(T &dest, const libcamera::YamlObject ¶ms, char const *name)\n> > > +{\n> > > + auto value = params[name].get();\n> > > + if (value)\n> > > + dest = *value;\n> > > + else\n> > > + LOG(RPiAf, Warning) << \"Missing parameter \\\"\" << name << \"\\\"\";\n> > > +}\n> > > +\n> > > +void Af::RangeDependentParams::read(const libcamera::YamlObject ¶ms)\n> > > +{\n> > > +\n> > > + readNumber(focusMin, params, \"min\");\n> > > + readNumber(focusMax, params, \"max\");\n> > > + readNumber(focusDefault, params, \"default\");\n> > > +}\n> > > +\n> > > +void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms)\n> > > +{\n> > > + readNumber(stepCoarse, params, \"step_coarse\");\n> > > + readNumber(stepFine, params, \"step_fine\");\n> > > + readNumber(contrastRatio, params, \"contrast_ratio\");\n> > > + readNumber(pdafGain, params, \"pdaf_gain\");\n> > > + readNumber(pdafSquelch, params, \"pdaf_squelch\");\n> > > + readNumber(maxSlew, params, \"max_slew\");\n> > > + readNumber(pdafFrames, params, \"pdaf_frames\");\n> > > + readNumber(dropoutFrames, params, \"dropout_frames\");\n> > > + readNumber(stepFrames, params, \"step_frames\");\n> > > +}\n> > > +\n> > > +int Af::CfgParams::read(const libcamera::YamlObject ¶ms)\n> > > +{\n> > > + if (params.contains(\"ranges\")) {\n> > > + auto &rr = params[\"ranges\"];\n> > > +\n> > > + if (rr.contains(\"normal\"))\n> > > + ranges[AfRangeNormal].read(rr[\"normal\"]);\n> > > + else\n> > > + LOG(RPiAf, Warning) << \"Missing range \\\"normal\\\"\";\n> > > +\n> > > + ranges[AfRangeMacro] = ranges[AfRangeNormal];\n> > > + if (rr.contains(\"macro\"))\n> > > + ranges[AfRangeMacro].read(rr[\"macro\"]);\n> > > +\n> > > + ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,\n> > > + ranges[AfRangeMacro].focusMin);\n> > > + ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,\n> > > + ranges[AfRangeMacro].focusMax);\n> > > + ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;\n> > > + if (rr.contains(\"full\"))\n> > > + ranges[AfRangeFull].read(rr[\"full\"]);\n> > > + } else\n> > > + LOG(RPiAf, Warning) << \"No ranges defined\";\n> > > +\n> > > + if (params.contains(\"speeds\")) {\n> > > + auto &ss = params[\"speeds\"];\n> > > +\n> > > + if (ss.contains(\"normal\"))\n> > > + speeds[AfSpeedNormal].read(ss[\"normal\"]);\n> > > + else\n> > > + LOG(RPiAf, Warning) << \"Missing speed \\\"normal\\\"\";\n> > > +\n> > > + speeds[AfSpeedFast] = speeds[AfSpeedNormal];\n> > > + if (ss.contains(\"fast\"))\n> > > + speeds[AfSpeedFast].read(ss[\"fast\"]);\n> > > + } else\n> > > + LOG(RPiAf, Warning) << \"No speeds defined\";\n> > > +\n> > > + readNumber(confEpsilon, params, \"conf_epsilon\");\n> > > + readNumber(confThresh, params, \"conf_thresh\");\n> > > + readNumber(confClip, params, \"conf_clip\");\n> > > + readNumber(skipFrames, params, \"skip_frames\");\n> > > +\n> > > + if (params.contains(\"map\"))\n> > > + map.read(params[\"map\"]);\n> > > + else\n> > > + LOG(RPiAf, Warning) << \"No map defined\";\n> > > +\n> > > + return 0;\n> > > +}\n> > > +\n> > > +void Af::CfgParams::initialise()\n> > > +{\n> > > + if (map.empty()) {\n> > > + /* Default mapping from dioptres to hardware setting */\n> > > + static constexpr double DefaultMapX0 = 0.0;\n> > > + static constexpr double DefaultMapY0 = 445.0;\n> > > + static constexpr double DefaultMapX1 = 15.0;\n> > > + static constexpr double DefaultMapY1 = 925.0;\n> > > +\n> > > + map.append(DefaultMapX0, DefaultMapY0);\n> > > + map.append(DefaultMapX1, DefaultMapY1);\n> > > +\n> > > + LOG(RPiAf, Warning) << \"af.map is not defined, \";\n> > > + }\n> > > +}\n> > > +\n> > > +/* Af Algorithm class */\n> > > +\n> > > +Af::Af(Controller *controller)\n> > > + : AfAlgorithm(controller),\n> > > + cfg_(),\n> > > + range_(AfRangeNormal),\n> > > + speed_(AfSpeedNormal),\n> > > + mode_(AfAlgorithm::AfModeManual),\n> > > + pauseFlag_(false),\n> > > + sensorSize_{ 0, 0 },\n> >\n> > sensorSize_ needs to be set before setWindows() is called. For this\n> > version it suffices to initialize it to\n> >\n> > sensorSize_ { 4608, 2502 },\n> >\n> > a more general fix may follow later.\n>\n> Can you send a tested update to this patch please? No need to send the\n> whole series again.\n>\n> If that can include the fix I highlighted, (Remove pdafSeen_) I believe\n> I can merge this whole series.\n>\n> --\n> Kieran\n\nI have done a bit of re-work to fix this \"properly\" and am testing it\nnow. I'll also remove pdafSeen_.\nThe re-work touches \"af.cpp\", \"af.h\" only so it affects only this patch.\n\nRegards,\n\n Nick\n\n\n","headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id BE6AEC3240\n\tfor ;\n\tFri, 20 Jan 2023 16:50:54 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 84036625E4;\n\tFri, 20 Jan 2023 17:50:54 +0100 (CET)","from mail-ej1-x62f.google.com (mail-ej1-x62f.google.com\n\t[IPv6:2a00:1450:4864:20::62f])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 1F34E61EFD\n\tfor ;\n\tFri, 20 Jan 2023 17:50:53 +0100 (CET)","by mail-ej1-x62f.google.com with SMTP id ss4so15385873ejb.11\n\tfor ;\n\tFri, 20 Jan 2023 08:50:53 -0800 (PST)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674233454;\n\tbh=/KUbGD8AMOVIBz5Z3Y/eqzNB/QML2fYDNcpT+P0HTkA=;\n\th=References:In-Reply-To:Date:To:Subject:List-Id:List-Unsubscribe:\n\tList-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc:\n\tFrom;\n\tb=tp+nwZBqBGIU91vXX40FYJqjWZqZJnzrvF3GqQYsH5RZaTaapvJA10s6SyJjtmV2J\n\tleXBHQrZInEPgLXwtRhDBiIEXintMdn6oUMwEh7pJtUPJbNMoGDxGujTXrb5ol6GW0\n\tS8wAtfVVrp+rBCm4d+JMK7UpddJNvBrsMCjUT/ON3MsxB86DKKe4buON8aoUKSLKeI\n\t1BZZbh3HDgcQ+6Qwd0AqahBAOZ1zK55u10Xmu1iW3ydE0Nky5Z803fxpUp/HPDqeR6\n\t7gPk07h3frJ2hf7zFO0oDs2qNs+a8r0tX76ujLeceEFeR6Cvb63BwRwWS7Q5wrulNJ\n\tPEz4gqJOoK/JA==","v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=raspberrypi.com; s=google;\n\th=cc:to:subject:message-id:date:from:in-reply-to:references\n\t:mime-version:from:to:cc:subject:date:message-id:reply-to;\n\tbh=FldViyc0Dr0vn6musAojHvNVdWtMvrhkAiMmUoh6FqY=;\n\tb=SGNw5ti73+0fKYx3CQD/wbVsg3LHMx/lIW2LvsB/JET/rcmId7fttSDpKh9WJk2Uv3\n\tLphBSa3rr7BDBFgur3vsNGK4AgCcrCM7Df+ffabJ29Eho34zhp0Hr1HEq58k/BmmRoQI\n\tYfYnpDx0xqNeHUqZv3mH9ZAvhNOyjpd1+KIygG0E+YtzXlXzpgzq3zTvMlBHnLLR0PZ1\n\ttC6ScX+AxpZX7CyFOM+sMqIfi13RC6vCWe4gB1jmVv9QeIYxmKiiUcWialofHyyo2pNR\n\tGa1VFBLnnhzOSueM0j7/SY/DRrKAKOfOVSruuU/ghTQbAlS155jgGaAxgt52r7tMcyX8\n\taWQg=="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key; \n\tunprotected) header.d=raspberrypi.com\n\theader.i=@raspberrypi.com\n\theader.b=\"SGNw5ti7\"; dkim-atps=neutral","X-Google-DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=1e100.net; s=20210112;\n\th=cc:to:subject:message-id:date:from:in-reply-to:references\n\t:mime-version:x-gm-message-state:from:to:cc:subject:date:message-id\n\t:reply-to;\n\tbh=FldViyc0Dr0vn6musAojHvNVdWtMvrhkAiMmUoh6FqY=;\n\tb=Zec3X9BYvjfDoG4K7wSi7Q2v4FaqMFBmzOlEjsyAyvvgn5S90sMLGItPC87WM8yuyp\n\tq9Q65Dt9g+SBWNBoAMT2fRkcXv+a9mDO2+QN6VPtOxHDRcyqBxnQd7AudoEunTTGL8YQ\n\tcbQQT68aQGryUzCmRhJmCadQJsHP9ArLr2GtLOsriJTMtvBc+9tUMBXiCMIWUFp0jcSL\n\tkLB7rdlKBES3aKwR45XhmV6zoOXoZCqBuK//zRTr1Rrv7REYLJ4v8z+0uMyGkdjk20bL\n\t9YT4GF4KgAJluOYMpCtXsG/nT1BZBh7urOeKh/UqGc7gVSBPjmahyrH8IURUCacJxfOY\n\tQ88w==","X-Gm-Message-State":"AFqh2kp9G55IJ3r+BM20ycBFvJX/04oJCw3lA9BbAtN0TfIeHzBNhaf3\n\tncJg+7PVoub5lylXZZ+PJLDI2BE2yNdj5ZuULoxP/A==","X-Google-Smtp-Source":"AMrXdXu2bgibhlE7g+rsNjVwJpjBBT00TuBGURunr5dfwFXmh7MVqayB/DhrihWxcXejIlzEIlujaqNH/uF5Ey10B4U=","X-Received":"by 2002:a17:906:3a44:b0:86a:b31a:25e6 with SMTP id\n\ta4-20020a1709063a4400b0086ab31a25e6mr1404640ejf.603.1674233452738;\n\tFri, 20 Jan 2023 08:50:52 -0800 (PST)","MIME-Version":"1.0","References":"<20230119104544.9456-1-naush@raspberrypi.com>\n\t<20230119104544.9456-12-naush@raspberrypi.com>\n\t\n\t<167423275289.42371.7103945458596783257@Monstersaurus>","In-Reply-To":"<167423275289.42371.7103945458596783257@Monstersaurus>","Date":"Fri, 20 Jan 2023 16:50:41 +0000","Message-ID":"","To":"Kieran Bingham ","Content-Type":"text/plain; charset=\"UTF-8\"","Subject":"Re: [libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First\n\tversion of autofocus algorithm using PDAF","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","From":"Nick Hollinghurst via libcamera-devel\n\t","Reply-To":"Nick Hollinghurst ","Cc":"Nick Hollinghurst via libcamera-devel\n\t","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "}}]