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

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

2、_reg1.def p id_out_reg1;DP_USER .set 5 ;DP指向 280h-300h 单元Kp_REG1_ .set 1000h ;for Kp_reg1Ki_HI_REG1_ .set 0005h ;for Ki_low_reg1(Ki=0 for PDKi_LO_REG1_ .set 0DEADh ;for Ki_high_reg1 (Ki=0 for PDKd_REG1_ .set 0000h ;for Kd_reg1(Kd=0 for PIP ID_REF_REG1_ .set 0000h ;for pid_ref_reg1P ID_OUT_MAX_ .set

3、0666h ;for p id_out_maxP ID_OUT_MIN_ .set 0000h ;for p id_out_min.in elude "F2407REGS.H" ;Perip heral RegistersQefi niti on in "F2407REGS.H"Variable Defini tio ns for P ID_REG1 moduleKp_reg1 .usect " pid_reg1",1Ki_low_reg1 .usect " pid_reg1",1Ki_high_reg1 .use

4、ct " pid_reg1",1Kd_reg1 .usect " pid_reg1",1K0_low_reg1 .usect " pid_reg1",1K0_high_reg1 .usect " pi d_reg1",1K1_reg1 .usect " pid_reg1",1P id_fb_reg1 .usect" pi d_reg1",1p id_ref_reg1 .usect " pi d_reg1",1p id_out_reg1 .usect &qu

5、ot; pid_reg1",1p id_out1_reg1 .usect " pid_reg1",1P id_e0_reg1 .usect" pid_reg1",1P id_e1_reg1 .usect" pid_reg1",1P id_e2_reg1 .usect" pid_reg1",1tmp 1_low_reg1 .usect " pid_reg1",1tmp 1_high_reg1 .usect " pi d_reg1",1tmp2_lo w_reg1 .u

6、sect " pid_reg1",1tmp 2_high_reg1 .usect " pid_reg1",1tmp 3_reg1 .usect " pid_reg1",1abs_e0_reg1 .usect " pid_reg1",1sig n_reg1 .usect " pid_reg1",1.def _c_i nt0;P ID_REG1_INIT:.text_c_in t0:;相当于主程序的入口LDP #Kp_reg1SPLK #Kp_REG1_,Kp_reg1SPLK #Ki_L0_REG

7、1_,Ki_low_reg1SPLK #Ki_HI_REG1_,Ki_high_reg1SPLK #Kd_REG1_,Kd_reg1SPLK #O,pi d_e1_reg1SP LK #0, pid_e2_reg1SPLK #0, pid_out1_reg1SPLK #P ID_REF_REG1_, pid_ref_reg1RET;P ID_REG1:SETC SXM ;Sig n exte nsio n modeSETC OVM Overflow modeSPM 0 ;Reset SPM;Con verti ng from Kp, Ki, Kd to K0, K1 (Note: K2 = K

8、dLDP #Kp_reg1LACC Ki_high_reg1,16 ;ACC = Ki (Q31 ADDS Ki_low_reg1 ;ACC = Ki (Q31ADD Kp_reg1,16 ;ACC = Kp + Ki (Q31 ADD Kd_reg1,16 ;ACC = Kp + Ki + Kd (Q31SACH K0_high_reg1 ;K0 = Kp + Ki + Kd (Q31SACL K0_low_reg1 ;K0 = Kp + Ki + Kd (Q31 LACC Kd_reg1,16 ;ACC = Kd (Q15 SFL ;ADD = 2*Kd (Q15 ADD Kp_reg1,

9、16 ;ACC = 2*Kd+K p (Q15 SACHK1_reg1 ;K1 = 2*Kd+K p (Q15 ;e(k = ref(k-fb(k => Q15 = Q15 - Q15LACC pid_ref_reg1 ;ACC = pid_ref_reg1 (Q15 SUB p id_fb_reg1 ;ACC = p id_ref_reg1- pid_fb_reg1;(Q15SACL pid_e0_reg1 ;e(k = pid_ref_reg1 - pid_fb_reg1;(Q15;tmp1 = -K1*e(k-1+K2*e(k-2 => Q31 = -Q15*Q15 + Q1

10、5*Q15LT Kd_reg1 ;TREG = K2 (Q15MPY pid_e2_reg1 ;P REG = K2*e(k-2 (Q30 PAC ;ACC = K2*e(k-2 (Q30LT K1_reg1 ;TREG = K1 (Q15 MPY pi d_e1_reg1 ;P REG = K1*e(k-1 (Q30SPAC ;ACC = -K1*e(k-1+K2*e(k-2 (Q30 SACH tmp 1_high_reg1,1;tm p1 = -K1*e(k-1+K2*e(k-2 (Q31SACL tmp 1_low_reg1,1 ;tm p1 = -K1*e(k-1+K2*e(k-2

11、(Q31 ;tm p2 = K0*e(k =>Q31 = Q31*Q15;check sig n for "error" onlyLACC p id_e0_reg1 ;ACC = e(kSACL sign_reg1 ;Sig n (0=+,1=- for K0*e(k;take absolute for " pid_e2_reg1" only because "K0" is always p ositiveABS ;ACC = |e(k|SACL abs_e0_reg1 ;|e(k| = ACC low;now they'

12、;re p ositive.LT abs_e0_reg1 ;TREG = |e(k|MPYU K0_low_reg1 ;P REG = K0_low*|e(k|SPH tmp 2_low_reg1 ;tmp 2_low = P REG highMPYU K0_high_reg1 ;P REG = K0_high*|e(k|PAC ;ACC = K0_high*|e(k|ADDS tmp 2_low_reg1 ;ACC = K0_high*|e(k| + tmp 2_low SACH tmp 2_high_reg1,1;tmp 2_high = ACC high (Q31SACL tmp 2_l

13、ow_reg1,1 ;tmp 2_low= ACC low (Q31 ;check the sign con ditionLACC sign_reg1 ;ACC = signBCND DONE_REG1, GT ;Check sign = positive ?LACC tmp 2_high_reg1,16;ACC high = tmp 2_highADDS tmp 2_low_reg1 ;ACC low= tmp 2_lowNEG ;Make the result negativeSACH tmp 2_high_reg1 ;tmp 2_high= ACC highSACL tmp 2_low_

14、reg1 ;tmp 2_low= ACC lowDONE_REG1;tmp2 + tmp1 = tmp3 => Q31 + Q31 = Q15LACC tmp 1_high_reg1,16;ACC high = tmp 1_high (Q31ADDS tmp1_lo w_reg1 ;ACC low= tmp 1_low (Q31ADDS tmp 2_low_reg1 ;ACC = tmp 1_low+t mp2_low (Q31 ADDH tmp 2_high_reg1 ;ACC = tmp 1_high+tm p2_high (Q31 SACH tmp 3_reg1 ;tm p3 =

15、tmp 1_high+tm p1_high (Q15 ;u(k = u(k-1 + tmp3 => Q15 = Q15 + Q15LACC p id_out1_reg1,16 ;ACC = u(k-1 (Q15ADD tmp 3_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

16、 p id_out_reg1 ;ACC = u(k (Q15SUB #P ID_OUT_MAX_ ;ACC = u(k-u_max (Q15 BCND SAT_MAX,GT ;Bra nch if saturated at maxLACC p id_out_reg1 ;ACC = u(k (Q15 SUB #P ID_OUT_MIN_ ;ACC = u(k-u_min (Q15 BCND SAT_MIN,LT ;Brnch if saturated at minB REG1 ENDSAT_MINSPLK #P ID_OUT_M IN_,p id_out_reg1;u(k = u_min (Q15B REG1_ENDSAT MAXSPLK #P ID_OUT_MAX_, pid_out_reg1;u(k = u_max (Q15REG1_END;Up dati ng the er