通用 PID 控制器类。 Generic PID controller. More...

#include <pid.hpp>

Collaboration diagram for LibXR::PID< Scalar >:

Data Structures
struct	Param
	PID 参数结构体。 Structure holding PID parameters. More...

Public Member Functions
template<typename P >
	PID (P &&p)
	构造 PID 控制器。 Construct a PID controller.

Scalar	Calculate (Scalar sp, Scalar fb, Scalar dt)
	使用反馈值计算 PID 输出。 Compute output from feedback only.

Scalar	Calculate (Scalar sp, Scalar fb, Scalar fb_dot, Scalar dt)
	使用外部导数计算 PID 输出。 Compute output using external feedback derivative.

void	SetK (Scalar k)
	设置全局比例系数 Set global proportional gain

void	SetP (Scalar p)
	设置 P 项系数 Set proportional gain

void	SetI (Scalar i)
	设置 I 项系数 Set integral gain

void	SetD (Scalar d)
	设置 D 项系数 Set derivative gain

void	SetILimit (Scalar limit)
	设置积分限幅 Set integral limit

void	SetOutLimit (Scalar limit)
	设置输出限幅 Set output limit

Scalar	K () const
	获取全局比例系数 Get global proportional gain

Scalar	P () const
	获取 P 项系数 Get proportional gain

Scalar	I () const
	获取 I 项系数 Get integral gain

Scalar	D () const
	获取 D 项系数 Get derivative gain

Scalar	ILimit () const
	获取积分限幅 Get integral limit

Scalar	OutLimit () const
	获取输出限幅 Get output limit

Scalar	LastError () const
	获取上一次误差 Get last error

Scalar	LastFeedback () const
	获取上一次反馈值（未缩放）Get last feedback (raw)

Scalar	LastOutput () const
	获取上一次输出 Get last output

Scalar	LastDerivative () const
	获取上一次导数（k * d(fb)/dt 或 k * fb_dot）Get last derivative (scaled by k)

void	Reset ()
	重置控制器状态。 Reset all internal states.

void	SetIntegralError (Scalar err)
	设置累计误差 Set integral error

Scalar	GetIntegralError () const
	获取累计误差 Get integral error

void	SetFeedForward (Scalar feed_forward)
	设置前馈项 Set feedforward

Scalar	GetFeedForward () const
	获取前馈项 Get feedforward

Private Attributes
Param	param_
	PID 参数 PID parameter set.

Scalar	i_ = 0
	积分状态 Integral state

Scalar	last_err_ = 0
	上次误差 Last error

Scalar	last_fb_ = 0
	上次反馈（未缩放）Last feedback (raw)

Scalar	last_der_ = 0
	上次导数（k 缩放）Last derivative (scaled by k)

Scalar	last_out_ = 0
	上次输出 Last output

Scalar	feed_forward_ = 0
	前馈项 Feedforward term

Detailed Description

template<typename Scalar = DefaultScalar>
class LibXR::PID< Scalar >

通用 PID 控制器类。 Generic PID controller.

支持周期角度处理、积分限幅与输出限幅，适用于基础控制应用。 Supports cyclic inputs, integral/output saturation. No derivative filtering.

Template Parameters

Scalar 控制器标量类型，默认为 DefaultScalar。 Scalar type for internal calculations.

Definition at line 26 of file pid.hpp.

Constructor & Destructor Documentation

◆ PID()

template<typename Scalar = DefaultScalar>

template<typename P >

LibXR::PID< Scalar >::PID ( P && p )

inline

构造 PID 控制器。 Construct a PID controller.

Template Parameters

Param PID 参数结构体。 PID parameter struct.

Parameters

PARAM

Definition at line 53 of file pid.hpp.

             : param_(std::forward<P>(p))
  {
    Reset();
  }

Member Function Documentation

◆ Calculate() [1/2]

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::Calculate	(	Scalar	sp,
		Scalar	fb,
		Scalar	dt )

inline

使用反馈值计算 PID 输出。 Compute output from feedback only.

Define: err = (cycle ? CycleValue(sp) - fb : sp - fb) e_k = k * err fb_d_k = k * (fb - last_fb) / dt

out = p * e_k + i * ∫(e_k)dt - d * fb_d_k + feed_forward

Parameters

sp	期望值 Setpoint
fb	反馈值 Feedback
dt	控制周期（秒） Time step in seconds

Returns: 控制器输出 Controller output

Definition at line 74 of file pid.hpp.

  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(dt) || dt <= Scalar(0))
    {
      return last_out_;
    }
 
    // Compute error
    const Scalar ERR = param_.cycle ? (CycleValue<Scalar>(sp) - fb) : (sp - fb);
    const Scalar E_K = ERR * param_.k;
 
    // Derivative from feedback change (scaled by k)
    Scalar fb_dot = (fb - last_fb_) / dt;
    if (!std::isfinite(fb_dot))
    {
      fb_dot = Scalar(0);
    }
 
    Scalar fb_d_k = fb_dot * param_.k;
    if (!std::isfinite(fb_d_k))
    {
      fb_d_k = Scalar(0);
    }
 
    // Compute PD
    const Scalar OUTPUT_PD = (E_K * param_.p) - (fb_d_k * param_.d);
 
    // -------------------------------
    // Integrator update: anti-windup + allow unwind
    // Rule: i_limit == 0 disables I (force i_ = 0)
    // -------------------------------
    if (param_.i > PID_SIGMA && param_.i_limit > PID_SIGMA)
    {
      Scalar i_candidate = i_ + E_K * dt;
 
      if (std::isfinite(i_candidate))
      {
        // Clamp integrator state
        i_candidate = std::clamp(i_candidate, -param_.i_limit, param_.i_limit);
 
        bool accept = true;
 
        // Output-limit-aware gating (windup prevention + unwind)
        if (param_.out_limit > PID_SIGMA)
        {
          const Scalar OUT_BEFORE = OUTPUT_PD + (i_ * param_.i) + feed_forward_;
          const Scalar OUT_AFTER = OUTPUT_PD + (i_candidate * param_.i) + feed_forward_;
 
          if (std::isfinite(OUT_BEFORE) && std::isfinite(OUT_AFTER))
          {
            const bool BEFORE_SAT = (std::abs(OUT_BEFORE) > param_.out_limit);
            const bool AFTER_SAT = (std::abs(OUT_AFTER) > param_.out_limit);
 
            if (AFTER_SAT)
            {
              // If saturated (or would be), only allow integral update if it reduces
              // saturation magnitude
              // - If not saturated before but would saturate after: reject (prevent
              // windup)
              // - If already saturated: allow only if |out_after| < |out_before| (unwind)
              accept = BEFORE_SAT && (std::abs(OUT_AFTER) < std::abs(OUT_BEFORE));
            }
          }
          else
          {
            accept = false;
          }
        }
 
        if (accept)
        {
          i_ = i_candidate;
        }
      }
    }
    else
    {
      // Disable I
      i_ = Scalar(0);
    }
 
    const Scalar I_OUT = i_ * param_.i;
 
    // Apply output limits
    Scalar output = OUTPUT_PD + I_OUT + feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = ERR;
    last_fb_ = fb;  // store raw feedback
    last_out_ = output;
    last_der_ = fb_d_k;  // store scaled derivative: k * d(fb)/dt
    return output;
  }

◆ Calculate() [2/2]

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::Calculate	(	Scalar	sp,
		Scalar	fb,
		Scalar	fb_dot,
		Scalar	dt )

inline

使用外部导数计算 PID 输出。 Compute output using external feedback derivative.

Define: err = (cycle ? CycleValue(sp) - fb : sp - fb) e_k = k * err fb_d_k = k * fb_dot

out = p * e_k + i * ∫(e_k)dt - d * fb_d_k + feed_forward

Parameters

sp	期望值 Setpoint
fb	反馈值 Feedback
fb_dot	反馈导数 Feedback rate (d(fb)/dt)
dt	控制周期 Delta time

Returns: 控制器输出 Controller output

Definition at line 189 of file pid.hpp.

  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(fb_dot) ||
        !std::isfinite(dt) || dt <= Scalar(0))
    {
      return last_out_;
    }
 
    // Compute error
    const Scalar ERR = param_.cycle ? (CycleValue<Scalar>(sp) - fb) : (sp - fb);
    const Scalar E_K = ERR * param_.k;
 
    // Use externally provided derivative (scaled by k)
    Scalar fb_d_k = fb_dot * param_.k;
    if (!std::isfinite(fb_d_k))
    {
      fb_d_k = Scalar(0);
    }
 
    // Compute PD
    const Scalar OUTPUT_PD = (E_K * param_.p) - (fb_d_k * param_.d);
 
    // -------------------------------
    // Integrator update: anti-windup + allow unwind
    // Rule: i_limit == 0 disables I (force i_ = 0)
    // -------------------------------
    if (param_.i > PID_SIGMA && param_.i_limit > PID_SIGMA)
    {
      Scalar i_candidate = i_ + E_K * dt;
 
      if (std::isfinite(i_candidate))
      {
        // Clamp integrator state
        i_candidate = std::clamp(i_candidate, -param_.i_limit, param_.i_limit);
 
        bool accept = true;
 
        if (param_.out_limit > PID_SIGMA)
        {
          const Scalar OUT_BEFORE = OUTPUT_PD + (i_ * param_.i) + feed_forward_;
          const Scalar OUT_AFTER = OUTPUT_PD + (i_candidate * param_.i) + feed_forward_;
 
          if (std::isfinite(OUT_BEFORE) && std::isfinite(OUT_AFTER))
          {
            const bool BEFORE_SAT = (std::abs(OUT_BEFORE) > param_.out_limit);
            const bool AFTER_SAT = (std::abs(OUT_AFTER) > param_.out_limit);
 
            if (AFTER_SAT)
            {
              accept = BEFORE_SAT && (std::abs(OUT_AFTER) < std::abs(OUT_BEFORE));
            }
          }
          else
          {
            accept = false;
          }
        }
 
        if (accept)
        {
          i_ = i_candidate;
        }
      }
    }
    else
    {
      // Disable I
      i_ = Scalar(0);
    }
 
    const Scalar I_OUT = i_ * param_.i;
 
    // Apply output limits
    Scalar output = OUTPUT_PD + I_OUT + feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = ERR;
    last_fb_ = fb;  // store raw feedback
    last_out_ = output;
    last_der_ = fb_d_k;  // store scaled derivative: k * d(fb)/dt
    return output;
  }

◆ D()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::D ( ) const

inline

获取 D 项系数 Get derivative gain

Definition at line 296 of file pid.hpp.

296{ return param_.d; }

◆ GetFeedForward()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::GetFeedForward ( ) const

inline

获取前馈项 Get feedforward

Returns: 前馈项 Feedforward

Definition at line 349 of file pid.hpp.

349{ return feed_forward_; }

◆ GetIntegralError()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::GetIntegralError ( ) const

inline

获取累计误差 Get integral error

Returns: 累计误差 Integral error

Definition at line 335 of file pid.hpp.

335{ return i_; }

◆ I()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::I ( ) const

inline

获取 I 项系数 Get integral gain

Definition at line 294 of file pid.hpp.

294{ return param_.i; }

◆ ILimit()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::ILimit ( ) const

inline

获取积分限幅 Get integral limit

Definition at line 298 of file pid.hpp.

298{ return param_.i_limit; }

◆ K()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::K ( ) const

inline

获取全局比例系数 Get global proportional gain

Definition at line 290 of file pid.hpp.

290{ return param_.k; }

◆ LastDerivative()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::LastDerivative ( ) const

inline

获取上一次导数（k * d(fb)/dt 或 k * fb_dot）Get last derivative (scaled by k)

Definition at line 308 of file pid.hpp.

308{ return last_der_; }

◆ LastError()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::LastError ( ) const

inline

获取上一次误差 Get last error

Definition at line 302 of file pid.hpp.

302{ return last_err_; }

◆ LastFeedback()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::LastFeedback ( ) const

inline

获取上一次反馈值（未缩放）Get last feedback (raw)

Definition at line 304 of file pid.hpp.

304{ return last_fb_; }

◆ LastOutput()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::LastOutput ( ) const

inline

获取上一次输出 Get last output

Definition at line 306 of file pid.hpp.

306{ return last_out_; }

◆ OutLimit()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::OutLimit ( ) const

inline

获取输出限幅 Get output limit

Definition at line 300 of file pid.hpp.

300{ return param_.out_limit; }

◆ P()

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::P ( ) const

inline

获取 P 项系数 Get proportional gain

Definition at line 292 of file pid.hpp.

292{ return param_.p; }

◆ Reset()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::Reset ( )

inline

重置控制器状态。 Reset all internal states.

Definition at line 314 of file pid.hpp.

  {
    i_ = 0;
    last_err_ = 0;
    last_fb_ = 0;
    last_out_ = 0;
    last_der_ = 0;
  }

◆ SetD()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetD ( Scalar d )

inline

设置 D 项系数 Set derivative gain

Definition at line 283 of file pid.hpp.

283{ param_.d = d; }

◆ SetFeedForward()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetFeedForward ( Scalar feed_forward )

inline

设置前馈项 Set feedforward

Parameters

feed_forward 前馈项 Feedforward

Definition at line 342 of file pid.hpp.

342{ feed_forward_ = feed_forward; }

◆ SetI()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetI ( Scalar i )

inline

设置 I 项系数 Set integral gain

Definition at line 281 of file pid.hpp.

281{ param_.i = i; }

◆ SetILimit()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetILimit ( Scalar limit )

inline

设置积分限幅 Set integral limit

Definition at line 285 of file pid.hpp.

285{ param_.i_limit = limit; }

◆ SetIntegralError()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetIntegralError ( Scalar err )

inline

设置累计误差 Set integral error

Parameters

err	累计误差 Integral error

Definition at line 328 of file pid.hpp.

328{ i_ = err; }

◆ SetK()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetK ( Scalar k )

inline

设置全局比例系数 Set global proportional gain

Definition at line 277 of file pid.hpp.

277{ param_.k = k; }

◆ SetOutLimit()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetOutLimit ( Scalar limit )

inline

设置输出限幅 Set output limit

Definition at line 287 of file pid.hpp.

287{ param_.out_limit = limit; }

◆ SetP()

template<typename Scalar = DefaultScalar>

void LibXR::PID< Scalar >::SetP ( Scalar p )

inline

设置 P 项系数 Set proportional gain

Definition at line 279 of file pid.hpp.

279{ param_.p = p; }

Field Documentation

◆ feed_forward_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::feed_forward_ = 0

private

前馈项 Feedforward term

Definition at line 358 of file pid.hpp.

◆ i_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::i_ = 0

private

积分状态 Integral state

Definition at line 353 of file pid.hpp.

◆ last_der_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::last_der_ = 0

private

上次导数（k 缩放）Last derivative (scaled by k)

Definition at line 356 of file pid.hpp.

◆ last_err_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::last_err_ = 0

private

上次误差 Last error

Definition at line 354 of file pid.hpp.

◆ last_fb_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::last_fb_ = 0

private

上次反馈（未缩放）Last feedback (raw)

Definition at line 355 of file pid.hpp.

◆ last_out_

template<typename Scalar = DefaultScalar>

Scalar LibXR::PID< Scalar >::last_out_ = 0

private

上次输出 Last output

Definition at line 357 of file pid.hpp.

◆ param_

template<typename Scalar = DefaultScalar>

Param LibXR::PID< Scalar >::param_

private

PID 参数 PID parameter set.

Definition at line 352 of file pid.hpp.

The documentation for this class was generated from the following file:

src/utils/pid.hpp

Data Structures

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PID()

Member Function Documentation

◆ Calculate() [1/2]

◆ Calculate() [2/2]

◆ D()

◆ GetFeedForward()

◆ GetIntegralError()

◆ I()

◆ ILimit()

◆ K()

◆ LastDerivative()

◆ LastError()

◆ LastFeedback()

◆ LastOutput()

◆ OutLimit()

◆ P()

◆ Reset()

◆ SetD()

◆ SetFeedForward()

◆ SetI()

◆ SetILimit()

◆ SetIntegralError()

◆ SetK()

◆ SetOutLimit()

◆ SetP()

Field Documentation

◆ feed_forward_

◆ i_

◆ last_der_

◆ last_err_

◆ last_fb_

◆ last_out_

◆ param_