[3/8] libcamera: Add PID controller class

23170 diff mbox series

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

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
>