DSP的PID控制算法实现例程(精)

1、个例程 PID 程序，请教高手，怎么样设置 Q 格式? 一下是我附上的一个 pid 控制程序例程(2407 的，请教高手，这里边 ki 是 Q31 格式的，如果我的 ki 是个大于 1 的整数，该怎么设置？ 还有在寄存器里写个 16 进 制数，怎么样写识别它的格式？比如设置成 Q0,Q8,Q15 该怎么操作？谢谢了 ;File n ame: pig.asm ;.ref PID_REG1, PID_REG1_INIT ;.ref pid_fb_reg1, pid_ref_reg1 ;.ref pid_out_reg1 ;.def PID_REG1, PID_REG1_INIT .def pid_

2、fb_reg1, pid_ref_reg1 .def pid_out_reg1 ;DP_USER .set 5 ;DP 指向 280h-300h 单元 Kp_REG1_ .set 1000h ;for Kp_reg1 Ki_HI_REG1_ .set 0005h ;for Ki_low_reg1(Ki=0 for PD Ki_LO_REG1_ .set 0DEADh ;for Ki_high_reg1 (Ki=0 for PD Kd_REG1_ .set 0000h ;for Kd_reg1(Kd=0 for PI PID_REF_REG1_ .set 0000h ;for pid_ref_r

3、eg1 PID_OUT_MAX_ .set 0666h ;for pid_out_max PID_OUT_MIN_ .set 0000h ;for pid_out_min .in elude F2407REGS.H Peripheral Registers ;Definition in F2407REGS.H Variable Definitions for PID_REG1 module Kp_reg1 .usect pid_reg1,1 Kio w_reg1 .usect pid_reg1,1 Ki_high_reg1 .usect pid_reg1,1 Kd_reg1 .usect pi

4、d_reg1,1 K0_low_reg1 .usect pid_reg1,1 K0_high_reg1 .usect pid_reg1,1 K1_reg1 .usect pid_reg1,1 pid_fb_reg1 .usect pid_reg1,1 pid_ref_reg1 .usect pid_reg1,1 pid_out_reg1 .usect pid_reg1,1 pid_out1_reg1 .usect pid_reg1,1 pid_e0_reg1 .usect pid_reg1,1 pid_e1_reg1 .usect pid_reg1,1 pid_e2_reg1 .usect p

5、id_reg1,1 tmplo w_reg1 .usect pid_reg1,1 tmp1_high_reg1 .usect pid_reg1,1 tmp2ow_reg1 .usect pid_reg1,1 tmp2_high_reg1 .usect pid_reg1,1 tmp3_reg1 .usect pid_reg1,1 abs_e0_reg1 .usect pid_reg1,1 sig n_reg1 .usect pid_reg1,1 .def _c_i nt0 ;PID_REG1NIT: .text _c_in t0:;相当于主程序的入口 LDP #Kp_reg1 SPLK #Kp_

6、REG1_,Kp_reg1 SPLK #Ki_LO_REG1_,Ki_low_reg1 SPLK #Ki_HI_REG1_,Ki_high_reg1 SPLK #Kd_REG1_,Kd_reg1 SPLK #0,pid_e1_reg1 SPLK #0,pid_e2_reg1 SPLK #0,pid_out1_reg1 SPLK #PID_REF_REG1_,pid_ref_reg1 RET ;PID_REG1: SETC SXM ;Sig n exte nsio n mode SETC OVM Overflow mode SPM 0 ;Reset SPM ;Converting from Kp

7、, Ki, Kd to KO, K1 (Note: K2 = Kd LDP #Kp_reg1 LACC Ki_high_reg1,16 ;ACC = Ki (Q31 ADDS Ki_low_reg1 ;ACC = Ki (Q31 ADD Kp_reg1,16 ;ACC = Kp + Ki (Q31 ADD Kd_reg1,16 ;ACC = Kp + Ki + Kd (Q31 SACH KO_high_reg1 ;K0 = Kp + Ki + Kd (Q31 SACL K0_low_reg1 ;K0 = Kp + Ki + Kd (Q31 LACC Kd_reg1,16 ;ACC = Kd (

8、Q15 SFL ;ADD = 2*Kd (Q15 ADD Kp_reg1,16 ;ACC = 2*Kd+Kp (Q15 SACH K1_reg1 ;K1 = 2*Kd+Kp (Q15 ;e(k = ref(k-fb(k = Q15 = Q15 - Q15 LACC pid_ref_reg1 ;ACC = pid_ref_reg1 (Q15 SUB pid_fb_reg1 ;ACC = pid_ref_reg1-pid_fb_reg1 ;(Q15 SACL pid_e0_reg1 ;e(k = pid_ref_reg1 - pid_fb_reg1 ;(Q15 ;tmp1 = -K1*e(k-1+

9、K2*e(k-2 = Q31 = -Q15*Q15 + Q15*Q15 LT Kd_reg1 ;TREG = K2 (Q15 MPY pid_e2_reg1 ;PREG = K2*e(k-2 (Q30 PAC ;ACC = K2*e(k-2 (Q30 LT K1_reg1 ;TREG = K1 (Q15 MPY pid_e1_reg1 ;PREG = K1*e(k-1 (Q30 SPAC ;ACC = -K1*e(k-1+K2*e(k-2 (Q30 SACH tmp1_high_reg1,1 ;tmp1 = -K1*e(k-1+K2*e(k-2 (Q31 SACL tmp1_low_reg1,

10、1 ;tmp1 = -K1*e(k-1+K2*e(k-2 (Q31 ;tmp2 = K0*e(k = Q31 = Q31*Q15 ;check sign for error only LACC pid_e0_reg1 ;ACC = e(k SACL sign_reg1 ;Sign (0=+,1=- for K0*e(k ;take absolute for pid_e2_reg1 only because K0 is always positive ABS ;ACC = |e(k| SACL abs_e0_reg1 ;|e(k| = ACC low ;now theyre positive.

11、LT abs_e0_reg1 ;TREG = |e(k| MPYU K0_low_reg1 ;PREG = K0_low*|e(k| SPH tmp2_low_reg1 ;tmp2ow = PREG high MPYU K0_high_reg1 ;PREG = K0_high*|e(k| PAC ;ACC = K0_high*|e(k| ADDS tmp2_low_reg1 ;ACC = K0_high*|e(k| + tmp2ow SACH tmp2_high_reg1,1 ;tmp2_high = ACC high (Q31 SACL tmp2_low_reg1,1 ;tmp2_low=

12、ACC low (Q31 ;check the sign con dition LACC sign_reg1 ;ACC = sig n BCND DONE_REG1, GT ;Check sign = positive ? LACC tmp2_high_reg1,16 ;ACC high = tmp2_high ADDS tmp2_low_reg1 ;ACC low= tmp2_low NEG ;Make the result negative SACH tmp2_high_reg1 ;tmp2_high= ACC high SACL tmp2_low_reg1 ;tmp2_low= ACC

13、low DONE_REG1 ;tmp2 + tmpl = tmp3 = Q31 + Q31 = Q15 LACC tmp1_high_reg1,16 ;ACC high = tmp1_high (Q31 ADDS tmp1_low_reg1 ;ACC low= tmp1_low (Q31 ADDS tmp2_low_reg1 ;ACC = tmplo w+tmp2_low (Q31 ADDH tmp2_high_reg1 ;ACC = tmp1_high+tmp2_high (Q31 SACH tmp3_reg1 ;tmp3 = tmp1_high+tmp1_high (Q15 ;u(k =

14、u(k-1 + tmp3 = Q15 = Q15 + Q15 LACC pid_out1_reg1,16 ;ACC = u(k-1 (Q15 ADD tmp3_reg1,16 ;ACC = u(k-1+K0*e(k-K1*e(k-1+K2*e(k-2 (Q15 SACH pid_out_reg1 ;u(k = u(k-1+K0*e(k-K1*e(k-1+K2*e(k-2 (Q15 ;If u(k u_max, u(k = u_max. If u(k u_mi n, u(k = u_mi n. LACC pid_out_reg1 ;ACC = u(k (Q15 SUB #PID_OUT_MAX_

15、 ;ACC = u(k-u_max (Q15 BCND SAT_MAX,GT ;Bra nch if saturated at max LACC pid_out_reg1 ;ACC = u(k (Q15 SUB #PID_OUT_MIN_ ;ACC = u(k-u_min (Q15 BCND SAT_MIN,LT ;Brnch if saturated at min B REG1_END SAT_MIN SPLK #PID_OUT_MIN_,pid_out_reg1 ;u(k = u_min (Q15 B REG1_END SAT_MAX SPLK #PID_OUT_MAX_,pid_out_reg1 ;u(k = u_max (Q15 REG1_END ;Updati ng the errors e(k-1, e(k-2 and