STM32的DSP库中的pid源代码分析

1万 · 2020-6-8 09:52

一、PID基本公式：
stm32 DSP库中的PID公式如下：

二、转化成Z域：

三、转化成差分方程：

1万 · 2020-6-8 09:54

本帖最后由叶春勇于 2020-6-8 09:56 编辑

四、源代码分析：
初始化程序，系数计算

复制

void arm_pid_init_f32(

  arm_pid_instance_f32 * S,

  int32_t resetStateFlag)

{



  /* Derived coefficient A0 */

  S->A0 = S->Kp + S->Ki + S->Kd;



  /* Derived coefficient A1 */

  S->A1 = (-S->Kp) - ((float32_t) 2.0 * S->Kd);



  /* Derived coefficient A2 */

  S->A2 = S->Kd;



  /* Check whether state needs reset or not */

  if(resetStateFlag)

  {

    /* Clear the state buffer.  The size will be always 3 samples */

    memset(S->state, 0, 3u * sizeof(float32_t));

  }



}

此为A0，A1，A2的计算，同上式第三步算出的系数。当采样率为1时。

1万 · 2020-6-8 10:00

此为增量PID
采用步骤三的差分方程：

复制

  static float32_t arm_pid_f32(

  arm_pid_instance_f32 * S,

  float32_t in)

  {

    float32_t out;



    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */

    out = (S->A0 * in) +

      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);



    /* Update state */

    S->state[1] = S->state[0];

    S->state[0] = in;

    S->state[2] = out;



    /* return to application */

    return (out);



  }

其中
S->state[0]为e(n-1)
S->state[1]为e(n-2)
S->state[2]存储的是上一周期的输出。

STM32的DSP库中的pid源代码分析

奔腾之江水

十世金身

技术奇才奖章

精英会员奖章