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..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 */ @@ -715,11 +872,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_); @@ -763,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(); } } @@ -779,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 317a51f3e..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 { @@ -54,7 +62,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; @@ -65,7 +75,8 @@ class Af : public AfAlgorithm Idle = 0, Trigger, Pdaf, - Coarse, + Coarse1, + Coarse2, Fine, Settle }; @@ -80,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 */ @@ -100,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(); @@ -129,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; @@ -150,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_; }; 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 +}