DSP的PID控制算法實(shí)現(xiàn)例程

1、個(gè)例程 PID 程序，請(qǐng)教高手，怎么樣設(shè)置 Q 格式？一下是我附上的一個(gè) pid 控制程序例程 (2407 的 ，請(qǐng)教高手，這里邊 ki 是 Q31 格式的，如果我的 ki 是個(gè)大于 1 的整數(shù)，該怎么設(shè)置？ 還有在寄存器里寫個(gè) 16進(jìn) 制數(shù)，怎么樣寫識(shí)別它的格式？比如設(shè)置成 Q0,Q8,Q15 該怎么操作？謝謝了; File name: 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_fb

2、_reg1, pid_ref_reg1.def pid_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 PIPID_REF_REG1_ .set 0000h ;for pid_ref_reg1PID_OU

3、T_MAX_ .set 0666h ;for pid_out_maxPID_OUT_MIN_ .set 0000h ;for pid_out_min .include F2407REGS.H ;Peripheral Registers ;Definition in F2407REGS.H;Variable Definitions for PID_REG1 moduleKp_reg1 .usect pid_reg1,1Ki_low_reg1 .usect pid_reg1,1Ki_high_reg1 .usect pid_reg1,1Kd_reg1 .usect pid_reg1,1K0_low

4、_reg1 .usect pid_reg1,1K0_high_reg1 .usect pid_reg1,1K1_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 pid_reg1,1 tmp1_lo

5、w_reg1 .usect pid_reg1,1 tmp1_high_reg1 .usect pid_reg1,1 tmp2_low_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 sign_reg1 .usect pid_reg1,1 .def _c_int0;PID_REG1_INIT:.text_c_int0: ;相當(dāng)于主程序的入口LDP #Kp_reg1SPLK #Kp_REG1_,Kp_reg1SPLK #

6、Ki_LO_REG1_,Ki_low_reg1SPLK #Ki_HI_REG1_,Ki_high_reg1SPLK #Kd_REG1_,Kd_reg1SPLK #0,pid_e1_reg1SPLK #0,pid_e2_reg1SPLK #0,pid_out1_reg1SPLK #PID_REF_REG1_,pid_ref_reg1RET;PID_REG1:SETC SXM ;Sign extension modeSETC OVM ;Overflow modeSPM 0 ;Reset SPM;Converting from Kp, Ki, Kd to K0, K1 (Note: K2 = KdL

7、DP #Kp_reg1LACC 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 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,

8、16 ;ACC = 2*Kd+Kp (Q15 SACH K1_reg1 ;K1 = 2*Kd+Kp (Q15 ;e(k = ref(k-fb(k = Q15 = Q15 - Q15LACC pid_ref_reg1 ;ACC = pid_ref_reg1 (Q15 SUB pid_fb_reg1 ;ACC = pid_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 + Q15*Q15LT Kd

9、_reg1 ;TREG = K2 (Q15MPY pid_e2_reg1 ;PREG = K2*e(k-2 (Q30 PAC ;ACC = K2*e(k-2 (Q30LT K1_reg1 ;TREG = K1 (Q15 MPY pid_e1_reg1 ;PREG = K1*e(k-1 (Q30SPAC ;ACC = -K1*e(k-1+K2*e(k-2 (Q30 SACH tmp1_high_reg1,1;tmp1 = -K1*e(k-1+K2*e(k-2 (Q31SACL tmp1_low_reg1,1 ;tmp1 = -K1*e(k-1+K2*e(k-2 (Q31 ;tmp2 = K0*e

10、(k =Q31 = Q31*Q15;check sign for error onlyLACC pid_e0_reg1 ;ACC = e(kSACL sign_reg1 ;Sign (0=+,1=- for K0*e(k;take absolute for pid_e2_reg1 only because K0 is always positiveABS ;ACC = |e(k|SACL abs_e0_reg1 ;|e(k| = ACC low;now theyre positive.LT abs_e0_reg1 ;TREG = |e(k|MPYU K0_low_reg1 ;PREG = K0

11、_low*|e(k|SPH tmp2_low_reg1 ;tmp2_low = PREG highMPYU K0_high_reg1 ;PREG = K0_high*|e(k|PAC ;ACC = K0_high*|e(k|ADDS tmp2_low_reg1 ;ACC = K0_high*|e(k| + tmp2_low SACH tmp2_high_reg1,1;tmp2_high = ACC high (Q31SACL tmp2_low_reg1,1 ;tmp2_low= ACC low (Q31 ;check the sign conditionLACC sign_reg1 ;ACC

12、= signBCND DONE_REG1, GT ;Check sign = positive ?LACC tmp2_high_reg1,16;ACC high = tmp2_highADDS tmp2_low_reg1 ;ACC low= tmp2_lowNEG ;Make the result negativeSACH tmp2_high_reg1 ;tmp2_high= ACC highSACL tmp2_low_reg1 ;tmp2_low= ACC lowDONE_REG1;tmp2 + tmp1 = tmp3 = Q31 + Q31 = Q15LACC tmp1_high_reg1

13、,16;ACC high = tmp1_high (Q31ADDS tmp1_low_reg1 ;ACC low= tmp1_low (Q31ADDS tmp2_low_reg1 ;ACC = tmp1_low+tmp2_low (Q31 ADDH tmp2_high_reg1 ;ACC = tmp1_high+tmp2_high (Q31 SACH tmp3_reg1 ;tmp3 = tmp1_high+tmp1_high (Q15 ;u(k = u(k-1 + tmp3 = Q15 = Q15 + Q15LACC pid_out1_reg1,16 ;ACC = u(k-1 (Q15ADD

14、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_min, u(k = u_min.LACC pid_out_reg1 ;ACC = u(k (Q15SUB #PID_OUT_MAX_ ;ACC = u(k-u_max (Q15 BCND SAT_MAX,GT ;Branch if saturated at maxLACC 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 minB REG1_ENDSAT_MINSPLK #PID_OUT_MIN_,pid_out_reg1;u(k = u_min (Q15B REG1_ENDSAT_MAXSPLK #PID_OUT_MAX_,pid_out_reg1;u(k = u_max (Q15REG1_END;Updating the errors e(k-1,