[libcamera-devel,v1,11/14] ipa: raspberrypi: First version of autofocus algorithm using PDAF
diff mbox series

Message ID 20230119104544.9456-12-naush@raspberrypi.com
State Superseded
Headers show
Series
  • Raspberry Pi: Camera Module 3 support
Related show

Commit Message

Naushir Patuck Jan. 19, 2023, 10:45 a.m. UTC
From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>

Provide the first version of the Raspberry Pi autofocus algorithm. This
implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
having less "hunting" behavior.

Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
---
 src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
 src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++
 src/ipa/raspberrypi/meson.build           |   1 +
 3 files changed, 909 insertions(+)
 create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
 create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h

Comments

Kieran Bingham Jan. 19, 2023, 1 p.m. UTC | #1
Hi Nick, Naush,

I was soo close to merging this series in one... but alas there's a minor
compiler issue in this one.


Quoting Naushir Patuck via libcamera-devel (2023-01-19 10:45:41)
> From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> 
> Provide the first version of the Raspberry Pi autofocus algorithm. This
> implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
> detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
> having less "hunting" behavior.
> 
> Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
> Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
> Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
> ---
>  src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
>  src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++
>  src/ipa/raspberrypi/meson.build           |   1 +
>  3 files changed, 909 insertions(+)
>  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
>  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h
> 
> diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
> new file mode 100644
> index 000000000000..7e2e8961085a
> --- /dev/null
> +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
> @@ -0,0 +1,755 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> + *
> + * af.cpp - Autofocus control algorithm
> + */
> +
> +#include "af.h"
> +
> +#include <iomanip>
> +#include <math.h>
> +#include <stdlib.h>
> +
> +#include <libcamera/base/log.h>
> +
> +#include <libcamera/control_ids.h>
> +
> +using namespace RPiController;
> +using namespace libcamera;
> +
> +LOG_DEFINE_CATEGORY(RPiAf)
> +
> +#define NAME "rpi.af"
> +
> +/*
> + * Default values for parameters. All may be overridden in the tuning file.
> + * Many of these values are sensor- or module-dependent; the defaults here
> + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
> + *
> + * Here all focus values are in dioptres (1/m). They are converted to hardware
> + * units when written to status.lensSetting or returned from setLensPosition().
> + *
> + * Gain and delay values are relative to the update rate, since much (not all)
> + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
> + * but note that algorithms are updated at no more than 30 Hz.
> + */
> +
> +Af::RangeDependentParams::RangeDependentParams()
> +       : focusMin(0.0),
> +         focusMax(12.0),
> +         focusDefault(1.0)
> +{
> +}
> +
> +Af::SpeedDependentParams::SpeedDependentParams()
> +       : stepCoarse(1.0),
> +         stepFine(0.25),
> +         contrastRatio(0.75),
> +         pdafGain(-0.02),
> +         pdafSquelch(0.125),
> +         maxSlew(2.0),
> +         pdafFrames(20),
> +         dropoutFrames(6),
> +         stepFrames(4)
> +{
> +}
> +
> +Af::CfgParams::CfgParams()
> +       : confEpsilon(8),
> +         confThresh(16),
> +         confClip(512),
> +         skipFrames(5),
> +         map()
> +{
> +}
> +
> +template<typename T>
> +static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
> +{
> +       auto value = params[name].get<T>();
> +       if (value)
> +               dest = *value;
> +       else
> +               LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
> +}
> +
> +void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
> +{
> +
> +       readNumber<double>(focusMin, params, "min");
> +       readNumber<double>(focusMax, params, "max");
> +       readNumber<double>(focusDefault, params, "default");
> +}
> +
> +void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
> +{
> +       readNumber<double>(stepCoarse, params, "step_coarse");
> +       readNumber<double>(stepFine, params, "step_fine");
> +       readNumber<double>(contrastRatio, params, "contrast_ratio");
> +       readNumber<double>(pdafGain, params, "pdaf_gain");
> +       readNumber<double>(pdafSquelch, params, "pdaf_squelch");
> +       readNumber<double>(maxSlew, params, "max_slew");
> +       readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
> +       readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
> +       readNumber<uint32_t>(stepFrames, params, "step_frames");
> +}
> +
> +int Af::CfgParams::read(const libcamera::YamlObject &params)
> +{
> +       if (params.contains("ranges")) {
> +               auto &rr = params["ranges"];
> +
> +               if (rr.contains("normal"))
> +                       ranges[AfRangeNormal].read(rr["normal"]);
> +               else
> +                       LOG(RPiAf, Warning) << "Missing range \"normal\"";
> +
> +               ranges[AfRangeMacro] = ranges[AfRangeNormal];
> +               if (rr.contains("macro"))
> +                       ranges[AfRangeMacro].read(rr["macro"]);
> +
> +               ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
> +                                                       ranges[AfRangeMacro].focusMin);
> +               ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
> +                                                       ranges[AfRangeMacro].focusMax);
> +               ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
> +               if (rr.contains("full"))
> +                       ranges[AfRangeFull].read(rr["full"]);
> +       } else
> +               LOG(RPiAf, Warning) << "No ranges defined";
> +
> +       if (params.contains("speeds")) {
> +               auto &ss = params["speeds"];
> +
> +               if (ss.contains("normal"))
> +                       speeds[AfSpeedNormal].read(ss["normal"]);
> +               else
> +                       LOG(RPiAf, Warning) << "Missing speed \"normal\"";
> +
> +               speeds[AfSpeedFast] = speeds[AfSpeedNormal];
> +               if (ss.contains("fast"))
> +                       speeds[AfSpeedFast].read(ss["fast"]);
> +       } else
> +               LOG(RPiAf, Warning) << "No speeds defined";
> +
> +       readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
> +       readNumber<uint32_t>(confThresh, params, "conf_thresh");
> +       readNumber<uint32_t>(confClip, params, "conf_clip");
> +       readNumber<uint32_t>(skipFrames, params, "skip_frames");
> +
> +       if (params.contains("map"))
> +               map.read(params["map"]);
> +       else
> +               LOG(RPiAf, Warning) << "No map defined";
> +
> +       return 0;
> +}
> +
> +void Af::CfgParams::initialise()
> +{
> +       if (map.empty()) {
> +               /* Default mapping from dioptres to hardware setting */
> +               static constexpr double DefaultMapX0 = 0.0;
> +               static constexpr double DefaultMapY0 = 445.0;
> +               static constexpr double DefaultMapX1 = 15.0;
> +               static constexpr double DefaultMapY1 = 925.0;
> +
> +               map.append(DefaultMapX0, DefaultMapY0);
> +               map.append(DefaultMapX1, DefaultMapY1);
> +
> +               LOG(RPiAf, Warning) << "af.map is not defined, ";
> +       }
> +}
> +
> +/* Af Algorithm class */
> +
> +Af::Af(Controller *controller)
> +       : AfAlgorithm(controller),
> +         cfg_(),
> +         range_(AfRangeNormal),
> +         speed_(AfSpeedNormal),
> +         mode_(AfAlgorithm::AfModeManual),
> +         pauseFlag_(false),
> +         sensorSize_{ 0, 0 },
> +         useWeights_(false),
> +         phaseWeights_{},
> +         contrastWeights_{},
> +         scanState_(ScanState::Idle),
> +         initted_(false),
> +         ftarget_(-1.0),
> +         fsmooth_(-1.0),
> +         prevContrast_(0.0),
> +         skipCount_(0),
> +         stepCount_(0),
> +         dropCount_(0),
> +         scanMaxContrast_(0.0),
> +         scanMinContrast_(1.0e9),
> +         scanData_(),
> +         reportState_(AfState::Idle)
> +{
> +       scanData_.reserve(24);
> +}
> +
> +Af::~Af()
> +{
> +}
> +
> +char const *Af::name() const
> +{
> +       return NAME;
> +}
> +
> +int Af::read(const libcamera::YamlObject &params)
> +{
> +       return cfg_.read(params);
> +}
> +
> +void Af::initialise()
> +{
> +       cfg_.initialise();
> +}
> +
> +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
> +{
> +       (void)metadata;
> +       sensorSize_.width = cameraMode.sensorWidth;
> +       sensorSize_.height = cameraMode.sensorHeight;
> +
> +       if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
> +               /*
> +                * If a scan was in progress, re-start it, as CDAF statistics
> +                * may have changed. Though if the application is just about
> +                * to take a still picture, this will not help...
> +                */
> +               startProgrammedScan();
> +       }
> +       skipCount_ = cfg_.skipFrames;
> +}
> +
> +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const
> +{
> +       static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
> +       };
> +       int32_t sumW = 0;
> +       int32_t sumWc = 0;
> +       int32_t sumWcp = 0;
> +       auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;
> +
> +       for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {
> +               for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {
> +                       if (wgts[i][j]) {
> +                               uint32_t c = data.conf[i][j];
> +                               if (c >= cfg_.confThresh) {
> +                                       if (c > cfg_.confClip)
> +                                               c = cfg_.confClip;
> +                                       sumWc += wgts[i][j] * (int32_t)c;
> +                                       c -= (cfg_.confThresh >> 1);
> +                                       sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;
> +                               }
> +                               sumW += wgts[i][j];
> +                       }
> +               }
> +       }
> +
> +       if (sumWc > 0) {
> +               phase = (double)sumWcp / (double)sumWc;
> +               conf = (double)sumWc / (double)sumW;
> +               return true;
> +       } else {
> +               phase = 0.0;
> +               conf = 0.0;
> +               return false;
> +       }
> +}
> +
> +void Af::doPDAF(double phase, double conf)
> +{
> +       /* Apply loop gain */
> +       phase *= cfg_.speeds[speed_].pdafGain;
> +
> +       if (mode_ == AfModeContinuous) {
> +               /*
> +                * PDAF in Continuous mode. Scale down lens movement when
> +                * delta is small or confidence is low, to suppress wobble.
> +                */
> +               if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
> +                       double a = phase / cfg_.speeds[speed_].pdafSquelch;
> +                       phase *= a * a;
> +               }
> +               phase *= conf / (conf + cfg_.confEpsilon);
> +       } else {
> +               /*
> +                * PDAF in triggered-auto mode. Allow early termination when
> +                * phase delta is small; scale down lens movements towards
> +                * the end of the sequence, to ensure a stable image.
> +                */
> +               if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
> +                       if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
> +                               stepCount_ = cfg_.speeds[speed_].stepFrames;
> +               } else
> +                       phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
> +       }
> +
> +       /* Apply slew rate limit. Report failure if out of bounds. */
> +       if (phase < -cfg_.speeds[speed_].maxSlew) {
> +               phase = -cfg_.speeds[speed_].maxSlew;
> +               reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
> +                                                                         : AfState::Scanning;
> +       } else if (phase > cfg_.speeds[speed_].maxSlew) {
> +               phase = cfg_.speeds[speed_].maxSlew;
> +               reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
> +                                                                         : AfState::Scanning;
> +       } else
> +               reportState_ = AfState::Focused;
> +
> +       ftarget_ = fsmooth_ + phase;
> +}
> +
> +bool Af::earlyTerminationByPhase(double phase)
> +{
> +       if (scanData_.size() > 0 &&
> +           scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
> +               double oldFocus = scanData_[scanData_.size() - 1].focus;
> +               double oldPhase = scanData_[scanData_.size() - 1].phase;
> +
> +               /*
> +                * Check that the gradient is finite and has the expected sign;
> +                * Interpolate/extrapolate the lens position for zero phase.
> +                * Check that the extrapolation is well-conditioned.
> +                */
> +               if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
> +                       double param = phase / (phase - oldPhase);
> +                       if (-3.0 <= param && param <= 3.5) {
> +                               ftarget_ += param * (oldFocus - ftarget_);
> +                               LOG(RPiAf, Debug) << "ETBP: param=" << param;
> +                               return true;
> +                       }
> +               }
> +       }
> +
> +       return false;
> +}
> +
> +double Af::findPeak(unsigned i) const
> +{
> +       double f = scanData_[i].focus;
> +
> +       if (i > 0 && i + 1 < scanData_.size()) {
> +               double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
> +               double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
> +               if (dropLo < dropHi) {
> +                       double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
> +                       f += param * (scanData_[i - 1].focus - f);
> +               } else if (dropHi < dropLo) {
> +                       double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
> +                       f += param * (scanData_[i + 1].focus - f);
> +               }
> +       }
> +
> +       LOG(RPiAf, Debug) << "FindPeak: " << f;
> +       return f;
> +}
> +
> +void Af::doScan(double contrast, double phase, double conf)
> +{
> +       /* Record lens position, contrast and phase values for the current scan */
> +       if (scanData_.empty() || contrast > scanMaxContrast_) {
> +               scanMaxContrast_ = contrast;
> +               scanMaxIndex_ = scanData_.size();
> +       }
> +       if (contrast < scanMinContrast_)
> +               scanMinContrast_ = contrast;
> +       scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
> +
> +       if (scanState_ == ScanState::Coarse) {
> +               if (ftarget_ >= cfg_.ranges[range_].focusMax ||
> +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> +                       /*
> +                        * Finished course scan, or termination based on contrast.
> +                        * Jump to just after max contrast and start fine scan.
> +                        */
> +                       ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
> +                                       2.0 * cfg_.speeds[speed_].stepFine);
> +                       scanState_ = ScanState::Fine;
> +                       scanData_.clear();
> +               } else
> +                       ftarget_ += cfg_.speeds[speed_].stepCoarse;
> +       } else { /* ScanState::Fine */
> +               if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
> +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> +                       /*
> +                        * Finished fine scan, or termination based on contrast.
> +                        * Use quadratic peak-finding to find best contrast position.
> +                        */
> +                       ftarget_ = findPeak(scanMaxIndex_);
> +                       scanState_ = ScanState::Settle;
> +               } else
> +                       ftarget_ -= cfg_.speeds[speed_].stepFine;
> +       }
> +
> +       stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
> +}
> +
> +void Af::doAF(double contrast, double phase, double conf)
> +{
> +       /* Skip frames at startup and after mode change */
> +       if (skipCount_ > 0) {
> +               LOG(RPiAf, Debug) << "SKIP";
> +               skipCount_--;
> +               return;
> +       }
> +
> +       if (scanState_ == ScanState::Pdaf) {
> +               /*
> +                * Use PDAF closed-loop control whenever available, in both CAF
> +                * mode and (for a limited number of iterations) when triggered.
> +                * If PDAF fails (due to poor contrast, noise or large defocus),
> +                * fall back to a CDAF-based scan. To avoid "nuisance" scans,
> +                * scan only after a number of frames with low PDAF confidence.
> +                */
> +               if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {
> +                       doPDAF(phase, conf);
> +                       if (stepCount_ > 0)
> +                               stepCount_--;
> +                       else if (mode_ != AfModeContinuous)
> +                               scanState_ = ScanState::Idle;
> +                       dropCount_ = 0;
> +               } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
> +                       startProgrammedScan();
> +       } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
> +               /*
> +                * Scanning sequence. This means PDAF has become unavailable.
> +                * Allow a delay between steps for CDAF FoM statistics to be
> +                * updated, and a "settling time" at the end of the sequence.
> +                * [A coarse or fine scan can be abandoned if two PDAF samples
> +                * allow direct interpolation of the zero-phase lens position.]
> +                */
> +               if (stepCount_ > 0)
> +                       stepCount_--;
> +               else if (scanState_ == ScanState::Settle) {
> +                       if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
> +                           scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
> +                               reportState_ = AfState::Focused;
> +                       else
> +                               reportState_ = AfState::Failed;
> +                       if (mode_ == AfModeContinuous && !pauseFlag_ &&
> +                           cfg_.speeds[speed_].dropoutFrames > 0)
> +                               scanState_ = ScanState::Pdaf;
> +                       else
> +                               scanState_ = ScanState::Idle;
> +                       scanData_.clear();
> +               } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
> +                       scanState_ = ScanState::Settle;
> +                       stepCount_ = (mode_ == AfModeContinuous) ? 0
> +                                                                : cfg_.speeds[speed_].stepFrames;
> +               } else
> +                       doScan(contrast, phase, conf);
> +       }
> +}
> +
> +void Af::updateLensPosition()
> +{
> +       if (mode_ != AfModeManual) {
> +               ftarget_ = std::clamp(ftarget_,
> +                               cfg_.ranges[range_].focusMin,
> +                               cfg_.ranges[range_].focusMax);
> +       }
> +
> +       /* \todo Add a clip for manual lens position to be within the PWL limits. */
> +
> +       if (initted_) {
> +               /* from a known lens position: apply slew rate limit */
> +               fsmooth_ = std::clamp(ftarget_,
> +                                     fsmooth_ - cfg_.speeds[speed_].maxSlew,
> +                                     fsmooth_ + cfg_.speeds[speed_].maxSlew);
> +       } else {
> +               /* from an unknown position: go straight to target, but add delay */
> +               fsmooth_ = ftarget_;
> +               initted_ = true;
> +               skipCount_ = cfg_.skipFrames;
> +       }
> +}
> +
> +/*
> + * PDAF phase data are available in prepare(), but CDAF statistics are not
> + * available until process(). We are gambling on the availability of PDAF.
> + * To expedite feedback control using PDAF, issue the V4L2 lens control from
> + * prepare(). Conversely, during scans, we must allow an extra frame delay
> + * between steps, to retrieve CDAF statistics from the previous process()
> + * so we can terminate the scan early without having to change our minds.
> + */
> +
> +void Af::prepare(Metadata *imageMetadata)
> +{
> +       if (initted_) {
> +               /* Get PDAF from the embedded metadata, and run AF algorithm core */
> +               PdafData data;
> +               double phase = 0.0, conf = 0.0;
> +               double oldFt = ftarget_;
> +               double oldFs = fsmooth_;
> +               ScanState oldSs = scanState_;
> +               uint32_t oldSt = stepCount_;
> +               if (imageMetadata->get("pdaf.data", data) == 0)
> +                       getPhase(data, phase, conf);
> +               doAF(prevContrast_, phase, conf);
> +               updateLensPosition();
> +               LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
> +                                 << static_cast<unsigned int>(reportState_)
> +                                 << " sst" << static_cast<unsigned int>(oldSs)
> +                                 << "->" << static_cast<unsigned int>(scanState_)
> +                                 << " stp" << oldSt << "->" << stepCount_
> +                                 << " ft" << oldFt << "->" << ftarget_
> +                                 << " fs" << oldFs << "->" << fsmooth_
> +                                 << " cont=" << (int)prevContrast_
> +                                 << " phase=" << (int)phase << " conf=" << (int)conf;
> +       }
> +
> +       /* Report status and produce new lens setting */
> +       AfStatus status;
> +       if (pauseFlag_)
> +               status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
> +                                                                   : AfPauseState::Pausing;
> +       else
> +               status.pauseState = AfPauseState::Running;
> +
> +       if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
> +               status.state = AfState::Scanning;
> +       else
> +               status.state = reportState_;
> +       status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
> +                                     : std::nullopt;
> +       imageMetadata->set("af.status", status);
> +}
> +
> +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const
> +{
> +       uint32_t totW = 0, totWc = 0;
> +
> +       if (useWeights_) {
> +               for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {
> +                       unsigned w = contrastWeights_[i];
> +                       totW += w;
> +                       totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);
> +               }
> +       }
> +       if (totW == 0) {
> +               totW = 2;
> +               totWc = (focus_stats[5].contrast_val[1][1] >> 10) +
> +                        (focus_stats[6].contrast_val[1][1] >> 10);
> +       }
> +
> +       return (double)totWc / (double)totW;
> +}
> +
> +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
> +{
> +       (void)imageMetadata;
> +       prevContrast_ = getContrast(stats->focus_stats);
> +}
> +
> +/* Controls */
> +
> +void Af::setRange(AfRange r)
> +{
> +       LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
> +       if (r < AfAlgorithm::AfRangeMax)
> +               range_ = r;
> +}
> +
> +void Af::setSpeed(AfSpeed s)
> +{
> +       LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
> +       if (s < AfAlgorithm::AfSpeedMax) {
> +               if (scanState_ == ScanState::Pdaf &&
> +                   cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
> +                       stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
> +               speed_ = s;
> +       }
> +}
> +
> +void Af::setMetering(bool mode)
> +{
> +       useWeights_ = mode;
> +}
> +
> +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
> +{
> +       /*
> +        * Here we just merge all of the given windows, weighted by area.
> +        * If there are more than 15 overlapping windows, overflow can occur.
> +        * TODO: A better approach might be to find the phase in each window
> +        * and choose either the closest or the highest-confidence one?
> +        *
> +        * Using mostly "int" arithmetic, because Rectangle has signed x, y
> +        */
> +       ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);
> +       int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);
> +       int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);
> +       int gridA = gridY * gridX;
> +
> +       for (int i = 0; i < PDAF_DATA_ROWS; ++i)
> +               std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);
> +       std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);
> +
> +       for (auto &w : wins) {
> +               for (int i = 0; i < PDAF_DATA_ROWS; ++i) {
> +                       int y0 = std::max(gridY * i, w.y);
> +                       int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));
> +                       if (y0 >= y1)
> +                               continue;
> +                       y1 -= y0;
> +                       for (int j = 0; j < PDAF_DATA_COLS; ++j) {
> +                               int x0 = std::max(gridX * j, w.x);
> +                               int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));
> +                               if (x0 >= x1)
> +                                       continue;
> +                               int a = y1 * (x1 - x0);
> +                               a = (16 * a + gridA - 1) / gridA;
> +                               phaseWeights_[i][j] += a;
> +                               contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;
> +                       }
> +               }
> +       }
> +}
> +
> +bool Af::setLensPosition(double dioptres, int *hwpos)
> +{
> +       bool changed = false;
> +
> +       if (mode_ == AfModeManual) {
> +               LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
> +               changed = !(initted_ && fsmooth_ == dioptres);
> +               ftarget_ = dioptres;
> +               updateLensPosition();
> +       }
> +
> +       if (hwpos)
> +               *hwpos = cfg_.map.eval(fsmooth_);
> +
> +       return changed;
> +}
> +
> +std::optional<double> Af::getLensPosition() const
> +{
> +       /*
> +        * \todo We ought to perform some precise timing here to determine
> +        * the current lens position.
> +        */
> +       return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
> +}
> +
> +void Af::startCAF()
> +{
> +       /* Try PDAF if the tuning file permits it for CAF; else CDAF */
> +       if (cfg_.speeds[speed_].dropoutFrames > 0) {
> +               if (!initted_) {
> +                       ftarget_ = cfg_.ranges[range_].focusDefault;
> +                       updateLensPosition();
> +               }
> +               scanState_ = ScanState::Pdaf;
> +               scanData_.clear();
> +               dropCount_ = 0;
> +               reportState_ = AfState::Scanning;
> +       } else {
> +               startProgrammedScan();
> +       }
> +}
> +
> +void Af::startProgrammedScan()
> +{
> +       ftarget_ = cfg_.ranges[range_].focusMin;
> +       updateLensPosition();
> +       scanState_ = ScanState::Coarse;
> +       scanMaxContrast_ = 0.0;
> +       scanMinContrast_ = 1.0e9;
> +       scanMaxIndex_ = 0;
> +       scanData_.clear();
> +       stepCount_ = cfg_.speeds[speed_].stepFrames;
> +       reportState_ = AfState::Scanning;
> +}
> +
> +void Af::goIdle()
> +{
> +       scanState_ = ScanState::Idle;
> +       reportState_ = AfState::Idle;
> +       scanData_.clear();
> +}
> +
> +void Af::cancelScan()
> +{
> +       LOG(RPiAf, Debug) << "cancelScan";
> +       if (mode_ == AfModeAuto)
> +               goIdle();
> +}
> +
> +void Af::triggerScan()
> +{
> +       LOG(RPiAf, Debug) << "triggerScan";
> +       if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {
> +               /* Try PDAF if the tuning file permits it for Auto; else CDAF */
> +               if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {
> +                       if (!initted_) {
> +                               ftarget_ = cfg_.ranges[range_].focusDefault;
> +                               updateLensPosition();
> +                       }
> +                       stepCount_ = cfg_.speeds[speed_].pdafFrames;
> +                       scanState_ = ScanState::Pdaf;
> +                       dropCount_ = 0;
> +               } else
> +                       startProgrammedScan();
> +               reportState_ = AfState::Scanning;
> +       }
> +}
> +
> +void Af::setMode(AfAlgorithm::AfMode mode)
> +{
> +       LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
> +       if (mode_ != mode) {
> +               mode_ = mode;
> +               pauseFlag_ = false;
> +               if (mode == AfModeContinuous)
> +                       startCAF();
> +               else if (mode != AfModeAuto)
> +                       goIdle();
> +       }
> +}
> +
> +AfAlgorithm::AfMode Af::getMode() const
> +{
> +       return mode_;
> +}
> +
> +void Af::pause(AfAlgorithm::AfPause pause)
> +{
> +       LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
> +       if (mode_ == AfModeContinuous) {
> +               if (pause == AfPauseResume && pauseFlag_) {
> +                       startCAF();
> +                       pauseFlag_ = false;
> +               } else if (pause != AfPauseResume && !pauseFlag_) {
> +                       if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)
> +                               goIdle();
> +                       pauseFlag_ = true;
> +               }
> +       }
> +}
> +
> +// Register algorithm with the system.
> +static Algorithm *create(Controller *controller)
> +{
> +       return (Algorithm *)new Af(controller);
> +}
> +static RegisterAlgorithm reg(NAME, &create);
> diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
> new file mode 100644
> index 000000000000..0431bd70ae29
> --- /dev/null
> +++ b/src/ipa/raspberrypi/controller/rpi/af.h
> @@ -0,0 +1,153 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> + *
> + * af.h - Autofocus control algorithm
> + */
> +#pragma once
> +
> +#include "../af_algorithm.h"
> +#include "../af_status.h"
> +#include "../pdaf_data.h"
> +#include "../pwl.h"
> +
> +/*
> + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
> + *
> + * Whenever PDAF is available, it is used in a continuous feedback loop.
> + * When triggered in auto mode, we simply enable AF for a limited number
> + * of frames (it may terminate early if the delta becomes small enough).
> + *
> + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
> + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
> + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
> + * estimate the lens position with peak contrast. This is slower due to
> + * extra latency in the ISP, and requires a settling time between steps.
> + *
> + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
> + * "nuisance" scans. During each interval where PDAF is not working, only
> + * ONE scan will be performed; CAF cannot track objects using CDAF alone.
> + *
> + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
> + */
> +
> +namespace RPiController {
> +
> +class Af : public AfAlgorithm
> +{
> +public:
> +       Af(Controller *controller = NULL);
> +       ~Af();
> +       char const *name() const override;
> +       int read(const libcamera::YamlObject &params) override;
> +       void initialise() override;
> +
> +       /* IPA calls */
> +       void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
> +       void prepare(Metadata *imageMetadata) override;
> +       void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
> +
> +       /* controls */
> +       void setRange(AfRange range) override;
> +       void setSpeed(AfSpeed speed) override;
> +       void setMetering(bool use_windows) override;
> +       void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
> +       void setMode(AfMode mode) override;
> +       AfMode getMode() const override;
> +       bool setLensPosition(double dioptres, int32_t *hwpos) override;
> +       std::optional<double> getLensPosition() const override;
> +       void triggerScan() override;
> +       void cancelScan() override;
> +       void pause(AfPause pause) override;
> +
> +private:
> +       enum class ScanState {
> +               Idle,
> +               Pdaf,
> +               Coarse,
> +               Fine,
> +               Settle
> +       };
> +
> +       struct RangeDependentParams {
> +               double focusMin;                /* lower (far) limit in dipotres */
> +               double focusMax;                /* upper (near) limit in dioptres */
> +               double focusDefault;            /* default setting ("hyperfocal") */
> +
> +               RangeDependentParams();
> +               void read(const libcamera::YamlObject &params);
> +       };
> +
> +       struct SpeedDependentParams {
> +               double stepCoarse;              /* used for scans */
> +               double stepFine;                /* used for scans */
> +               double contrastRatio;           /* used for scan termination and reporting */
> +               double pdafGain;                /* coefficient for PDAF feedback loop */
> +               double pdafSquelch;             /* PDAF stability parameter (device-specific) */
> +               double maxSlew;                 /* limit for lens movement per frame */
> +               uint32_t pdafFrames;            /* number of iterations when triggered */
> +               uint32_t dropoutFrames;         /* number of non-PDAF frames to switch to CDAF */
> +               uint32_t stepFrames;            /* frames to skip in between steps of a scan */
> +
> +               SpeedDependentParams();
> +               void read(const libcamera::YamlObject &params);
> +       };
> +
> +       struct CfgParams {
> +               RangeDependentParams ranges[AfRangeMax];
> +               SpeedDependentParams speeds[AfSpeedMax];
> +               uint32_t confEpsilon;           /* PDAF hysteresis threshold (sensor-specific) */
> +               uint32_t confThresh;            /* PDAF confidence cell min (sensor-specific) */
> +               uint32_t confClip;              /* PDAF confidence cell max (sensor-specific) */
> +               uint32_t skipFrames;            /* frames to skip at start or modeswitch */
> +               Pwl map;                        /* converts dioptres -> lens driver position */
> +
> +               CfgParams();
> +               int read(const libcamera::YamlObject &params);
> +               void initialise();
> +       };
> +
> +       struct ScanRecord {
> +               double focus;
> +               double contrast;
> +               double phase;
> +               double conf;
> +       };
> +
> +       bool getPhase(PdafData const &data, double &phase, double &conf) const;
> +       double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
> +       void doPDAF(double phase, double conf);
> +       bool earlyTerminationByPhase(double phase);
> +       void doScan(double contrast, double phase, double conf);
> +       double findPeak(unsigned index) const;
> +       void doAF(double contrast, double phase, double conf);
> +       void updateLensPosition();
> +       void startProgrammedScan();
> +       void startCAF();
> +       void goIdle();
> +
> +       /* Configuration and settings */
> +       CfgParams cfg_;
> +       AfRange range_;
> +       AfSpeed speed_;
> +       AfMode mode_;
> +       bool pauseFlag_;
> +       libcamera::Size sensorSize_;
> +       bool useWeights_;
> +       uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
> +       uint16_t contrastWeights_[FOCUS_REGIONS];
> +
> +       /* Working state. */
> +       ScanState scanState_;
> +       bool initted_;
> +       double ftarget_, fsmooth_;
> +       double prevContrast_;
> +       bool pdafSeen_;

This pdafSeen_ variable is unused and gets noticed by Clang.

FAILED: src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o
clang++-11 -Isrc/ipa/raspberrypi/ipa_rpi.so.p -Isrc/ipa/raspberrypi -I../../../src/libcamera/src/ipa/raspberrypi -Iinclude -I../../../src/libcamera/include -Isrc/ipa -I../../../src/libcamera/src/ipa -I../../../src/libcamera/src/ipa/raspberrypi/controller -Iinclude/libcamera/ipa -Iinclude/libcamera -fcolor-diagnostics -D_FILE_OFFSET_BITS=64 -Wall -Winvalid-pch -Wnon-virtual-dtor -Wextra -Werror -std=c++17 -O0 -g -Wextra-semi -Wthread-safety -Wshadow -include /home/kbingham/iob/libcamera/ci/integrator/builds/build-matrix/clang-11/config.h -Wno-c99-designator -fPIC -DLIBCAMERA_BASE_PRIVATE -MD -MQ src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o -MF src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o.d -o src/ipa/raspberrypi/ipa_rpi.so.p/controller_rpi_af.cpp.o -c ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.cpp
In file included from ../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.cpp:8:
../../../src/libcamera/src/ipa/raspberrypi/controller/rpi/af.h:145:7: error: private field 'pdafSeen_' is not used [-Werror,-Wunused-private-field]
        bool pdafSeen_;
             ^
1 error generated.

As this is a single line to remove, and I don't think that's
controversial - I think that could be fixed while applying if you're ok
with that.

--
Kieran



> +       unsigned skipCount_, stepCount_, dropCount_;
> +       unsigned scanMaxIndex_;
> +       double scanMaxContrast_, scanMinContrast_;
> +       std::vector<ScanRecord> scanData_;
> +       AfState reportState_;
> +};
> +
> +} // namespace RPiController
> diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
> index 517d815bb98c..4e2689536257 100644
> --- a/src/ipa/raspberrypi/meson.build
> +++ b/src/ipa/raspberrypi/meson.build
> @@ -27,6 +27,7 @@ rpi_ipa_sources = files([
>      'controller/controller.cpp',
>      'controller/histogram.cpp',
>      'controller/algorithm.cpp',
> +    'controller/rpi/af.cpp',
>      'controller/rpi/alsc.cpp',
>      'controller/rpi/awb.cpp',
>      'controller/rpi/sharpen.cpp',
> -- 
> 2.25.1
>
Nick Hollinghurst Jan. 19, 2023, 1:07 p.m. UTC | #2
Hi all,

This patch needs a tweak to handle the case where setWindows is called
before configure -- see Af::Af(Controller *controller) constructor
below.

On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote:
>
> From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
>
> Provide the first version of the Raspberry Pi autofocus algorithm. This
> implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
> detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
> having less "hunting" behavior.
>
> Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
> Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
> Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
> ---
>  src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
>  src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++
>  src/ipa/raspberrypi/meson.build           |   1 +
>  3 files changed, 909 insertions(+)
>  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
>  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h
>
> diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
> new file mode 100644
> index 000000000000..7e2e8961085a
> --- /dev/null
> +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
> @@ -0,0 +1,755 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> + *
> + * af.cpp - Autofocus control algorithm
> + */
> +
> +#include "af.h"
> +
> +#include <iomanip>
> +#include <math.h>
> +#include <stdlib.h>
> +
> +#include <libcamera/base/log.h>
> +
> +#include <libcamera/control_ids.h>
> +
> +using namespace RPiController;
> +using namespace libcamera;
> +
> +LOG_DEFINE_CATEGORY(RPiAf)
> +
> +#define NAME "rpi.af"
> +
> +/*
> + * Default values for parameters. All may be overridden in the tuning file.
> + * Many of these values are sensor- or module-dependent; the defaults here
> + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
> + *
> + * Here all focus values are in dioptres (1/m). They are converted to hardware
> + * units when written to status.lensSetting or returned from setLensPosition().
> + *
> + * Gain and delay values are relative to the update rate, since much (not all)
> + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
> + * but note that algorithms are updated at no more than 30 Hz.
> + */
> +
> +Af::RangeDependentParams::RangeDependentParams()
> +       : focusMin(0.0),
> +         focusMax(12.0),
> +         focusDefault(1.0)
> +{
> +}
> +
> +Af::SpeedDependentParams::SpeedDependentParams()
> +       : stepCoarse(1.0),
> +         stepFine(0.25),
> +         contrastRatio(0.75),
> +         pdafGain(-0.02),
> +         pdafSquelch(0.125),
> +         maxSlew(2.0),
> +         pdafFrames(20),
> +         dropoutFrames(6),
> +         stepFrames(4)
> +{
> +}
> +
> +Af::CfgParams::CfgParams()
> +       : confEpsilon(8),
> +         confThresh(16),
> +         confClip(512),
> +         skipFrames(5),
> +         map()
> +{
> +}
> +
> +template<typename T>
> +static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
> +{
> +       auto value = params[name].get<T>();
> +       if (value)
> +               dest = *value;
> +       else
> +               LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
> +}
> +
> +void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
> +{
> +
> +       readNumber<double>(focusMin, params, "min");
> +       readNumber<double>(focusMax, params, "max");
> +       readNumber<double>(focusDefault, params, "default");
> +}
> +
> +void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
> +{
> +       readNumber<double>(stepCoarse, params, "step_coarse");
> +       readNumber<double>(stepFine, params, "step_fine");
> +       readNumber<double>(contrastRatio, params, "contrast_ratio");
> +       readNumber<double>(pdafGain, params, "pdaf_gain");
> +       readNumber<double>(pdafSquelch, params, "pdaf_squelch");
> +       readNumber<double>(maxSlew, params, "max_slew");
> +       readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
> +       readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
> +       readNumber<uint32_t>(stepFrames, params, "step_frames");
> +}
> +
> +int Af::CfgParams::read(const libcamera::YamlObject &params)
> +{
> +       if (params.contains("ranges")) {
> +               auto &rr = params["ranges"];
> +
> +               if (rr.contains("normal"))
> +                       ranges[AfRangeNormal].read(rr["normal"]);
> +               else
> +                       LOG(RPiAf, Warning) << "Missing range \"normal\"";
> +
> +               ranges[AfRangeMacro] = ranges[AfRangeNormal];
> +               if (rr.contains("macro"))
> +                       ranges[AfRangeMacro].read(rr["macro"]);
> +
> +               ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
> +                                                       ranges[AfRangeMacro].focusMin);
> +               ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
> +                                                       ranges[AfRangeMacro].focusMax);
> +               ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
> +               if (rr.contains("full"))
> +                       ranges[AfRangeFull].read(rr["full"]);
> +       } else
> +               LOG(RPiAf, Warning) << "No ranges defined";
> +
> +       if (params.contains("speeds")) {
> +               auto &ss = params["speeds"];
> +
> +               if (ss.contains("normal"))
> +                       speeds[AfSpeedNormal].read(ss["normal"]);
> +               else
> +                       LOG(RPiAf, Warning) << "Missing speed \"normal\"";
> +
> +               speeds[AfSpeedFast] = speeds[AfSpeedNormal];
> +               if (ss.contains("fast"))
> +                       speeds[AfSpeedFast].read(ss["fast"]);
> +       } else
> +               LOG(RPiAf, Warning) << "No speeds defined";
> +
> +       readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
> +       readNumber<uint32_t>(confThresh, params, "conf_thresh");
> +       readNumber<uint32_t>(confClip, params, "conf_clip");
> +       readNumber<uint32_t>(skipFrames, params, "skip_frames");
> +
> +       if (params.contains("map"))
> +               map.read(params["map"]);
> +       else
> +               LOG(RPiAf, Warning) << "No map defined";
> +
> +       return 0;
> +}
> +
> +void Af::CfgParams::initialise()
> +{
> +       if (map.empty()) {
> +               /* Default mapping from dioptres to hardware setting */
> +               static constexpr double DefaultMapX0 = 0.0;
> +               static constexpr double DefaultMapY0 = 445.0;
> +               static constexpr double DefaultMapX1 = 15.0;
> +               static constexpr double DefaultMapY1 = 925.0;
> +
> +               map.append(DefaultMapX0, DefaultMapY0);
> +               map.append(DefaultMapX1, DefaultMapY1);
> +
> +               LOG(RPiAf, Warning) << "af.map is not defined, ";
> +       }
> +}
> +
> +/* Af Algorithm class */
> +
> +Af::Af(Controller *controller)
> +       : AfAlgorithm(controller),
> +         cfg_(),
> +         range_(AfRangeNormal),
> +         speed_(AfSpeedNormal),
> +         mode_(AfAlgorithm::AfModeManual),
> +         pauseFlag_(false),
> +         sensorSize_{ 0, 0 },

sensorSize_ needs to be set before setWindows() is called. For this
version it suffices to initialize it to

        sensorSize_ { 4608, 2502 },

a more general fix may follow later.


> +         useWeights_(false),
> +         phaseWeights_{},
> +         contrastWeights_{},
> +         scanState_(ScanState::Idle),
> +         initted_(false),
> +         ftarget_(-1.0),
> +         fsmooth_(-1.0),
> +         prevContrast_(0.0),
> +         skipCount_(0),
> +         stepCount_(0),
> +         dropCount_(0),
> +         scanMaxContrast_(0.0),
> +         scanMinContrast_(1.0e9),
> +         scanData_(),
> +         reportState_(AfState::Idle)
> +{
> +       scanData_.reserve(24);
> +}
> +
> +Af::~Af()
> +{
> +}
> +
> +char const *Af::name() const
> +{
> +       return NAME;
> +}
> +
> +int Af::read(const libcamera::YamlObject &params)
> +{
> +       return cfg_.read(params);
> +}
> +
> +void Af::initialise()
> +{
> +       cfg_.initialise();
> +}
> +
> +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
> +{
> +       (void)metadata;
> +       sensorSize_.width = cameraMode.sensorWidth;
> +       sensorSize_.height = cameraMode.sensorHeight;
> +
> +       if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
> +               /*
> +                * If a scan was in progress, re-start it, as CDAF statistics
> +                * may have changed. Though if the application is just about
> +                * to take a still picture, this will not help...
> +                */
> +               startProgrammedScan();
> +       }
> +       skipCount_ = cfg_.skipFrames;
> +}
> +
> +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const
> +{
> +       static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
> +       };
> +       int32_t sumW = 0;
> +       int32_t sumWc = 0;
> +       int32_t sumWcp = 0;
> +       auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;
> +
> +       for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {
> +               for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {
> +                       if (wgts[i][j]) {
> +                               uint32_t c = data.conf[i][j];
> +                               if (c >= cfg_.confThresh) {
> +                                       if (c > cfg_.confClip)
> +                                               c = cfg_.confClip;
> +                                       sumWc += wgts[i][j] * (int32_t)c;
> +                                       c -= (cfg_.confThresh >> 1);
> +                                       sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;
> +                               }
> +                               sumW += wgts[i][j];
> +                       }
> +               }
> +       }
> +
> +       if (sumWc > 0) {
> +               phase = (double)sumWcp / (double)sumWc;
> +               conf = (double)sumWc / (double)sumW;
> +               return true;
> +       } else {
> +               phase = 0.0;
> +               conf = 0.0;
> +               return false;
> +       }
> +}
> +
> +void Af::doPDAF(double phase, double conf)
> +{
> +       /* Apply loop gain */
> +       phase *= cfg_.speeds[speed_].pdafGain;
> +
> +       if (mode_ == AfModeContinuous) {
> +               /*
> +                * PDAF in Continuous mode. Scale down lens movement when
> +                * delta is small or confidence is low, to suppress wobble.
> +                */
> +               if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
> +                       double a = phase / cfg_.speeds[speed_].pdafSquelch;
> +                       phase *= a * a;
> +               }
> +               phase *= conf / (conf + cfg_.confEpsilon);
> +       } else {
> +               /*
> +                * PDAF in triggered-auto mode. Allow early termination when
> +                * phase delta is small; scale down lens movements towards
> +                * the end of the sequence, to ensure a stable image.
> +                */
> +               if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
> +                       if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
> +                               stepCount_ = cfg_.speeds[speed_].stepFrames;
> +               } else
> +                       phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
> +       }
> +
> +       /* Apply slew rate limit. Report failure if out of bounds. */
> +       if (phase < -cfg_.speeds[speed_].maxSlew) {
> +               phase = -cfg_.speeds[speed_].maxSlew;
> +               reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
> +                                                                         : AfState::Scanning;
> +       } else if (phase > cfg_.speeds[speed_].maxSlew) {
> +               phase = cfg_.speeds[speed_].maxSlew;
> +               reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
> +                                                                         : AfState::Scanning;
> +       } else
> +               reportState_ = AfState::Focused;
> +
> +       ftarget_ = fsmooth_ + phase;
> +}
> +
> +bool Af::earlyTerminationByPhase(double phase)
> +{
> +       if (scanData_.size() > 0 &&
> +           scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
> +               double oldFocus = scanData_[scanData_.size() - 1].focus;
> +               double oldPhase = scanData_[scanData_.size() - 1].phase;
> +
> +               /*
> +                * Check that the gradient is finite and has the expected sign;
> +                * Interpolate/extrapolate the lens position for zero phase.
> +                * Check that the extrapolation is well-conditioned.
> +                */
> +               if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
> +                       double param = phase / (phase - oldPhase);
> +                       if (-3.0 <= param && param <= 3.5) {
> +                               ftarget_ += param * (oldFocus - ftarget_);
> +                               LOG(RPiAf, Debug) << "ETBP: param=" << param;
> +                               return true;
> +                       }
> +               }
> +       }
> +
> +       return false;
> +}
> +
> +double Af::findPeak(unsigned i) const
> +{
> +       double f = scanData_[i].focus;
> +
> +       if (i > 0 && i + 1 < scanData_.size()) {
> +               double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
> +               double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
> +               if (dropLo < dropHi) {
> +                       double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
> +                       f += param * (scanData_[i - 1].focus - f);
> +               } else if (dropHi < dropLo) {
> +                       double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
> +                       f += param * (scanData_[i + 1].focus - f);
> +               }
> +       }
> +
> +       LOG(RPiAf, Debug) << "FindPeak: " << f;
> +       return f;
> +}
> +
> +void Af::doScan(double contrast, double phase, double conf)
> +{
> +       /* Record lens position, contrast and phase values for the current scan */
> +       if (scanData_.empty() || contrast > scanMaxContrast_) {
> +               scanMaxContrast_ = contrast;
> +               scanMaxIndex_ = scanData_.size();
> +       }
> +       if (contrast < scanMinContrast_)
> +               scanMinContrast_ = contrast;
> +       scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
> +
> +       if (scanState_ == ScanState::Coarse) {
> +               if (ftarget_ >= cfg_.ranges[range_].focusMax ||
> +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> +                       /*
> +                        * Finished course scan, or termination based on contrast.
> +                        * Jump to just after max contrast and start fine scan.
> +                        */
> +                       ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
> +                                       2.0 * cfg_.speeds[speed_].stepFine);
> +                       scanState_ = ScanState::Fine;
> +                       scanData_.clear();
> +               } else
> +                       ftarget_ += cfg_.speeds[speed_].stepCoarse;
> +       } else { /* ScanState::Fine */
> +               if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
> +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> +                       /*
> +                        * Finished fine scan, or termination based on contrast.
> +                        * Use quadratic peak-finding to find best contrast position.
> +                        */
> +                       ftarget_ = findPeak(scanMaxIndex_);
> +                       scanState_ = ScanState::Settle;
> +               } else
> +                       ftarget_ -= cfg_.speeds[speed_].stepFine;
> +       }
> +
> +       stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
> +}
> +
> +void Af::doAF(double contrast, double phase, double conf)
> +{
> +       /* Skip frames at startup and after mode change */
> +       if (skipCount_ > 0) {
> +               LOG(RPiAf, Debug) << "SKIP";
> +               skipCount_--;
> +               return;
> +       }
> +
> +       if (scanState_ == ScanState::Pdaf) {
> +               /*
> +                * Use PDAF closed-loop control whenever available, in both CAF
> +                * mode and (for a limited number of iterations) when triggered.
> +                * If PDAF fails (due to poor contrast, noise or large defocus),
> +                * fall back to a CDAF-based scan. To avoid "nuisance" scans,
> +                * scan only after a number of frames with low PDAF confidence.
> +                */
> +               if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {
> +                       doPDAF(phase, conf);
> +                       if (stepCount_ > 0)
> +                               stepCount_--;
> +                       else if (mode_ != AfModeContinuous)
> +                               scanState_ = ScanState::Idle;
> +                       dropCount_ = 0;
> +               } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
> +                       startProgrammedScan();
> +       } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
> +               /*
> +                * Scanning sequence. This means PDAF has become unavailable.
> +                * Allow a delay between steps for CDAF FoM statistics to be
> +                * updated, and a "settling time" at the end of the sequence.
> +                * [A coarse or fine scan can be abandoned if two PDAF samples
> +                * allow direct interpolation of the zero-phase lens position.]
> +                */
> +               if (stepCount_ > 0)
> +                       stepCount_--;
> +               else if (scanState_ == ScanState::Settle) {
> +                       if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
> +                           scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
> +                               reportState_ = AfState::Focused;
> +                       else
> +                               reportState_ = AfState::Failed;
> +                       if (mode_ == AfModeContinuous && !pauseFlag_ &&
> +                           cfg_.speeds[speed_].dropoutFrames > 0)
> +                               scanState_ = ScanState::Pdaf;
> +                       else
> +                               scanState_ = ScanState::Idle;
> +                       scanData_.clear();
> +               } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
> +                       scanState_ = ScanState::Settle;
> +                       stepCount_ = (mode_ == AfModeContinuous) ? 0
> +                                                                : cfg_.speeds[speed_].stepFrames;
> +               } else
> +                       doScan(contrast, phase, conf);
> +       }
> +}
> +
> +void Af::updateLensPosition()
> +{
> +       if (mode_ != AfModeManual) {
> +               ftarget_ = std::clamp(ftarget_,
> +                               cfg_.ranges[range_].focusMin,
> +                               cfg_.ranges[range_].focusMax);
> +       }
> +
> +       /* \todo Add a clip for manual lens position to be within the PWL limits. */
> +
> +       if (initted_) {
> +               /* from a known lens position: apply slew rate limit */
> +               fsmooth_ = std::clamp(ftarget_,
> +                                     fsmooth_ - cfg_.speeds[speed_].maxSlew,
> +                                     fsmooth_ + cfg_.speeds[speed_].maxSlew);
> +       } else {
> +               /* from an unknown position: go straight to target, but add delay */
> +               fsmooth_ = ftarget_;
> +               initted_ = true;
> +               skipCount_ = cfg_.skipFrames;
> +       }
> +}
> +
> +/*
> + * PDAF phase data are available in prepare(), but CDAF statistics are not
> + * available until process(). We are gambling on the availability of PDAF.
> + * To expedite feedback control using PDAF, issue the V4L2 lens control from
> + * prepare(). Conversely, during scans, we must allow an extra frame delay
> + * between steps, to retrieve CDAF statistics from the previous process()
> + * so we can terminate the scan early without having to change our minds.
> + */
> +
> +void Af::prepare(Metadata *imageMetadata)
> +{
> +       if (initted_) {
> +               /* Get PDAF from the embedded metadata, and run AF algorithm core */
> +               PdafData data;
> +               double phase = 0.0, conf = 0.0;
> +               double oldFt = ftarget_;
> +               double oldFs = fsmooth_;
> +               ScanState oldSs = scanState_;
> +               uint32_t oldSt = stepCount_;
> +               if (imageMetadata->get("pdaf.data", data) == 0)
> +                       getPhase(data, phase, conf);
> +               doAF(prevContrast_, phase, conf);
> +               updateLensPosition();
> +               LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
> +                                 << static_cast<unsigned int>(reportState_)
> +                                 << " sst" << static_cast<unsigned int>(oldSs)
> +                                 << "->" << static_cast<unsigned int>(scanState_)
> +                                 << " stp" << oldSt << "->" << stepCount_
> +                                 << " ft" << oldFt << "->" << ftarget_
> +                                 << " fs" << oldFs << "->" << fsmooth_
> +                                 << " cont=" << (int)prevContrast_
> +                                 << " phase=" << (int)phase << " conf=" << (int)conf;
> +       }
> +
> +       /* Report status and produce new lens setting */
> +       AfStatus status;
> +       if (pauseFlag_)
> +               status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
> +                                                                   : AfPauseState::Pausing;
> +       else
> +               status.pauseState = AfPauseState::Running;
> +
> +       if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
> +               status.state = AfState::Scanning;
> +       else
> +               status.state = reportState_;
> +       status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
> +                                     : std::nullopt;
> +       imageMetadata->set("af.status", status);
> +}
> +
> +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const
> +{
> +       uint32_t totW = 0, totWc = 0;
> +
> +       if (useWeights_) {
> +               for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {
> +                       unsigned w = contrastWeights_[i];
> +                       totW += w;
> +                       totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);
> +               }
> +       }
> +       if (totW == 0) {
> +               totW = 2;
> +               totWc = (focus_stats[5].contrast_val[1][1] >> 10) +
> +                        (focus_stats[6].contrast_val[1][1] >> 10);
> +       }
> +
> +       return (double)totWc / (double)totW;
> +}
> +
> +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
> +{
> +       (void)imageMetadata;
> +       prevContrast_ = getContrast(stats->focus_stats);
> +}
> +
> +/* Controls */
> +
> +void Af::setRange(AfRange r)
> +{
> +       LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
> +       if (r < AfAlgorithm::AfRangeMax)
> +               range_ = r;
> +}
> +
> +void Af::setSpeed(AfSpeed s)
> +{
> +       LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
> +       if (s < AfAlgorithm::AfSpeedMax) {
> +               if (scanState_ == ScanState::Pdaf &&
> +                   cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
> +                       stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
> +               speed_ = s;
> +       }
> +}
> +
> +void Af::setMetering(bool mode)
> +{
> +       useWeights_ = mode;
> +}
> +
> +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
> +{
> +       /*
> +        * Here we just merge all of the given windows, weighted by area.
> +        * If there are more than 15 overlapping windows, overflow can occur.
> +        * TODO: A better approach might be to find the phase in each window
> +        * and choose either the closest or the highest-confidence one?
> +        *
> +        * Using mostly "int" arithmetic, because Rectangle has signed x, y
> +        */
> +       ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);
> +       int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);
> +       int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);
> +       int gridA = gridY * gridX;
> +
> +       for (int i = 0; i < PDAF_DATA_ROWS; ++i)
> +               std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);
> +       std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);
> +
> +       for (auto &w : wins) {
> +               for (int i = 0; i < PDAF_DATA_ROWS; ++i) {
> +                       int y0 = std::max(gridY * i, w.y);
> +                       int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));
> +                       if (y0 >= y1)
> +                               continue;
> +                       y1 -= y0;
> +                       for (int j = 0; j < PDAF_DATA_COLS; ++j) {
> +                               int x0 = std::max(gridX * j, w.x);
> +                               int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));
> +                               if (x0 >= x1)
> +                                       continue;
> +                               int a = y1 * (x1 - x0);
> +                               a = (16 * a + gridA - 1) / gridA;
> +                               phaseWeights_[i][j] += a;
> +                               contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;
> +                       }
> +               }
> +       }
> +}
> +
> +bool Af::setLensPosition(double dioptres, int *hwpos)
> +{
> +       bool changed = false;
> +
> +       if (mode_ == AfModeManual) {
> +               LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
> +               changed = !(initted_ && fsmooth_ == dioptres);
> +               ftarget_ = dioptres;
> +               updateLensPosition();
> +       }
> +
> +       if (hwpos)
> +               *hwpos = cfg_.map.eval(fsmooth_);
> +
> +       return changed;
> +}
> +
> +std::optional<double> Af::getLensPosition() const
> +{
> +       /*
> +        * \todo We ought to perform some precise timing here to determine
> +        * the current lens position.
> +        */
> +       return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
> +}
> +
> +void Af::startCAF()
> +{
> +       /* Try PDAF if the tuning file permits it for CAF; else CDAF */
> +       if (cfg_.speeds[speed_].dropoutFrames > 0) {
> +               if (!initted_) {
> +                       ftarget_ = cfg_.ranges[range_].focusDefault;
> +                       updateLensPosition();
> +               }
> +               scanState_ = ScanState::Pdaf;
> +               scanData_.clear();
> +               dropCount_ = 0;
> +               reportState_ = AfState::Scanning;
> +       } else {
> +               startProgrammedScan();
> +       }
> +}
> +
> +void Af::startProgrammedScan()
> +{
> +       ftarget_ = cfg_.ranges[range_].focusMin;
> +       updateLensPosition();
> +       scanState_ = ScanState::Coarse;
> +       scanMaxContrast_ = 0.0;
> +       scanMinContrast_ = 1.0e9;
> +       scanMaxIndex_ = 0;
> +       scanData_.clear();
> +       stepCount_ = cfg_.speeds[speed_].stepFrames;
> +       reportState_ = AfState::Scanning;
> +}
> +
> +void Af::goIdle()
> +{
> +       scanState_ = ScanState::Idle;
> +       reportState_ = AfState::Idle;
> +       scanData_.clear();
> +}
> +
> +void Af::cancelScan()
> +{
> +       LOG(RPiAf, Debug) << "cancelScan";
> +       if (mode_ == AfModeAuto)
> +               goIdle();
> +}
> +
> +void Af::triggerScan()
> +{
> +       LOG(RPiAf, Debug) << "triggerScan";
> +       if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {
> +               /* Try PDAF if the tuning file permits it for Auto; else CDAF */
> +               if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {
> +                       if (!initted_) {
> +                               ftarget_ = cfg_.ranges[range_].focusDefault;
> +                               updateLensPosition();
> +                       }
> +                       stepCount_ = cfg_.speeds[speed_].pdafFrames;
> +                       scanState_ = ScanState::Pdaf;
> +                       dropCount_ = 0;
> +               } else
> +                       startProgrammedScan();
> +               reportState_ = AfState::Scanning;
> +       }
> +}
> +
> +void Af::setMode(AfAlgorithm::AfMode mode)
> +{
> +       LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
> +       if (mode_ != mode) {
> +               mode_ = mode;
> +               pauseFlag_ = false;
> +               if (mode == AfModeContinuous)
> +                       startCAF();
> +               else if (mode != AfModeAuto)
> +                       goIdle();
> +       }
> +}
> +
> +AfAlgorithm::AfMode Af::getMode() const
> +{
> +       return mode_;
> +}
> +
> +void Af::pause(AfAlgorithm::AfPause pause)
> +{
> +       LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
> +       if (mode_ == AfModeContinuous) {
> +               if (pause == AfPauseResume && pauseFlag_) {
> +                       startCAF();
> +                       pauseFlag_ = false;
> +               } else if (pause != AfPauseResume && !pauseFlag_) {
> +                       if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)
> +                               goIdle();
> +                       pauseFlag_ = true;
> +               }
> +       }
> +}
> +
> +// Register algorithm with the system.
> +static Algorithm *create(Controller *controller)
> +{
> +       return (Algorithm *)new Af(controller);
> +}
> +static RegisterAlgorithm reg(NAME, &create);
> diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
> new file mode 100644
> index 000000000000..0431bd70ae29
> --- /dev/null
> +++ b/src/ipa/raspberrypi/controller/rpi/af.h
> @@ -0,0 +1,153 @@
> +/* SPDX-License-Identifier: BSD-2-Clause */
> +/*
> + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> + *
> + * af.h - Autofocus control algorithm
> + */
> +#pragma once
> +
> +#include "../af_algorithm.h"
> +#include "../af_status.h"
> +#include "../pdaf_data.h"
> +#include "../pwl.h"
> +
> +/*
> + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
> + *
> + * Whenever PDAF is available, it is used in a continuous feedback loop.
> + * When triggered in auto mode, we simply enable AF for a limited number
> + * of frames (it may terminate early if the delta becomes small enough).
> + *
> + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
> + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
> + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
> + * estimate the lens position with peak contrast. This is slower due to
> + * extra latency in the ISP, and requires a settling time between steps.
> + *
> + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
> + * "nuisance" scans. During each interval where PDAF is not working, only
> + * ONE scan will be performed; CAF cannot track objects using CDAF alone.
> + *
> + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
> + */
> +
> +namespace RPiController {
> +
> +class Af : public AfAlgorithm
> +{
> +public:
> +       Af(Controller *controller = NULL);
> +       ~Af();
> +       char const *name() const override;
> +       int read(const libcamera::YamlObject &params) override;
> +       void initialise() override;
> +
> +       /* IPA calls */
> +       void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
> +       void prepare(Metadata *imageMetadata) override;
> +       void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
> +
> +       /* controls */
> +       void setRange(AfRange range) override;
> +       void setSpeed(AfSpeed speed) override;
> +       void setMetering(bool use_windows) override;
> +       void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
> +       void setMode(AfMode mode) override;
> +       AfMode getMode() const override;
> +       bool setLensPosition(double dioptres, int32_t *hwpos) override;
> +       std::optional<double> getLensPosition() const override;
> +       void triggerScan() override;
> +       void cancelScan() override;
> +       void pause(AfPause pause) override;
> +
> +private:
> +       enum class ScanState {
> +               Idle,
> +               Pdaf,
> +               Coarse,
> +               Fine,
> +               Settle
> +       };
> +
> +       struct RangeDependentParams {
> +               double focusMin;                /* lower (far) limit in dipotres */
> +               double focusMax;                /* upper (near) limit in dioptres */
> +               double focusDefault;            /* default setting ("hyperfocal") */
> +
> +               RangeDependentParams();
> +               void read(const libcamera::YamlObject &params);
> +       };
> +
> +       struct SpeedDependentParams {
> +               double stepCoarse;              /* used for scans */
> +               double stepFine;                /* used for scans */
> +               double contrastRatio;           /* used for scan termination and reporting */
> +               double pdafGain;                /* coefficient for PDAF feedback loop */
> +               double pdafSquelch;             /* PDAF stability parameter (device-specific) */
> +               double maxSlew;                 /* limit for lens movement per frame */
> +               uint32_t pdafFrames;            /* number of iterations when triggered */
> +               uint32_t dropoutFrames;         /* number of non-PDAF frames to switch to CDAF */
> +               uint32_t stepFrames;            /* frames to skip in between steps of a scan */
> +
> +               SpeedDependentParams();
> +               void read(const libcamera::YamlObject &params);
> +       };
> +
> +       struct CfgParams {
> +               RangeDependentParams ranges[AfRangeMax];
> +               SpeedDependentParams speeds[AfSpeedMax];
> +               uint32_t confEpsilon;           /* PDAF hysteresis threshold (sensor-specific) */
> +               uint32_t confThresh;            /* PDAF confidence cell min (sensor-specific) */
> +               uint32_t confClip;              /* PDAF confidence cell max (sensor-specific) */
> +               uint32_t skipFrames;            /* frames to skip at start or modeswitch */
> +               Pwl map;                        /* converts dioptres -> lens driver position */
> +
> +               CfgParams();
> +               int read(const libcamera::YamlObject &params);
> +               void initialise();
> +       };
> +
> +       struct ScanRecord {
> +               double focus;
> +               double contrast;
> +               double phase;
> +               double conf;
> +       };
> +
> +       bool getPhase(PdafData const &data, double &phase, double &conf) const;
> +       double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
> +       void doPDAF(double phase, double conf);
> +       bool earlyTerminationByPhase(double phase);
> +       void doScan(double contrast, double phase, double conf);
> +       double findPeak(unsigned index) const;
> +       void doAF(double contrast, double phase, double conf);
> +       void updateLensPosition();
> +       void startProgrammedScan();
> +       void startCAF();
> +       void goIdle();
> +
> +       /* Configuration and settings */
> +       CfgParams cfg_;
> +       AfRange range_;
> +       AfSpeed speed_;
> +       AfMode mode_;
> +       bool pauseFlag_;
> +       libcamera::Size sensorSize_;
> +       bool useWeights_;
> +       uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
> +       uint16_t contrastWeights_[FOCUS_REGIONS];
> +
> +       /* Working state. */
> +       ScanState scanState_;
> +       bool initted_;
> +       double ftarget_, fsmooth_;
> +       double prevContrast_;
> +       bool pdafSeen_;
> +       unsigned skipCount_, stepCount_, dropCount_;
> +       unsigned scanMaxIndex_;
> +       double scanMaxContrast_, scanMinContrast_;
> +       std::vector<ScanRecord> scanData_;
> +       AfState reportState_;
> +};
> +
> +} // namespace RPiController
> diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
> index 517d815bb98c..4e2689536257 100644
> --- a/src/ipa/raspberrypi/meson.build
> +++ b/src/ipa/raspberrypi/meson.build
> @@ -27,6 +27,7 @@ rpi_ipa_sources = files([
>      'controller/controller.cpp',
>      'controller/histogram.cpp',
>      'controller/algorithm.cpp',
> +    'controller/rpi/af.cpp',
>      'controller/rpi/alsc.cpp',
>      'controller/rpi/awb.cpp',
>      'controller/rpi/sharpen.cpp',
> --
> 2.25.1
>
Kieran Bingham Jan. 20, 2023, 4:39 p.m. UTC | #3
Hi Nick, / Naush,

Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44)
> Hi all,
> 
> This patch needs a tweak to handle the case where setWindows is called
> before configure -- see Af::Af(Controller *controller) constructor
> below.
> 
> On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote:
> >
> > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> >
> > Provide the first version of the Raspberry Pi autofocus algorithm. This
> > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
> > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
> > having less "hunting" behavior.
> >
> > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> > Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
> > Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
> > Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
> > ---
> >  src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
> >  src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++
> >  src/ipa/raspberrypi/meson.build           |   1 +
> >  3 files changed, 909 insertions(+)
> >  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
> >  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h
> >
> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > new file mode 100644
> > index 000000000000..7e2e8961085a
> > --- /dev/null
> > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > @@ -0,0 +1,755 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> > + *
> > + * af.cpp - Autofocus control algorithm
> > + */
> > +
> > +#include "af.h"
> > +
> > +#include <iomanip>
> > +#include <math.h>
> > +#include <stdlib.h>
> > +
> > +#include <libcamera/base/log.h>
> > +
> > +#include <libcamera/control_ids.h>
> > +
> > +using namespace RPiController;
> > +using namespace libcamera;
> > +
> > +LOG_DEFINE_CATEGORY(RPiAf)
> > +
> > +#define NAME "rpi.af"
> > +
> > +/*
> > + * Default values for parameters. All may be overridden in the tuning file.
> > + * Many of these values are sensor- or module-dependent; the defaults here
> > + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
> > + *
> > + * Here all focus values are in dioptres (1/m). They are converted to hardware
> > + * units when written to status.lensSetting or returned from setLensPosition().
> > + *
> > + * Gain and delay values are relative to the update rate, since much (not all)
> > + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
> > + * but note that algorithms are updated at no more than 30 Hz.
> > + */
> > +
> > +Af::RangeDependentParams::RangeDependentParams()
> > +       : focusMin(0.0),
> > +         focusMax(12.0),
> > +         focusDefault(1.0)
> > +{
> > +}
> > +
> > +Af::SpeedDependentParams::SpeedDependentParams()
> > +       : stepCoarse(1.0),
> > +         stepFine(0.25),
> > +         contrastRatio(0.75),
> > +         pdafGain(-0.02),
> > +         pdafSquelch(0.125),
> > +         maxSlew(2.0),
> > +         pdafFrames(20),
> > +         dropoutFrames(6),
> > +         stepFrames(4)
> > +{
> > +}
> > +
> > +Af::CfgParams::CfgParams()
> > +       : confEpsilon(8),
> > +         confThresh(16),
> > +         confClip(512),
> > +         skipFrames(5),
> > +         map()
> > +{
> > +}
> > +
> > +template<typename T>
> > +static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
> > +{
> > +       auto value = params[name].get<T>();
> > +       if (value)
> > +               dest = *value;
> > +       else
> > +               LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
> > +}
> > +
> > +void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
> > +{
> > +
> > +       readNumber<double>(focusMin, params, "min");
> > +       readNumber<double>(focusMax, params, "max");
> > +       readNumber<double>(focusDefault, params, "default");
> > +}
> > +
> > +void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
> > +{
> > +       readNumber<double>(stepCoarse, params, "step_coarse");
> > +       readNumber<double>(stepFine, params, "step_fine");
> > +       readNumber<double>(contrastRatio, params, "contrast_ratio");
> > +       readNumber<double>(pdafGain, params, "pdaf_gain");
> > +       readNumber<double>(pdafSquelch, params, "pdaf_squelch");
> > +       readNumber<double>(maxSlew, params, "max_slew");
> > +       readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
> > +       readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
> > +       readNumber<uint32_t>(stepFrames, params, "step_frames");
> > +}
> > +
> > +int Af::CfgParams::read(const libcamera::YamlObject &params)
> > +{
> > +       if (params.contains("ranges")) {
> > +               auto &rr = params["ranges"];
> > +
> > +               if (rr.contains("normal"))
> > +                       ranges[AfRangeNormal].read(rr["normal"]);
> > +               else
> > +                       LOG(RPiAf, Warning) << "Missing range \"normal\"";
> > +
> > +               ranges[AfRangeMacro] = ranges[AfRangeNormal];
> > +               if (rr.contains("macro"))
> > +                       ranges[AfRangeMacro].read(rr["macro"]);
> > +
> > +               ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
> > +                                                       ranges[AfRangeMacro].focusMin);
> > +               ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
> > +                                                       ranges[AfRangeMacro].focusMax);
> > +               ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
> > +               if (rr.contains("full"))
> > +                       ranges[AfRangeFull].read(rr["full"]);
> > +       } else
> > +               LOG(RPiAf, Warning) << "No ranges defined";
> > +
> > +       if (params.contains("speeds")) {
> > +               auto &ss = params["speeds"];
> > +
> > +               if (ss.contains("normal"))
> > +                       speeds[AfSpeedNormal].read(ss["normal"]);
> > +               else
> > +                       LOG(RPiAf, Warning) << "Missing speed \"normal\"";
> > +
> > +               speeds[AfSpeedFast] = speeds[AfSpeedNormal];
> > +               if (ss.contains("fast"))
> > +                       speeds[AfSpeedFast].read(ss["fast"]);
> > +       } else
> > +               LOG(RPiAf, Warning) << "No speeds defined";
> > +
> > +       readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
> > +       readNumber<uint32_t>(confThresh, params, "conf_thresh");
> > +       readNumber<uint32_t>(confClip, params, "conf_clip");
> > +       readNumber<uint32_t>(skipFrames, params, "skip_frames");
> > +
> > +       if (params.contains("map"))
> > +               map.read(params["map"]);
> > +       else
> > +               LOG(RPiAf, Warning) << "No map defined";
> > +
> > +       return 0;
> > +}
> > +
> > +void Af::CfgParams::initialise()
> > +{
> > +       if (map.empty()) {
> > +               /* Default mapping from dioptres to hardware setting */
> > +               static constexpr double DefaultMapX0 = 0.0;
> > +               static constexpr double DefaultMapY0 = 445.0;
> > +               static constexpr double DefaultMapX1 = 15.0;
> > +               static constexpr double DefaultMapY1 = 925.0;
> > +
> > +               map.append(DefaultMapX0, DefaultMapY0);
> > +               map.append(DefaultMapX1, DefaultMapY1);
> > +
> > +               LOG(RPiAf, Warning) << "af.map is not defined, ";
> > +       }
> > +}
> > +
> > +/* Af Algorithm class */
> > +
> > +Af::Af(Controller *controller)
> > +       : AfAlgorithm(controller),
> > +         cfg_(),
> > +         range_(AfRangeNormal),
> > +         speed_(AfSpeedNormal),
> > +         mode_(AfAlgorithm::AfModeManual),
> > +         pauseFlag_(false),
> > +         sensorSize_{ 0, 0 },
> 
> sensorSize_ needs to be set before setWindows() is called. For this
> version it suffices to initialize it to
> 
>         sensorSize_ { 4608, 2502 },
> 
> a more general fix may follow later.

Can you send a tested update to this patch please? No need to send the
whole series again.

If that can include the fix I highlighted, (Remove pdafSeen_) I believe
I can merge this whole series.

--
Kieran


> 
> 
> > +         useWeights_(false),
> > +         phaseWeights_{},
> > +         contrastWeights_{},
> > +         scanState_(ScanState::Idle),
> > +         initted_(false),
> > +         ftarget_(-1.0),
> > +         fsmooth_(-1.0),
> > +         prevContrast_(0.0),
> > +         skipCount_(0),
> > +         stepCount_(0),
> > +         dropCount_(0),
> > +         scanMaxContrast_(0.0),
> > +         scanMinContrast_(1.0e9),
> > +         scanData_(),
> > +         reportState_(AfState::Idle)
> > +{
> > +       scanData_.reserve(24);
> > +}
> > +
> > +Af::~Af()
> > +{
> > +}
> > +
> > +char const *Af::name() const
> > +{
> > +       return NAME;
> > +}
> > +
> > +int Af::read(const libcamera::YamlObject &params)
> > +{
> > +       return cfg_.read(params);
> > +}
> > +
> > +void Af::initialise()
> > +{
> > +       cfg_.initialise();
> > +}
> > +
> > +void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
> > +{
> > +       (void)metadata;
> > +       sensorSize_.width = cameraMode.sensorWidth;
> > +       sensorSize_.height = cameraMode.sensorHeight;
> > +
> > +       if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
> > +               /*
> > +                * If a scan was in progress, re-start it, as CDAF statistics
> > +                * may have changed. Though if the application is just about
> > +                * to take a still picture, this will not help...
> > +                */
> > +               startProgrammedScan();
> > +       }
> > +       skipCount_ = cfg_.skipFrames;
> > +}
> > +
> > +bool Af::getPhase(PdafData const &data, double &phase, double &conf) const
> > +{
> > +       static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
> > +               { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
> > +       };
> > +       int32_t sumW = 0;
> > +       int32_t sumWc = 0;
> > +       int32_t sumWcp = 0;
> > +       auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;
> > +
> > +       for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {
> > +               for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {
> > +                       if (wgts[i][j]) {
> > +                               uint32_t c = data.conf[i][j];
> > +                               if (c >= cfg_.confThresh) {
> > +                                       if (c > cfg_.confClip)
> > +                                               c = cfg_.confClip;
> > +                                       sumWc += wgts[i][j] * (int32_t)c;
> > +                                       c -= (cfg_.confThresh >> 1);
> > +                                       sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;
> > +                               }
> > +                               sumW += wgts[i][j];
> > +                       }
> > +               }
> > +       }
> > +
> > +       if (sumWc > 0) {
> > +               phase = (double)sumWcp / (double)sumWc;
> > +               conf = (double)sumWc / (double)sumW;
> > +               return true;
> > +       } else {
> > +               phase = 0.0;
> > +               conf = 0.0;
> > +               return false;
> > +       }
> > +}
> > +
> > +void Af::doPDAF(double phase, double conf)
> > +{
> > +       /* Apply loop gain */
> > +       phase *= cfg_.speeds[speed_].pdafGain;
> > +
> > +       if (mode_ == AfModeContinuous) {
> > +               /*
> > +                * PDAF in Continuous mode. Scale down lens movement when
> > +                * delta is small or confidence is low, to suppress wobble.
> > +                */
> > +               if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
> > +                       double a = phase / cfg_.speeds[speed_].pdafSquelch;
> > +                       phase *= a * a;
> > +               }
> > +               phase *= conf / (conf + cfg_.confEpsilon);
> > +       } else {
> > +               /*
> > +                * PDAF in triggered-auto mode. Allow early termination when
> > +                * phase delta is small; scale down lens movements towards
> > +                * the end of the sequence, to ensure a stable image.
> > +                */
> > +               if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
> > +                       if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
> > +                               stepCount_ = cfg_.speeds[speed_].stepFrames;
> > +               } else
> > +                       phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
> > +       }
> > +
> > +       /* Apply slew rate limit. Report failure if out of bounds. */
> > +       if (phase < -cfg_.speeds[speed_].maxSlew) {
> > +               phase = -cfg_.speeds[speed_].maxSlew;
> > +               reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
> > +                                                                         : AfState::Scanning;
> > +       } else if (phase > cfg_.speeds[speed_].maxSlew) {
> > +               phase = cfg_.speeds[speed_].maxSlew;
> > +               reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
> > +                                                                         : AfState::Scanning;
> > +       } else
> > +               reportState_ = AfState::Focused;
> > +
> > +       ftarget_ = fsmooth_ + phase;
> > +}
> > +
> > +bool Af::earlyTerminationByPhase(double phase)
> > +{
> > +       if (scanData_.size() > 0 &&
> > +           scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
> > +               double oldFocus = scanData_[scanData_.size() - 1].focus;
> > +               double oldPhase = scanData_[scanData_.size() - 1].phase;
> > +
> > +               /*
> > +                * Check that the gradient is finite and has the expected sign;
> > +                * Interpolate/extrapolate the lens position for zero phase.
> > +                * Check that the extrapolation is well-conditioned.
> > +                */
> > +               if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
> > +                       double param = phase / (phase - oldPhase);
> > +                       if (-3.0 <= param && param <= 3.5) {
> > +                               ftarget_ += param * (oldFocus - ftarget_);
> > +                               LOG(RPiAf, Debug) << "ETBP: param=" << param;
> > +                               return true;
> > +                       }
> > +               }
> > +       }
> > +
> > +       return false;
> > +}
> > +
> > +double Af::findPeak(unsigned i) const
> > +{
> > +       double f = scanData_[i].focus;
> > +
> > +       if (i > 0 && i + 1 < scanData_.size()) {
> > +               double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
> > +               double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
> > +               if (dropLo < dropHi) {
> > +                       double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
> > +                       f += param * (scanData_[i - 1].focus - f);
> > +               } else if (dropHi < dropLo) {
> > +                       double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
> > +                       f += param * (scanData_[i + 1].focus - f);
> > +               }
> > +       }
> > +
> > +       LOG(RPiAf, Debug) << "FindPeak: " << f;
> > +       return f;
> > +}
> > +
> > +void Af::doScan(double contrast, double phase, double conf)
> > +{
> > +       /* Record lens position, contrast and phase values for the current scan */
> > +       if (scanData_.empty() || contrast > scanMaxContrast_) {
> > +               scanMaxContrast_ = contrast;
> > +               scanMaxIndex_ = scanData_.size();
> > +       }
> > +       if (contrast < scanMinContrast_)
> > +               scanMinContrast_ = contrast;
> > +       scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
> > +
> > +       if (scanState_ == ScanState::Coarse) {
> > +               if (ftarget_ >= cfg_.ranges[range_].focusMax ||
> > +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> > +                       /*
> > +                        * Finished course scan, or termination based on contrast.
> > +                        * Jump to just after max contrast and start fine scan.
> > +                        */
> > +                       ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
> > +                                       2.0 * cfg_.speeds[speed_].stepFine);
> > +                       scanState_ = ScanState::Fine;
> > +                       scanData_.clear();
> > +               } else
> > +                       ftarget_ += cfg_.speeds[speed_].stepCoarse;
> > +       } else { /* ScanState::Fine */
> > +               if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
> > +                   contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
> > +                       /*
> > +                        * Finished fine scan, or termination based on contrast.
> > +                        * Use quadratic peak-finding to find best contrast position.
> > +                        */
> > +                       ftarget_ = findPeak(scanMaxIndex_);
> > +                       scanState_ = ScanState::Settle;
> > +               } else
> > +                       ftarget_ -= cfg_.speeds[speed_].stepFine;
> > +       }
> > +
> > +       stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
> > +}
> > +
> > +void Af::doAF(double contrast, double phase, double conf)
> > +{
> > +       /* Skip frames at startup and after mode change */
> > +       if (skipCount_ > 0) {
> > +               LOG(RPiAf, Debug) << "SKIP";
> > +               skipCount_--;
> > +               return;
> > +       }
> > +
> > +       if (scanState_ == ScanState::Pdaf) {
> > +               /*
> > +                * Use PDAF closed-loop control whenever available, in both CAF
> > +                * mode and (for a limited number of iterations) when triggered.
> > +                * If PDAF fails (due to poor contrast, noise or large defocus),
> > +                * fall back to a CDAF-based scan. To avoid "nuisance" scans,
> > +                * scan only after a number of frames with low PDAF confidence.
> > +                */
> > +               if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {
> > +                       doPDAF(phase, conf);
> > +                       if (stepCount_ > 0)
> > +                               stepCount_--;
> > +                       else if (mode_ != AfModeContinuous)
> > +                               scanState_ = ScanState::Idle;
> > +                       dropCount_ = 0;
> > +               } else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
> > +                       startProgrammedScan();
> > +       } else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
> > +               /*
> > +                * Scanning sequence. This means PDAF has become unavailable.
> > +                * Allow a delay between steps for CDAF FoM statistics to be
> > +                * updated, and a "settling time" at the end of the sequence.
> > +                * [A coarse or fine scan can be abandoned if two PDAF samples
> > +                * allow direct interpolation of the zero-phase lens position.]
> > +                */
> > +               if (stepCount_ > 0)
> > +                       stepCount_--;
> > +               else if (scanState_ == ScanState::Settle) {
> > +                       if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
> > +                           scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
> > +                               reportState_ = AfState::Focused;
> > +                       else
> > +                               reportState_ = AfState::Failed;
> > +                       if (mode_ == AfModeContinuous && !pauseFlag_ &&
> > +                           cfg_.speeds[speed_].dropoutFrames > 0)
> > +                               scanState_ = ScanState::Pdaf;
> > +                       else
> > +                               scanState_ = ScanState::Idle;
> > +                       scanData_.clear();
> > +               } else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
> > +                       scanState_ = ScanState::Settle;
> > +                       stepCount_ = (mode_ == AfModeContinuous) ? 0
> > +                                                                : cfg_.speeds[speed_].stepFrames;
> > +               } else
> > +                       doScan(contrast, phase, conf);
> > +       }
> > +}
> > +
> > +void Af::updateLensPosition()
> > +{
> > +       if (mode_ != AfModeManual) {
> > +               ftarget_ = std::clamp(ftarget_,
> > +                               cfg_.ranges[range_].focusMin,
> > +                               cfg_.ranges[range_].focusMax);
> > +       }
> > +
> > +       /* \todo Add a clip for manual lens position to be within the PWL limits. */
> > +
> > +       if (initted_) {
> > +               /* from a known lens position: apply slew rate limit */
> > +               fsmooth_ = std::clamp(ftarget_,
> > +                                     fsmooth_ - cfg_.speeds[speed_].maxSlew,
> > +                                     fsmooth_ + cfg_.speeds[speed_].maxSlew);
> > +       } else {
> > +               /* from an unknown position: go straight to target, but add delay */
> > +               fsmooth_ = ftarget_;
> > +               initted_ = true;
> > +               skipCount_ = cfg_.skipFrames;
> > +       }
> > +}
> > +
> > +/*
> > + * PDAF phase data are available in prepare(), but CDAF statistics are not
> > + * available until process(). We are gambling on the availability of PDAF.
> > + * To expedite feedback control using PDAF, issue the V4L2 lens control from
> > + * prepare(). Conversely, during scans, we must allow an extra frame delay
> > + * between steps, to retrieve CDAF statistics from the previous process()
> > + * so we can terminate the scan early without having to change our minds.
> > + */
> > +
> > +void Af::prepare(Metadata *imageMetadata)
> > +{
> > +       if (initted_) {
> > +               /* Get PDAF from the embedded metadata, and run AF algorithm core */
> > +               PdafData data;
> > +               double phase = 0.0, conf = 0.0;
> > +               double oldFt = ftarget_;
> > +               double oldFs = fsmooth_;
> > +               ScanState oldSs = scanState_;
> > +               uint32_t oldSt = stepCount_;
> > +               if (imageMetadata->get("pdaf.data", data) == 0)
> > +                       getPhase(data, phase, conf);
> > +               doAF(prevContrast_, phase, conf);
> > +               updateLensPosition();
> > +               LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
> > +                                 << static_cast<unsigned int>(reportState_)
> > +                                 << " sst" << static_cast<unsigned int>(oldSs)
> > +                                 << "->" << static_cast<unsigned int>(scanState_)
> > +                                 << " stp" << oldSt << "->" << stepCount_
> > +                                 << " ft" << oldFt << "->" << ftarget_
> > +                                 << " fs" << oldFs << "->" << fsmooth_
> > +                                 << " cont=" << (int)prevContrast_
> > +                                 << " phase=" << (int)phase << " conf=" << (int)conf;
> > +       }
> > +
> > +       /* Report status and produce new lens setting */
> > +       AfStatus status;
> > +       if (pauseFlag_)
> > +               status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
> > +                                                                   : AfPauseState::Pausing;
> > +       else
> > +               status.pauseState = AfPauseState::Running;
> > +
> > +       if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
> > +               status.state = AfState::Scanning;
> > +       else
> > +               status.state = reportState_;
> > +       status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
> > +                                     : std::nullopt;
> > +       imageMetadata->set("af.status", status);
> > +}
> > +
> > +double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const
> > +{
> > +       uint32_t totW = 0, totWc = 0;
> > +
> > +       if (useWeights_) {
> > +               for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {
> > +                       unsigned w = contrastWeights_[i];
> > +                       totW += w;
> > +                       totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);
> > +               }
> > +       }
> > +       if (totW == 0) {
> > +               totW = 2;
> > +               totWc = (focus_stats[5].contrast_val[1][1] >> 10) +
> > +                        (focus_stats[6].contrast_val[1][1] >> 10);
> > +       }
> > +
> > +       return (double)totWc / (double)totW;
> > +}
> > +
> > +void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
> > +{
> > +       (void)imageMetadata;
> > +       prevContrast_ = getContrast(stats->focus_stats);
> > +}
> > +
> > +/* Controls */
> > +
> > +void Af::setRange(AfRange r)
> > +{
> > +       LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
> > +       if (r < AfAlgorithm::AfRangeMax)
> > +               range_ = r;
> > +}
> > +
> > +void Af::setSpeed(AfSpeed s)
> > +{
> > +       LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
> > +       if (s < AfAlgorithm::AfSpeedMax) {
> > +               if (scanState_ == ScanState::Pdaf &&
> > +                   cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
> > +                       stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
> > +               speed_ = s;
> > +       }
> > +}
> > +
> > +void Af::setMetering(bool mode)
> > +{
> > +       useWeights_ = mode;
> > +}
> > +
> > +void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
> > +{
> > +       /*
> > +        * Here we just merge all of the given windows, weighted by area.
> > +        * If there are more than 15 overlapping windows, overflow can occur.
> > +        * TODO: A better approach might be to find the phase in each window
> > +        * and choose either the closest or the highest-confidence one?
> > +        *
> > +        * Using mostly "int" arithmetic, because Rectangle has signed x, y
> > +        */
> > +       ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);
> > +       int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);
> > +       int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);
> > +       int gridA = gridY * gridX;
> > +
> > +       for (int i = 0; i < PDAF_DATA_ROWS; ++i)
> > +               std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);
> > +       std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);
> > +
> > +       for (auto &w : wins) {
> > +               for (int i = 0; i < PDAF_DATA_ROWS; ++i) {
> > +                       int y0 = std::max(gridY * i, w.y);
> > +                       int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));
> > +                       if (y0 >= y1)
> > +                               continue;
> > +                       y1 -= y0;
> > +                       for (int j = 0; j < PDAF_DATA_COLS; ++j) {
> > +                               int x0 = std::max(gridX * j, w.x);
> > +                               int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));
> > +                               if (x0 >= x1)
> > +                                       continue;
> > +                               int a = y1 * (x1 - x0);
> > +                               a = (16 * a + gridA - 1) / gridA;
> > +                               phaseWeights_[i][j] += a;
> > +                               contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;
> > +                       }
> > +               }
> > +       }
> > +}
> > +
> > +bool Af::setLensPosition(double dioptres, int *hwpos)
> > +{
> > +       bool changed = false;
> > +
> > +       if (mode_ == AfModeManual) {
> > +               LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
> > +               changed = !(initted_ && fsmooth_ == dioptres);
> > +               ftarget_ = dioptres;
> > +               updateLensPosition();
> > +       }
> > +
> > +       if (hwpos)
> > +               *hwpos = cfg_.map.eval(fsmooth_);
> > +
> > +       return changed;
> > +}
> > +
> > +std::optional<double> Af::getLensPosition() const
> > +{
> > +       /*
> > +        * \todo We ought to perform some precise timing here to determine
> > +        * the current lens position.
> > +        */
> > +       return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
> > +}
> > +
> > +void Af::startCAF()
> > +{
> > +       /* Try PDAF if the tuning file permits it for CAF; else CDAF */
> > +       if (cfg_.speeds[speed_].dropoutFrames > 0) {
> > +               if (!initted_) {
> > +                       ftarget_ = cfg_.ranges[range_].focusDefault;
> > +                       updateLensPosition();
> > +               }
> > +               scanState_ = ScanState::Pdaf;
> > +               scanData_.clear();
> > +               dropCount_ = 0;
> > +               reportState_ = AfState::Scanning;
> > +       } else {
> > +               startProgrammedScan();
> > +       }
> > +}
> > +
> > +void Af::startProgrammedScan()
> > +{
> > +       ftarget_ = cfg_.ranges[range_].focusMin;
> > +       updateLensPosition();
> > +       scanState_ = ScanState::Coarse;
> > +       scanMaxContrast_ = 0.0;
> > +       scanMinContrast_ = 1.0e9;
> > +       scanMaxIndex_ = 0;
> > +       scanData_.clear();
> > +       stepCount_ = cfg_.speeds[speed_].stepFrames;
> > +       reportState_ = AfState::Scanning;
> > +}
> > +
> > +void Af::goIdle()
> > +{
> > +       scanState_ = ScanState::Idle;
> > +       reportState_ = AfState::Idle;
> > +       scanData_.clear();
> > +}
> > +
> > +void Af::cancelScan()
> > +{
> > +       LOG(RPiAf, Debug) << "cancelScan";
> > +       if (mode_ == AfModeAuto)
> > +               goIdle();
> > +}
> > +
> > +void Af::triggerScan()
> > +{
> > +       LOG(RPiAf, Debug) << "triggerScan";
> > +       if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {
> > +               /* Try PDAF if the tuning file permits it for Auto; else CDAF */
> > +               if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {
> > +                       if (!initted_) {
> > +                               ftarget_ = cfg_.ranges[range_].focusDefault;
> > +                               updateLensPosition();
> > +                       }
> > +                       stepCount_ = cfg_.speeds[speed_].pdafFrames;
> > +                       scanState_ = ScanState::Pdaf;
> > +                       dropCount_ = 0;
> > +               } else
> > +                       startProgrammedScan();
> > +               reportState_ = AfState::Scanning;
> > +       }
> > +}
> > +
> > +void Af::setMode(AfAlgorithm::AfMode mode)
> > +{
> > +       LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
> > +       if (mode_ != mode) {
> > +               mode_ = mode;
> > +               pauseFlag_ = false;
> > +               if (mode == AfModeContinuous)
> > +                       startCAF();
> > +               else if (mode != AfModeAuto)
> > +                       goIdle();
> > +       }
> > +}
> > +
> > +AfAlgorithm::AfMode Af::getMode() const
> > +{
> > +       return mode_;
> > +}
> > +
> > +void Af::pause(AfAlgorithm::AfPause pause)
> > +{
> > +       LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
> > +       if (mode_ == AfModeContinuous) {
> > +               if (pause == AfPauseResume && pauseFlag_) {
> > +                       startCAF();
> > +                       pauseFlag_ = false;
> > +               } else if (pause != AfPauseResume && !pauseFlag_) {
> > +                       if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)
> > +                               goIdle();
> > +                       pauseFlag_ = true;
> > +               }
> > +       }
> > +}
> > +
> > +// Register algorithm with the system.
> > +static Algorithm *create(Controller *controller)
> > +{
> > +       return (Algorithm *)new Af(controller);
> > +}
> > +static RegisterAlgorithm reg(NAME, &create);
> > diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
> > new file mode 100644
> > index 000000000000..0431bd70ae29
> > --- /dev/null
> > +++ b/src/ipa/raspberrypi/controller/rpi/af.h
> > @@ -0,0 +1,153 @@
> > +/* SPDX-License-Identifier: BSD-2-Clause */
> > +/*
> > + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> > + *
> > + * af.h - Autofocus control algorithm
> > + */
> > +#pragma once
> > +
> > +#include "../af_algorithm.h"
> > +#include "../af_status.h"
> > +#include "../pdaf_data.h"
> > +#include "../pwl.h"
> > +
> > +/*
> > + * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
> > + *
> > + * Whenever PDAF is available, it is used in a continuous feedback loop.
> > + * When triggered in auto mode, we simply enable AF for a limited number
> > + * of frames (it may terminate early if the delta becomes small enough).
> > + *
> > + * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
> > + * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
> > + * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
> > + * estimate the lens position with peak contrast. This is slower due to
> > + * extra latency in the ISP, and requires a settling time between steps.
> > + *
> > + * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
> > + * "nuisance" scans. During each interval where PDAF is not working, only
> > + * ONE scan will be performed; CAF cannot track objects using CDAF alone.
> > + *
> > + * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
> > + */
> > +
> > +namespace RPiController {
> > +
> > +class Af : public AfAlgorithm
> > +{
> > +public:
> > +       Af(Controller *controller = NULL);
> > +       ~Af();
> > +       char const *name() const override;
> > +       int read(const libcamera::YamlObject &params) override;
> > +       void initialise() override;
> > +
> > +       /* IPA calls */
> > +       void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
> > +       void prepare(Metadata *imageMetadata) override;
> > +       void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
> > +
> > +       /* controls */
> > +       void setRange(AfRange range) override;
> > +       void setSpeed(AfSpeed speed) override;
> > +       void setMetering(bool use_windows) override;
> > +       void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
> > +       void setMode(AfMode mode) override;
> > +       AfMode getMode() const override;
> > +       bool setLensPosition(double dioptres, int32_t *hwpos) override;
> > +       std::optional<double> getLensPosition() const override;
> > +       void triggerScan() override;
> > +       void cancelScan() override;
> > +       void pause(AfPause pause) override;
> > +
> > +private:
> > +       enum class ScanState {
> > +               Idle,
> > +               Pdaf,
> > +               Coarse,
> > +               Fine,
> > +               Settle
> > +       };
> > +
> > +       struct RangeDependentParams {
> > +               double focusMin;                /* lower (far) limit in dipotres */
> > +               double focusMax;                /* upper (near) limit in dioptres */
> > +               double focusDefault;            /* default setting ("hyperfocal") */
> > +
> > +               RangeDependentParams();
> > +               void read(const libcamera::YamlObject &params);
> > +       };
> > +
> > +       struct SpeedDependentParams {
> > +               double stepCoarse;              /* used for scans */
> > +               double stepFine;                /* used for scans */
> > +               double contrastRatio;           /* used for scan termination and reporting */
> > +               double pdafGain;                /* coefficient for PDAF feedback loop */
> > +               double pdafSquelch;             /* PDAF stability parameter (device-specific) */
> > +               double maxSlew;                 /* limit for lens movement per frame */
> > +               uint32_t pdafFrames;            /* number of iterations when triggered */
> > +               uint32_t dropoutFrames;         /* number of non-PDAF frames to switch to CDAF */
> > +               uint32_t stepFrames;            /* frames to skip in between steps of a scan */
> > +
> > +               SpeedDependentParams();
> > +               void read(const libcamera::YamlObject &params);
> > +       };
> > +
> > +       struct CfgParams {
> > +               RangeDependentParams ranges[AfRangeMax];
> > +               SpeedDependentParams speeds[AfSpeedMax];
> > +               uint32_t confEpsilon;           /* PDAF hysteresis threshold (sensor-specific) */
> > +               uint32_t confThresh;            /* PDAF confidence cell min (sensor-specific) */
> > +               uint32_t confClip;              /* PDAF confidence cell max (sensor-specific) */
> > +               uint32_t skipFrames;            /* frames to skip at start or modeswitch */
> > +               Pwl map;                        /* converts dioptres -> lens driver position */
> > +
> > +               CfgParams();
> > +               int read(const libcamera::YamlObject &params);
> > +               void initialise();
> > +       };
> > +
> > +       struct ScanRecord {
> > +               double focus;
> > +               double contrast;
> > +               double phase;
> > +               double conf;
> > +       };
> > +
> > +       bool getPhase(PdafData const &data, double &phase, double &conf) const;
> > +       double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
> > +       void doPDAF(double phase, double conf);
> > +       bool earlyTerminationByPhase(double phase);
> > +       void doScan(double contrast, double phase, double conf);
> > +       double findPeak(unsigned index) const;
> > +       void doAF(double contrast, double phase, double conf);
> > +       void updateLensPosition();
> > +       void startProgrammedScan();
> > +       void startCAF();
> > +       void goIdle();
> > +
> > +       /* Configuration and settings */
> > +       CfgParams cfg_;
> > +       AfRange range_;
> > +       AfSpeed speed_;
> > +       AfMode mode_;
> > +       bool pauseFlag_;
> > +       libcamera::Size sensorSize_;
> > +       bool useWeights_;
> > +       uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
> > +       uint16_t contrastWeights_[FOCUS_REGIONS];
> > +
> > +       /* Working state. */
> > +       ScanState scanState_;
> > +       bool initted_;
> > +       double ftarget_, fsmooth_;
> > +       double prevContrast_;
> > +       bool pdafSeen_;
> > +       unsigned skipCount_, stepCount_, dropCount_;
> > +       unsigned scanMaxIndex_;
> > +       double scanMaxContrast_, scanMinContrast_;
> > +       std::vector<ScanRecord> scanData_;
> > +       AfState reportState_;
> > +};
> > +
> > +} // namespace RPiController
> > diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
> > index 517d815bb98c..4e2689536257 100644
> > --- a/src/ipa/raspberrypi/meson.build
> > +++ b/src/ipa/raspberrypi/meson.build
> > @@ -27,6 +27,7 @@ rpi_ipa_sources = files([
> >      'controller/controller.cpp',
> >      'controller/histogram.cpp',
> >      'controller/algorithm.cpp',
> > +    'controller/rpi/af.cpp',
> >      'controller/rpi/alsc.cpp',
> >      'controller/rpi/awb.cpp',
> >      'controller/rpi/sharpen.cpp',
> > --
> > 2.25.1
> >
Nick Hollinghurst Jan. 20, 2023, 4:50 p.m. UTC | #4
Hi Kieran,

On Fri, 20 Jan 2023 at 16:39, Kieran Bingham
<kieran.bingham@ideasonboard.com> wrote:
>
> Hi Nick, / Naush,
>
> Quoting Nick Hollinghurst via libcamera-devel (2023-01-19 13:07:44)
> > Hi all,
> >
> > This patch needs a tweak to handle the case where setWindows is called
> > before configure -- see Af::Af(Controller *controller) constructor
> > below.
> >
> > On Thu, 19 Jan 2023 at 10:46, Naushir Patuck <naush@raspberrypi.com> wrote:
> > >
> > > From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> > >
> > > Provide the first version of the Raspberry Pi autofocus algorithm. This
> > > implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
> > > detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
> > > having less "hunting" behavior.
> > >
> > > Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
> > > Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
> > > Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
> > > Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
> > > ---
> > >  src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++
> > >  src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++
> > >  src/ipa/raspberrypi/meson.build           |   1 +
> > >  3 files changed, 909 insertions(+)
> > >  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp
> > >  create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h
> > >
> > > diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > > new file mode 100644
> > > index 000000000000..7e2e8961085a
> > > --- /dev/null
> > > +++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
> > > @@ -0,0 +1,755 @@
> > > +/* SPDX-License-Identifier: BSD-2-Clause */
> > > +/*
> > > + * Copyright (C) 2022-2023, Raspberry Pi Ltd
> > > + *
> > > + * af.cpp - Autofocus control algorithm
> > > + */
> > > +
> > > +#include "af.h"
> > > +
> > > +#include <iomanip>
> > > +#include <math.h>
> > > +#include <stdlib.h>
> > > +
> > > +#include <libcamera/base/log.h>
> > > +
> > > +#include <libcamera/control_ids.h>
> > > +
> > > +using namespace RPiController;
> > > +using namespace libcamera;
> > > +
> > > +LOG_DEFINE_CATEGORY(RPiAf)
> > > +
> > > +#define NAME "rpi.af"
> > > +
> > > +/*
> > > + * Default values for parameters. All may be overridden in the tuning file.
> > > + * Many of these values are sensor- or module-dependent; the defaults here
> > > + * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
> > > + *
> > > + * Here all focus values are in dioptres (1/m). They are converted to hardware
> > > + * units when written to status.lensSetting or returned from setLensPosition().
> > > + *
> > > + * Gain and delay values are relative to the update rate, since much (not all)
> > > + * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
> > > + * but note that algorithms are updated at no more than 30 Hz.
> > > + */
> > > +
> > > +Af::RangeDependentParams::RangeDependentParams()
> > > +       : focusMin(0.0),
> > > +         focusMax(12.0),
> > > +         focusDefault(1.0)
> > > +{
> > > +}
> > > +
> > > +Af::SpeedDependentParams::SpeedDependentParams()
> > > +       : stepCoarse(1.0),
> > > +         stepFine(0.25),
> > > +         contrastRatio(0.75),
> > > +         pdafGain(-0.02),
> > > +         pdafSquelch(0.125),
> > > +         maxSlew(2.0),
> > > +         pdafFrames(20),
> > > +         dropoutFrames(6),
> > > +         stepFrames(4)
> > > +{
> > > +}
> > > +
> > > +Af::CfgParams::CfgParams()
> > > +       : confEpsilon(8),
> > > +         confThresh(16),
> > > +         confClip(512),
> > > +         skipFrames(5),
> > > +         map()
> > > +{
> > > +}
> > > +
> > > +template<typename T>
> > > +static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
> > > +{
> > > +       auto value = params[name].get<T>();
> > > +       if (value)
> > > +               dest = *value;
> > > +       else
> > > +               LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
> > > +}
> > > +
> > > +void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
> > > +{
> > > +
> > > +       readNumber<double>(focusMin, params, "min");
> > > +       readNumber<double>(focusMax, params, "max");
> > > +       readNumber<double>(focusDefault, params, "default");
> > > +}
> > > +
> > > +void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
> > > +{
> > > +       readNumber<double>(stepCoarse, params, "step_coarse");
> > > +       readNumber<double>(stepFine, params, "step_fine");
> > > +       readNumber<double>(contrastRatio, params, "contrast_ratio");
> > > +       readNumber<double>(pdafGain, params, "pdaf_gain");
> > > +       readNumber<double>(pdafSquelch, params, "pdaf_squelch");
> > > +       readNumber<double>(maxSlew, params, "max_slew");
> > > +       readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
> > > +       readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
> > > +       readNumber<uint32_t>(stepFrames, params, "step_frames");
> > > +}
> > > +
> > > +int Af::CfgParams::read(const libcamera::YamlObject &params)
> > > +{
> > > +       if (params.contains("ranges")) {
> > > +               auto &rr = params["ranges"];
> > > +
> > > +               if (rr.contains("normal"))
> > > +                       ranges[AfRangeNormal].read(rr["normal"]);
> > > +               else
> > > +                       LOG(RPiAf, Warning) << "Missing range \"normal\"";
> > > +
> > > +               ranges[AfRangeMacro] = ranges[AfRangeNormal];
> > > +               if (rr.contains("macro"))
> > > +                       ranges[AfRangeMacro].read(rr["macro"]);
> > > +
> > > +               ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
> > > +                                                       ranges[AfRangeMacro].focusMin);
> > > +               ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
> > > +                                                       ranges[AfRangeMacro].focusMax);
> > > +               ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
> > > +               if (rr.contains("full"))
> > > +                       ranges[AfRangeFull].read(rr["full"]);
> > > +       } else
> > > +               LOG(RPiAf, Warning) << "No ranges defined";
> > > +
> > > +       if (params.contains("speeds")) {
> > > +               auto &ss = params["speeds"];
> > > +
> > > +               if (ss.contains("normal"))
> > > +                       speeds[AfSpeedNormal].read(ss["normal"]);
> > > +               else
> > > +                       LOG(RPiAf, Warning) << "Missing speed \"normal\"";
> > > +
> > > +               speeds[AfSpeedFast] = speeds[AfSpeedNormal];
> > > +               if (ss.contains("fast"))
> > > +                       speeds[AfSpeedFast].read(ss["fast"]);
> > > +       } else
> > > +               LOG(RPiAf, Warning) << "No speeds defined";
> > > +
> > > +       readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
> > > +       readNumber<uint32_t>(confThresh, params, "conf_thresh");
> > > +       readNumber<uint32_t>(confClip, params, "conf_clip");
> > > +       readNumber<uint32_t>(skipFrames, params, "skip_frames");
> > > +
> > > +       if (params.contains("map"))
> > > +               map.read(params["map"]);
> > > +       else
> > > +               LOG(RPiAf, Warning) << "No map defined";
> > > +
> > > +       return 0;
> > > +}
> > > +
> > > +void Af::CfgParams::initialise()
> > > +{
> > > +       if (map.empty()) {
> > > +               /* Default mapping from dioptres to hardware setting */
> > > +               static constexpr double DefaultMapX0 = 0.0;
> > > +               static constexpr double DefaultMapY0 = 445.0;
> > > +               static constexpr double DefaultMapX1 = 15.0;
> > > +               static constexpr double DefaultMapY1 = 925.0;
> > > +
> > > +               map.append(DefaultMapX0, DefaultMapY0);
> > > +               map.append(DefaultMapX1, DefaultMapY1);
> > > +
> > > +               LOG(RPiAf, Warning) << "af.map is not defined, ";
> > > +       }
> > > +}
> > > +
> > > +/* Af Algorithm class */
> > > +
> > > +Af::Af(Controller *controller)
> > > +       : AfAlgorithm(controller),
> > > +         cfg_(),
> > > +         range_(AfRangeNormal),
> > > +         speed_(AfSpeedNormal),
> > > +         mode_(AfAlgorithm::AfModeManual),
> > > +         pauseFlag_(false),
> > > +         sensorSize_{ 0, 0 },
> >
> > sensorSize_ needs to be set before setWindows() is called. For this
> > version it suffices to initialize it to
> >
> >         sensorSize_ { 4608, 2502 },
> >
> > a more general fix may follow later.
>
> Can you send a tested update to this patch please? No need to send the
> whole series again.
>
> If that can include the fix I highlighted, (Remove pdafSeen_) I believe
> I can merge this whole series.
>
> --
> Kieran

I have done a bit of re-work to fix this "properly" and am testing it
now. I'll also remove pdafSeen_.
The re-work touches "af.cpp", "af.h" only so it affects only this patch.

Regards,

 Nick


<snipped remainder>

Patch
diff mbox series

diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp
new file mode 100644
index 000000000000..7e2e8961085a
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/rpi/af.cpp
@@ -0,0 +1,755 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022-2023, Raspberry Pi Ltd
+ *
+ * af.cpp - Autofocus control algorithm
+ */
+
+#include "af.h"
+
+#include <iomanip>
+#include <math.h>
+#include <stdlib.h>
+
+#include <libcamera/base/log.h>
+
+#include <libcamera/control_ids.h>
+
+using namespace RPiController;
+using namespace libcamera;
+
+LOG_DEFINE_CATEGORY(RPiAf)
+
+#define NAME "rpi.af"
+
+/*
+ * Default values for parameters. All may be overridden in the tuning file.
+ * Many of these values are sensor- or module-dependent; the defaults here
+ * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.
+ *
+ * Here all focus values are in dioptres (1/m). They are converted to hardware
+ * units when written to status.lensSetting or returned from setLensPosition().
+ *
+ * Gain and delay values are relative to the update rate, since much (not all)
+ * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;
+ * but note that algorithms are updated at no more than 30 Hz.
+ */
+
+Af::RangeDependentParams::RangeDependentParams()
+	: focusMin(0.0),
+	  focusMax(12.0),
+	  focusDefault(1.0)
+{
+}
+
+Af::SpeedDependentParams::SpeedDependentParams()
+	: stepCoarse(1.0),
+	  stepFine(0.25),
+	  contrastRatio(0.75),
+	  pdafGain(-0.02),
+	  pdafSquelch(0.125),
+	  maxSlew(2.0),
+	  pdafFrames(20),
+	  dropoutFrames(6),
+	  stepFrames(4)
+{
+}
+
+Af::CfgParams::CfgParams()
+	: confEpsilon(8),
+	  confThresh(16),
+	  confClip(512),
+	  skipFrames(5),
+	  map()
+{
+}
+
+template<typename T>
+static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)
+{
+	auto value = params[name].get<T>();
+	if (value)
+		dest = *value;
+	else
+		LOG(RPiAf, Warning) << "Missing parameter \"" << name << "\"";
+}
+
+void Af::RangeDependentParams::read(const libcamera::YamlObject &params)
+{
+
+	readNumber<double>(focusMin, params, "min");
+	readNumber<double>(focusMax, params, "max");
+	readNumber<double>(focusDefault, params, "default");
+}
+
+void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)
+{
+	readNumber<double>(stepCoarse, params, "step_coarse");
+	readNumber<double>(stepFine, params, "step_fine");
+	readNumber<double>(contrastRatio, params, "contrast_ratio");
+	readNumber<double>(pdafGain, params, "pdaf_gain");
+	readNumber<double>(pdafSquelch, params, "pdaf_squelch");
+	readNumber<double>(maxSlew, params, "max_slew");
+	readNumber<uint32_t>(pdafFrames, params, "pdaf_frames");
+	readNumber<uint32_t>(dropoutFrames, params, "dropout_frames");
+	readNumber<uint32_t>(stepFrames, params, "step_frames");
+}
+
+int Af::CfgParams::read(const libcamera::YamlObject &params)
+{
+	if (params.contains("ranges")) {
+		auto &rr = params["ranges"];
+
+		if (rr.contains("normal"))
+			ranges[AfRangeNormal].read(rr["normal"]);
+		else
+			LOG(RPiAf, Warning) << "Missing range \"normal\"";
+
+		ranges[AfRangeMacro] = ranges[AfRangeNormal];
+		if (rr.contains("macro"))
+			ranges[AfRangeMacro].read(rr["macro"]);
+
+		ranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,
+							ranges[AfRangeMacro].focusMin);
+		ranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,
+							ranges[AfRangeMacro].focusMax);
+		ranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;
+		if (rr.contains("full"))
+			ranges[AfRangeFull].read(rr["full"]);
+	} else
+		LOG(RPiAf, Warning) << "No ranges defined";
+
+	if (params.contains("speeds")) {
+		auto &ss = params["speeds"];
+
+		if (ss.contains("normal"))
+			speeds[AfSpeedNormal].read(ss["normal"]);
+		else
+			LOG(RPiAf, Warning) << "Missing speed \"normal\"";
+
+		speeds[AfSpeedFast] = speeds[AfSpeedNormal];
+		if (ss.contains("fast"))
+			speeds[AfSpeedFast].read(ss["fast"]);
+	} else
+		LOG(RPiAf, Warning) << "No speeds defined";
+
+	readNumber<uint32_t>(confEpsilon, params, "conf_epsilon");
+	readNumber<uint32_t>(confThresh, params, "conf_thresh");
+	readNumber<uint32_t>(confClip, params, "conf_clip");
+	readNumber<uint32_t>(skipFrames, params, "skip_frames");
+
+	if (params.contains("map"))
+		map.read(params["map"]);
+	else
+		LOG(RPiAf, Warning) << "No map defined";
+
+	return 0;
+}
+
+void Af::CfgParams::initialise()
+{
+	if (map.empty()) {
+		/* Default mapping from dioptres to hardware setting */
+		static constexpr double DefaultMapX0 = 0.0;
+		static constexpr double DefaultMapY0 = 445.0;
+		static constexpr double DefaultMapX1 = 15.0;
+		static constexpr double DefaultMapY1 = 925.0;
+
+		map.append(DefaultMapX0, DefaultMapY0);
+		map.append(DefaultMapX1, DefaultMapY1);
+
+		LOG(RPiAf, Warning) << "af.map is not defined, ";
+	}
+}
+
+/* Af Algorithm class */
+
+Af::Af(Controller *controller)
+	: AfAlgorithm(controller),
+	  cfg_(),
+	  range_(AfRangeNormal),
+	  speed_(AfSpeedNormal),
+	  mode_(AfAlgorithm::AfModeManual),
+	  pauseFlag_(false),
+	  sensorSize_{ 0, 0 },
+	  useWeights_(false),
+	  phaseWeights_{},
+	  contrastWeights_{},
+	  scanState_(ScanState::Idle),
+	  initted_(false),
+	  ftarget_(-1.0),
+	  fsmooth_(-1.0),
+	  prevContrast_(0.0),
+	  skipCount_(0),
+	  stepCount_(0),
+	  dropCount_(0),
+	  scanMaxContrast_(0.0),
+	  scanMinContrast_(1.0e9),
+	  scanData_(),
+	  reportState_(AfState::Idle)
+{
+	scanData_.reserve(24);
+}
+
+Af::~Af()
+{
+}
+
+char const *Af::name() const
+{
+	return NAME;
+}
+
+int Af::read(const libcamera::YamlObject &params)
+{
+	return cfg_.read(params);
+}
+
+void Af::initialise()
+{
+	cfg_.initialise();
+}
+
+void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)
+{
+	(void)metadata;
+	sensorSize_.width = cameraMode.sensorWidth;
+	sensorSize_.height = cameraMode.sensorHeight;
+
+	if (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {
+		/*
+		 * If a scan was in progress, re-start it, as CDAF statistics
+		 * may have changed. Though if the application is just about
+		 * to take a still picture, this will not help...
+		 */
+		startProgrammedScan();
+	}
+	skipCount_ = cfg_.skipFrames;
+}
+
+bool Af::getPhase(PdafData const &data, double &phase, double &conf) const
+{
+	static const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
+		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
+	};
+	int32_t sumW = 0;
+	int32_t sumWc = 0;
+	int32_t sumWcp = 0;
+	auto wgts = useWeights_ ? phaseWeights_ : defaultWeights;
+
+	for (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {
+		for (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {
+			if (wgts[i][j]) {
+				uint32_t c = data.conf[i][j];
+				if (c >= cfg_.confThresh) {
+					if (c > cfg_.confClip)
+						c = cfg_.confClip;
+					sumWc += wgts[i][j] * (int32_t)c;
+					c -= (cfg_.confThresh >> 1);
+					sumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;
+				}
+				sumW += wgts[i][j];
+			}
+		}
+	}
+
+	if (sumWc > 0) {
+		phase = (double)sumWcp / (double)sumWc;
+		conf = (double)sumWc / (double)sumW;
+		return true;
+	} else {
+		phase = 0.0;
+		conf = 0.0;
+		return false;
+	}
+}
+
+void Af::doPDAF(double phase, double conf)
+{
+	/* Apply loop gain */
+	phase *= cfg_.speeds[speed_].pdafGain;
+
+	if (mode_ == AfModeContinuous) {
+		/*
+		 * PDAF in Continuous mode. Scale down lens movement when
+		 * delta is small or confidence is low, to suppress wobble.
+		 */
+		if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {
+			double a = phase / cfg_.speeds[speed_].pdafSquelch;
+			phase *= a * a;
+		}
+		phase *= conf / (conf + cfg_.confEpsilon);
+	} else {
+		/*
+		 * PDAF in triggered-auto mode. Allow early termination when
+		 * phase delta is small; scale down lens movements towards
+		 * the end of the sequence, to ensure a stable image.
+		 */
+		if (stepCount_ >= cfg_.speeds[speed_].stepFrames) {
+			if (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)
+				stepCount_ = cfg_.speeds[speed_].stepFrames;
+		} else
+			phase *= stepCount_ / cfg_.speeds[speed_].stepFrames;
+	}
+
+	/* Apply slew rate limit. Report failure if out of bounds. */
+	if (phase < -cfg_.speeds[speed_].maxSlew) {
+		phase = -cfg_.speeds[speed_].maxSlew;
+		reportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed
+									  : AfState::Scanning;
+	} else if (phase > cfg_.speeds[speed_].maxSlew) {
+		phase = cfg_.speeds[speed_].maxSlew;
+		reportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed
+									  : AfState::Scanning;
+	} else
+		reportState_ = AfState::Focused;
+
+	ftarget_ = fsmooth_ + phase;
+}
+
+bool Af::earlyTerminationByPhase(double phase)
+{
+	if (scanData_.size() > 0 &&
+	    scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {
+		double oldFocus = scanData_[scanData_.size() - 1].focus;
+		double oldPhase = scanData_[scanData_.size() - 1].phase;
+
+		/*
+		 * Check that the gradient is finite and has the expected sign;
+		 * Interpolate/extrapolate the lens position for zero phase.
+		 * Check that the extrapolation is well-conditioned.
+		 */
+		if ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {
+			double param = phase / (phase - oldPhase);
+			if (-3.0 <= param && param <= 3.5) {
+				ftarget_ += param * (oldFocus - ftarget_);
+				LOG(RPiAf, Debug) << "ETBP: param=" << param;
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+double Af::findPeak(unsigned i) const
+{
+	double f = scanData_[i].focus;
+
+	if (i > 0 && i + 1 < scanData_.size()) {
+		double dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;
+		double dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;
+		if (dropLo < dropHi) {
+			double param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);
+			f += param * (scanData_[i - 1].focus - f);
+		} else if (dropHi < dropLo) {
+			double param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);
+			f += param * (scanData_[i + 1].focus - f);
+		}
+	}
+
+	LOG(RPiAf, Debug) << "FindPeak: " << f;
+	return f;
+}
+
+void Af::doScan(double contrast, double phase, double conf)
+{
+	/* Record lens position, contrast and phase values for the current scan */
+	if (scanData_.empty() || contrast > scanMaxContrast_) {
+		scanMaxContrast_ = contrast;
+		scanMaxIndex_ = scanData_.size();
+	}
+	if (contrast < scanMinContrast_)
+		scanMinContrast_ = contrast;
+	scanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });
+
+	if (scanState_ == ScanState::Coarse) {
+		if (ftarget_ >= cfg_.ranges[range_].focusMax ||
+		    contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
+			/*
+			 * Finished course scan, or termination based on contrast.
+			 * Jump to just after max contrast and start fine scan.
+			 */
+			ftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +
+					2.0 * cfg_.speeds[speed_].stepFine);
+			scanState_ = ScanState::Fine;
+			scanData_.clear();
+		} else
+			ftarget_ += cfg_.speeds[speed_].stepCoarse;
+	} else { /* ScanState::Fine */
+		if (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||
+		    contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {
+			/*
+			 * Finished fine scan, or termination based on contrast.
+			 * Use quadratic peak-finding to find best contrast position.
+			 */
+			ftarget_ = findPeak(scanMaxIndex_);
+			scanState_ = ScanState::Settle;
+		} else
+			ftarget_ -= cfg_.speeds[speed_].stepFine;
+	}
+
+	stepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;
+}
+
+void Af::doAF(double contrast, double phase, double conf)
+{
+	/* Skip frames at startup and after mode change */
+	if (skipCount_ > 0) {
+		LOG(RPiAf, Debug) << "SKIP";
+		skipCount_--;
+		return;
+	}
+
+	if (scanState_ == ScanState::Pdaf) {
+		/*
+		 * Use PDAF closed-loop control whenever available, in both CAF
+		 * mode and (for a limited number of iterations) when triggered.
+		 * If PDAF fails (due to poor contrast, noise or large defocus),
+		 * fall back to a CDAF-based scan. To avoid "nuisance" scans,
+		 * scan only after a number of frames with low PDAF confidence.
+		 */
+		if (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {
+			doPDAF(phase, conf);
+			if (stepCount_ > 0)
+				stepCount_--;
+			else if (mode_ != AfModeContinuous)
+				scanState_ = ScanState::Idle;
+			dropCount_ = 0;
+		} else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)
+			startProgrammedScan();
+	} else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {
+		/*
+		 * Scanning sequence. This means PDAF has become unavailable.
+		 * Allow a delay between steps for CDAF FoM statistics to be
+		 * updated, and a "settling time" at the end of the sequence.
+		 * [A coarse or fine scan can be abandoned if two PDAF samples
+		 * allow direct interpolation of the zero-phase lens position.]
+		 */
+		if (stepCount_ > 0)
+			stepCount_--;
+		else if (scanState_ == ScanState::Settle) {
+			if (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&
+			    scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)
+				reportState_ = AfState::Focused;
+			else
+				reportState_ = AfState::Failed;
+			if (mode_ == AfModeContinuous && !pauseFlag_ &&
+			    cfg_.speeds[speed_].dropoutFrames > 0)
+				scanState_ = ScanState::Pdaf;
+			else
+				scanState_ = ScanState::Idle;
+			scanData_.clear();
+		} else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {
+			scanState_ = ScanState::Settle;
+			stepCount_ = (mode_ == AfModeContinuous) ? 0
+								 : cfg_.speeds[speed_].stepFrames;
+		} else
+			doScan(contrast, phase, conf);
+	}
+}
+
+void Af::updateLensPosition()
+{
+	if (mode_ != AfModeManual) {
+		ftarget_ = std::clamp(ftarget_,
+				cfg_.ranges[range_].focusMin,
+				cfg_.ranges[range_].focusMax);
+	}
+
+	/* \todo Add a clip for manual lens position to be within the PWL limits. */
+
+	if (initted_) {
+		/* from a known lens position: apply slew rate limit */
+		fsmooth_ = std::clamp(ftarget_,
+				      fsmooth_ - cfg_.speeds[speed_].maxSlew,
+				      fsmooth_ + cfg_.speeds[speed_].maxSlew);
+	} else {
+		/* from an unknown position: go straight to target, but add delay */
+		fsmooth_ = ftarget_;
+		initted_ = true;
+		skipCount_ = cfg_.skipFrames;
+	}
+}
+
+/*
+ * PDAF phase data are available in prepare(), but CDAF statistics are not
+ * available until process(). We are gambling on the availability of PDAF.
+ * To expedite feedback control using PDAF, issue the V4L2 lens control from
+ * prepare(). Conversely, during scans, we must allow an extra frame delay
+ * between steps, to retrieve CDAF statistics from the previous process()
+ * so we can terminate the scan early without having to change our minds.
+ */
+
+void Af::prepare(Metadata *imageMetadata)
+{
+	if (initted_) {
+		/* Get PDAF from the embedded metadata, and run AF algorithm core */
+		PdafData data;
+		double phase = 0.0, conf = 0.0;
+		double oldFt = ftarget_;
+		double oldFs = fsmooth_;
+		ScanState oldSs = scanState_;
+		uint32_t oldSt = stepCount_;
+		if (imageMetadata->get("pdaf.data", data) == 0)
+			getPhase(data, phase, conf);
+		doAF(prevContrast_, phase, conf);
+		updateLensPosition();
+		LOG(RPiAf, Debug) << std::fixed << std::setprecision(2)
+				  << static_cast<unsigned int>(reportState_)
+				  << " sst" << static_cast<unsigned int>(oldSs)
+				  << "->" << static_cast<unsigned int>(scanState_)
+				  << " stp" << oldSt << "->" << stepCount_
+				  << " ft" << oldFt << "->" << ftarget_
+				  << " fs" << oldFs << "->" << fsmooth_
+				  << " cont=" << (int)prevContrast_
+				  << " phase=" << (int)phase << " conf=" << (int)conf;
+	}
+
+	/* Report status and produce new lens setting */
+	AfStatus status;
+	if (pauseFlag_)
+		status.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused
+								    : AfPauseState::Pausing;
+	else
+		status.pauseState = AfPauseState::Running;
+
+	if (mode_ == AfModeAuto && scanState_ != ScanState::Idle)
+		status.state = AfState::Scanning;
+	else
+		status.state = reportState_;
+	status.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))
+				      : std::nullopt;
+	imageMetadata->set("af.status", status);
+}
+
+double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const
+{
+	uint32_t totW = 0, totWc = 0;
+
+	if (useWeights_) {
+		for (unsigned i = 0; i < FOCUS_REGIONS; ++i) {
+			unsigned w = contrastWeights_[i];
+			totW += w;
+			totWc += w * (focus_stats[i].contrast_val[1][1] >> 10);
+		}
+	}
+	if (totW == 0) {
+		totW = 2;
+		totWc = (focus_stats[5].contrast_val[1][1] >> 10) +
+			 (focus_stats[6].contrast_val[1][1] >> 10);
+	}
+
+	return (double)totWc / (double)totW;
+}
+
+void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)
+{
+	(void)imageMetadata;
+	prevContrast_ = getContrast(stats->focus_stats);
+}
+
+/* Controls */
+
+void Af::setRange(AfRange r)
+{
+	LOG(RPiAf, Debug) << "setRange: " << (unsigned)r;
+	if (r < AfAlgorithm::AfRangeMax)
+		range_ = r;
+}
+
+void Af::setSpeed(AfSpeed s)
+{
+	LOG(RPiAf, Debug) << "setSpeed: " << (unsigned)s;
+	if (s < AfAlgorithm::AfSpeedMax) {
+		if (scanState_ == ScanState::Pdaf &&
+		    cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)
+			stepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;
+		speed_ = s;
+	}
+}
+
+void Af::setMetering(bool mode)
+{
+	useWeights_ = mode;
+}
+
+void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)
+{
+	/*
+	 * Here we just merge all of the given windows, weighted by area.
+	 * If there are more than 15 overlapping windows, overflow can occur.
+	 * TODO: A better approach might be to find the phase in each window
+	 * and choose either the closest or the highest-confidence one?
+	 *
+	 * Using mostly "int" arithmetic, because Rectangle has signed x, y
+	 */
+	ASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);
+	int gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);
+	int gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);
+	int gridA = gridY * gridX;
+
+	for (int i = 0; i < PDAF_DATA_ROWS; ++i)
+		std::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);
+	std::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);
+
+	for (auto &w : wins) {
+		for (int i = 0; i < PDAF_DATA_ROWS; ++i) {
+			int y0 = std::max(gridY * i, w.y);
+			int y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));
+			if (y0 >= y1)
+				continue;
+			y1 -= y0;
+			for (int j = 0; j < PDAF_DATA_COLS; ++j) {
+				int x0 = std::max(gridX * j, w.x);
+				int x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));
+				if (x0 >= x1)
+					continue;
+				int a = y1 * (x1 - x0);
+				a = (16 * a + gridA - 1) / gridA;
+				phaseWeights_[i][j] += a;
+				contrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;
+			}
+		}
+	}
+}
+
+bool Af::setLensPosition(double dioptres, int *hwpos)
+{
+	bool changed = false;
+
+	if (mode_ == AfModeManual) {
+		LOG(RPiAf, Debug) << "setLensPosition: " << dioptres;
+		changed = !(initted_ && fsmooth_ == dioptres);
+		ftarget_ = dioptres;
+		updateLensPosition();
+	}
+
+	if (hwpos)
+		*hwpos = cfg_.map.eval(fsmooth_);
+
+	return changed;
+}
+
+std::optional<double> Af::getLensPosition() const
+{
+	/*
+	 * \todo We ought to perform some precise timing here to determine
+	 * the current lens position.
+	 */
+	return initted_ ? std::optional<double>(fsmooth_) : std::nullopt;
+}
+
+void Af::startCAF()
+{
+	/* Try PDAF if the tuning file permits it for CAF; else CDAF */
+	if (cfg_.speeds[speed_].dropoutFrames > 0) {
+		if (!initted_) {
+			ftarget_ = cfg_.ranges[range_].focusDefault;
+			updateLensPosition();
+		}
+		scanState_ = ScanState::Pdaf;
+		scanData_.clear();
+		dropCount_ = 0;
+		reportState_ = AfState::Scanning;
+	} else {
+		startProgrammedScan();
+	}
+}
+
+void Af::startProgrammedScan()
+{
+	ftarget_ = cfg_.ranges[range_].focusMin;
+	updateLensPosition();
+	scanState_ = ScanState::Coarse;
+	scanMaxContrast_ = 0.0;
+	scanMinContrast_ = 1.0e9;
+	scanMaxIndex_ = 0;
+	scanData_.clear();
+	stepCount_ = cfg_.speeds[speed_].stepFrames;
+	reportState_ = AfState::Scanning;
+}
+
+void Af::goIdle()
+{
+	scanState_ = ScanState::Idle;
+	reportState_ = AfState::Idle;
+	scanData_.clear();
+}
+
+void Af::cancelScan()
+{
+	LOG(RPiAf, Debug) << "cancelScan";
+	if (mode_ == AfModeAuto)
+		goIdle();
+}
+
+void Af::triggerScan()
+{
+	LOG(RPiAf, Debug) << "triggerScan";
+	if (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {
+		/* Try PDAF if the tuning file permits it for Auto; else CDAF */
+		if (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {
+			if (!initted_) {
+				ftarget_ = cfg_.ranges[range_].focusDefault;
+				updateLensPosition();
+			}
+			stepCount_ = cfg_.speeds[speed_].pdafFrames;
+			scanState_ = ScanState::Pdaf;
+			dropCount_ = 0;
+		} else
+			startProgrammedScan();
+		reportState_ = AfState::Scanning;
+	}
+}
+
+void Af::setMode(AfAlgorithm::AfMode mode)
+{
+	LOG(RPiAf, Debug) << "setMode: " << (unsigned)mode;
+	if (mode_ != mode) {
+		mode_ = mode;
+		pauseFlag_ = false;
+		if (mode == AfModeContinuous)
+			startCAF();
+		else if (mode != AfModeAuto)
+			goIdle();
+	}
+}
+
+AfAlgorithm::AfMode Af::getMode() const
+{
+	return mode_;
+}
+
+void Af::pause(AfAlgorithm::AfPause pause)
+{
+	LOG(RPiAf, Debug) << "pause: " << (unsigned)pause;
+	if (mode_ == AfModeContinuous) {
+		if (pause == AfPauseResume && pauseFlag_) {
+			startCAF();
+			pauseFlag_ = false;
+		} else if (pause != AfPauseResume && !pauseFlag_) {
+			if (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)
+				goIdle();
+			pauseFlag_ = true;
+		}
+	}
+}
+
+// Register algorithm with the system.
+static Algorithm *create(Controller *controller)
+{
+	return (Algorithm *)new Af(controller);
+}
+static RegisterAlgorithm reg(NAME, &create);
diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
new file mode 100644
index 000000000000..0431bd70ae29
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/rpi/af.h
@@ -0,0 +1,153 @@ 
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022-2023, Raspberry Pi Ltd
+ *
+ * af.h - Autofocus control algorithm
+ */
+#pragma once
+
+#include "../af_algorithm.h"
+#include "../af_status.h"
+#include "../pdaf_data.h"
+#include "../pwl.h"
+
+/*
+ * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
+ *
+ * Whenever PDAF is available, it is used in a continuous feedback loop.
+ * When triggered in auto mode, we simply enable AF for a limited number
+ * of frames (it may terminate early if the delta becomes small enough).
+ *
+ * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
+ * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
+ * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
+ * estimate the lens position with peak contrast. This is slower due to
+ * extra latency in the ISP, and requires a settling time between steps.
+ *
+ * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
+ * "nuisance" scans. During each interval where PDAF is not working, only
+ * ONE scan will be performed; CAF cannot track objects using CDAF alone.
+ *
+ * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
+ */
+
+namespace RPiController {
+
+class Af : public AfAlgorithm
+{
+public:
+	Af(Controller *controller = NULL);
+	~Af();
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+
+	/* IPA calls */
+	void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
+	void prepare(Metadata *imageMetadata) override;
+	void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
+
+	/* controls */
+	void setRange(AfRange range) override;
+	void setSpeed(AfSpeed speed) override;
+	void setMetering(bool use_windows) override;
+	void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
+	void setMode(AfMode mode) override;
+	AfMode getMode() const override;
+	bool setLensPosition(double dioptres, int32_t *hwpos) override;
+	std::optional<double> getLensPosition() const override;
+	void triggerScan() override;
+	void cancelScan() override;
+	void pause(AfPause pause) override;
+
+private:
+	enum class ScanState {
+		Idle,
+		Pdaf,
+		Coarse,
+		Fine,
+		Settle
+	};
+
+	struct RangeDependentParams {
+		double focusMin;       		/* lower (far) limit in dipotres */
+		double focusMax;	       	/* upper (near) limit in dioptres */
+		double focusDefault;		/* default setting ("hyperfocal") */
+
+		RangeDependentParams();
+		void read(const libcamera::YamlObject &params);
+	};
+
+	struct SpeedDependentParams {
+		double stepCoarse;		/* used for scans */
+		double stepFine;		/* used for scans */
+		double contrastRatio;		/* used for scan termination and reporting */
+		double pdafGain;		/* coefficient for PDAF feedback loop */
+		double pdafSquelch;		/* PDAF stability parameter (device-specific) */
+		double maxSlew;			/* limit for lens movement per frame */
+		uint32_t pdafFrames;		/* number of iterations when triggered */
+		uint32_t dropoutFrames;		/* number of non-PDAF frames to switch to CDAF */
+		uint32_t stepFrames;		/* frames to skip in between steps of a scan */
+
+		SpeedDependentParams();
+		void read(const libcamera::YamlObject &params);
+	};
+
+	struct CfgParams {
+		RangeDependentParams ranges[AfRangeMax];
+		SpeedDependentParams speeds[AfSpeedMax];
+		uint32_t confEpsilon;	       	/* PDAF hysteresis threshold (sensor-specific) */
+		uint32_t confThresh;	       	/* PDAF confidence cell min (sensor-specific) */
+		uint32_t confClip;	       	/* PDAF confidence cell max (sensor-specific) */
+		uint32_t skipFrames;	       	/* frames to skip at start or modeswitch */
+		Pwl map;		       	/* converts dioptres -> lens driver position */
+
+		CfgParams();
+		int read(const libcamera::YamlObject &params);
+		void initialise();
+	};
+
+	struct ScanRecord {
+		double focus;
+		double contrast;
+		double phase;
+		double conf;
+	};
+
+	bool getPhase(PdafData const &data, double &phase, double &conf) const;
+	double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
+	void doPDAF(double phase, double conf);
+	bool earlyTerminationByPhase(double phase);
+	void doScan(double contrast, double phase, double conf);
+	double findPeak(unsigned index) const;
+	void doAF(double contrast, double phase, double conf);
+	void updateLensPosition();
+	void startProgrammedScan();
+	void startCAF();
+	void goIdle();
+
+	/* Configuration and settings */
+	CfgParams cfg_;
+	AfRange range_;
+	AfSpeed speed_;
+	AfMode mode_;
+	bool pauseFlag_;
+	libcamera::Size sensorSize_;
+	bool useWeights_;
+	uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
+	uint16_t contrastWeights_[FOCUS_REGIONS];
+
+	/* Working state. */
+	ScanState scanState_;
+	bool initted_;
+	double ftarget_, fsmooth_;
+	double prevContrast_;
+	bool pdafSeen_;
+	unsigned skipCount_, stepCount_, dropCount_;
+	unsigned scanMaxIndex_;
+	double scanMaxContrast_, scanMinContrast_;
+	std::vector<ScanRecord> scanData_;
+	AfState reportState_;
+};
+
+} // namespace RPiController
diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
index 517d815bb98c..4e2689536257 100644
--- a/src/ipa/raspberrypi/meson.build
+++ b/src/ipa/raspberrypi/meson.build
@@ -27,6 +27,7 @@  rpi_ipa_sources = files([
     'controller/controller.cpp',
     'controller/histogram.cpp',
     'controller/algorithm.cpp',
+    'controller/rpi/af.cpp',
     'controller/rpi/alsc.cpp',
     'controller/rpi/awb.cpp',
     'controller/rpi/sharpen.cpp',