记录TI开源PID程序

/* =================================================================================
File name:       PID_GRANDO.H 
===================================================================================*/


#ifndef __PID_H__
#define __PID_H__

typedef struct {  _iq  Ref;   			// Input: reference set-point 参考点
				  _iq  Fbk;   			// Input: feedback 反馈
				  _iq  Out;   			// Output: controller output 控制器输出 
				  _iq  c1;   			// Internal: derivative filter coefficient 1 导数滤波系数1
				  _iq  c2;   			// Internal: derivative filter coefficient 2 导数滤波系数2
				} PID_TERMINALS;
				// note: c1 & c2 placed here to keep structure size under 8 words

typedef struct {  _iq  Kr;				// Parameter: reference set-point weighting 参考设定点权重
				  _iq  Kp;				// Parameter: proportional loop gain 比例回路增益
				  _iq  Ki;			    // Parameter: integral gain 积分增益
				  _iq  Kd; 		        // Parameter: derivative gain 导数增益
				  _iq  Km; 		        // Parameter: derivative weighting 导数加权
				  _iq  Umax;			// Parameter: upper saturation limit 饱和上限
				  _iq  Umin;			// Parameter: lower saturation limit 饱和下限
				} PID_PARAMETERS;

typedef struct {  _iq  up;				// Data: proportional term 比例项
				  _iq  ui;				// Data: integral term 积分项
				  _iq  ud;				// Data: derivative term 导数项
				  _iq  v1;				// Data: pre-saturated controller output 预饱和控制器输出
				  _iq  i1;				// Data: integrator storage: ui(k-1) 积分器存储:ui(k-1)
				  _iq  d1;				// Data: differentiator storage: ud(k-1) 积分器存储:ui(k-1)
				  _iq  d2;				// Data: differentiator storage: d2(k-1) 微分器存储:d2(k-1)
				  _iq  w1;				// Data: saturation record: [u(k-1) - v(k-1)] 微分器存储:d2(k-1)
				} PID_DATA;


typedef struct {  PID_TERMINALS	term;
				  PID_PARAMETERS param;
				  PID_DATA		data;
				} PID_CONTROLLER;

/*-----------------------------------------------------------------------------
Default initalisation values for the PID objects
-----------------------------------------------------------------------------*/                     

#define PID_TERM_DEFAULTS {				\
						   0, 			\
                           0, 			\
                           0, 			\
                           0, 			\
						   0 			\
              			  }

#define PID_PARAM_DEFAULTS {			\
                           _IQ(1.0),	\
                           _IQ(1.0), 	\
                           _IQ(0.0),	\
                           _IQ(0.0),	\
                           _IQ(1.0),	\
                           _IQ(1.0),	\
                           _IQ(-1.0) 	\
              			  }

#define PID_DATA_DEFAULTS {			    \
                           _IQ(0.0),	\
                           _IQ(0.0), 	\
                           _IQ(0.0),	\
                           _IQ(0.0),	\
                           _IQ(0.0), 	\
                           _IQ(0.0),	\
                           _IQ(0.0),	\
                           _IQ(1.0) 	\
              			  }


/*------------------------------------------------------------------------------
 	PID Macro Definition
------------------------------------------------------------------------------*/

#define PID_MACRO(v)																				\
																									\
	/* proportional term */ 																		\
	v.data.up = _IQmpy(v.param.Kr, v.term.Ref) - v.term.Fbk;										\
																									\
	/* integral term */ 																			\
	v.data.ui = _IQmpy(v.param.Ki, _IQmpy(v.data.w1, (v.term.Ref - v.term.Fbk))) + v.data.i1;		\
	v.data.i1 = v.data.ui;																			\
																									\
	/* derivative term */ 																			\
	v.data.d2 = _IQmpy(v.param.Kd, _IQmpy(v.term.c1, (_IQmpy(v.term.Ref, v.param.Km) - v.term.Fbk))) - v.data.d2;	\
	v.data.ud = v.data.d2 + v.data.d1;																\
	v.data.d1 = _IQmpy(v.data.ud, v.term.c2);														\
																									\
	/* control output */ 																			\
	v.data.v1 = _IQmpy(v.param.Kp, (v.data.up + v.data.ui + v.data.ud));							\
	v.term.Out= _IQsat(v.data.v1, v.param.Umax, v.param.Umin);										\
	v.data.w1 = (v.term.Out == v.data.v1) ? _IQ(1.0) : _IQ(0.0);									\
	
#endif // __PID_H__

Example
The following pseudo code provides the information about the module usage. 
 /* Instance the PID module */
 PID pid1={ PID_TERM_DEFAULTS, PID_PARAM_DEFAULTS, PID_DATA_DEFAULTS };

main()
{
  pid1.param.Kp = _IQ(0.5);
  pid1.param.Ki = _IQ(0.005);
  pid1.param.Kd = _IQ(0);
  pid1.param.Kr = _IQ(1.0);
  pid1.param.Km =_IQ(1.0);
  pid1.param.Umax= _IQ(1.0);
  pid1.param.Umin= _IQ(-1.0);
}
void interrupt periodic_interrupt_isr()
{
  pid1.Ref = input1_1; // Pass _iq inputs to pid1
  pid1.Fbk = input1_2; // Pass _iq inputs to pid1 
  PID_MACRO(pid1); // Call compute macro for pid1 
  output1 = pid1.Out; // Access the output of pid1 
}