From patchwork Fri Aug 8 14:12:50 2025 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Stefan Klug X-Patchwork-Id: 24083 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 54438BDCC1 for ; Fri, 8 Aug 2025 14:14:20 +0000 (UTC) Received: from lancelot.ideasonboard.com (localhost [IPv6:::1]) by lancelot.ideasonboard.com (Postfix) with ESMTP id 1594E69237; Fri, 8 Aug 2025 16:14:20 +0200 (CEST) Authentication-Results: lancelot.ideasonboard.com; dkim=pass (1024-bit key; unprotected) header.d=ideasonboard.com header.i=@ideasonboard.com header.b="WczthZf+"; dkim-atps=neutral Received: from perceval.ideasonboard.com (perceval.ideasonboard.com [213.167.242.64]) by lancelot.ideasonboard.com (Postfix) with ESMTPS id 38EE669227 for ; Fri, 8 Aug 2025 16:14:19 +0200 (CEST) Received: from ideasonboard.com (unknown [IPv6:2a00:6020:448c:6c00:3ea1:35ac:90da:a221]) by perceval.ideasonboard.com (Postfix) with UTF8SMTPSA id 4F61C208E; Fri, 8 Aug 2025 16:13:29 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=ideasonboard.com; s=mail; t=1754662409; bh=sTIEc/0QGr/bX2YAqYMQGdoogbsGmOTnAoiP0E7dHGU=; h=From:To:Cc:Subject:Date:In-Reply-To:References:From; b=WczthZf+OAW3iD+cTP5zQGwnazdiTTA1qudgHbTbh1+A0tVpgtK/ZQofUMZ1m67Bx MhW+l04tVuKqarOr9D9y8s3nYtT3EMFM4ZO8ozuAX4fYwQt2jDVIkoMVQbjPht0asq 8Ljv4fmFsMmfZU1uerSb5ewZzUt9Oizlc21sR3o8= From: Stefan Klug To: libcamera-devel@lists.libcamera.org Cc: Stefan Klug , Paul 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 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 Reviewed-by: Paul Elder Reviewed-by: Kieran Bingham --- Changes in v2: - Collected tag - Fix default min limit to be numeric_limits::lowest() instead of numeric_limits::min() - Added debug log for anti-windup --- include/libcamera/internal/meson.build | 1 + include/libcamera/internal/pid_controller.h | 46 ++++++ src/libcamera/meson.build | 1 + src/libcamera/pid_controller.cpp | 172 ++++++++++++++++++++ 4 files changed, 220 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 5c80a28c4cbe..adbd665ec77b 100644 --- a/include/libcamera/internal/meson.build +++ b/include/libcamera/internal/meson.build @@ -34,6 +34,7 @@ libcamera_internal_headers = files([ 'media_device.h', 'media_object.h', 'media_pipeline.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..6c2c4d3c4565 --- /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::lowest(), + 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::lowest(), + 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 de1eb99b28fd..a821eff2a29b 100644 --- a/src/libcamera/meson.build +++ b/src/libcamera/meson.build @@ -45,6 +45,7 @@ libcamera_internal_sources = files([ 'media_device.cpp', 'media_object.cpp', 'media_pipeline.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..3ade02051902 --- /dev/null +++ b/src/libcamera/pid_controller.cpp @@ -0,0 +1,172 @@ +/* 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; + else + LOG(PidController, Debug) << "Disable integrative part: " + << ", clamped_top_: " << clamped_top_ + << ", clamped_bottom_: " << clamped_bottom_; + 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 */