[{"id":35328,"web_url":"https://patchwork.libcamera.org/comment/35328/","msgid":"<175489740375.4081879.15950150563596762998@ping.linuxembedded.co.uk>","date":"2025-08-11T07:30:03","subject":"Re: [PATCH v2 12/16] libcamera: Add PID controller class","submitter":{"id":4,"url":"https://patchwork.libcamera.org/api/people/4/","name":"Kieran Bingham","email":"kieran.bingham@ideasonboard.com"},"content":"Quoting Stefan Klug (2025-08-08 15:12:50)\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> Reviewed-by: Paul Elder \n> \n\nAs a self containted object, I wonder if this should have a\ncorresponding unit test ... but ... I'm not sure what it would actually\ntest. Perhaps some edge cases on overflow possibilities maybe ?\n\nThe scope is too wide to test ever possible combination for the Kx\nvariables...\n\nI'm not too concerned at the moment so...\n\nReviewed-by: Kieran Bingham \n\n> ---\n> \n> Changes 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\n> \n> diff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build\n> index 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',\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..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> + PidController(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0,\n> + double min = std::numeric_limits::lowest(),\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::lowest(),\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 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',\n> diff --git a/src/libcamera/pid_controller.cpp b/src/libcamera/pid_controller.cpp\n> new file mode 100644\n> index 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> + 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> + else\n> + LOG(PidController, Debug) << \"Disable integrative part: \"\n> + << \", clamped_top_: \" << clamped_top_\n> + << \", clamped_bottom_: \" << clamped_bottom_;\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.48.1\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 F14C1BDCC1\n\tfor ;\n\tMon, 11 Aug 2025 07:30:08 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id A21BD6921E;\n\tMon, 11 Aug 2025 09:30:07 +0200 (CEST)","from perceval.ideasonboard.com (perceval.ideasonboard.com\n\t[IPv6:2001:4b98:dc2:55:216:3eff:fef7:d647])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 3483161464\n\tfor ;\n\tMon, 11 Aug 2025 09:30:06 +0200 (CEST)","from pendragon.ideasonboard.com\n\t(cpc89244-aztw30-2-0-cust6594.18-1.cable.virginm.net [86.31.185.195])\n\tby perceval.ideasonboard.com (Postfix) with ESMTPSA id 0B9D54C7;\n\tMon, 11 Aug 2025 09:29:14 +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=\"SEcxPB8Q\"; dkim-atps=neutral","DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com;\n\ts=mail; t=1754897354;\n\tbh=tmUBP1dzqr73tUtOD7P+MKm7kfO3thizmll6y7WS70A=;\n\th=In-Reply-To:References:Subject:From:Cc:To:Date:From;\n\tb=SEcxPB8QpNMrEf8R6U4sHfgDW+rEzLsKo8CzsFOPPT/gGLHnLoIb+4kQkg/7yzLpn\n\tHwIG1QJ/clMpMOdqsiIv6KSlDm8rz6tUccU2bVbt6im5LWP7gbNmGwVvCUYt+9qCQv\n\tYjDUzJKK6BN/jJnJq6HzTv5aGbPsy7S/WDDSYbLY=","Content-Type":"text/plain; charset=\"utf-8\"","MIME-Version":"1.0","Content-Transfer-Encoding":"quoted-printable","In-Reply-To":"<20250808141315.413839-13-stefan.klug@ideasonboard.com>","References":"<20250808141315.413839-1-stefan.klug@ideasonboard.com>\n\t<20250808141315.413839-13-stefan.klug@ideasonboard.com>","Subject":"Re: [PATCH v2 12/16] libcamera: Add PID controller class","From":"Kieran Bingham ","Cc":"Stefan Klug ,\n\tPaul Elder ","To":"Stefan Klug ,\n\tlibcamera-devel@lists.libcamera.org","Date":"Mon, 11 Aug 2025 08:30:03 +0100","Message-ID":"<175489740375.4081879.15950150563596762998@ping.linuxembedded.co.uk>","User-Agent":"alot/0.9.1","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\" "}}]