定点PID算法

前言

  之前使用有硬件浮点单元的单片机，就一直在用浮点PID，最近在STM32F1上面跑FOC，再使用浮点肯定是不太合适的了，就研究了下STM32官方的定点PID。由于FOC算法一般使用PI就够了，所以下面算法只有PI（比例，积分）部分，没有加入D(微分)部分。

结构体

	typedef struct {
		int16_t h_kp;
		int16_t h_ki;
		
		int32_t w_integral_term;
		
		// limit
		int32_t w_upper_integral_limit;
		int32_t w_lower_integral_limit;
		int16_t h_upper_output_limit;
		int16_t h_lower_output_limit;
		
		uint16_t h_kp_divisor_pow2;
		uint16_t h_ki_divisor_pow2;
		
	} mc_pid_t;

   ST的代码喜欢用h开头表示16位数据，w开头表示32位数据，对于需要做定点乘法的代码来说，这种表示方法能避免出错，所以我们这里沿用这种表示方法。

变量名	含义
h_kp	当前使用的kp值
h_ki	当前使用的ki值
w_integral_term	积分项的值，由于积分项需要累加，所以将其值保存在结构体中
w_upper_integral_limit	积分项的最大值
w_lower_integral_limit	积分项的最小值
h_upper_output_limit	输出的最大值
h_lower_output_limit	输出的最小值
h_kp_divisor_pow2	比例项右移的位数
h_ki_divisor_pow2	比例项右移的位数

   在我们使用浮点pid算法时，kp，ki的值我们都能够设置成小数，而定点不行。那怎么处理呢？最简单的办法就是，将比例项和积分项都除以一个数，如果kp为1，除以10就相当于kp是0.1了。
   在STM32这样的单片机中，进行除法运算效率也是比较低的，所以采用右移运算，右移1位表示除以2，右移2位表示除以2^2，一次类推。。。我们变量名中的pow2就是这个意思了，表示以2为底数，变量值为指数的幂。

Show you the code

	int16_t mc_pi_controller(mc_pid_t *pid, int32_t w_process_var_error)
	{
		int32_t w_proportional_term, w_integral_term, w_output_32, w_integral_sum_tmp;
		int32_t w_discharge = 0;
		int16_t h_upper_output_limit = pid->h_upper_output_limit;
		int16_t h_lower_output_limit = pid->h_lower_output_limit;
		
		/* 1 - 比例项计算 */
		w_proportional_term = pid->h_kp * w_process_var_error;
		
		/* 2 - 积分项计算 */
		if (pid->h_ki == 0) {
			pid->w_integral_term = 0;
		} else {
			w_integral_term = pid->h_ki * w_process_var_error;
			w_integral_sum_tmp = pid->w_integral_term + w_integral_term;
			
			// 处理w_integral_sum_tmp溢出的情况
			if (w_integral_sum_tmp < 0) {
				if (pid->w_integral_term > 0) {
					if (w_integral_term > 0) {
						/* 正常情况下 pid->w_integral_term > 0 , w_integral_term > 0，其求和结果w_integral_sum_tmp必 > 0；
						w_integral_sum_tmp < 0，只可能是求和的值超过了INT32范围而溢出，所以这里将其赋值为INT32_MAX来纠正这个错误 */
						w_integral_term = INT32_MAX;	
					}
				}
			} else {
				if (pid->w_integral_term < 0) {
					if (w_integral_term < 0) {
						/* 正常情况下 pid->w_integral_term < 0 , w_integral_term < 0，其求和结果w_integral_sum_tmp必 < 0；
						w_integral_sum_tmp >= 0，只可能是求和的值超过了INT32范围而溢出，所以这里将其赋值为-INT32_MAX来纠正这个错误 */
						w_integral_term = -INT32_MAX;	
					}
				}
			}
			
			// 限制积分项范围
			if (w_integral_sum_tmp > pid->w_upper_integral_limit) {
				pid->w_integral_term = pid->w_upper_integral_limit;
			} else if (w_integral_sum_tmp < pid->w_lower_integral_limit) {
				pid->w_integral_term = pid->w_lower_integral_limit;
			} else {
				pid->w_integral_term = w_integral_sum_tmp;
			}
		}
		
		/* 3 - 计算pid运算的输出 */
		/* 注意：下面这行代码不满足 MISRA规范；用户需要验证编译器是使用Cortex-M3 ASR(算术右移指令)而不是
		LSR(逻辑右移指令)来处理右移操作，否则结果会得到错误的结果。 
		笔者验证过，绝大多数ARM系列的编译器(gcc ARMCC ARMClang)，对于这行代码都会使用ASR，如不需要进行MISRA认证，这里可以放心使用。 */
		w_output_32 = (w_proportional_term >> pid->h_kp_divisor_pow2) + (pid->w_integral_term >> pid->h_ki_divisor_pow2);
		
		
		// 限制pid输出的范围
		if (w_output_32 > h_upper_output_limit) {
			w_discharge = h_upper_output_limit - w_output_32;
			w_output_32 = h_upper_output_limit;
		} else if (w_output_32 < h_lower_output_limit) {
			w_discharge = h_lower_output_limit - w_output_32;
			w_output_32 = h_lower_output_limit;
		} else {
			/* Nothing to do here*/
		}
		
		pid->w_integral_term += w_discharge;
		
		return ((int16_t) (w_output_32));
	}

   代码中都加入了注释，直接看代码中的注释就好啦。