Message ID | 20250411130423.2164577-4-stefan.klug@ideasonboard.com |
---|---|
State | New |
Headers | show |
Series |
|
Related | show |
Quoting Stefan Klug (2025-04-11 15:04:10) > 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@ideasonboard.com> Reviewed-by: Paul Elder <paul.elder@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 >
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 */
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@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