From 1a1dff5af3bf3f4be8de599af8e7901be3782c64 Mon Sep 17 00:00:00 2001 From: Nick Hollinghurst Date: Fri, 30 May 2025 16:30:14 +0100 Subject: [PATCH 1/3] ipa: rpi: Defer initialising AF LensPosition ControlInfo and value This fixes two small bugs: We previously populated LensPosition's ControlInfo with hard-coded values, ignoring the tuning file. Now we query the AfAlgorithm to get limits (over all AF ranges) and default (for AfRangeNormal). We previosuly sent a default position to the lens driver, even when a user-specified starting position would follow. Defer doing this, to reduce unnecessary lens movement at startup (for some drivers). Signed-off-by: Nick Hollinghurst --- src/ipa/rpi/common/ipa_base.cpp | 53 +++++++++++++++++---------- src/ipa/rpi/controller/af_algorithm.h | 8 +++- src/ipa/rpi/controller/rpi/af.cpp | 16 +++++++- src/ipa/rpi/controller/rpi/af.h | 4 +- 4 files changed, 57 insertions(+), 24 deletions(-) diff --git a/src/ipa/rpi/common/ipa_base.cpp b/src/ipa/rpi/common/ipa_base.cpp index 4c09a0931..a221dda1c 100644 --- a/src/ipa/rpi/common/ipa_base.cpp +++ b/src/ipa/rpi/common/ipa_base.cpp @@ -237,25 +237,6 @@ int32_t IpaBase::configure(const IPACameraSensorInfo &sensorInfo, const ConfigPa agcStatus.analogueGain = defaultAnalogueGain; applyAGC(&agcStatus, ctrls); - /* - * Set the lens to the default (typically hyperfocal) position - * on first start. - */ - if (lensPresent_) { - RPiController::AfAlgorithm *af = - dynamic_cast(controller_.getAlgorithm("af")); - - if (af) { - float defaultPos = - ipaAfControls.at(&controls::LensPosition).def().get(); - ControlList lensCtrl(lensCtrls_); - int32_t hwpos; - - af->setLensPosition(defaultPos, &hwpos); - lensCtrl.set(V4L2_CID_FOCUS_ABSOLUTE, hwpos); - result->lensControls = std::move(lensCtrl); - } - } } result->sensorControls = std::move(ctrls); @@ -285,8 +266,20 @@ int32_t IpaBase::configure(const IPACameraSensorInfo &sensorInfo, const ConfigPa ctrlMap.merge(ControlInfoMap::Map(ipaColourControls)); /* Declare Autofocus controls, only if we have a controllable lens */ - if (lensPresent_) + if (lensPresent_) { ctrlMap.merge(ControlInfoMap::Map(ipaAfControls)); + RPiController::AfAlgorithm *af = + dynamic_cast(controller_.getAlgorithm("af")); + if (af) { + double min, max, dflt; + af->getLensLimits(min, max); + dflt = af->getDefaultLensPosition(); + ctrlMap[&controls::LensPosition] = + ControlInfo(static_cast(min), + static_cast(max), + static_cast(dflt)); + } + } result->controlInfo = ControlInfoMap(std::move(ctrlMap), controls::controls); @@ -324,6 +317,26 @@ void IpaBase::start(const ControlList &controls, StartResult *result) /* Make a note of this as it tells us the HDR status of the first few frames. */ hdrStatus_ = agcStatus.hdr; + /* + * AF: If no lens position was specified, drive lens to a default position. + * This had to be deferred (not initialised by a constructor) until here + * to ensure that exactly ONE starting position is sent to the lens driver. + * It should be the static API default, not dependent on AF range or mode. + */ + if (firstStart_ && lensPresent_) { + RPiController::AfAlgorithm *af = dynamic_cast( + controller_.getAlgorithm("af")); + if (af && !af->getLensPosition()) { + int32_t hwpos; + double pos = af->getDefaultLensPosition(); + if (af->setLensPosition(pos, &hwpos, true)) { + ControlList lensCtrls(lensCtrls_); + lensCtrls.set(V4L2_CID_FOCUS_ABSOLUTE, hwpos); + setLensControls.emit(lensCtrls); + } + } + } + /* * Initialise frame counts, and decide how many frames must be hidden or * "mistrusted", which depends on whether this is a startup from cold, diff --git a/src/ipa/rpi/controller/af_algorithm.h b/src/ipa/rpi/controller/af_algorithm.h index ad9b57545..382609f9b 100644 --- a/src/ipa/rpi/controller/af_algorithm.h +++ b/src/ipa/rpi/controller/af_algorithm.h @@ -33,6 +33,10 @@ class AfAlgorithm : public Algorithm * * getMode() is provided mainly for validating controls. * getLensPosition() is provided for populating DeviceStatus. + * + * getDefaultlensPosition() and getLensLimits() were added for + * populating ControlInfoMap. They return the static API limits + * which should be independent of the current range or mode. */ enum AfRange { AfRangeNormal = 0, @@ -66,7 +70,9 @@ class AfAlgorithm : public Algorithm } virtual void setMode(AfMode mode) = 0; virtual AfMode getMode() const = 0; - virtual bool setLensPosition(double dioptres, int32_t *hwpos) = 0; + virtual double getDefaultLensPosition() const = 0; + virtual void getLensLimits(double &min, double &max) const = 0; + virtual bool setLensPosition(double dioptres, int32_t *hwpos, bool force = false) = 0; virtual std::optional getLensPosition() const = 0; virtual void triggerScan() = 0; virtual void cancelScan() = 0; diff --git a/src/ipa/rpi/controller/rpi/af.cpp b/src/ipa/rpi/controller/rpi/af.cpp index 2157eb94f..041cb51db 100644 --- a/src/ipa/rpi/controller/rpi/af.cpp +++ b/src/ipa/rpi/controller/rpi/af.cpp @@ -715,11 +715,23 @@ void Af::setWindows(libcamera::Span const &wins) invalidateWeights(); } -bool Af::setLensPosition(double dioptres, int *hwpos) +double Af::getDefaultLensPosition() const +{ + return cfg_.ranges[AfRangeNormal].focusDefault; +} + +void Af::getLensLimits(double &min, double &max) const +{ + /* Limits for manual focus are set by map, not by ranges */ + min = cfg_.map.domain().start; + max = cfg_.map.domain().end; +} + +bool Af::setLensPosition(double dioptres, int *hwpos, bool force) { bool changed = false; - if (mode_ == AfModeManual) { + if (mode_ == AfModeManual || force) { LOG(RPiAf, Debug) << "setLensPosition: " << dioptres; ftarget_ = cfg_.map.domain().clamp(dioptres); changed = !(initted_ && fsmooth_ == ftarget_); diff --git a/src/ipa/rpi/controller/rpi/af.h b/src/ipa/rpi/controller/rpi/af.h index 317a51f3e..d8ece1ab1 100644 --- a/src/ipa/rpi/controller/rpi/af.h +++ b/src/ipa/rpi/controller/rpi/af.h @@ -54,7 +54,9 @@ class Af : public AfAlgorithm void setWindows(libcamera::Span const &wins) override; void setMode(AfMode mode) override; AfMode getMode() const override; - bool setLensPosition(double dioptres, int32_t *hwpos) override; + double getDefaultLensPosition() const override; + void getLensLimits(double &min, double &max) const override; + bool setLensPosition(double dioptres, int32_t *hwpos, bool force) override; std::optional getLensPosition() const override; void triggerScan() override; void cancelScan() override; From 310bf3f2d921fcbb16184f64453b83b554f27e0e Mon Sep 17 00:00:00 2001 From: Nick Hollinghurst Date: Fri, 30 May 2025 17:53:01 +0100 Subject: [PATCH 2/3] ipa: rpi: controller: AutoFocus improvements In getPhase(), stop using different weights for sumWc and sumWcp. This should improve linearity e.g. in earlyTerminationByPhase(). Use AWB statistics, and optionally try to detect IR lighting and suppress PDAF in that case, as IR interferes with PDAF on IMX708. Make earlyTerminationByPhase() harder to trigger; extrapolate less. Improve quadratic peak fitting in findPeak(). The old approximation was good but only really valid when points were equally spaced and the MAX was not at one end of the series. For PDAF-based CAF, suppress lens movement unless "phase" has had the same sign for 4 frames in a row. This suppresses noise/wobble. Change the logic for falling back from PDAF to CDAF, and for re- triggering a CDAF-based scan. It is now triggered by image change (based on both Focus and AWB statistics) followed by stability. Modify CDAF scan pattern so it doesn't always start at the lowest setting, but can sometimes start from the current lens position. (Though in the worst case this requires two coarse scans, if the first one went in the wrong direction.) Shorten fine scan and allow it to be skipped when step_fine == 0 Signed-off-by: Nick Hollinghurst --- src/ipa/rpi/controller/rpi/af.cpp | 285 +++++++++++++++++++++++------- src/ipa/rpi/controller/rpi/af.h | 49 +++-- 2 files changed, 254 insertions(+), 80 deletions(-) diff --git a/src/ipa/rpi/controller/rpi/af.cpp b/src/ipa/rpi/controller/rpi/af.cpp index 041cb51db..791ec0bc7 100644 --- a/src/ipa/rpi/controller/rpi/af.cpp +++ b/src/ipa/rpi/controller/rpi/af.cpp @@ -46,6 +46,8 @@ Af::SpeedDependentParams::SpeedDependentParams() : stepCoarse(1.0), stepFine(0.25), contrastRatio(0.75), + retriggerRatio(0.75), + retriggerDelay(10), pdafGain(-0.02), pdafSquelch(0.125), maxSlew(2.0), @@ -60,6 +62,7 @@ Af::CfgParams::CfgParams() confThresh(16), confClip(512), skipFrames(5), + checkForIR(false), map() { } @@ -87,6 +90,8 @@ void Af::SpeedDependentParams::read(const libcamera::YamlObject ¶ms) readNumber(stepCoarse, params, "step_coarse"); readNumber(stepFine, params, "step_fine"); readNumber(contrastRatio, params, "contrast_ratio"); + readNumber(retriggerRatio, params, "retrigger_ratio"); + readNumber(retriggerDelay, params, "retrigger_delay"); readNumber(pdafGain, params, "pdaf_gain"); readNumber(pdafSquelch, params, "pdaf_squelch"); readNumber(maxSlew, params, "max_slew"); @@ -137,6 +142,7 @@ int Af::CfgParams::read(const libcamera::YamlObject ¶ms) readNumber(confThresh, params, "conf_thresh"); readNumber(confClip, params, "conf_clip"); readNumber(skipFrames, params, "skip_frames"); + readNumber(checkForIR, params, "check_for_ir"); if (params.contains("map")) map = params["map"].get(ipa::Pwl{}); @@ -176,27 +182,38 @@ Af::Af(Controller *controller) useWindows_(false), phaseWeights_(), contrastWeights_(), + awbWeights_(), scanState_(ScanState::Idle), initted_(false), + irFlag_(false), ftarget_(-1.0), fsmooth_(-1.0), prevContrast_(0.0), + oldSceneContrast_(0.0), + prevAverage_{ 0.0, 0.0, 0.0 }, + oldSceneAverage_{ 0.0, 0.0, 0.0 }, + prevPhase_(0.0), skipCount_(0), stepCount_(0), dropCount_(0), + sameSignCount_(0), + sceneChangeCount_(0), scanMaxContrast_(0.0), scanMinContrast_(1.0e9), + scanStep_(0.0), scanData_(), reportState_(AfState::Idle) { /* - * Reserve space for data, to reduce memory fragmentation. It's too early - * to query the size of the PDAF (from camera) and Contrast (from ISP) - * statistics, but these are plausible upper bounds. + * Reserve space for data structures, to reduce memory fragmentation. + * It's too early to query the size of the PDAF sensor data, so guess. */ + windows_.reserve(1); phaseWeights_.w.reserve(16 * 12); contrastWeights_.w.reserve(getHardwareConfig().focusRegions.width * getHardwareConfig().focusRegions.height); + contrastWeights_.w.reserve(getHardwareConfig().awbRegions.width * + getHardwareConfig().awbRegions.height); scanData_.reserve(32); } @@ -235,13 +252,14 @@ void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *met << statsRegion_.height; invalidateWeights(); - if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) { + if (scanState_ >= ScanState::Coarse1 && scanState_ < ScanState::Settle) { /* * If a scan was in progress, re-start it, as CDAF statistics * may have changed. Though if the application is just about * to take a still picture, this will not help... */ startProgrammedScan(); + updateLensPosition(); } skipCount_ = cfg_.skipFrames; } @@ -307,6 +325,7 @@ void Af::invalidateWeights() { phaseWeights_.sum = 0; contrastWeights_.sum = 0; + awbWeights_.sum = 0; } bool Af::getPhase(PdafRegions const ®ions, double &phase, double &conf) @@ -328,9 +347,8 @@ bool Af::getPhase(PdafRegions const ®ions, double &phase, double &conf) if (c >= cfg_.confThresh) { if (c > cfg_.confClip) c = cfg_.confClip; - c -= (cfg_.confThresh >> 2); + c -= (cfg_.confThresh >> 1); sumWc += w * c; - c -= (cfg_.confThresh >> 2); sumWcp += (int64_t)(w * c) * (int64_t)data.phase; } } @@ -364,6 +382,54 @@ double Af::getContrast(const FocusRegions &focusStats) return (contrastWeights_.sum > 0) ? ((double)sumWc / (double)contrastWeights_.sum) : 0.0; } +/* + * Get the average R, G, B values in AF window[s] (from AWB statistics). + * Optionally, check if all of {R,G,B} are within 4:5 of each other + * across more than 50% of the counted area and within the AF window: + * for an RGB sensor this strongly suggests that IR lighting is in use. + */ + +bool Af::getAverageAndTestIr(const RgbyRegions &awbStats, double rgb[3]) +{ + libcamera::Size size = awbStats.size(); + if (size.height != awbWeights_.rows || + size.width != awbWeights_.cols || awbWeights_.sum == 0) { + LOG(RPiAf, Debug) << "Recompute RGB weights " << size.width << 'x' << size.height; + computeWeights(&awbWeights_, size.height, size.width); + } + + uint64_t sr = 0, sg = 0, sb = 0, sw = 1; + uint64_t greyCount = 0, allCount = 0; + for (unsigned i = 0; i < awbStats.numRegions(); ++i) { + uint64_t r = awbStats.get(i).val.rSum; + uint64_t g = awbStats.get(i).val.gSum; + uint64_t b = awbStats.get(i).val.bSum; + uint64_t w = awbWeights_.w[i]; + if (w) { + sw += w; + sr += w * awbStats.get(i).val.rSum; + sg += w * awbStats.get(i).val.gSum; + sb += w * awbStats.get(i).val.bSum; + } + if (cfg_.checkForIR) { + if (4 * r < 5 * b && 4 * b < 5 * r && + 4 * r < 5 * g && 4 * g < 5 * r && + 4 * b < 5 * g && 4 * g < 5 * b) + greyCount += awbStats.get(i).counted; + allCount += awbStats.get(i).counted; + } + } + + rgb[0] = sr / (double)sw; + rgb[1] = sg / (double)sw; + rgb[2] = sb / (double)sw; + + return (cfg_.checkForIR && 2 * greyCount > allCount && + 4 * sr < 5 * sb && 4 * sb < 5 * sr && + 4 * sr < 5 * sg && 4 * sg < 5 * sr && + 4 * sb < 5 * sg && 4 * sg < 5 * sb); +} + void Af::doPDAF(double phase, double conf) { /* Apply loop gain */ @@ -410,7 +476,7 @@ void Af::doPDAF(double phase, double conf) bool Af::earlyTerminationByPhase(double phase) { if (scanData_.size() > 0 && - scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) { + scanData_[scanData_.size() - 1].conf >= cfg_.confThresh) { double oldFocus = scanData_[scanData_.size() - 1].focus; double oldPhase = scanData_[scanData_.size() - 1].phase; @@ -421,7 +487,7 @@ bool Af::earlyTerminationByPhase(double phase) */ if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) { double param = phase / (phase - oldPhase); - if (-3.0 <= param && param <= 3.5) { + if (-2.5 <= param && param <= 3.0) { ftarget_ += param * (oldFocus - ftarget_); LOG(RPiAf, Debug) << "ETBP: param=" << param; return true; @@ -436,15 +502,28 @@ double Af::findPeak(unsigned i) const { double f = scanData_[i].focus; - if (i > 0 && i + 1 < scanData_.size()) { - double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast; - double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast; - if (0.0 <= dropLo && dropLo < dropHi) { - double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi); - f += param * (scanData_[i - 1].focus - f); - } else if (0.0 <= dropHi && dropHi < dropLo) { - double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo); - f += param * (scanData_[i + 1].focus - f); + if (scanData_.size() >= 3) { + /* + * Given the sample with the highest contrast score and its two + * neighbours either side (or same side if at the end of a scan), + * solve for the best lens position by fitting a parabola. + * Adapted from awb.cpp: interpolateQaudaratic() + */ + + if (i == 0) + i++; + else if (i + 1 >= scanData_.size()) + i--; + + double abx = scanData_[i - 1].focus - scanData_[i].focus; + double aby = scanData_[i - 1].contrast - scanData_[i].contrast; + double cbx = scanData_[i + 1].focus - scanData_[i].focus; + double cby = scanData_[i + 1].contrast - scanData_[i].contrast; + double denom = 2.0 * (aby * cbx - cby * abx); + if (std::abs(denom) >= (1.0 / 64.0) && denom * abx > 0.0) { + f = (aby * cbx * cbx - cby * abx * abx) / denom; + f = std::clamp(f, std::min(abx, cbx), std::max(abx, cbx)); + f += scanData_[i].focus; } } @@ -458,36 +537,49 @@ void Af::doScan(double contrast, double phase, double conf) if (scanData_.empty() || contrast > scanMaxContrast_) { scanMaxContrast_ = contrast; scanMaxIndex_ = scanData_.size(); + if (scanState_ != ScanState::Fine) + std::copy(prevAverage_, prevAverage_ + 3, oldSceneAverage_); } if (contrast < scanMinContrast_) scanMinContrast_ = contrast; scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf }); - if (scanState_ == ScanState::Coarse) { - if (ftarget_ >= cfg_.ranges[range_].focusMax || - contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { - /* - * Finished course scan, or termination based on contrast. - * Jump to just after max contrast and start fine scan. - */ - ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) + - 2.0 * cfg_.speeds[speed_].stepFine); - scanState_ = ScanState::Fine; - scanData_.clear(); - } else - ftarget_ += cfg_.speeds[speed_].stepCoarse; - } else { /* ScanState::Fine */ - if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 || - contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { - /* - * Finished fine scan, or termination based on contrast. - * Use quadratic peak-finding to find best contrast position. - */ - ftarget_ = findPeak(scanMaxIndex_); + if ((scanStep_ >= 0.0 && ftarget_ >= cfg_.ranges[range_].focusMax) || + (scanStep_ <= 0.0 && ftarget_ <= cfg_.ranges[range_].focusMin) || + (scanState_ == ScanState::Fine && scanData_.size() >= 3) || + contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { + double pk = findPeak(scanMaxIndex_); + /* + * Finished a scan, by hitting a limit or due to constrast dropping off. + * If this is a first coarse scan and we didn't bracket the peak, reverse! + * If this is a fine scan, or no fine step was defined, we've finished. + * Otherwise, start fine scan in opposite direction. + */ + if (scanState_ == ScanState::Coarse1 && + scanData_[0].contrast >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { + scanStep_ = -scanStep_; + scanState_ = ScanState::Coarse2; + } else if (scanState_ == ScanState::Fine || cfg_.speeds[speed_].stepFine <= 0.0) { + ftarget_ = pk; scanState_ = ScanState::Settle; - } else - ftarget_ -= cfg_.speeds[speed_].stepFine; - } + } else if (scanState_ == ScanState::Coarse1 && + scanData_[0].contrast >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) { + scanStep_ = -scanStep_; + scanState_ = ScanState::Coarse2; + } else if (scanStep_ >= 0.0) { + ftarget_ = std::min(pk + cfg_.speeds[speed_].stepFine, + cfg_.ranges[range_].focusMax); + scanStep_ = -cfg_.speeds[speed_].stepFine; + scanState_ = ScanState::Fine; + } else { + ftarget_ = std::max(pk - cfg_.speeds[speed_].stepFine, + cfg_.ranges[range_].focusMin); + scanStep_ = cfg_.speeds[speed_].stepFine; + scanState_ = ScanState::Fine; + } + scanData_.clear(); + } else + ftarget_ += scanStep_; stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames; } @@ -501,26 +593,70 @@ void Af::doAF(double contrast, double phase, double conf) return; } + /* Count frames for which PDAF phase has had same sign */ + if (phase * prevPhase_ <= 0.0) + sameSignCount_ = 0; + else + sameSignCount_++; + prevPhase_ = phase; + + if (mode_ == AfModeManual) + return; /* nothing to do */ + if (scanState_ == ScanState::Pdaf) { /* * Use PDAF closed-loop control whenever available, in both CAF * mode and (for a limited number of iterations) when triggered. - * If PDAF fails (due to poor contrast, noise or large defocus), - * fall back to a CDAF-based scan. To avoid "nuisance" scans, - * scan only after a number of frames with low PDAF confidence. + * If PDAF fails (due to poor contrast, noise or large defocus) + * for at least dropoutFrames, fall back to a CDAF-based scan + * immediately (in triggered-auto) or on scene change (in CAF). */ - if (conf > (dropCount_ ? 1.0 : 0.25) * cfg_.confEpsilon) { - doPDAF(phase, conf); + if (conf >= cfg_.confEpsilon) { + if (mode_ == AfModeAuto || sameSignCount_ >= 3) + doPDAF(phase, conf); if (stepCount_ > 0) stepCount_--; else if (mode_ != AfModeContinuous) scanState_ = ScanState::Idle; + oldSceneContrast_ = contrast; + std::copy(prevAverage_, prevAverage_ + 3, oldSceneAverage_); + sceneChangeCount_ = 0; dropCount_ = 0; - } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames) + return; + } else { + dropCount_++; + if (dropCount_ < cfg_.speeds[speed_].dropoutFrames) + return; + if (mode_ != AfModeContinuous) { + startProgrammedScan(); + return; + } + /* else fall through to waiting for a scene change */ + } + } + if (scanState_ < ScanState::Coarse1 && mode_ == AfModeContinuous) { + /* + * In CAF mode, not in a scan, and PDAF is unavailable. + * Wait for a scene change, followed by stability. + */ + if (contrast + 1.0 < cfg_.speeds[speed_].retriggerRatio * oldSceneContrast_ || + oldSceneContrast_ + 1.0 < cfg_.speeds[speed_].retriggerRatio * contrast || + prevAverage_[0] + 1.0 < cfg_.speeds[speed_].retriggerRatio * oldSceneAverage_[0] || + oldSceneAverage_[0] + 1.0 < cfg_.speeds[speed_].retriggerRatio * prevAverage_[0] || + prevAverage_[1] + 1.0 < cfg_.speeds[speed_].retriggerRatio * oldSceneAverage_[1] || + oldSceneAverage_[1] + 1.0 < cfg_.speeds[speed_].retriggerRatio * prevAverage_[1] || + prevAverage_[2] + 1.0 < cfg_.speeds[speed_].retriggerRatio * oldSceneAverage_[2] || + oldSceneAverage_[2] + 1.0 < cfg_.speeds[speed_].retriggerRatio * prevAverage_[2]) { + oldSceneContrast_ = contrast; + std::copy(prevAverage_, prevAverage_ + 3, oldSceneAverage_); + sceneChangeCount_ = 1; + } else if (sceneChangeCount_) + sceneChangeCount_++; + if (sceneChangeCount_ >= cfg_.speeds[speed_].retriggerDelay) startProgrammedScan(); - } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) { + } else if (scanState_ >= ScanState::Coarse1 && fsmooth_ == ftarget_) { /* - * Scanning sequence. This means PDAF has become unavailable. + * CDAF-based scanning sequence. * Allow a delay between steps for CDAF FoM statistics to be * updated, and a "settling time" at the end of the sequence. * [A coarse or fine scan can be abandoned if two PDAF samples @@ -539,11 +675,14 @@ void Af::doAF(double contrast, double phase, double conf) scanState_ = ScanState::Pdaf; else scanState_ = ScanState::Idle; + dropCount_ = 0; + sceneChangeCount_ = 0; + oldSceneContrast_ = scanMaxContrast_; scanData_.clear(); - } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) { + } else if (conf >= 1.5 * cfg_.confThresh && earlyTerminationByPhase(phase)) { + std::copy(prevAverage_, prevAverage_ + 3, oldSceneAverage_); scanState_ = ScanState::Settle; - stepCount_ = (mode_ == AfModeContinuous) ? 0 - : cfg_.speeds[speed_].stepFrames; + stepCount_ = (mode_ == AfModeContinuous) ? 0 : cfg_.speeds[speed_].stepFrames; } else doScan(contrast, phase, conf); } @@ -573,7 +712,8 @@ void Af::updateLensPosition() void Af::startAF() { /* Use PDAF if the tuning file allows it; else CDAF. */ - if (cfg_.speeds[speed_].dropoutFrames > 0 && + if (cfg_.speeds[speed_].pdafGain != 0.0 && + cfg_.speeds[speed_].dropoutFrames > 0 && (mode_ == AfModeContinuous || cfg_.speeds[speed_].pdafFrames > 0)) { if (!initted_) { ftarget_ = cfg_.ranges[range_].focusDefault; @@ -583,16 +723,30 @@ void Af::startAF() scanState_ = ScanState::Pdaf; scanData_.clear(); dropCount_ = 0; + oldSceneContrast_ = 0; + sceneChangeCount_ = 0; reportState_ = AfState::Scanning; - } else + } else { startProgrammedScan(); + updateLensPosition(); + } } void Af::startProgrammedScan() { - ftarget_ = cfg_.ranges[range_].focusMin; - updateLensPosition(); - scanState_ = ScanState::Coarse; + if (!initted_ || mode_ != AfModeContinuous || + fsmooth_ <= cfg_.ranges[range_].focusMin + 2.0 * cfg_.speeds[speed_].stepCoarse) { + ftarget_ = cfg_.ranges[range_].focusMin; + scanStep_ = cfg_.speeds[speed_].stepCoarse; + scanState_ = ScanState::Coarse2; + } else if (fsmooth_ >= cfg_.ranges[range_].focusMax - 2.0 * cfg_.speeds[speed_].stepCoarse) { + ftarget_ = cfg_.ranges[range_].focusMax; + scanStep_ = -cfg_.speeds[speed_].stepCoarse; + scanState_ = ScanState::Coarse2; + } else { + scanStep_ = -cfg_.speeds[speed_].stepCoarse; + scanState_ = ScanState::Coarse1; + } scanMaxContrast_ = 0.0; scanMinContrast_ = 1.0e9; scanMaxIndex_ = 0; @@ -633,7 +787,7 @@ void Af::prepare(Metadata *imageMetadata) uint32_t oldSt = stepCount_; if (imageMetadata->get("pdaf.regions", regions) == 0) getPhase(regions, phase, conf); - doAF(prevContrast_, phase, conf); + doAF(prevContrast_, phase, irFlag_ ? 0 : conf); updateLensPosition(); LOG(RPiAf, Debug) << std::fixed << std::setprecision(2) << static_cast(reportState_) @@ -643,7 +797,8 @@ void Af::prepare(Metadata *imageMetadata) << " ft" << oldFt << "->" << ftarget_ << " fs" << oldFs << "->" << fsmooth_ << " cont=" << (int)prevContrast_ - << " phase=" << (int)phase << " conf=" << (int)conf; + << " phase=" << (int)phase << " conf=" << (int)conf + << (irFlag_ ? " IR" : ""); } /* Report status and produce new lens setting */ @@ -654,7 +809,9 @@ void Af::prepare(Metadata *imageMetadata) else status.pauseState = AfPauseState::Running; - if (mode_ == AfModeAuto && scanState_ != ScanState::Idle) + if (scanState_ == ScanState::Idle) + status.state = AfState::Idle; + else if (mode_ == AfModeAuto) status.state = AfState::Scanning; else status.state = reportState_; @@ -665,8 +822,8 @@ void Af::prepare(Metadata *imageMetadata) void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata) { - (void)imageMetadata; prevContrast_ = getContrast(stats->focusRegions); + irFlag_ = getAverageAndTestIr(stats->awbRegions, prevAverage_); } /* Controls */ @@ -775,7 +932,7 @@ void Af::setMode(AfAlgorithm::AfMode mode) pauseFlag_ = false; if (mode == AfModeContinuous) scanState_ = ScanState::Trigger; - else if (mode != AfModeAuto || scanState_ < ScanState::Coarse) + else if (mode != AfModeAuto || scanState_ < ScanState::Coarse1) goIdle(); } } @@ -791,11 +948,11 @@ void Af::pause(AfAlgorithm::AfPause pause) if (mode_ == AfModeContinuous) { if (pause == AfPauseResume && pauseFlag_) { pauseFlag_ = false; - if (scanState_ < ScanState::Coarse) + if (scanState_ < ScanState::Coarse1) scanState_ = ScanState::Trigger; } else if (pause != AfPauseResume && !pauseFlag_) { pauseFlag_ = true; - if (pause == AfPauseImmediate || scanState_ < ScanState::Coarse) + if (pause == AfPauseImmediate || scanState_ < ScanState::Coarse1) goIdle(); } } diff --git a/src/ipa/rpi/controller/rpi/af.h b/src/ipa/rpi/controller/rpi/af.h index d8ece1ab1..20b62dbaa 100644 --- a/src/ipa/rpi/controller/rpi/af.h +++ b/src/ipa/rpi/controller/rpi/af.h @@ -15,20 +15,28 @@ /* * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF. * - * Whenever PDAF is available, it is used in a continuous feedback loop. - * When triggered in auto mode, we simply enable AF for a limited number - * of frames (it may terminate early if the delta becomes small enough). + * Whenever PDAF is available (and reports sufficiently high confidence), + * it is used for continuous feedback control of the lens position. When + * triggered in Auto mode, we enable the loop for a limited number of frames + * (it may terminate sooner if the phase becomes small). In CAF mode, the + * PDAF loop runs continuously. Very small lens movements are suppressed. * * When PDAF confidence is low (due e.g. to low contrast or extreme defocus) * or PDAF data are absent, fall back to CDAF with a programmed scan pattern. - * A coarse and fine scan are performed, using ISP's CDAF focus FoM to - * estimate the lens position with peak contrast. This is slower due to - * extra latency in the ISP, and requires a settling time between steps. + * A coarse and fine scan are performed, using the ISP's CDAF contrast FoM + * to estimate the lens position with peak contrast. (This is slower due to + * extra latency in the ISP, and requires a settling time between steps.) + * The scan may terminate early if PDAF recovers and allows the zero-phase + * lens position to be interpolated. * - * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid - * "nuisance" scans. During each interval where PDAF is not working, only - * ONE scan will be performed; CAF cannot track objects using CDAF alone. + * In CAF mode, the fallback to a CDAF scan is triggered when PDAF fails to + * report high confidence and a configurable number of frames have elapsed + * since the last image change since either PDAF was working or a previous + * scan found peak contrast. Image changes are detected using both contrast + * and AWB statistics (within the AF window[s]). * + * IR lighting can interfere with the correct operation of PDAF, so we + * optionally try to detect it (from AWB statistics). */ namespace RPiController { @@ -67,7 +75,8 @@ class Af : public AfAlgorithm Idle = 0, Trigger, Pdaf, - Coarse, + Coarse1, + Coarse2, Fine, Settle }; @@ -82,9 +91,11 @@ class Af : public AfAlgorithm }; struct SpeedDependentParams { - double stepCoarse; /* used for scans */ - double stepFine; /* used for scans */ + double stepCoarse; /* in dioptres; used for scans */ + double stepFine; /* in dioptres; used for scans */ double contrastRatio; /* used for scan termination and reporting */ + double retriggerRatio; /* contrast and RGB ratio for re-triggering */ + uint32_t retriggerDelay; /* frames of stability before re-triggering */ double pdafGain; /* coefficient for PDAF feedback loop */ double pdafSquelch; /* PDAF stability parameter (device-specific) */ double maxSlew; /* limit for lens movement per frame */ @@ -102,7 +113,8 @@ class Af : public AfAlgorithm uint32_t confEpsilon; /* PDAF hysteresis threshold (sensor-specific) */ uint32_t confThresh; /* PDAF confidence cell min (sensor-specific) */ uint32_t confClip; /* PDAF confidence cell max (sensor-specific) */ - uint32_t skipFrames; /* frames to skip at start or modeswitch */ + uint32_t skipFrames; /* frames to skip at start or mode-switch */ + bool checkForIR; /* Set this if PDAF is unreliable in IR light */ libcamera::ipa::Pwl map; /* converts dioptres -> lens driver position */ CfgParams(); @@ -131,6 +143,7 @@ class Af : public AfAlgorithm void invalidateWeights(); bool getPhase(PdafRegions const ®ions, double &phase, double &conf); double getContrast(const FocusRegions &focusStats); + bool getAverageAndTestIr(const RgbyRegions &awbStats, double rgb[3]); void doPDAF(double phase, double conf); bool earlyTerminationByPhase(double phase); double findPeak(unsigned index) const; @@ -152,15 +165,19 @@ class Af : public AfAlgorithm bool useWindows_; RegionWeights phaseWeights_; RegionWeights contrastWeights_; + RegionWeights awbWeights_; /* Working state. */ ScanState scanState_; - bool initted_; + bool initted_, irFlag_; double ftarget_, fsmooth_; - double prevContrast_; + double prevContrast_, oldSceneContrast_; + double prevAverage_[3], oldSceneAverage_[3]; + double prevPhase_; unsigned skipCount_, stepCount_, dropCount_; + unsigned sameSignCount_, sceneChangeCount_; unsigned scanMaxIndex_; - double scanMaxContrast_, scanMinContrast_; + double scanMaxContrast_, scanMinContrast_, scanStep_; std::vector scanData_; AfState reportState_; }; From 06d67e8304f0f5d20631c9040e9552c13c2cfeb1 Mon Sep 17 00:00:00 2001 From: Nick Hollinghurst Date: Wed, 4 Jun 2025 16:00:52 +0100 Subject: [PATCH 3/3] ipa: rpi: Update IMX708 camera tuning files for AutoFocus changes Explicitly add new parameters: "retrigger_ratio", "retrigger_delay", "check_for_ir". Tweak other parameters to suit algorithm changes. (Though existing tuning files should still work acceptably.) Add AfSpeedFast parameters for the Raspberry Pi V3 standard lens. Signed-off-by: Nick Hollinghurst --- src/ipa/rpi/pisp/data/imx708.json | 23 ++++++++++++++++++--- src/ipa/rpi/pisp/data/imx708_noir.json | 23 ++++++++++++++++++--- src/ipa/rpi/pisp/data/imx708_wide.json | 17 +++++++++------ src/ipa/rpi/pisp/data/imx708_wide_noir.json | 17 +++++++++------ src/ipa/rpi/vc4/data/imx708.json | 23 ++++++++++++++++++--- src/ipa/rpi/vc4/data/imx708_noir.json | 23 ++++++++++++++++++--- src/ipa/rpi/vc4/data/imx708_wide.json | 17 +++++++++------ src/ipa/rpi/vc4/data/imx708_wide_noir.json | 17 +++++++++------ 8 files changed, 124 insertions(+), 36 deletions(-) diff --git a/src/ipa/rpi/pisp/data/imx708.json b/src/ipa/rpi/pisp/data/imx708.json index 3fe99aa31..d3e8286d5 100644 --- a/src/ipa/rpi/pisp/data/imx708.json +++ b/src/ipa/rpi/pisp/data/imx708.json @@ -1139,11 +1139,27 @@ "step_coarse": 1.0, "step_fine": 0.25, "contrast_ratio": 0.75, - "pdaf_gain": -0.02, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, + "pdaf_gain": -0.018, "pdaf_squelch": 0.125, - "max_slew": 2.0, + "max_slew": 1.5, "pdaf_frames": 20, "dropout_frames": 6, + "step_frames": 5 + }, + "fast": + { + "step_coarse": 1.0, + "step_fine": 0.0, + "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, + "pdaf_gain": -0.02, + "pdaf_squelch": 0.125, + "max_slew": 2.0, + "pdaf_frames": 16, + "dropout_frames": 4, "step_frames": 4 } }, @@ -1151,6 +1167,7 @@ "conf_thresh": 16, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": false, "map": [ 0.0, 445, 15.0, 925 ] } }, @@ -1272,4 +1289,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/pisp/data/imx708_noir.json b/src/ipa/rpi/pisp/data/imx708_noir.json index 127e5c58c..0d8e79adc 100644 --- a/src/ipa/rpi/pisp/data/imx708_noir.json +++ b/src/ipa/rpi/pisp/data/imx708_noir.json @@ -1156,11 +1156,27 @@ "step_coarse": 1.0, "step_fine": 0.25, "contrast_ratio": 0.75, - "pdaf_gain": -0.02, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, + "pdaf_gain": -0.018, "pdaf_squelch": 0.125, - "max_slew": 2.0, + "max_slew": 1.5, "pdaf_frames": 20, "dropout_frames": 6, + "step_frames": 5 + }, + "fast": + { + "step_coarse": 1.0, + "step_fine": 0.0, + "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, + "pdaf_gain": -0.02, + "pdaf_squelch": 0.125, + "max_slew": 2.0, + "pdaf_frames": 16, + "dropout_frames": 4, "step_frames": 4 } }, @@ -1168,6 +1184,7 @@ "conf_thresh": 16, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": true, "map": [ 0.0, 445, 15.0, 925 ] } }, @@ -1235,4 +1252,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/pisp/data/imx708_wide.json b/src/ipa/rpi/pisp/data/imx708_wide.json index eaf2f3472..db9cdd8f0 100644 --- a/src/ipa/rpi/pisp/data/imx708_wide.json +++ b/src/ipa/rpi/pisp/data/imx708_wide.json @@ -1148,23 +1148,27 @@ "step_coarse": 2.0, "step_fine": 0.5, "contrast_ratio": 0.75, + "retrigger_ratio" : 0.75, + "retrigger_delay" : 10, "pdaf_gain": -0.03, "pdaf_squelch": 0.2, - "max_slew": 4.0, + "max_slew": 3.0, "pdaf_frames": 20, "dropout_frames": 6, - "step_frames": 4 + "step_frames": 5 }, "fast": { "step_coarse": 2.0, - "step_fine": 0.5, + "step_fine": 0.0, "contrast_ratio": 0.75, + "retrigger_ratio" : 0.75, + "retrigger_delay" : 8, "pdaf_gain": -0.05, "pdaf_squelch": 0.2, - "max_slew": 5.0, + "max_slew": 4.0, "pdaf_frames": 16, - "dropout_frames": 6, + "dropout_frames": 4, "step_frames": 4 } }, @@ -1172,6 +1176,7 @@ "conf_thresh": 12, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": false, "map": [ 0.0, 420, 35.0, 920 ] } }, @@ -1295,4 +1300,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/pisp/data/imx708_wide_noir.json b/src/ipa/rpi/pisp/data/imx708_wide_noir.json index 631a1e680..33472859d 100644 --- a/src/ipa/rpi/pisp/data/imx708_wide_noir.json +++ b/src/ipa/rpi/pisp/data/imx708_wide_noir.json @@ -1057,23 +1057,27 @@ "step_coarse": 2.0, "step_fine": 0.5, "contrast_ratio": 0.75, + "retrigger_ratio" : 0.75, + "retrigger_delay" : 10, "pdaf_gain": -0.03, "pdaf_squelch": 0.2, - "max_slew": 4.0, + "max_slew": 3.0, "pdaf_frames": 20, "dropout_frames": 6, - "step_frames": 4 + "step_frames": 5 }, "fast": { "step_coarse": 2.0, - "step_fine": 0.5, + "step_fine": 0.0, "contrast_ratio": 0.75, + "retrigger_ratio" : 0.75, + "retrigger_delay" : 8, "pdaf_gain": -0.05, "pdaf_squelch": 0.2, - "max_slew": 5.0, + "max_slew": 4.0, "pdaf_frames": 16, - "dropout_frames": 6, + "dropout_frames": 4, "step_frames": 4 } }, @@ -1081,6 +1085,7 @@ "conf_thresh": 12, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": true, "map": [ 0.0, 420, 35.0, 920 ] } }, @@ -1150,4 +1155,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/vc4/data/imx708.json b/src/ipa/rpi/vc4/data/imx708.json index e38763e2c..158faac19 100644 --- a/src/ipa/rpi/vc4/data/imx708.json +++ b/src/ipa/rpi/vc4/data/imx708.json @@ -638,11 +638,27 @@ "step_coarse": 1.0, "step_fine": 0.25, "contrast_ratio": 0.75, - "pdaf_gain": -0.02, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, + "pdaf_gain": -0.018, "pdaf_squelch": 0.125, - "max_slew": 2.0, + "max_slew": 1.5, "pdaf_frames": 20, "dropout_frames": 6, + "step_frames": 5 + }, + "fast": + { + "step_coarse": 1.0, + "step_fine": 0.0, + "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, + "pdaf_gain": -0.02, + "pdaf_squelch": 0.125, + "max_slew": 2.0, + "pdaf_frames": 16, + "dropout_frames": 4, "step_frames": 4 } }, @@ -650,6 +666,7 @@ "conf_thresh": 16, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": false, "map": [ 0.0, 445, 15.0, 925 ] } }, @@ -673,4 +690,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/vc4/data/imx708_noir.json b/src/ipa/rpi/vc4/data/imx708_noir.json index bac64766a..20754eb26 100644 --- a/src/ipa/rpi/vc4/data/imx708_noir.json +++ b/src/ipa/rpi/vc4/data/imx708_noir.json @@ -737,11 +737,27 @@ "step_coarse": 1.0, "step_fine": 0.25, "contrast_ratio": 0.75, - "pdaf_gain": -0.02, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, + "pdaf_gain": -0.018, "pdaf_squelch": 0.125, - "max_slew": 2.0, + "max_slew": 1.5, "pdaf_frames": 20, "dropout_frames": 6, + "step_frames": 5 + }, + "fast": + { + "step_coarse": 1.0, + "step_fine": 0.0, + "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, + "pdaf_gain": -0.02, + "pdaf_squelch": 0.125, + "max_slew": 2.0, + "pdaf_frames": 16, + "dropout_frames": 4, "step_frames": 4 } }, @@ -749,6 +765,7 @@ "conf_thresh": 16, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": true, "map": [ 0.0, 445, 15.0, 925 ] } }, @@ -772,4 +789,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/vc4/data/imx708_wide.json b/src/ipa/rpi/vc4/data/imx708_wide.json index 4d2c906c2..c01279290 100644 --- a/src/ipa/rpi/vc4/data/imx708_wide.json +++ b/src/ipa/rpi/vc4/data/imx708_wide.json @@ -637,23 +637,27 @@ "step_coarse": 2.0, "step_fine": 0.5, "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, "pdaf_gain": -0.03, "pdaf_squelch": 0.2, - "max_slew": 4.0, + "max_slew": 3.0, "pdaf_frames": 20, "dropout_frames": 6, - "step_frames": 4 + "step_frames": 5 }, "fast": { "step_coarse": 2.0, - "step_fine": 0.5, + "step_fine": 0.0, "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, "pdaf_gain": -0.05, "pdaf_squelch": 0.2, - "max_slew": 5.0, + "max_slew": 4.0, "pdaf_frames": 16, - "dropout_frames": 6, + "dropout_frames": 4, "step_frames": 4 } }, @@ -661,6 +665,7 @@ "conf_thresh": 12, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": false, "map": [ 0.0, 420, 35.0, 920 ] } }, @@ -684,4 +689,4 @@ } } ] -} \ No newline at end of file +} diff --git a/src/ipa/rpi/vc4/data/imx708_wide_noir.json b/src/ipa/rpi/vc4/data/imx708_wide_noir.json index 4cc88df1f..8f362f800 100644 --- a/src/ipa/rpi/vc4/data/imx708_wide_noir.json +++ b/src/ipa/rpi/vc4/data/imx708_wide_noir.json @@ -628,23 +628,27 @@ "step_coarse": 2.0, "step_fine": 0.5, "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 10, "pdaf_gain": -0.03, "pdaf_squelch": 0.2, - "max_slew": 4.0, + "max_slew": 3.0, "pdaf_frames": 20, "dropout_frames": 6, - "step_frames": 4 + "step_frames": 5 }, "fast": { "step_coarse": 2.0, - "step_fine": 0.5, + "step_fine": 0.0, "contrast_ratio": 0.75, + "retrigger_ratio": 0.75, + "retrigger_delay": 8, "pdaf_gain": -0.05, "pdaf_squelch": 0.2, - "max_slew": 5.0, + "max_slew": 4.0, "pdaf_frames": 16, - "dropout_frames": 6, + "dropout_frames": 4, "step_frames": 4 } }, @@ -652,6 +656,7 @@ "conf_thresh": 12, "conf_clip": 512, "skip_frames": 5, + "check_for_ir": true, "map": [ 0.0, 420, 35.0, 920 ] } }, @@ -675,4 +680,4 @@ } } ] -} \ No newline at end of file +}