From patchwork Fri Apr 11 13:04:10 2025 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Stefan Klug X-Patchwork-Id: 23170 Return-Path: X-Original-To: parsemail@patchwork.libcamera.org Delivered-To: parsemail@patchwork.libcamera.org Received: from lancelot.ideasonboard.com (lancelot.ideasonboard.com [92.243.16.209]) by patchwork.libcamera.org (Postfix) with ESMTPS id 33C67C3213 for ; Fri, 11 Apr 2025 13:04:38 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id E442268AA8; Fri, 11 Apr 2025 15:04:37 +0200 (CEST) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (1024-bit key; unprotected) header.d=ideasonboard.com header.i=@ideasonboard.com header.b="WCJoIusD"; dkim-atps=neutral Received: from perceval.ideasonboard.com (perceval.ideasonboard.com [IPv6:2001:4b98:dc2:55:216:3eff:fef7:d647]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 5B27668A90 for ; Fri, 11 Apr 2025 15:04:36 +0200 (CEST) Received: from ideasonboard.com (unknown [IPv6:2a00:6020:448c:6c00:5b21:2ad5:1023:7179]) by perceval.ideasonboard.com (Postfix) with ESMTPSA id 0C723735; Fri, 11 Apr 2025 15:02:36 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com; s=mail; t=1744376557; bh=TmqTRaOVWD1etF2XOjcJeFWQ1ul6T1CHZaICjXw3c0A=; h=From:To:Cc:Subject:Date:In-Reply-To:References:From; b=WCJoIusD4fscWJmzOWmGUvm2iic0F3HvNIJMy3DOL9K3kCHNaUKsu/uWRfakqgUnF zT5yxYVfTiNb1ujJ28Dc64Sp/LfTC8TiKSPix5jN79ifhIfGjynP2bHuRZrhRo34TW xkgGxgn1eAVutxRLhnKArkLQPGI/6DzsCXpUAmrE= From: Stefan Klug To: libcamera-devel@lists.libcamera.org Cc: Stefan Klug Subject: [PATCH 3/8] libcamera: Add PID controller class Date: Fri, 11 Apr 2025 15:04:10 +0200 Message-ID: <20250411130423.2164577-4-stefan.klug@ideasonboard.com> X-Mailer: git-send-email 2.43.0 In-Reply-To: <20250411130423.2164577-1-stefan.klug@ideasonboard.com> References: <20250411130423.2164577-1-stefan.klug@ideasonboard.com> MIME-Version: 1.0 X-BeenThere: libcamera-devel@lists.libcamera.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: libcamera-devel-bounces@lists.libcamera.org Sender: "libcamera-devel" A PID controller is a practical and proven solution for many regulation tasks. This implementation can be parameterized either using the standard form or normal form [1]. Additionally output limits can be specified that are used to clamp the output and to prevent integrator windup. [1]: https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller Signed-off-by: Stefan Klug --- include/libcamera/internal/meson.build | 1 + include/libcamera/internal/pid_controller.h | 46 ++++++ src/libcamera/meson.build | 1 + src/libcamera/pid_controller.cpp | 169 ++++++++++++++++++++ 4 files changed, 217 insertions(+) create mode 100644 include/libcamera/internal/pid_controller.h create mode 100644 src/libcamera/pid_controller.cpp diff --git a/include/libcamera/internal/meson.build b/include/libcamera/internal/meson.build index 45408b313848..fa4c515332ee 100644 --- a/include/libcamera/internal/meson.build +++ b/include/libcamera/internal/meson.build @@ -32,6 +32,7 @@ libcamera_internal_headers = files([ 'matrix.h', 'media_device.h', 'media_object.h', + 'pid_controller.h', 'pipeline_handler.h', 'process.h', 'pub_key.h', diff --git a/include/libcamera/internal/pid_controller.h b/include/libcamera/internal/pid_controller.h new file mode 100644 index 000000000000..c1212aeb0946 --- /dev/null +++ b/include/libcamera/internal/pid_controller.h @@ -0,0 +1,46 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2025, Ideas On Board Oy + * + * PID Controller + */ +#pragma once + +#include + +#include + +namespace libcamera { + +LOG_DECLARE_CATEGORY(PidController) +class PidController +{ +public: + PidController(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0, + double min = std::numeric_limits::min(), + double max = std::numeric_limits::max()); + + void setNormalParameters(double Kp = 1.0, double Ki = 1e6, double Kd = 0.0); + void setStandardParameters(double Kp = 1.0, double Ti = 1e6, double Td = 0.0); + void setOutputLimits(double min = std::numeric_limits::min(), + double max = std::numeric_limits::max()); + void reset(); + + void setTarget(double target); + double process(double value, double dt = 1.0); + +private: + double Kp_; + double Ki_; + double Kd_; + double max_; + double min_; + double target_; + + bool clamped_bottom_; + bool clamped_top_; + double integral_; + double last_error_; +}; + +} /* namespace libcamera */ diff --git a/src/libcamera/meson.build b/src/libcamera/meson.build index de22b8e60dde..cb9c384d5062 100644 --- a/src/libcamera/meson.build +++ b/src/libcamera/meson.build @@ -43,6 +43,7 @@ libcamera_internal_sources = files([ 'matrix.cpp', 'media_device.cpp', 'media_object.cpp', + 'pid_controller.cpp', 'pipeline_handler.cpp', 'process.cpp', 'pub_key.cpp', diff --git a/src/libcamera/pid_controller.cpp b/src/libcamera/pid_controller.cpp new file mode 100644 index 000000000000..edfa7efafdfd --- /dev/null +++ b/src/libcamera/pid_controller.cpp @@ -0,0 +1,169 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * Copyright (C) 2025, Ideas on Board Oy + * + * PID controller + */ + +#include "libcamera/internal/pid_controller.h" + +#include + +#include + +/** + * \file pid_controller.h + * \brief PID controller class + */ + +namespace libcamera { + +LOG_DEFINE_CATEGORY(PidController) +/** + * \class PidController + * \brief Implementation of a PID controller + * + * Implementation of a Proportional Integral Derivative controller. + * See https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller + * for the underlying details. + * + */ + +/** + * \brief Construct a PidController with optional normal parameters + * \param[in] Kp Proportional gain + * \param[in] Ki Integral gain + * \param[in] Kd Derivative gain + * \param[in] min Minimum output value + * \param[in] max Maximum output value + * + * For the parameters \see setNormalParameters() and \see setOutputLimits(). + */ +PidController::PidController(double Kp, double Ki, double Kd, double min, double max) +{ + reset(); + setNormalParameters(Kp, Ki, Kd); + setOutputLimits(min, max); +} + +/** + * \brief Set the normal parameters + * \param[in] Kp Proportional gain + * \param[in] Ki Integral gain + * \param[in] Kd Derivative gain + * + * Set the normal parameters of the PID controller. + */ +void PidController::setNormalParameters(double Kp, double Ki, double Kd) +{ + Kp_ = Kp; + Ki_ = Ki; + Kd_ = Kd; +} + +/** + * \brief Set the standard parameters + * \param[in] Kp Proportional gain + * \param[in] Ti Integration time constant + * \param[in] Td Derivative time constant + * + * Set the standard parameters of the PID controller. Functionally it is + * identical to the normal parameters but has the added benefit that it is + * easier to understand the values. The \a Ti parameter specifies the time the + * controller will tolerate the output value to be away from the target. Td is + * the time in which the controller tries to approach the target value. + * + * \see https://en.wikipedia.org/wiki/Proportional-integral-derivative_controller#Standard_form + */ +void PidController::setStandardParameters(double Kp, double Ti, double Td) +{ + Kp_ = Kp; + Ki_ = Kp / Ti; + Kd_ = Kp / Td; +} + +/** + * \brief Set the output limits + * \param[in] min Minimum output value + * \param[in] max Maximum output value + * + * Set the minimum and maximum output values of the controller. The controller + * will clamp the output to these values and ensure that no windup of the + * integral part occurs. + */ +void PidController::setOutputLimits(double min, double max) +{ + min_ = min; + max_ = max; +} + +/** + * \brief Reset the controller + * + * Reset the internal state of the controller. + */ +void PidController::reset() +{ + last_error_ = 0; + integral_ = 0; + clamped_bottom_ = false; + clamped_top_ = false; +} + +/** + * \brief Set the target value + * \param[in] target Target value + * + * Set the target value the controller shall reach. In controller theory this is + * usually called the set point. + */ +void PidController::setTarget(double target) +{ + target_ = target; +} + +/** + * \brief Run a regulation step + * \param[in] value Measured value + * \param[in] dt Time since last call + * \return Output value + * + * Process the last measurement (also called process variable PV) and return the + * new regulation value. The \a dt parameter specifies the time since the last + * call. It defaults to 1.0, so in cases that are not time but frame based, it + * can be left out. + */ +double PidController::process(double value, double dt) +{ + double error = target_ - value; + double derivative = (error - last_error_) / dt; + + /* If we hit a limit disable the integrative part in that direction */ + if ((error * Ki_ > 0 && !clamped_top_) || + (error * Ki_ < 0 && !clamped_bottom_)) + integral_ += error * dt; + + double ret = Kp_ * error + Ki_ * integral_ + Kd_ * derivative; + + clamped_top_ = false; + if (ret >= max_) { + clamped_top_ = true; + ret = max_; + } + + clamped_bottom_ = false; + if (ret <= min_) { + clamped_bottom_ = true; + ret = min_; + } + + LOG(PidController, Debug) << "Value: " << value << ", Target: " << target_ + << ", Error: " << error << ", Integral: " << integral_ + << ", Derivative: " << derivative << ", Output: " << ret; + + last_error_ = error; + + return ret; +} + +} /* namespace libcamera */