{"id":24083,"url":"https://patchwork.libcamera.org/api/1.1/patches/24083/?format=json","web_url":"https://patchwork.libcamera.org/patch/24083/","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":"<20250808141315.413839-13-stefan.klug@ideasonboard.com>","date":"2025-08-08T14:12:50","name":"[v2,12/16] libcamera: Add PID controller class","commit_ref":null,"pull_url":null,"state":"superseded","archived":false,"hash":"7de603f24cc0422648724fbfd1b6c58a37358b7f","submitter":{"id":184,"url":"https://patchwork.libcamera.org/api/1.1/people/184/?format=json","name":"Stefan Klug","email":"stefan.klug@ideasonboard.com"},"delegate":null,"mbox":"https://patchwork.libcamera.org/patch/24083/mbox/","series":[{"id":5364,"url":"https://patchwork.libcamera.org/api/1.1/series/5364/?format=json","web_url":"https://patchwork.libcamera.org/project/libcamera/list/?series=5364","date":"2025-08-08T14:12:38","name":"Implement WDR algorithm","version":2,"mbox":"https://patchwork.libcamera.org/series/5364/mbox/"}],"comments":"https://patchwork.libcamera.org/api/patches/24083/comments/","check":"pending","checks":"https://patchwork.libcamera.org/api/patches/24083/checks/","tags":{},"headers":{"Return-Path":"","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 54438BDCC1\n\tfor ;\n\tFri, 8 Aug 2025 14:14:20 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 1594E69237;\n\tFri, 8 Aug 2025 16:14:20 +0200 (CEST)","from perceval.ideasonboard.com (perceval.ideasonboard.com\n\t[213.167.242.64])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 38EE669227\n\tfor ;\n\tFri, 8 Aug 2025 16:14:19 +0200 (CEST)","from ideasonboard.com (unknown\n\t[IPv6:2a00:6020:448c:6c00:3ea1:35ac:90da:a221])\n\tby perceval.ideasonboard.com (Postfix) with UTF8SMTPSA id 4F61C208E; \n\tFri, 8 Aug 2025 16:13:29 +0200 (CEST)"],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (1024-bit key;\n\tunprotected) header.d=ideasonboard.com header.i=@ideasonboard.com\n\theader.b=\"WczthZf+\"; dkim-atps=neutral","DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com;\n\ts=mail; t=1754662409;\n\tbh=sTIEc/0QGr/bX2YAqYMQGdoogbsGmOTnAoiP0E7dHGU=;\n\th=From:To:Cc:Subject:Date:In-Reply-To:References:From;\n\tb=WczthZf+OAW3iD+cTP5zQGwnazdiTTA1qudgHbTbh1+A0tVpgtK/ZQofUMZ1m67Bx\n\tMhW+l04tVuKqarOr9D9y8s3nYtT3EMFM4ZO8ozuAX4fYwQt2jDVIkoMVQbjPht0asq\n\t8Ljv4fmFsMmfZU1uerSb5ewZzUt9Oizlc21sR3o8=","From":"Stefan Klug ","To":"libcamera-devel@lists.libcamera.org","Cc":"Stefan Klug ,\n\tPaul Elder ","Subject":"[PATCH v2 12/16] libcamera: Add PID controller class","Date":"Fri, 8 Aug 2025 16:12:50 +0200","Message-ID":"<20250808141315.413839-13-stefan.klug@ideasonboard.com>","X-Mailer":"git-send-email 2.48.1","In-Reply-To":"<20250808141315.413839-1-stefan.klug@ideasonboard.com>","References":"<20250808141315.413839-1-stefan.klug@ideasonboard.com>","MIME-Version":"1.0","Content-Transfer-Encoding":"8bit","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "},"content":"A PID controller is a practical and proven solution for many regulation\ntasks. This implementation can be parameterized either using the\nstandard form or normal form [1].\n\nAdditionally output limits can be specified that are used to clamp the\noutput and to prevent integrator windup.\n\n[1]: https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller\n\nSigned-off-by: Stefan Klug \nReviewed-by: Paul Elder \n\n---\n\nChanges in v2:\n- Collected tag\n- Fix default min limit to be numeric_limits::lowest() instead of\n numeric_limits::min()\n- Added debug log for anti-windup\n---\n include/libcamera/internal/meson.build | 1 +\n include/libcamera/internal/pid_controller.h | 46 ++++++\n src/libcamera/meson.build | 1 +\n src/libcamera/pid_controller.cpp | 172 ++++++++++++++++++++\n 4 files changed, 220 insertions(+)\n create mode 100644 include/libcamera/internal/pid_controller.h\n create mode 100644 src/libcamera/pid_controller.cpp","diff":"diff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build\nindex 5c80a28c4cbe..adbd665ec77b 100644\n--- a/include/libcamera/internal/meson.build\n+++ b/include/libcamera/internal/meson.build\n@@ -34,6 +34,7 @@ libcamera_internal_headers = files([\n 'media_device.h',\n 'media_object.h',\n 'media_pipeline.h',\n+ 'pid_controller.h',\n 'pipeline_handler.h',\n 'process.h',\n 'pub_key.h',\ndiff --git a/include/libcamera/internal/pid_controller.h b/include/libcamera/internal/pid_controller.h\nnew file mode 100644\nindex 000000000000..6c2c4d3c4565\n--- /dev/null\n+++ b/include/libcamera/internal/pid_controller.h\n@@ -0,0 +1,46 @@\n+/* SPDX-License-Identifier: LGPL-2.1-or-later */\n+/*\n+ * Copyright (C) 2025, Ideas On Board Oy\n+ *\n+ * PID Controller\n+ */\n+#pragma once\n+\n+#include \n+\n+#include \n+\n+namespace libcamera {\n+\n+LOG_DECLARE_CATEGORY(PidController)\n+class PidController\n+{\n+public:\n+\tPidController(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0,\n+\t\t double min = std::numeric_limits::lowest(),\n+\t\t double max = std::numeric_limits::max());\n+\n+\tvoid setNormalParameters(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0);\n+\tvoid setStandardParameters(double Kp = 1.0, double Ti = 1e6, double Td = 0.0);\n+\tvoid setOutputLimits(double min = std::numeric_limits::lowest(),\n+\t\t\t double max = std::numeric_limits::max());\n+\tvoid reset();\n+\n+\tvoid setTarget(double target);\n+\tdouble process(double value, double dt = 1.0);\n+\n+private:\n+\tdouble Kp_;\n+\tdouble Ki_;\n+\tdouble Kd_;\n+\tdouble max_;\n+\tdouble min_;\n+\tdouble target_;\n+\n+\tbool clamped_bottom_;\n+\tbool clamped_top_;\n+\tdouble integral_;\n+\tdouble last_error_;\n+};\n+\n+} /* namespace libcamera */\ndiff --git a/src/libcamera/meson.build b/src/libcamera/meson.build\nindex de1eb99b28fd..a821eff2a29b 100644\n--- a/src/libcamera/meson.build\n+++ b/src/libcamera/meson.build\n@@ -45,6 +45,7 @@ libcamera_internal_sources = files([\n 'media_device.cpp',\n 'media_object.cpp',\n 'media_pipeline.cpp',\n+ 'pid_controller.cpp',\n 'pipeline_handler.cpp',\n 'process.cpp',\n 'pub_key.cpp',\ndiff --git a/src/libcamera/pid_controller.cpp b/src/libcamera/pid_controller.cpp\nnew file mode 100644\nindex 000000000000..3ade02051902\n--- /dev/null\n+++ b/src/libcamera/pid_controller.cpp\n@@ -0,0 +1,172 @@\n+/* SPDX-License-Identifier: LGPL-2.1-or-later */\n+/*\n+ * Copyright (C) 2025, Ideas on Board Oy\n+ *\n+ * PID controller\n+ */\n+\n+#include \"libcamera/internal/pid_controller.h\"\n+\n+#include \n+\n+#include \n+\n+/**\n+ * \\file pid_controller.h\n+ * \\brief PID controller class\n+ */\n+\n+namespace libcamera {\n+\n+LOG_DEFINE_CATEGORY(PidController)\n+/**\n+ * \\class PidController\n+ * \\brief Implementation of a PID controller\n+ *\n+ * Implementation of a Proportional Integral Derivative controller.\n+ * See https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller\n+ * for the underlying details.\n+ *\n+ */\n+\n+/**\n+ * \\brief Construct a PidController with optional normal parameters\n+ * \\param[in] Kp Proportional gain\n+ * \\param[in] Ki Integral gain\n+ * \\param[in] Kd Derivative gain\n+ * \\param[in] min Minimum output value\n+ * \\param[in] max Maximum output value\n+ *\n+ * For the parameters \\see setNormalParameters() and \\see setOutputLimits().\n+ */\n+PidController::PidController(double Kp, double Ki, double Kd, double min, double max)\n+{\n+\treset();\n+\tsetNormalParameters(Kp, Ki, Kd);\n+\tsetOutputLimits(min, max);\n+}\n+\n+/**\n+ * \\brief Set the normal parameters\n+ * \\param[in] Kp Proportional gain\n+ * \\param[in] Ki Integral gain\n+ * \\param[in] Kd Derivative gain\n+ *\n+ * Set the normal parameters of the PID controller.\n+ */\n+void PidController::setNormalParameters(double Kp, double Ki, double Kd)\n+{\n+\tKp_ = Kp;\n+\tKi_ = Ki;\n+\tKd_ = Kd;\n+}\n+\n+/**\n+ * \\brief Set the standard parameters\n+ * \\param[in] Kp Proportional gain\n+ * \\param[in] Ti Integration time constant\n+ * \\param[in] Td Derivative time constant\n+ *\n+ * Set the standard parameters of the PID controller. Functionally it is\n+ * identical to the normal parameters but has the added benefit that it is\n+ * easier to understand the values. The \\a Ti parameter specifies the time the\n+ * controller will tolerate the output value to be away from the target. Td is\n+ * the time in which the controller tries to approach the target value.\n+ *\n+ * \\see https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller#Standard_form\n+ */\n+void PidController::setStandardParameters(double Kp, double Ti, double Td)\n+{\n+\tKp_ = Kp;\n+\tKi_ = Kp / Ti;\n+\tKd_ = Kp / Td;\n+}\n+\n+/**\n+ * \\brief Set the output limits\n+ * \\param[in] min Minimum output value\n+ * \\param[in] max Maximum output value\n+ *\n+ * Set the minimum and maximum output values of the controller. The controller\n+ * will clamp the output to these values and ensure that no windup of the\n+ * integral part occurs.\n+ */\n+void PidController::setOutputLimits(double min, double max)\n+{\n+\tmin_ = min;\n+\tmax_ = max;\n+}\n+\n+/**\n+ * \\brief Reset the controller\n+ *\n+ * Reset the internal state of the controller.\n+ */\n+void PidController::reset()\n+{\n+\tlast_error_ = 0;\n+\tintegral_ = 0;\n+\tclamped_bottom_ = false;\n+\tclamped_top_ = false;\n+}\n+\n+/**\n+ * \\brief Set the target value\n+ * \\param[in] target Target value\n+ *\n+ * Set the target value the controller shall reach. In controller theory this is\n+ * usually called the set point.\n+ */\n+void PidController::setTarget(double target)\n+{\n+\ttarget_ = target;\n+}\n+\n+/**\n+ * \\brief Run a regulation step\n+ * \\param[in] value Measured value\n+ * \\param[in] dt Time since last call\n+ * \\return Output value\n+ *\n+ * Process the last measurement (also called process variable PV) and return the\n+ * new regulation value. The \\a dt parameter specifies the time since the last\n+ * call. It defaults to 1.0, so in cases that are not time but frame based, it\n+ * can be left out.\n+ */\n+double PidController::process(double value, double dt)\n+{\n+\tdouble error = target_ - value;\n+\tdouble derivative = (error - last_error_) / dt;\n+\n+\t/* If we hit a limit disable the integrative part in that direction */\n+\tif ((error * Ki_ > 0 && !clamped_top_) ||\n+\t (error * Ki_ < 0 && !clamped_bottom_))\n+\t\tintegral_ += error * dt;\n+\telse\n+\t\tLOG(PidController, Debug) << \"Disable integrative part: \"\n+\t\t\t\t\t << \", clamped_top_: \" << clamped_top_\n+\t\t\t\t\t << \", clamped_bottom_: \" << clamped_bottom_;\n+\tdouble ret = Kp_ * error + Ki_ * integral_ + Kd_ * derivative;\n+\n+\tclamped_top_ = false;\n+\tif (ret >= max_) {\n+\t\tclamped_top_ = true;\n+\t\tret = max_;\n+\t}\n+\n+\tclamped_bottom_ = false;\n+\tif (ret <= min_) {\n+\t\tclamped_bottom_ = true;\n+\t\tret = min_;\n+\t}\n+\n+\tLOG(PidController, Debug) << \"Value: \" << value << \", Target: \" << target_\n+\t\t\t\t << \", Error: \" << error << \", Integral: \" << integral_\n+\t\t\t\t << \", Derivative: \" << derivative << \", Output: \" << ret;\n+\n+\tlast_error_ = error;\n+\n+\treturn ret;\n+}\n+\n+} /* namespace libcamera */\n","prefixes":["v2","12/16"]}