[PATCH 3/8] libcamera: Add PID controller class
Stefan Klug
stefan.klug at ideasonboard.com
Fri Apr 11 15:04:10 CEST 2025
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 <stefan.klug at ideasonboard.com>
---
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 <limits>
+
+#include <libcamera/base/log.h>
+
+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<double>::min(),
+ double max = std::numeric_limits<double>::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<double>::min(),
+ double max = std::numeric_limits<double>::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 <algorithm>
+
+#include <libcamera/base/log.h>
+
+/**
+ * \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 */
--
2.43.0
More information about the libcamera-devel
mailing list