废话不多说,先看以下主要内容。
typedef struct
{
float target_val; //目标值
float actual_val; //实际值
float err; //定义当前偏差值
float err_last; //定义最后一个偏差值
float err_Previous;
float Kp, Ki, Kd; //定义比例、积分、微分系数
}_pid;
/**
* @brief PID算法实现
* @param val 目标值
* @note 无
* @retval 通过PID计算后的输出
*/
float PI_realize(_pid *pid)
{
/*目标值只在这里参与计算,计算目标值与实际值的误差*/
pid->err= pid->actual_val - pid->target_val;
/*PID算法实现*/
pid->actual_val += pid->Kp*(pid->err - pid->err_last) + pid->Ki*pid->err + pid->Kd*(pid->Kd * (pid->err - 2 * pid->err_Previous + pid->err_last)); //微分D);
/*传递误差*/
pid->err_Previous = pid->err_last;
pid->err_last = pid->err;
/*返回的值是经过pid运算以后的值*/
return pid->actual_val;
}
以上是PID自定义结构体及函数,下面是DSP库相关的。
/**
* @brief Instance structure for the Q15 PID Control.
*/
typedef struct
{
q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
#if !defined (ARM_MATH_DSP)
q15_t A1;
q15_t A2;
#else
q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
#endif
q15_t state[3]; /**< The state array of length 3. */
q15_t Kp; /**< The proportional gain. */
q15_t Ki; /**< The integral gain. */
q15_t Kd; /**< The derivative gain. */
} arm_pid_instance_q15;
/**
* @brief Instance structure for the Q31 PID Control.
*/
typedef struct
{
q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
q31_t A2; /**< The derived gain, A2 = Kd . */
q31_t state[3]; /**< The state array of length 3. */
q31_t Kp; /**< The proportional gain. */
q31_t Ki; /**< The integral gain. */
q31_t Kd; /**< The derivative gain. */
} arm_pid_instance_q31;
/**
* @brief Instance structure for the floating-point PID Control.
*/
typedef struct
{
float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
float32_t A2; /**< The derived gain, A2 = Kd . */
float32_t state[3]; /**< The state array of length 3. */
float32_t Kp; /**< The proportional gain. */
float32_t Ki; /**< The integral gain. */
float32_t Kd; /**< The derivative gain. */
} arm_pid_instance_f32;
上面为我们展示了三种结构体,后缀依次是q15、q31、f32。分别是定点16位、定点32位、浮点32位。浮点具体得看MCU是否自带浮点数。其中A0、A1、A2介绍为结构体里介绍。state数组在arm_pid_init_f32中可以进行清零或者手动清零。KP、KI、KD是PID参数。接下来看DSP库中的PID函数。
/**
* @brief Process function for the floating-point PID Control.
* @param[in,out] S is an instance of the floating-point PID Control structure
* @param[in] in input sample to process
* @return out processed output sample.
*/
CMSIS_INLINE __STATIC_INLINE 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);
}
接下来看PID初始化函数,在此我们定义目标值为1250,实际值是4000,我们允许负值计算,强制所有系数为0.5(浮点计算)。
以下是我们测试的条件,以LED亮灭时间(示波器抓波形)来计算函数执行时间。
以下是用户PID执行一次的时间
以下是dsp库执行时间
实测dsp库运行时间会比常规下快,但比常规消耗多一点的ram