{"id":18159,"url":"https://patchwork.libcamera.org/api/1.1/patches/18159/?format=json","web_url":"https://patchwork.libcamera.org/patch/18159/","project":{"id":1,"url":"https://patchwork.libcamera.org/api/1.1/projects/1/?format=json","name":"libcamera","link_name":"libcamera","list_id":"libcamera_core","list_email":"libcamera-devel@lists.libcamera.org","web_url":"","scm_url":"","webscm_url":""},"msgid":"<20230119104544.9456-12-naush@raspberrypi.com>","date":"2023-01-19T10:45:41","name":"[libcamera-devel,v1,11/14] ipa: raspberrypi: First version of autofocus algorithm using PDAF","commit_ref":null,"pull_url":null,"state":"superseded","archived":false,"hash":"98fcad97751c747f6d03e3e7b05dd2949dddbf7f","submitter":{"id":34,"url":"https://patchwork.libcamera.org/api/1.1/people/34/?format=json","name":"Naushir Patuck","email":"naush@raspberrypi.com"},"delegate":null,"mbox":"https://patchwork.libcamera.org/patch/18159/mbox/","series":[{"id":3715,"url":"https://patchwork.libcamera.org/api/1.1/series/3715/?format=json","web_url":"https://patchwork.libcamera.org/project/libcamera/list/?series=3715","date":"2023-01-19T10:45:30","name":"Raspberry Pi: Camera Module 3 support","version":1,"mbox":"https://patchwork.libcamera.org/series/3715/mbox/"}],"comments":"https://patchwork.libcamera.org/api/patches/18159/comments/","check":"pending","checks":"https://patchwork.libcamera.org/api/patches/18159/checks/","tags":{},"headers":{"Return-Path":"<libcamera-devel-bounces@lists.libcamera.org>","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id 72E33C3298\n\tfor <parsemail@patchwork.libcamera.org>;\n\tThu, 19 Jan 2023 10:46:07 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 3203362604;\n\tThu, 19 Jan 2023 11:46:07 +0100 (CET)","from mail-wm1-x336.google.com (mail-wm1-x336.google.com\n\t[IPv6:2a00:1450:4864:20::336])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 1AF3A625FE\n\tfor <libcamera-devel@lists.libcamera.org>;\n\tThu, 19 Jan 2023 11:46:01 +0100 (CET)","by mail-wm1-x336.google.com with SMTP id\n\to17-20020a05600c511100b003db021ef437so919332wms.4\n\tfor <libcamera-devel@lists.libcamera.org>;\n\tThu, 19 Jan 2023 02:46:01 -0800 (PST)","from localhost.localdomain ([93.93.133.154])\n\tby smtp.gmail.com with ESMTPSA id\n\tbe12-20020a05600c1e8c00b003db06224953sm5332029wmb.41.2023.01.19.02.45.59\n\t(version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256);\n\tThu, 19 Jan 2023 02:45:59 -0800 (PST)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674125167;\n\tbh=2oOoSnV3xIVoIEas16og8XSCi4NRg40bj0ZImlWQOUE=;\n\th=To:Date:In-Reply-To:References:Subject:List-Id:List-Unsubscribe:\n\tList-Archive:List-Post:List-Help:List-Subscribe:From:Reply-To:Cc:\n\tFrom;\n\tb=TZhxgCKCcwKMtWrqL86P+Ydxbfkjp6G1P6m3AAKbVF+/Ct80LUg6T2d0odXBrUVb6\n\tqa4QCIMBYGFeUgjNc1/cbRo9fYCU5xD5laPR1JgOPuZBgX3woQe+akZg2vTroEAVqU\n\tkmq3i6iL0ONncPP3/6gfc5I1HjwmvSYCNRkHrezPs7y5InK6J6wB5ZD4pEiH4ovMhx\n\t6omZlwqJ0EmAZ316xlYLzaVYp/gEnbrpNcyXz/x49vKNOdqvfvDr8FO4MnI4ru2yOH\n\tlhXJb8FLaD1vIt0EtQlhxWCKNgSaI/R02Q5f72OiXLjupZGckn2axKbf+eDEb0sg9l\n\t8ICmjAOxjFYJA==","v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=raspberrypi.com; s=google;\n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:from:to:cc:subject:date\n\t:message-id:reply-to;\n\tbh=Q8vHDGV07pCNO6Y463aCfAMBsyQiadIBgsNDWxvUI7o=;\n\tb=Rb317x0MdiDAcElffSNnSs89CSiizMZCluflkIlxMGWuSKVoI9ShiNu3E1ujemgE3p\n\tc143kHuynEj40C/DaAhI7xfXYHiQkK0fJ1sadPYJAHTkDotvh2vswRUj0OUgmAVYCOWi\n\tdbf72Vf+aQom1rOfpxwDug386JULFAbnhwTl10pt3oukXO40W5TLi8KdtY8sDpClDv9d\n\tMPu3AAotMcEi3xOjZ3sjKA4F2+g8bfmCfacBiyzP3L966CS03Cjy9r5ukgyx8BqjUyRX\n\tQILrHov4ffTS8RTBZbso1IP0eDjstNaL9wjlP53waZ/8jGBP3cP0Vp9tPNRJU1AQ26ut\n\t7Plw=="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key; \n\tunprotected) header.d=raspberrypi.com\n\theader.i=@raspberrypi.com\n\theader.b=\"Rb317x0M\"; dkim-atps=neutral","X-Google-DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=1e100.net; s=20210112;\n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc\n\t:subject:date:message-id:reply-to;\n\tbh=Q8vHDGV07pCNO6Y463aCfAMBsyQiadIBgsNDWxvUI7o=;\n\tb=dgypd8eUFV1KUKEIu0SYciINc2RGkk1H7L7RSjrhESIU1r+jbdHb0N5IHRaiufOQyM\n\tnhdFySy7+nY3ZrLziKZxoQyHm61IS2aOMlCtnV9Uxx7y0npyV6+P7WJWnS1bGh8K0DCv\n\t/zqT8zzmJuTqycIvR7d4LpvEHm9J3GvDsWNaWrat1k0TM2IMdS5VdNU2oz9eHU5hvGHd\n\t+h2isEglRynwTo+AndY2zHySZSPeNm8r89/J+hLMgOIuYluFCOLVozEQCpSYVtevtRxY\n\tO6NCIhyqPQp8Xx8kCpazYnMsZhF35tckFfd01SVjyP9rO/u5rZboxi9S6ykLRr87VzLm\n\tZ1pQ==","X-Gm-Message-State":"AFqh2koonVV6t6Dpp+WYrHllL0z6fd8Jg0a3X8xtvtx/Nj7UDD7l5xUa\n\t6zSducnIKdP3sf3L9lV3/bcF5GREpTOZMM9tn7I=","X-Google-Smtp-Source":"AMrXdXtPjdRrA8DjNo56r973hwNpJ7x2g+jbLm++zNN0i7wf0X86V8/+S6Dp9vDDeyQIeimVQMLmCg==","X-Received":"by 2002:a05:600c:181c:b0:3da:f9e9:3a1a with SMTP id\n\tn28-20020a05600c181c00b003daf9e93a1amr10181397wmp.20.1674125159975; \n\tThu, 19 Jan 2023 02:45:59 -0800 (PST)","To":"libcamera-devel@lists.libcamera.org","Date":"Thu, 19 Jan 2023 10:45:41 +0000","Message-Id":"<20230119104544.9456-12-naush@raspberrypi.com>","X-Mailer":"git-send-email 2.25.1","In-Reply-To":"<20230119104544.9456-1-naush@raspberrypi.com>","References":"<20230119104544.9456-1-naush@raspberrypi.com>","MIME-Version":"1.0","Content-Transfer-Encoding":"8bit","Subject":"[libcamera-devel] [PATCH v1 11/14] ipa: raspberrypi: First version\n\tof autofocus algorithm using PDAF","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"<libcamera-devel.lists.libcamera.org>","List-Unsubscribe":"<https://lists.libcamera.org/options/libcamera-devel>,\n\t<mailto:libcamera-devel-request@lists.libcamera.org?subject=unsubscribe>","List-Archive":"<https://lists.libcamera.org/pipermail/libcamera-devel/>","List-Post":"<mailto:libcamera-devel@lists.libcamera.org>","List-Help":"<mailto:libcamera-devel-request@lists.libcamera.org?subject=help>","List-Subscribe":"<https://lists.libcamera.org/listinfo/libcamera-devel>,\n\t<mailto:libcamera-devel-request@lists.libcamera.org?subject=subscribe>","From":"Naushir Patuck via libcamera-devel\n\t<libcamera-devel@lists.libcamera.org>","Reply-To":"Naushir Patuck <naush@raspberrypi.com>","Cc":"Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" <libcamera-devel-bounces@lists.libcamera.org>"},"content":"From: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>\n\nProvide the first version of the Raspberry Pi autofocus algorithm. This\nimplementation uses a hybrid of contrast detect autofocus (CDAF) and phase\ndetect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to\nhaving less \"hunting\" behavior.\n\nSigned-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>\nSigned-off-by: Naushir Patuck <naush@raspberrypi.com>\nReviewed-by: Naushir Patuck <naush@raspberrypi.com>\nReviewed-by: David Plowman <david.plowman@raspberrypi.com>\n---\n src/ipa/raspberrypi/controller/rpi/af.cpp | 755 ++++++++++++++++++++++\n src/ipa/raspberrypi/controller/rpi/af.h   | 153 +++++\n src/ipa/raspberrypi/meson.build           |   1 +\n 3 files changed, 909 insertions(+)\n create mode 100644 src/ipa/raspberrypi/controller/rpi/af.cpp\n create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h","diff":"diff --git a/src/ipa/raspberrypi/controller/rpi/af.cpp b/src/ipa/raspberrypi/controller/rpi/af.cpp\nnew file mode 100644\nindex 000000000000..7e2e8961085a\n--- /dev/null\n+++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n@@ -0,0 +1,755 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2022-2023, Raspberry Pi Ltd\n+ *\n+ * af.cpp - Autofocus control algorithm\n+ */\n+\n+#include \"af.h\"\n+\n+#include <iomanip>\n+#include <math.h>\n+#include <stdlib.h>\n+\n+#include <libcamera/base/log.h>\n+\n+#include <libcamera/control_ids.h>\n+\n+using namespace RPiController;\n+using namespace libcamera;\n+\n+LOG_DEFINE_CATEGORY(RPiAf)\n+\n+#define NAME \"rpi.af\"\n+\n+/*\n+ * Default values for parameters. All may be overridden in the tuning file.\n+ * Many of these values are sensor- or module-dependent; the defaults here\n+ * assume IMX708 in a Raspberry Pi V3 camera with the standard lens.\n+ *\n+ * Here all focus values are in dioptres (1/m). They are converted to hardware\n+ * units when written to status.lensSetting or returned from setLensPosition().\n+ *\n+ * Gain and delay values are relative to the update rate, since much (not all)\n+ * of the delay is in the sensor and (for CDAF) ISP, not the lens mechanism;\n+ * but note that algorithms are updated at no more than 30 Hz.\n+ */\n+\n+Af::RangeDependentParams::RangeDependentParams()\n+\t: focusMin(0.0),\n+\t  focusMax(12.0),\n+\t  focusDefault(1.0)\n+{\n+}\n+\n+Af::SpeedDependentParams::SpeedDependentParams()\n+\t: stepCoarse(1.0),\n+\t  stepFine(0.25),\n+\t  contrastRatio(0.75),\n+\t  pdafGain(-0.02),\n+\t  pdafSquelch(0.125),\n+\t  maxSlew(2.0),\n+\t  pdafFrames(20),\n+\t  dropoutFrames(6),\n+\t  stepFrames(4)\n+{\n+}\n+\n+Af::CfgParams::CfgParams()\n+\t: confEpsilon(8),\n+\t  confThresh(16),\n+\t  confClip(512),\n+\t  skipFrames(5),\n+\t  map()\n+{\n+}\n+\n+template<typename T>\n+static void readNumber(T &dest, const libcamera::YamlObject &params, char const *name)\n+{\n+\tauto value = params[name].get<T>();\n+\tif (value)\n+\t\tdest = *value;\n+\telse\n+\t\tLOG(RPiAf, Warning) << \"Missing parameter \\\"\" << name << \"\\\"\";\n+}\n+\n+void Af::RangeDependentParams::read(const libcamera::YamlObject &params)\n+{\n+\n+\treadNumber<double>(focusMin, params, \"min\");\n+\treadNumber<double>(focusMax, params, \"max\");\n+\treadNumber<double>(focusDefault, params, \"default\");\n+}\n+\n+void Af::SpeedDependentParams::read(const libcamera::YamlObject &params)\n+{\n+\treadNumber<double>(stepCoarse, params, \"step_coarse\");\n+\treadNumber<double>(stepFine, params, \"step_fine\");\n+\treadNumber<double>(contrastRatio, params, \"contrast_ratio\");\n+\treadNumber<double>(pdafGain, params, \"pdaf_gain\");\n+\treadNumber<double>(pdafSquelch, params, \"pdaf_squelch\");\n+\treadNumber<double>(maxSlew, params, \"max_slew\");\n+\treadNumber<uint32_t>(pdafFrames, params, \"pdaf_frames\");\n+\treadNumber<uint32_t>(dropoutFrames, params, \"dropout_frames\");\n+\treadNumber<uint32_t>(stepFrames, params, \"step_frames\");\n+}\n+\n+int Af::CfgParams::read(const libcamera::YamlObject &params)\n+{\n+\tif (params.contains(\"ranges\")) {\n+\t\tauto &rr = params[\"ranges\"];\n+\n+\t\tif (rr.contains(\"normal\"))\n+\t\t\tranges[AfRangeNormal].read(rr[\"normal\"]);\n+\t\telse\n+\t\t\tLOG(RPiAf, Warning) << \"Missing range \\\"normal\\\"\";\n+\n+\t\tranges[AfRangeMacro] = ranges[AfRangeNormal];\n+\t\tif (rr.contains(\"macro\"))\n+\t\t\tranges[AfRangeMacro].read(rr[\"macro\"]);\n+\n+\t\tranges[AfRangeFull].focusMin = std::min(ranges[AfRangeNormal].focusMin,\n+\t\t\t\t\t\t\tranges[AfRangeMacro].focusMin);\n+\t\tranges[AfRangeFull].focusMax = std::max(ranges[AfRangeNormal].focusMax,\n+\t\t\t\t\t\t\tranges[AfRangeMacro].focusMax);\n+\t\tranges[AfRangeFull].focusDefault = ranges[AfRangeNormal].focusDefault;\n+\t\tif (rr.contains(\"full\"))\n+\t\t\tranges[AfRangeFull].read(rr[\"full\"]);\n+\t} else\n+\t\tLOG(RPiAf, Warning) << \"No ranges defined\";\n+\n+\tif (params.contains(\"speeds\")) {\n+\t\tauto &ss = params[\"speeds\"];\n+\n+\t\tif (ss.contains(\"normal\"))\n+\t\t\tspeeds[AfSpeedNormal].read(ss[\"normal\"]);\n+\t\telse\n+\t\t\tLOG(RPiAf, Warning) << \"Missing speed \\\"normal\\\"\";\n+\n+\t\tspeeds[AfSpeedFast] = speeds[AfSpeedNormal];\n+\t\tif (ss.contains(\"fast\"))\n+\t\t\tspeeds[AfSpeedFast].read(ss[\"fast\"]);\n+\t} else\n+\t\tLOG(RPiAf, Warning) << \"No speeds defined\";\n+\n+\treadNumber<uint32_t>(confEpsilon, params, \"conf_epsilon\");\n+\treadNumber<uint32_t>(confThresh, params, \"conf_thresh\");\n+\treadNumber<uint32_t>(confClip, params, \"conf_clip\");\n+\treadNumber<uint32_t>(skipFrames, params, \"skip_frames\");\n+\n+\tif (params.contains(\"map\"))\n+\t\tmap.read(params[\"map\"]);\n+\telse\n+\t\tLOG(RPiAf, Warning) << \"No map defined\";\n+\n+\treturn 0;\n+}\n+\n+void Af::CfgParams::initialise()\n+{\n+\tif (map.empty()) {\n+\t\t/* Default mapping from dioptres to hardware setting */\n+\t\tstatic constexpr double DefaultMapX0 = 0.0;\n+\t\tstatic constexpr double DefaultMapY0 = 445.0;\n+\t\tstatic constexpr double DefaultMapX1 = 15.0;\n+\t\tstatic constexpr double DefaultMapY1 = 925.0;\n+\n+\t\tmap.append(DefaultMapX0, DefaultMapY0);\n+\t\tmap.append(DefaultMapX1, DefaultMapY1);\n+\n+\t\tLOG(RPiAf, Warning) << \"af.map is not defined, \";\n+\t}\n+}\n+\n+/* Af Algorithm class */\n+\n+Af::Af(Controller *controller)\n+\t: AfAlgorithm(controller),\n+\t  cfg_(),\n+\t  range_(AfRangeNormal),\n+\t  speed_(AfSpeedNormal),\n+\t  mode_(AfAlgorithm::AfModeManual),\n+\t  pauseFlag_(false),\n+\t  sensorSize_{ 0, 0 },\n+\t  useWeights_(false),\n+\t  phaseWeights_{},\n+\t  contrastWeights_{},\n+\t  scanState_(ScanState::Idle),\n+\t  initted_(false),\n+\t  ftarget_(-1.0),\n+\t  fsmooth_(-1.0),\n+\t  prevContrast_(0.0),\n+\t  skipCount_(0),\n+\t  stepCount_(0),\n+\t  dropCount_(0),\n+\t  scanMaxContrast_(0.0),\n+\t  scanMinContrast_(1.0e9),\n+\t  scanData_(),\n+\t  reportState_(AfState::Idle)\n+{\n+\tscanData_.reserve(24);\n+}\n+\n+Af::~Af()\n+{\n+}\n+\n+char const *Af::name() const\n+{\n+\treturn NAME;\n+}\n+\n+int Af::read(const libcamera::YamlObject &params)\n+{\n+\treturn cfg_.read(params);\n+}\n+\n+void Af::initialise()\n+{\n+\tcfg_.initialise();\n+}\n+\n+void Af::switchMode(CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)\n+{\n+\t(void)metadata;\n+\tsensorSize_.width = cameraMode.sensorWidth;\n+\tsensorSize_.height = cameraMode.sensorHeight;\n+\n+\tif (scanState_ >= ScanState::Coarse && scanState_ < ScanState::Settle) {\n+\t\t/*\n+\t\t * If a scan was in progress, re-start it, as CDAF statistics\n+\t\t * may have changed. Though if the application is just about\n+\t\t * to take a still picture, this will not help...\n+\t\t */\n+\t\tstartProgrammedScan();\n+\t}\n+\tskipCount_ = cfg_.skipFrames;\n+}\n+\n+bool Af::getPhase(PdafData const &data, double &phase, double &conf) const\n+{\n+\tstatic const uint8_t defaultWeights[PDAF_DATA_ROWS][PDAF_DATA_COLS] = {\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 2, 4, 4, 4, 4, 4, 4, 2, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },\n+\t\t{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }\n+\t};\n+\tint32_t sumW = 0;\n+\tint32_t sumWc = 0;\n+\tint32_t sumWcp = 0;\n+\tauto wgts = useWeights_ ? phaseWeights_ : defaultWeights;\n+\n+\tfor (unsigned i = 0; i < PDAF_DATA_ROWS; ++i) {\n+\t\tfor (unsigned j = 0; j < PDAF_DATA_COLS; ++j) {\n+\t\t\tif (wgts[i][j]) {\n+\t\t\t\tuint32_t c = data.conf[i][j];\n+\t\t\t\tif (c >= cfg_.confThresh) {\n+\t\t\t\t\tif (c > cfg_.confClip)\n+\t\t\t\t\t\tc = cfg_.confClip;\n+\t\t\t\t\tsumWc += wgts[i][j] * (int32_t)c;\n+\t\t\t\t\tc -= (cfg_.confThresh >> 1);\n+\t\t\t\t\tsumWcp += wgts[i][j] * data.phase[i][j] * (int32_t)c;\n+\t\t\t\t}\n+\t\t\t\tsumW += wgts[i][j];\n+\t\t\t}\n+\t\t}\n+\t}\n+\n+\tif (sumWc > 0) {\n+\t\tphase = (double)sumWcp / (double)sumWc;\n+\t\tconf = (double)sumWc / (double)sumW;\n+\t\treturn true;\n+\t} else {\n+\t\tphase = 0.0;\n+\t\tconf = 0.0;\n+\t\treturn false;\n+\t}\n+}\n+\n+void Af::doPDAF(double phase, double conf)\n+{\n+\t/* Apply loop gain */\n+\tphase *= cfg_.speeds[speed_].pdafGain;\n+\n+\tif (mode_ == AfModeContinuous) {\n+\t\t/*\n+\t\t * PDAF in Continuous mode. Scale down lens movement when\n+\t\t * delta is small or confidence is low, to suppress wobble.\n+\t\t */\n+\t\tif (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch) {\n+\t\t\tdouble a = phase / cfg_.speeds[speed_].pdafSquelch;\n+\t\t\tphase *= a * a;\n+\t\t}\n+\t\tphase *= conf / (conf + cfg_.confEpsilon);\n+\t} else {\n+\t\t/*\n+\t\t * PDAF in triggered-auto mode. Allow early termination when\n+\t\t * phase delta is small; scale down lens movements towards\n+\t\t * the end of the sequence, to ensure a stable image.\n+\t\t */\n+\t\tif (stepCount_ >= cfg_.speeds[speed_].stepFrames) {\n+\t\t\tif (std::abs(phase) < cfg_.speeds[speed_].pdafSquelch)\n+\t\t\t\tstepCount_ = cfg_.speeds[speed_].stepFrames;\n+\t\t} else\n+\t\t\tphase *= stepCount_ / cfg_.speeds[speed_].stepFrames;\n+\t}\n+\n+\t/* Apply slew rate limit. Report failure if out of bounds. */\n+\tif (phase < -cfg_.speeds[speed_].maxSlew) {\n+\t\tphase = -cfg_.speeds[speed_].maxSlew;\n+\t\treportState_ = (ftarget_ <= cfg_.ranges[range_].focusMin) ? AfState::Failed\n+\t\t\t\t\t\t\t\t\t  : AfState::Scanning;\n+\t} else if (phase > cfg_.speeds[speed_].maxSlew) {\n+\t\tphase = cfg_.speeds[speed_].maxSlew;\n+\t\treportState_ = (ftarget_ >= cfg_.ranges[range_].focusMax) ? AfState::Failed\n+\t\t\t\t\t\t\t\t\t  : AfState::Scanning;\n+\t} else\n+\t\treportState_ = AfState::Focused;\n+\n+\tftarget_ = fsmooth_ + phase;\n+}\n+\n+bool Af::earlyTerminationByPhase(double phase)\n+{\n+\tif (scanData_.size() > 0 &&\n+\t    scanData_[scanData_.size() - 1].conf >= cfg_.confEpsilon) {\n+\t\tdouble oldFocus = scanData_[scanData_.size() - 1].focus;\n+\t\tdouble oldPhase = scanData_[scanData_.size() - 1].phase;\n+\n+\t\t/*\n+\t\t * Check that the gradient is finite and has the expected sign;\n+\t\t * Interpolate/extrapolate the lens position for zero phase.\n+\t\t * Check that the extrapolation is well-conditioned.\n+\t\t */\n+\t\tif ((ftarget_ - oldFocus) * (phase - oldPhase) > 0.0) {\n+\t\t\tdouble param = phase / (phase - oldPhase);\n+\t\t\tif (-3.0 <= param && param <= 3.5) {\n+\t\t\t\tftarget_ += param * (oldFocus - ftarget_);\n+\t\t\t\tLOG(RPiAf, Debug) << \"ETBP: param=\" << param;\n+\t\t\t\treturn true;\n+\t\t\t}\n+\t\t}\n+\t}\n+\n+\treturn false;\n+}\n+\n+double Af::findPeak(unsigned i) const\n+{\n+\tdouble f = scanData_[i].focus;\n+\n+\tif (i > 0 && i + 1 < scanData_.size()) {\n+\t\tdouble dropLo = scanData_[i].contrast - scanData_[i - 1].contrast;\n+\t\tdouble dropHi = scanData_[i].contrast - scanData_[i + 1].contrast;\n+\t\tif (dropLo < dropHi) {\n+\t\t\tdouble param = 0.3125 * (1.0 - dropLo / dropHi) * (1.6 - dropLo / dropHi);\n+\t\t\tf += param * (scanData_[i - 1].focus - f);\n+\t\t} else if (dropHi < dropLo) {\n+\t\t\tdouble param = 0.3125 * (1.0 - dropHi / dropLo) * (1.6 - dropHi / dropLo);\n+\t\t\tf += param * (scanData_[i + 1].focus - f);\n+\t\t}\n+\t}\n+\n+\tLOG(RPiAf, Debug) << \"FindPeak: \" << f;\n+\treturn f;\n+}\n+\n+void Af::doScan(double contrast, double phase, double conf)\n+{\n+\t/* Record lens position, contrast and phase values for the current scan */\n+\tif (scanData_.empty() || contrast > scanMaxContrast_) {\n+\t\tscanMaxContrast_ = contrast;\n+\t\tscanMaxIndex_ = scanData_.size();\n+\t}\n+\tif (contrast < scanMinContrast_)\n+\t\tscanMinContrast_ = contrast;\n+\tscanData_.emplace_back(ScanRecord{ ftarget_, contrast, phase, conf });\n+\n+\tif (scanState_ == ScanState::Coarse) {\n+\t\tif (ftarget_ >= cfg_.ranges[range_].focusMax ||\n+\t\t    contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n+\t\t\t/*\n+\t\t\t * Finished course scan, or termination based on contrast.\n+\t\t\t * Jump to just after max contrast and start fine scan.\n+\t\t\t */\n+\t\t\tftarget_ = std::min(ftarget_, findPeak(scanMaxIndex_) +\n+\t\t\t\t\t2.0 * cfg_.speeds[speed_].stepFine);\n+\t\t\tscanState_ = ScanState::Fine;\n+\t\t\tscanData_.clear();\n+\t\t} else\n+\t\t\tftarget_ += cfg_.speeds[speed_].stepCoarse;\n+\t} else { /* ScanState::Fine */\n+\t\tif (ftarget_ <= cfg_.ranges[range_].focusMin || scanData_.size() >= 5 ||\n+\t\t    contrast < cfg_.speeds[speed_].contrastRatio * scanMaxContrast_) {\n+\t\t\t/*\n+\t\t\t * Finished fine scan, or termination based on contrast.\n+\t\t\t * Use quadratic peak-finding to find best contrast position.\n+\t\t\t */\n+\t\t\tftarget_ = findPeak(scanMaxIndex_);\n+\t\t\tscanState_ = ScanState::Settle;\n+\t\t} else\n+\t\t\tftarget_ -= cfg_.speeds[speed_].stepFine;\n+\t}\n+\n+\tstepCount_ = (ftarget_ == fsmooth_) ? 0 : cfg_.speeds[speed_].stepFrames;\n+}\n+\n+void Af::doAF(double contrast, double phase, double conf)\n+{\n+\t/* Skip frames at startup and after mode change */\n+\tif (skipCount_ > 0) {\n+\t\tLOG(RPiAf, Debug) << \"SKIP\";\n+\t\tskipCount_--;\n+\t\treturn;\n+\t}\n+\n+\tif (scanState_ == ScanState::Pdaf) {\n+\t\t/*\n+\t\t * Use PDAF closed-loop control whenever available, in both CAF\n+\t\t * mode and (for a limited number of iterations) when triggered.\n+\t\t * If PDAF fails (due to poor contrast, noise or large defocus),\n+\t\t * fall back to a CDAF-based scan. To avoid \"nuisance\" scans,\n+\t\t * scan only after a number of frames with low PDAF confidence.\n+\t\t */\n+\t\tif (conf > (dropCount_ ? cfg_.confEpsilon : 0.0)) {\n+\t\t\tdoPDAF(phase, conf);\n+\t\t\tif (stepCount_ > 0)\n+\t\t\t\tstepCount_--;\n+\t\t\telse if (mode_ != AfModeContinuous)\n+\t\t\t\tscanState_ = ScanState::Idle;\n+\t\t\tdropCount_ = 0;\n+\t\t} else if (++dropCount_ == cfg_.speeds[speed_].dropoutFrames)\n+\t\t\tstartProgrammedScan();\n+\t} else if (scanState_ >= ScanState::Coarse && fsmooth_ == ftarget_) {\n+\t\t/*\n+\t\t * Scanning sequence. This means PDAF has become unavailable.\n+\t\t * Allow a delay between steps for CDAF FoM statistics to be\n+\t\t * updated, and a \"settling time\" at the end of the sequence.\n+\t\t * [A coarse or fine scan can be abandoned if two PDAF samples\n+\t\t * allow direct interpolation of the zero-phase lens position.]\n+\t\t */\n+\t\tif (stepCount_ > 0)\n+\t\t\tstepCount_--;\n+\t\telse if (scanState_ == ScanState::Settle) {\n+\t\t\tif (prevContrast_ >= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_ &&\n+\t\t\t    scanMinContrast_ <= cfg_.speeds[speed_].contrastRatio * scanMaxContrast_)\n+\t\t\t\treportState_ = AfState::Focused;\n+\t\t\telse\n+\t\t\t\treportState_ = AfState::Failed;\n+\t\t\tif (mode_ == AfModeContinuous && !pauseFlag_ &&\n+\t\t\t    cfg_.speeds[speed_].dropoutFrames > 0)\n+\t\t\t\tscanState_ = ScanState::Pdaf;\n+\t\t\telse\n+\t\t\t\tscanState_ = ScanState::Idle;\n+\t\t\tscanData_.clear();\n+\t\t} else if (conf >= cfg_.confEpsilon && earlyTerminationByPhase(phase)) {\n+\t\t\tscanState_ = ScanState::Settle;\n+\t\t\tstepCount_ = (mode_ == AfModeContinuous) ? 0\n+\t\t\t\t\t\t\t\t : cfg_.speeds[speed_].stepFrames;\n+\t\t} else\n+\t\t\tdoScan(contrast, phase, conf);\n+\t}\n+}\n+\n+void Af::updateLensPosition()\n+{\n+\tif (mode_ != AfModeManual) {\n+\t\tftarget_ = std::clamp(ftarget_,\n+\t\t\t\tcfg_.ranges[range_].focusMin,\n+\t\t\t\tcfg_.ranges[range_].focusMax);\n+\t}\n+\n+\t/* \\todo Add a clip for manual lens position to be within the PWL limits. */\n+\n+\tif (initted_) {\n+\t\t/* from a known lens position: apply slew rate limit */\n+\t\tfsmooth_ = std::clamp(ftarget_,\n+\t\t\t\t      fsmooth_ - cfg_.speeds[speed_].maxSlew,\n+\t\t\t\t      fsmooth_ + cfg_.speeds[speed_].maxSlew);\n+\t} else {\n+\t\t/* from an unknown position: go straight to target, but add delay */\n+\t\tfsmooth_ = ftarget_;\n+\t\tinitted_ = true;\n+\t\tskipCount_ = cfg_.skipFrames;\n+\t}\n+}\n+\n+/*\n+ * PDAF phase data are available in prepare(), but CDAF statistics are not\n+ * available until process(). We are gambling on the availability of PDAF.\n+ * To expedite feedback control using PDAF, issue the V4L2 lens control from\n+ * prepare(). Conversely, during scans, we must allow an extra frame delay\n+ * between steps, to retrieve CDAF statistics from the previous process()\n+ * so we can terminate the scan early without having to change our minds.\n+ */\n+\n+void Af::prepare(Metadata *imageMetadata)\n+{\n+\tif (initted_) {\n+\t\t/* Get PDAF from the embedded metadata, and run AF algorithm core */\n+\t\tPdafData data;\n+\t\tdouble phase = 0.0, conf = 0.0;\n+\t\tdouble oldFt = ftarget_;\n+\t\tdouble oldFs = fsmooth_;\n+\t\tScanState oldSs = scanState_;\n+\t\tuint32_t oldSt = stepCount_;\n+\t\tif (imageMetadata->get(\"pdaf.data\", data) == 0)\n+\t\t\tgetPhase(data, phase, conf);\n+\t\tdoAF(prevContrast_, phase, conf);\n+\t\tupdateLensPosition();\n+\t\tLOG(RPiAf, Debug) << std::fixed << std::setprecision(2)\n+\t\t\t\t  << static_cast<unsigned int>(reportState_)\n+\t\t\t\t  << \" sst\" << static_cast<unsigned int>(oldSs)\n+\t\t\t\t  << \"->\" << static_cast<unsigned int>(scanState_)\n+\t\t\t\t  << \" stp\" << oldSt << \"->\" << stepCount_\n+\t\t\t\t  << \" ft\" << oldFt << \"->\" << ftarget_\n+\t\t\t\t  << \" fs\" << oldFs << \"->\" << fsmooth_\n+\t\t\t\t  << \" cont=\" << (int)prevContrast_\n+\t\t\t\t  << \" phase=\" << (int)phase << \" conf=\" << (int)conf;\n+\t}\n+\n+\t/* Report status and produce new lens setting */\n+\tAfStatus status;\n+\tif (pauseFlag_)\n+\t\tstatus.pauseState = (scanState_ == ScanState::Idle) ? AfPauseState::Paused\n+\t\t\t\t\t\t\t\t    : AfPauseState::Pausing;\n+\telse\n+\t\tstatus.pauseState = AfPauseState::Running;\n+\n+\tif (mode_ == AfModeAuto && scanState_ != ScanState::Idle)\n+\t\tstatus.state = AfState::Scanning;\n+\telse\n+\t\tstatus.state = reportState_;\n+\tstatus.lensSetting = initted_ ? std::optional<int>(cfg_.map.eval(fsmooth_))\n+\t\t\t\t      : std::nullopt;\n+\timageMetadata->set(\"af.status\", status);\n+}\n+\n+double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const\n+{\n+\tuint32_t totW = 0, totWc = 0;\n+\n+\tif (useWeights_) {\n+\t\tfor (unsigned i = 0; i < FOCUS_REGIONS; ++i) {\n+\t\t\tunsigned w = contrastWeights_[i];\n+\t\t\ttotW += w;\n+\t\t\ttotWc += w * (focus_stats[i].contrast_val[1][1] >> 10);\n+\t\t}\n+\t}\n+\tif (totW == 0) {\n+\t\ttotW = 2;\n+\t\ttotWc = (focus_stats[5].contrast_val[1][1] >> 10) +\n+\t\t\t (focus_stats[6].contrast_val[1][1] >> 10);\n+\t}\n+\n+\treturn (double)totWc / (double)totW;\n+}\n+\n+void Af::process(StatisticsPtr &stats, [[maybe_unused]] Metadata *imageMetadata)\n+{\n+\t(void)imageMetadata;\n+\tprevContrast_ = getContrast(stats->focus_stats);\n+}\n+\n+/* Controls */\n+\n+void Af::setRange(AfRange r)\n+{\n+\tLOG(RPiAf, Debug) << \"setRange: \" << (unsigned)r;\n+\tif (r < AfAlgorithm::AfRangeMax)\n+\t\trange_ = r;\n+}\n+\n+void Af::setSpeed(AfSpeed s)\n+{\n+\tLOG(RPiAf, Debug) << \"setSpeed: \" << (unsigned)s;\n+\tif (s < AfAlgorithm::AfSpeedMax) {\n+\t\tif (scanState_ == ScanState::Pdaf &&\n+\t\t    cfg_.speeds[s].pdafFrames > cfg_.speeds[speed_].pdafFrames)\n+\t\t\tstepCount_ += cfg_.speeds[s].pdafFrames - cfg_.speeds[speed_].pdafFrames;\n+\t\tspeed_ = s;\n+\t}\n+}\n+\n+void Af::setMetering(bool mode)\n+{\n+\tuseWeights_ = mode;\n+}\n+\n+void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)\n+{\n+\t/*\n+\t * Here we just merge all of the given windows, weighted by area.\n+\t * If there are more than 15 overlapping windows, overflow can occur.\n+\t * TODO: A better approach might be to find the phase in each window\n+\t * and choose either the closest or the highest-confidence one?\n+\t *\n+\t * Using mostly \"int\" arithmetic, because Rectangle has signed x, y\n+\t */\n+\tASSERT(sensorSize_.width > 0 && sensorSize_.height > 0);\n+\tint gridY = (int)(sensorSize_.height / PDAF_DATA_ROWS);\n+\tint gridX = (int)(sensorSize_.width / PDAF_DATA_COLS);\n+\tint gridA = gridY * gridX;\n+\n+\tfor (int i = 0; i < PDAF_DATA_ROWS; ++i)\n+\t\tstd::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);\n+\tstd::fill(contrastWeights_, contrastWeights_ + FOCUS_REGIONS, 0);\n+\n+\tfor (auto &w : wins) {\n+\t\tfor (int i = 0; i < PDAF_DATA_ROWS; ++i) {\n+\t\t\tint y0 = std::max(gridY * i, w.y);\n+\t\t\tint y1 = std::min(gridY * (i + 1), w.y + (int)(w.height));\n+\t\t\tif (y0 >= y1)\n+\t\t\t\tcontinue;\n+\t\t\ty1 -= y0;\n+\t\t\tfor (int j = 0; j < PDAF_DATA_COLS; ++j) {\n+\t\t\t\tint x0 = std::max(gridX * j, w.x);\n+\t\t\t\tint x1 = std::min(gridX * (j + 1), w.x + (int)(w.width));\n+\t\t\t\tif (x0 >= x1)\n+\t\t\t\t\tcontinue;\n+\t\t\t\tint a = y1 * (x1 - x0);\n+\t\t\t\ta = (16 * a + gridA - 1) / gridA;\n+\t\t\t\tphaseWeights_[i][j] += a;\n+\t\t\t\tcontrastWeights_[4 * ((3 * i) / PDAF_DATA_ROWS) + ((4 * j) / PDAF_DATA_COLS)] += a;\n+\t\t\t}\n+\t\t}\n+\t}\n+}\n+\n+bool Af::setLensPosition(double dioptres, int *hwpos)\n+{\n+\tbool changed = false;\n+\n+\tif (mode_ == AfModeManual) {\n+\t\tLOG(RPiAf, Debug) << \"setLensPosition: \" << dioptres;\n+\t\tchanged = !(initted_ && fsmooth_ == dioptres);\n+\t\tftarget_ = dioptres;\n+\t\tupdateLensPosition();\n+\t}\n+\n+\tif (hwpos)\n+\t\t*hwpos = cfg_.map.eval(fsmooth_);\n+\n+\treturn changed;\n+}\n+\n+std::optional<double> Af::getLensPosition() const\n+{\n+\t/*\n+\t * \\todo We ought to perform some precise timing here to determine\n+\t * the current lens position.\n+\t */\n+\treturn initted_ ? std::optional<double>(fsmooth_) : std::nullopt;\n+}\n+\n+void Af::startCAF()\n+{\n+\t/* Try PDAF if the tuning file permits it for CAF; else CDAF */\n+\tif (cfg_.speeds[speed_].dropoutFrames > 0) {\n+\t\tif (!initted_) {\n+\t\t\tftarget_ = cfg_.ranges[range_].focusDefault;\n+\t\t\tupdateLensPosition();\n+\t\t}\n+\t\tscanState_ = ScanState::Pdaf;\n+\t\tscanData_.clear();\n+\t\tdropCount_ = 0;\n+\t\treportState_ = AfState::Scanning;\n+\t} else {\n+\t\tstartProgrammedScan();\n+\t}\n+}\n+\n+void Af::startProgrammedScan()\n+{\n+\tftarget_ = cfg_.ranges[range_].focusMin;\n+\tupdateLensPosition();\n+\tscanState_ = ScanState::Coarse;\n+\tscanMaxContrast_ = 0.0;\n+\tscanMinContrast_ = 1.0e9;\n+\tscanMaxIndex_ = 0;\n+\tscanData_.clear();\n+\tstepCount_ = cfg_.speeds[speed_].stepFrames;\n+\treportState_ = AfState::Scanning;\n+}\n+\n+void Af::goIdle()\n+{\n+\tscanState_ = ScanState::Idle;\n+\treportState_ = AfState::Idle;\n+\tscanData_.clear();\n+}\n+\n+void Af::cancelScan()\n+{\n+\tLOG(RPiAf, Debug) << \"cancelScan\";\n+\tif (mode_ == AfModeAuto)\n+\t\tgoIdle();\n+}\n+\n+void Af::triggerScan()\n+{\n+\tLOG(RPiAf, Debug) << \"triggerScan\";\n+\tif (mode_ == AfModeAuto && scanState_ == ScanState::Idle) {\n+\t\t/* Try PDAF if the tuning file permits it for Auto; else CDAF */\n+\t\tif (cfg_.speeds[speed_].pdafFrames > 0 && cfg_.speeds[speed_].dropoutFrames > 0) {\n+\t\t\tif (!initted_) {\n+\t\t\t\tftarget_ = cfg_.ranges[range_].focusDefault;\n+\t\t\t\tupdateLensPosition();\n+\t\t\t}\n+\t\t\tstepCount_ = cfg_.speeds[speed_].pdafFrames;\n+\t\t\tscanState_ = ScanState::Pdaf;\n+\t\t\tdropCount_ = 0;\n+\t\t} else\n+\t\t\tstartProgrammedScan();\n+\t\treportState_ = AfState::Scanning;\n+\t}\n+}\n+\n+void Af::setMode(AfAlgorithm::AfMode mode)\n+{\n+\tLOG(RPiAf, Debug) << \"setMode: \" << (unsigned)mode;\n+\tif (mode_ != mode) {\n+\t\tmode_ = mode;\n+\t\tpauseFlag_ = false;\n+\t\tif (mode == AfModeContinuous)\n+\t\t\tstartCAF();\n+\t\telse if (mode != AfModeAuto)\n+\t\t\tgoIdle();\n+\t}\n+}\n+\n+AfAlgorithm::AfMode Af::getMode() const\n+{\n+\treturn mode_;\n+}\n+\n+void Af::pause(AfAlgorithm::AfPause pause)\n+{\n+\tLOG(RPiAf, Debug) << \"pause: \" << (unsigned)pause;\n+\tif (mode_ == AfModeContinuous) {\n+\t\tif (pause == AfPauseResume && pauseFlag_) {\n+\t\t\tstartCAF();\n+\t\t\tpauseFlag_ = false;\n+\t\t} else if (pause != AfPauseResume && !pauseFlag_) {\n+\t\t\tif (pause == AfPauseImmediate || scanState_ == ScanState::Pdaf)\n+\t\t\t\tgoIdle();\n+\t\t\tpauseFlag_ = true;\n+\t\t}\n+\t}\n+}\n+\n+// Register algorithm with the system.\n+static Algorithm *create(Controller *controller)\n+{\n+\treturn (Algorithm *)new Af(controller);\n+}\n+static RegisterAlgorithm reg(NAME, &create);\ndiff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h\nnew file mode 100644\nindex 000000000000..0431bd70ae29\n--- /dev/null\n+++ b/src/ipa/raspberrypi/controller/rpi/af.h\n@@ -0,0 +1,153 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2022-2023, Raspberry Pi Ltd\n+ *\n+ * af.h - Autofocus control algorithm\n+ */\n+#pragma once\n+\n+#include \"../af_algorithm.h\"\n+#include \"../af_status.h\"\n+#include \"../pdaf_data.h\"\n+#include \"../pwl.h\"\n+\n+/*\n+ * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.\n+ *\n+ * Whenever PDAF is available, it is used in a continuous feedback loop.\n+ * When triggered in auto mode, we simply enable AF for a limited number\n+ * of frames (it may terminate early if the delta becomes small enough).\n+ *\n+ * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)\n+ * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.\n+ * A coarse and fine scan are performed, using ISP's CDAF focus FoM to\n+ * estimate the lens position with peak contrast. This is slower due to\n+ * extra latency in the ISP, and requires a settling time between steps.\n+ *\n+ * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid\n+ * \"nuisance\" scans. During each interval where PDAF is not working, only\n+ * ONE scan will be performed; CAF cannot track objects using CDAF alone.\n+ *\n+ * This algorithm is unrelated to \"rpi.focus\" which merely reports CDAF FoM.\n+ */\n+\n+namespace RPiController {\n+\n+class Af : public AfAlgorithm\n+{\n+public:\n+\tAf(Controller *controller = NULL);\n+\t~Af();\n+\tchar const *name() const override;\n+\tint read(const libcamera::YamlObject &params) override;\n+\tvoid initialise() override;\n+\n+\t/* IPA calls */\n+\tvoid switchMode(CameraMode const &cameraMode, Metadata *metadata) override;\n+\tvoid prepare(Metadata *imageMetadata) override;\n+\tvoid process(StatisticsPtr &stats, Metadata *imageMetadata) override;\n+\n+\t/* controls */\n+\tvoid setRange(AfRange range) override;\n+\tvoid setSpeed(AfSpeed speed) override;\n+\tvoid setMetering(bool use_windows) override;\n+\tvoid setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;\n+\tvoid setMode(AfMode mode) override;\n+\tAfMode getMode() const override;\n+\tbool setLensPosition(double dioptres, int32_t *hwpos) override;\n+\tstd::optional<double> getLensPosition() const override;\n+\tvoid triggerScan() override;\n+\tvoid cancelScan() override;\n+\tvoid pause(AfPause pause) override;\n+\n+private:\n+\tenum class ScanState {\n+\t\tIdle,\n+\t\tPdaf,\n+\t\tCoarse,\n+\t\tFine,\n+\t\tSettle\n+\t};\n+\n+\tstruct RangeDependentParams {\n+\t\tdouble focusMin;       \t\t/* lower (far) limit in dipotres */\n+\t\tdouble focusMax;\t       \t/* upper (near) limit in dioptres */\n+\t\tdouble focusDefault;\t\t/* default setting (\"hyperfocal\") */\n+\n+\t\tRangeDependentParams();\n+\t\tvoid read(const libcamera::YamlObject &params);\n+\t};\n+\n+\tstruct SpeedDependentParams {\n+\t\tdouble stepCoarse;\t\t/* used for scans */\n+\t\tdouble stepFine;\t\t/* used for scans */\n+\t\tdouble contrastRatio;\t\t/* used for scan termination and reporting */\n+\t\tdouble pdafGain;\t\t/* coefficient for PDAF feedback loop */\n+\t\tdouble pdafSquelch;\t\t/* PDAF stability parameter (device-specific) */\n+\t\tdouble maxSlew;\t\t\t/* limit for lens movement per frame */\n+\t\tuint32_t pdafFrames;\t\t/* number of iterations when triggered */\n+\t\tuint32_t dropoutFrames;\t\t/* number of non-PDAF frames to switch to CDAF */\n+\t\tuint32_t stepFrames;\t\t/* frames to skip in between steps of a scan */\n+\n+\t\tSpeedDependentParams();\n+\t\tvoid read(const libcamera::YamlObject &params);\n+\t};\n+\n+\tstruct CfgParams {\n+\t\tRangeDependentParams ranges[AfRangeMax];\n+\t\tSpeedDependentParams speeds[AfSpeedMax];\n+\t\tuint32_t confEpsilon;\t       \t/* PDAF hysteresis threshold (sensor-specific) */\n+\t\tuint32_t confThresh;\t       \t/* PDAF confidence cell min (sensor-specific) */\n+\t\tuint32_t confClip;\t       \t/* PDAF confidence cell max (sensor-specific) */\n+\t\tuint32_t skipFrames;\t       \t/* frames to skip at start or modeswitch */\n+\t\tPwl map;\t\t       \t/* converts dioptres -> lens driver position */\n+\n+\t\tCfgParams();\n+\t\tint read(const libcamera::YamlObject &params);\n+\t\tvoid initialise();\n+\t};\n+\n+\tstruct ScanRecord {\n+\t\tdouble focus;\n+\t\tdouble contrast;\n+\t\tdouble phase;\n+\t\tdouble conf;\n+\t};\n+\n+\tbool getPhase(PdafData const &data, double &phase, double &conf) const;\n+\tdouble getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;\n+\tvoid doPDAF(double phase, double conf);\n+\tbool earlyTerminationByPhase(double phase);\n+\tvoid doScan(double contrast, double phase, double conf);\n+\tdouble findPeak(unsigned index) const;\n+\tvoid doAF(double contrast, double phase, double conf);\n+\tvoid updateLensPosition();\n+\tvoid startProgrammedScan();\n+\tvoid startCAF();\n+\tvoid goIdle();\n+\n+\t/* Configuration and settings */\n+\tCfgParams cfg_;\n+\tAfRange range_;\n+\tAfSpeed speed_;\n+\tAfMode mode_;\n+\tbool pauseFlag_;\n+\tlibcamera::Size sensorSize_;\n+\tbool useWeights_;\n+\tuint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];\n+\tuint16_t contrastWeights_[FOCUS_REGIONS];\n+\n+\t/* Working state. */\n+\tScanState scanState_;\n+\tbool initted_;\n+\tdouble ftarget_, fsmooth_;\n+\tdouble prevContrast_;\n+\tbool pdafSeen_;\n+\tunsigned skipCount_, stepCount_, dropCount_;\n+\tunsigned scanMaxIndex_;\n+\tdouble scanMaxContrast_, scanMinContrast_;\n+\tstd::vector<ScanRecord> scanData_;\n+\tAfState reportState_;\n+};\n+\n+} // namespace RPiController\ndiff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build\nindex 517d815bb98c..4e2689536257 100644\n--- a/src/ipa/raspberrypi/meson.build\n+++ b/src/ipa/raspberrypi/meson.build\n@@ -27,6 +27,7 @@ rpi_ipa_sources = files([\n     'controller/controller.cpp',\n     'controller/histogram.cpp',\n     'controller/algorithm.cpp',\n+    'controller/rpi/af.cpp',\n     'controller/rpi/alsc.cpp',\n     'controller/rpi/awb.cpp',\n     'controller/rpi/sharpen.cpp',\n","prefixes":["libcamera-devel","v1","11/14"]}