{"id":18187,"url":"https://patchwork.libcamera.org/api/patches/18187/?format=json","web_url":"https://patchwork.libcamera.org/patch/18187/","project":{"id":1,"url":"https://patchwork.libcamera.org/api/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":"<20230123154934.27803-12-naush@raspberrypi.com>","date":"2023-01-23T15:49:31","name":"[libcamera-devel,v2,11/14] ipa: raspberrypi: First version of autofocus algorithm using PDAF","commit_ref":"cc010b0c35bfa0eee0bafba6a5803d1405542456","pull_url":null,"state":"accepted","archived":false,"hash":"772f637359df7fa8f3e00722a8296199ff889d51","submitter":{"id":34,"url":"https://patchwork.libcamera.org/api/people/34/?format=json","name":"Naushir Patuck","email":"naush@raspberrypi.com"},"delegate":null,"mbox":"https://patchwork.libcamera.org/patch/18187/mbox/","series":[{"id":3719,"url":"https://patchwork.libcamera.org/api/series/3719/?format=json","web_url":"https://patchwork.libcamera.org/project/libcamera/list/?series=3719","date":"2023-01-23T15:49:20","name":"Raspberry Pi: Camera Module 3 support","version":2,"mbox":"https://patchwork.libcamera.org/series/3719/mbox/"}],"comments":"https://patchwork.libcamera.org/api/patches/18187/comments/","check":"pending","checks":"https://patchwork.libcamera.org/api/patches/18187/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 91730C3298\n\tfor <parsemail@patchwork.libcamera.org>;\n\tMon, 23 Jan 2023 15:50:02 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 42F0162614;\n\tMon, 23 Jan 2023 16:50:02 +0100 (CET)","from mail-wm1-x32e.google.com (mail-wm1-x32e.google.com\n\t[IPv6:2a00:1450:4864:20::32e])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 65F7F62601\n\tfor <libcamera-devel@lists.libcamera.org>;\n\tMon, 23 Jan 2023 16:49:55 +0100 (CET)","by mail-wm1-x32e.google.com with SMTP id\n\te19-20020a05600c439300b003db1cac0c1fso9438636wmn.5\n\tfor <libcamera-devel@lists.libcamera.org>;\n\tMon, 23 Jan 2023 07:49:55 -0800 (PST)","from localhost.localdomain ([93.93.133.154])\n\tby smtp.gmail.com with ESMTPSA id\n\tn16-20020a05600c181000b003d1de805de5sm10339689wmp.16.2023.01.23.07.49.53\n\t(version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256);\n\tMon, 23 Jan 2023 07:49:54 -0800 (PST)"],"DKIM-Signature":["v=1; a=rsa-sha256; c=relaxed/simple; d=libcamera.org;\n\ts=mail; t=1674489002;\n\tbh=6H96QZ3C86kUAU+1qICEYgxfvzvBTQ4373sHYfw1B38=;\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=f2t36r4I7IltoREN8gqRzl9BUb4/1QCujGyP8MaRy6WmWllDFdXmgnj27reMiERks\n\tAzQ88Y3bHJi19ELJlVgX///fDjbOJxBIDmTSM4SKZbd+yg/HhIIoHM/6Cks1KiSQl5\n\tb0hDHOe9XksuuGsWhN8dwaaTMoSPzvtcChrYy2fIvDFO0Oapnl676I53Ddo8bP1Ehg\n\tXYYmPtVG9GOXEOsoxXKAs6tV6lwTPU7HvQNurqQF2xJ4EcFxvn1qjl3kRZt4G09XzM\n\t2avAt+mASdtME2w5PDZ1jJgSTMXvZZ/wvTH8p7+tChCQ8K7bbP6kTPdywyNHsyYl73\n\tSHv9RsjzFPVPg==","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=hD6vL0z/OF8HyVNYrfq/gbSq1OW9v8NrBiKRZnVuqAk=;\n\tb=hOEpcB1bZRAsjazWK71kSVsKKcBFKY/lYBr/lPGwk2M6LAgUtMlxS6NP3aAFLEuQ2a\n\tZ45CH4e98Y58i9t3JbN08W32AaR7IpNbEJrjJ6luemEzvpSdrhjQ9G+BeKrYvXrMPgea\n\tSMhzodKrwBH+YHogEnzfepF5iV4pfOFhoQzxZWepicuBquEd+8a42/iXOaG3FSmE/yIa\n\tPEpkf4ctJQRNzIuW2li2PeNiYYihjAfgkTHmkphDyE14W+76HXolJqYyyeIw4nVMI3yu\n\tMEkK6WNjMYEQvXAdC21xGuvRzsu2+65h+qLOkuemPQy8pVYN8g3KX4kWa+d8aW4DC7xt\n\tyU+g=="],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key; \n\tunprotected) header.d=raspberrypi.com\n\theader.i=@raspberrypi.com\n\theader.b=\"hOEpcB1b\"; 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=hD6vL0z/OF8HyVNYrfq/gbSq1OW9v8NrBiKRZnVuqAk=;\n\tb=3NnJv0MpExayofrnyq5vfI/wGkX3fiWpEjItEjlbqGFHNTsf59sIEsznVHvRC9Ok8m\n\tTYjGqly6IYiW/heCA3Dg5e8lOOpSdZ14Fi+3RO/crVsPAY/NXhKcJzX8t2k5ehQ2JsDY\n\tDKB989eraLEUOza+s8eSnewJtmJquJ4BBFh4ytV3tfhBMsopsu+9Sx/ACoQsBAw0A3jb\n\tWsxgqqLgVgBmL6Rv3vJt8oZPk+8lT097wZG3CqRWcFVDcvMI1Q4K8RH9IzybdSBqW1fL\n\tSfDtrGoAZs6uXoUIueGHvYh+mN4fnZ/ANN6hc1JaBjSr6JPTc3ZYEn+htTqKnmyDsm0x\n\t5GVg==","X-Gm-Message-State":"AFqh2koSvCS3lEN14l3pSmib3dFb6tyffa0Pqmk9PVAKCVZebKvm7RNC\n\tHvLfGmth2nOFRhxVTGnNNJYdy1qXIUfu8/64TU0=","X-Google-Smtp-Source":"AMrXdXtYMmf8KniX9gOchU/J8Fbmwpd8MXbsLv7RfS1wjCsMSkeeNpUUd15fydHRC+uiDBVjjctcew==","X-Received":"by 2002:a05:600c:4687:b0:3db:2e06:4091 with SMTP id\n\tp7-20020a05600c468700b003db2e064091mr14173609wmo.37.1674488994549; \n\tMon, 23 Jan 2023 07:49:54 -0800 (PST)","To":"libcamera-devel@lists.libcamera.org","Date":"Mon, 23 Jan 2023 15:49:31 +0000","Message-Id":"<20230123154934.27803-12-naush@raspberrypi.com>","X-Mailer":"git-send-email 2.25.1","In-Reply-To":"<20230123154934.27803-1-naush@raspberrypi.com>","References":"<20230123154934.27803-1-naush@raspberrypi.com>","MIME-Version":"1.0","Content-Transfer-Encoding":"8bit","Subject":"[libcamera-devel] [PATCH v2 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 | 795 ++++++++++++++++++++++\n src/ipa/raspberrypi/controller/rpi/af.h   | 156 +++++\n src/ipa/raspberrypi/meson.build           |   1 +\n 3 files changed, 952 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..2e72f239fb25\n--- /dev/null\n+++ b/src/ipa/raspberrypi/controller/rpi/af.cpp\n@@ -0,0 +1,795 @@\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+\t}\n+}\n+\n+/* Af Algorithm class */\n+\n+static constexpr unsigned MaxWindows = 10;\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  statsRegion_(0, 0, 0, 0),\n+\t  windows_(),\n+\t  useWindows_(false),\n+\t  phaseWeights_{},\n+\t  contrastWeights_{},\n+\t  sumWeights_(0),\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+\n+\t/* Assume that PDAF and Focus stats grids cover the visible area */\n+\tstatsRegion_.x = (int)cameraMode.cropX;\n+\tstatsRegion_.y = (int)cameraMode.cropY;\n+\tstatsRegion_.width = (unsigned)(cameraMode.width * cameraMode.scaleX);\n+\tstatsRegion_.height = (unsigned)(cameraMode.height * cameraMode.scaleY);\n+\tLOG(RPiAf, Debug) << \"switchMode: statsRegion: \"\n+\t\t\t  << statsRegion_.x << ','\n+\t\t\t  << statsRegion_.y << ','\n+\t\t\t  << statsRegion_.width << ','\n+\t\t\t  << statsRegion_.height;\n+\tcomputeWeights();\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+void Af::computeWeights()\n+{\n+\tconstexpr int MaxCellWeight = 240 / (int)MaxWindows;\n+\n+\tsumWeights_ = 0;\n+\tfor (int i = 0; i < PDAF_DATA_ROWS; ++i)\n+\t\tstd::fill(phaseWeights_[i], phaseWeights_[i] + PDAF_DATA_COLS, 0);\n+\n+\tif (useWindows_ &&\n+\t    statsRegion_.width >= PDAF_DATA_COLS && statsRegion_.height >= PDAF_DATA_ROWS) {\n+\t\t/*\n+\t\t * Here we just merge all of the given windows, weighted by area.\n+\t\t * \\todo Perhaps a better approach might be to find the phase in each\n+\t\t * window and choose either the closest or the highest-confidence one?\n+\t\t *\n+\t\t * Using mostly \"int\" arithmetic, because Rectangle has signed x, y\n+\t\t */\n+\t\tint cellH = (int)(statsRegion_.height / PDAF_DATA_ROWS);\n+\t\tint cellW = (int)(statsRegion_.width / PDAF_DATA_COLS);\n+\t\tint cellA = cellH * cellW;\n+\n+\t\tfor (auto &w : windows_) {\n+\t\t\tfor (int i = 0; i < PDAF_DATA_ROWS; ++i) {\n+\t\t\t\tint y0 = std::max(statsRegion_.y + cellH * i, w.y);\n+\t\t\t\tint y1 = std::min(statsRegion_.y + cellH * (i + 1), w.y + (int)(w.height));\n+\t\t\t\tif (y0 >= y1)\n+\t\t\t\t\tcontinue;\n+\t\t\t\ty1 -= y0;\n+\t\t\t\tfor (int j = 0; j < PDAF_DATA_COLS; ++j) {\n+\t\t\t\t\tint x0 = std::max(statsRegion_.x + cellW * j, w.x);\n+\t\t\t\t\tint x1 = std::min(statsRegion_.x + cellW * (j + 1), w.x + (int)(w.width));\n+\t\t\t\t\tif (x0 >= x1)\n+\t\t\t\t\t\tcontinue;\n+\t\t\t\t\tint a = y1 * (x1 - x0);\n+\t\t\t\t\ta = (MaxCellWeight * a + cellA - 1) / cellA;\n+\t\t\t\t\tphaseWeights_[i][j] += a;\n+\t\t\t\t\tsumWeights_ += a;\n+\t\t\t\t}\n+\t\t\t}\n+\t\t}\n+\t}\n+\n+\tif (sumWeights_ == 0) {\n+\t\t/*\n+\t\t * Default AF window is the middle 1/2 width of the middle 1/3 height\n+\t\t * since this maps nicely to both PDAF (16x12) and Focus (4x3) grids.\n+\t\t */\n+\t\tfor (int i = PDAF_DATA_ROWS / 3; i < 2 * PDAF_DATA_ROWS / 3; ++i) {\n+\t\t\tfor (int j = PDAF_DATA_COLS / 4; j < 3 * PDAF_DATA_COLS / 4; ++j) {\n+\t\t\t\tphaseWeights_[i][j] = MaxCellWeight;\n+\t\t\t\tsumWeights_ += MaxCellWeight;\n+\t\t\t}\n+\t\t}\n+\t}\n+\n+\t/* Scale from PDAF to Focus Statistics grid (which has fixed size 4x3) */\n+\tconstexpr int FocusStatsRows = 3;\n+\tconstexpr int FocusStatsCols = 4;\n+\tstatic_assert(FOCUS_REGIONS == FocusStatsRows * FocusStatsCols);\n+\tstatic_assert(PDAF_DATA_ROWS % FocusStatsRows == 0);\n+\tstatic_assert(PDAF_DATA_COLS % FocusStatsCols == 0);\n+\tconstexpr int YFactor = PDAF_DATA_ROWS / FocusStatsRows;\n+\tconstexpr int XFactor = PDAF_DATA_COLS / FocusStatsCols;\n+\n+\tLOG(RPiAf, Debug) << \"Recomputed weights:\";\n+\tfor (int i = 0; i < FocusStatsRows; ++i) {\n+\t\tfor (int j = 0; j < FocusStatsCols; ++j) {\n+\t\t\tunsigned w = 0;\n+\t\t\tfor (int y = 0; y < YFactor; ++y)\n+\t\t\t\tfor (int x = 0; x < XFactor; ++x)\n+\t\t\t\t\tw += phaseWeights_[YFactor * i + y][XFactor * j + x];\n+\t\t\tcontrastWeights_[FocusStatsCols * i + j] = w;\n+\t\t}\n+\t\tLOG(RPiAf, Debug) << \"   \"\n+\t\t\t\t  << contrastWeights_[FocusStatsCols * i + 0] << \" \"\n+\t\t\t\t  << contrastWeights_[FocusStatsCols * i + 1] << \" \"\n+\t\t\t\t  << contrastWeights_[FocusStatsCols * i + 2] << \" \"\n+\t\t\t\t  << contrastWeights_[FocusStatsCols * i + 3];\n+\t}\n+}\n+\n+bool Af::getPhase(PdafData const &data, double &phase, double &conf) const\n+{\n+\tuint32_t sumWc = 0;\n+\tint64_t sumWcp = 0;\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 (phaseWeights_[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\tc -= (cfg_.confThresh >> 2);\n+\t\t\t\t\tsumWc += phaseWeights_[i][j] * c;\n+\t\t\t\t\tc -= (cfg_.confThresh >> 2);\n+\t\t\t\t\tsumWcp += phaseWeights_[i][j] * data.phase[i][j] * (int64_t)c;\n+\t\t\t\t}\n+\t\t\t}\n+\t\t}\n+\t}\n+\n+\tif (0 < sumWeights_ && sumWeights_ <= sumWc) {\n+\t\tphase = (double)sumWcp / (double)sumWc;\n+\t\tconf = (double)sumWc / (double)sumWeights_;\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+double Af::getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const\n+{\n+\tuint32_t sumWc = 0;\n+\n+\tfor (unsigned i = 0; i < FOCUS_REGIONS; ++i) {\n+\t\tunsigned w = contrastWeights_[i];\n+\t\tsumWc += w * (focus_stats[i].contrast_val[1][1] >> 10);\n+\t}\n+\n+\treturn (sumWeights_ == 0) ? 0.0 : (double)sumWc / (double)sumWeights_;\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\tphase *= conf / (conf + cfg_.confEpsilon);\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} 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 (0.0 <= dropLo && 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 (0.0 <= dropHi && 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 sensor 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_ ? 1.0 : 0.25) * cfg_.confEpsilon) {\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 (scanState_ >= ScanState::Pdaf) {\n+\t\tftarget_ = std::clamp(ftarget_,\n+\t\t\t\t      cfg_.ranges[range_].focusMin,\n+\t\t\t\t      cfg_.ranges[range_].focusMax);\n+\t}\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+void Af::startAF()\n+{\n+\t/* Use PDAF if the tuning file allows it; else CDAF. */\n+\tif (cfg_.speeds[speed_].dropoutFrames > 0 &&\n+\t    (mode_ == AfModeContinuous || cfg_.speeds[speed_].pdafFrames > 0)) {\n+\t\tif (!initted_) {\n+\t\t\tftarget_ = cfg_.ranges[range_].focusDefault;\n+\t\t\tupdateLensPosition();\n+\t\t}\n+\t\tstepCount_ = (mode_ == AfModeContinuous) ? 0 : cfg_.speeds[speed_].pdafFrames;\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+}\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+/*\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+\t/* Initialize for triggered scan or start of CAF mode */\n+\tif (scanState_ == ScanState::Trigger)\n+\t\tstartAF();\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+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+\tif (useWindows_ != mode) {\n+\t\tuseWindows_ = mode;\n+\t\tcomputeWeights();\n+\t}\n+}\n+\n+void Af::setWindows(libcamera::Span<libcamera::Rectangle const> const &wins)\n+{\n+\twindows_.clear();\n+\tfor (auto &w : wins) {\n+\t\tLOG(RPiAf, Debug) << \"Window: \"\n+\t\t\t\t  << w.x << \", \"\n+\t\t\t\t  << w.y << \", \"\n+\t\t\t\t  << w.width << \", \"\n+\t\t\t\t  << w.height;\n+\t\twindows_.push_back(w);\n+\t\tif (windows_.size() >= MaxWindows)\n+\t\t\tbreak;\n+\t}\n+\tcomputeWeights();\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\tftarget_ = cfg_.map.domain().clip(dioptres);\n+\t\tchanged = !(initted_ && fsmooth_ == ftarget_);\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::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\tscanState_ = ScanState::Trigger;\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\tscanState_ = ScanState::Trigger;\n+\t\telse if (mode != AfModeAuto || scanState_ < ScanState::Coarse)\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\tpauseFlag_ = false;\n+\t\t\tif (scanState_ < ScanState::Coarse)\n+\t\t\t\tscanState_ = ScanState::Trigger;\n+\t\t} else if (pause != AfPauseResume && !pauseFlag_) {\n+\t\t\tpauseFlag_ = true;\n+\t\t\tif (pause == AfPauseImmediate || scanState_ < ScanState::Coarse)\n+\t\t\t\tgoIdle();\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..f7baf8979873\n--- /dev/null\n+++ b/src/ipa/raspberrypi/controller/rpi/af.h\n@@ -0,0 +1,156 @@\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 = 0,\n+\t\tTrigger,\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+\tvoid computeWeights();\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+\tdouble findPeak(unsigned index) const;\n+\tvoid doScan(double contrast, double phase, double conf);\n+\tvoid doAF(double contrast, double phase, double conf);\n+\tvoid updateLensPosition();\n+\tvoid startAF();\n+\tvoid startProgrammedScan();\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::Rectangle statsRegion_;\n+\tstd::vector<libcamera::Rectangle> windows_;\n+\tbool useWindows_;\n+\tuint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];\n+\tuint16_t contrastWeights_[FOCUS_REGIONS];\n+\tuint32_t sumWeights_;\n+\n+\t/* Working state. */\n+\tScanState scanState_;\n+\tbool initted_;\n+\tdouble ftarget_, fsmooth_;\n+\tdouble prevContrast_;\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","v2","11/14"]}