[{"id":34201,"web_url":"https://patchwork.libcamera.org/comment/34201/","msgid":"<174705072359.521251.16506981732823714033@pyrite.rasen.tech>","date":"2025-05-12T11:52:03","subject":"Re: [PATCH 3/8] libcamera: Add PID controller class","submitter":{"id":17,"url":"https://patchwork.libcamera.org/api/people/17/","name":"Paul Elder","email":"paul.elder@ideasonboard.com"},"content":"Quoting Stefan Klug (2025-04-11 15:04:10)\n> A PID controller is a practical and proven solution for many regulation\n> tasks. This implementation can be parameterized either using the\n> standard form or normal form [1].\n> \n> Additionally output limits can be specified that are used to clamp the\n> output and to prevent integrator windup.\n> \n> [1]: https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller\n> \n> Signed-off-by: Stefan Klug \n\nReviewed-by: Paul Elder \n\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 | 169 ++++++++++++++++++++\n> 4 files changed, 217 insertions(+)\n> create mode 100644 include/libcamera/internal/pid_controller.h\n> create mode 100644 src/libcamera/pid_controller.cpp\n> \n> diff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build\n> index 45408b313848..fa4c515332ee 100644\n> --- a/include/libcamera/internal/meson.build\n> +++ b/include/libcamera/internal/meson.build\n> @@ -32,6 +32,7 @@ libcamera_internal_headers = files([\n> 'matrix.h',\n> 'media_device.h',\n> 'media_object.h',\n> + 'pid_controller.h',\n> 'pipeline_handler.h',\n> 'process.h',\n> 'pub_key.h',\n> diff --git a/include/libcamera/internal/pid_controller.h b/include/libcamera/internal/pid_controller.h\n> new file mode 100644\n> index 000000000000..c1212aeb0946\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> + PidController(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0,\n> + double min = std::numeric_limits::min(),\n> + double max = std::numeric_limits::max());\n> +\n> + void setNormalParameters(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0);\n> + void setStandardParameters(double Kp = 1.0, double Ti = 1e6, double Td = 0.0);\n> + void setOutputLimits(double min = std::numeric_limits::min(),\n> + double max = std::numeric_limits::max());\n> + void reset();\n> +\n> + void setTarget(double target);\n> + double process(double value, double dt = 1.0);\n> +\n> +private:\n> + double Kp_;\n> + double Ki_;\n> + double Kd_;\n> + double max_;\n> + double min_;\n> + double target_;\n> +\n> + bool clamped_bottom_;\n> + bool clamped_top_;\n> + double integral_;\n> + double last_error_;\n> +};\n> +\n> +} /* namespace libcamera */\n> diff --git a/src/libcamera/meson.build b/src/libcamera/meson.build\n> index de22b8e60dde..cb9c384d5062 100644\n> --- a/src/libcamera/meson.build\n> +++ b/src/libcamera/meson.build\n> @@ -43,6 +43,7 @@ libcamera_internal_sources = files([\n> 'matrix.cpp',\n> 'media_device.cpp',\n> 'media_object.cpp',\n> + 'pid_controller.cpp',\n> 'pipeline_handler.cpp',\n> 'process.cpp',\n> 'pub_key.cpp',\n> diff --git a/src/libcamera/pid_controller.cpp b/src/libcamera/pid_controller.cpp\n> new file mode 100644\n> index 000000000000..edfa7efafdfd\n> --- /dev/null\n> +++ b/src/libcamera/pid_controller.cpp\n> @@ -0,0 +1,169 @@\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> + reset();\n> + setNormalParameters(Kp, Ki, Kd);\n> + setOutputLimits(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> + Kp_ = Kp;\n> + Ki_ = Ki;\n> + Kd_ = 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> + Kp_ = Kp;\n> + Ki_ = Kp / Ti;\n> + Kd_ = 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> + min_ = min;\n> + max_ = 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> + last_error_ = 0;\n> + integral_ = 0;\n> + clamped_bottom_ = false;\n> + clamped_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> + target_ = 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> + double error = target_ - value;\n> + double derivative = (error - last_error_) / dt;\n> +\n> + /* If we hit a limit disable the integrative part in that direction */\n> + if ((error * Ki_ > 0 && !clamped_top_) ||\n> + (error * Ki_ < 0 && !clamped_bottom_))\n> + integral_ += error * dt;\n> +\n> + double ret = Kp_ * error + Ki_ * integral_ + Kd_ * derivative;\n> +\n> + clamped_top_ = false;\n> + if (ret >= max_) {\n> + clamped_top_ = true;\n> + ret = max_;\n> + }\n> +\n> + clamped_bottom_ = false;\n> + if (ret <= min_) {\n> + clamped_bottom_ = true;\n> + ret = min_;\n> + }\n> +\n> + LOG(PidController, Debug) << \"Value: \" << value << \", Target: \" << target_\n> + << \", Error: \" << error << \", Integral: \" << integral_\n> + << \", Derivative: \" << derivative << \", Output: \" << ret;\n> +\n> + last_error_ = error;\n> +\n> + return ret;\n> +}\n> +\n> +} /* namespace libcamera */\n> -- \n> 2.43.0\n>","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 D74A0C3226\n\tfor ;\n\tMon, 12 May 2025 11:52:09 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 175C968B40;\n\tMon, 12 May 2025 13:52:09 +0200 (CEST)","from perceval.ideasonboard.com (perceval.ideasonboard.com\n\t[213.167.242.64])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id A8234614E3\n\tfor ;\n\tMon, 12 May 2025 13:52:06 +0200 (CEST)","from pyrite.rasen.tech (unknown\n\t[IPv6:2001:861:3a80:3300:4f2f:8c2c:b3ef:17d4])\n\tby perceval.ideasonboard.com (Postfix) with ESMTPSA id 86FA76AF;\n\tMon, 12 May 2025 13:51:51 +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=\"p5+InyzU\"; dkim-atps=neutral","DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com;\n\ts=mail; t=1747050711;\n\tbh=9OL54YVlRFtmAP/PLavLLcx9t2DVvYFkEmEr3Dq9VsQ=;\n\th=In-Reply-To:References:Subject:From:Cc:To:Date:From;\n\tb=p5+InyzUup68XAh/5GfNUF5cJRkApNEpasdmgOGYbcaA7Ls+WkPGMuiFPrCMhoHio\n\t7mxt6Jx11eKvFrBkEFfbv6899k9mtgdKPxsGkZoyH+4cNFnrT+8XNAFQO+23f61WyW\n\tjGxmFeCsMWrZXsWL7W/u6TUB6thefQ3bpLyIXptE=","Content-Type":"text/plain; charset=\"utf-8\"","MIME-Version":"1.0","Content-Transfer-Encoding":"quoted-printable","In-Reply-To":"<20250411130423.2164577-4-stefan.klug@ideasonboard.com>","References":"<20250411130423.2164577-1-stefan.klug@ideasonboard.com>\n\t<20250411130423.2164577-4-stefan.klug@ideasonboard.com>","Subject":"Re: [PATCH 3/8] libcamera: Add PID controller class","From":"Paul Elder ","Cc":"Stefan Klug ","To":"Stefan Klug ,\n\tlibcamera-devel@lists.libcamera.org","Date":"Mon, 12 May 2025 13:52:03 +0200","Message-ID":"<174705072359.521251.16506981732823714033@pyrite.rasen.tech>","User-Agent":"alot/0.0.0","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\" "}}]