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

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

2、P_USER.set5;DP指向280h-300h单元Kp_REG1_.set1000h;forKp_reg1Ki_HI_REG1_.set0005h;forKi_low_reg1(Ki=0forPDKi_LO_REG1_.set0DEADh;forKi_high_reg1(Ki=0forPDKd_REG1_.set0000h;forKd_reg1(Kd=0forPIPID_REF_REG1_.set0000h;forpid_ref_reg1PID_OUT_MAX_.set0666h;forpid_out_maxPID_OUT_MIN_.set0000h;forpid_out_min.incl

3、ude"F2407REGS.H"PeripheralRegisters;Definitionin"F2407REGS.H"VariableDefinitionsforPID_REG1moduleKp_reg1.usect"pid_reg1",1Ki_low_reg1.usect"pid_reg1",1Ki_high_reg1.usect"pid_reg1",1Kd_reg1.usect"pid_reg1",1K0_low_reg1.usect"pid_reg1&qu

4、ot;,1K0_high_reg1.usect"pid_reg1",1K1_reg1.usect"pid_reg1",1pid_fb_reg1.usect"pid_reg1",1pid_ref_reg1.usect"pid_reg1",1pid_out_reg1.usect"pid_reg1",1pid_out1_reg1.usect"pid_reg1",1pid_e0_reg1.usect"pid_reg1",1pid_e1_reg1.usect&quo

5、t;pid_reg1",1pid_e2_reg1.usect"pid_reg1",1tmp1_low_reg1.usect"pid_reg1",1tmp2_low_reg1.usect"pid_reg1",1tmp2_high_reg1.usect"pid_reg1",1tmp3_reg1.usect"pid_reg1",1abs_e0_reg1.usect"pid_reg1",1sign_reg1.usect"pid_reg1",1.def_c

6、_int0;PID_REG1_INIT:.text_c_int0:;相当于主程序的入口LDP#Kp_reg1SPLK#Kp_REG1_,Kp_reg1SPLK#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:SETCSXM;SignextensionmodeSETCOVM;OverflowmodeSP

7、M0;ResetSPM;ConvertingfromKp,Ki,KdtoK0,K1(Note:K2=KdLDP#Kp_reg1LACCKi_high_reg1,16;ACC=Ki(Q31ADDSKi_low_reg1;ACC=Ki(Q31ADDKp_reg1,16;ACC=Kp+Ki(Q31ADDKd_reg1,16;ACC=Kp+Ki+Kd(Q31SACHK0_high_reg1;K0=Kp+Ki+Kd(Q31SACLK0_low_reg1;K0=Kp+Ki+Kd(Q31LACCKd_reg1,16;ACC=Kd(Q15SFL;ADD=2*Kd(Q15ADDKp_reg1,16;ACC=2*

8、Kd+Kp(Q15SACHK1_reg1;K1=2*Kd+Kp(Q15;e(k=ref(k-fb(k=>Q15=Q15-Q15LACCpid_ref_reg1;ACC=pid_ref_reg1(Q15SUBpid_fb_reg1;ACC=pid_ref_reg1-pid_fb_reg1;(Q15SACLpid_e0_reg1;e(k=pid_ref_reg1-pid_fb_reg1;(Q15;tmp1=-K1*e(k-1+K2*e(k-2=>Q31=-Q15*Q15+Q15*Q15LTKd_reg1;TREG=K2(Q15MPYpid_e2_reg1;PREG=K2*e(k-2(Q

9、30PAC;ACC=K2*e(k-2(Q30LTK1_reg1;TREG=K1(Q15MPYpid_e1_reg1;PREG=K1*e(k-1(Q30SPAC;ACC=-K1*e(k-1+K2*e(k-2(Q30SACHtmp1_high_reg1,1;tmp1=-K1*e(k-1+K2*e(k-2(Q31SACLtmp1_low_reg1,1;tmp1=-K1*e(k-1+K2*e(k-2(Q31;tmp2=K0*e(k=>Q31=Q31*Q15;checksignfor"error"onlyLACCpid_e0_reg1;ACC=e(kSACLsign_reg1;

10、Sign(0=+,1=-forK0*e(k;takeabsolutefor"pid_e2_reg1"onlybecause"K0"isalwayspositiveABS;ACC=|e(k|SACLabs_e0_reg1;|e(k|=ACClow;nowthey'repositive.LTabs_e0_reg1;TREG=|e(k|MPYUK0_low_reg1;PREG=K0_low*|e(k|SPHtmp2_low_reg1;tmp2_low=PREGhighMPYUK0_high_reg1;PREG=K0_high*|e(k|PAC;ACC=

11、K0_high*|e(k|ADDStmp2_low_reg1;ACC=K0_high*|e(k|+tmp2_lowSACHtmp2_high_reg1,1SACLtmp2_low_reg1,1;tmp2_low=ACClow(Q31;checkthesignconditionLACCsign_reg1;ACC=signBCNDDONE_REG1,GT;Checksign=positive?LACCtmp2_high_reg1,16;ACChigh=tmp2_highADDStmp2_low_reg1;ACClow=tmp2_lowNEG;MaketheresultnegativeSACHtmp

12、2_high_reg1;tmp2_high=ACChighSACLtmp2_low_reg1;tmp2_low=ACClowDONE_REG1;tmp2+tmp1=tmp3=>Q31+Q31=Q15LACCtmp1_high_reg1,16;ACChigh=tmp1_high(Q31ADDStmp1_low_reg1;ACClow=tmp1_low(Q31ADDStmp2_low_reg1;ACC=tmp1_low+tmp2_low(Q31ADDHtmp2_high_reg1;ACC=tmp1_high+tmp2_high(Q31SACHtmp3_reg1;tmp3=tmp1_high+

13、tmp1_high(Q15;u(k=u(k-1+tmp3=>Q15=Q15+Q15LACCpid_out1_reg1,16;ACC=u(k-1(Q15ADDtmp3_reg1,16;ACC=u(k-1+K0*e(k-K1*e(k-1+K2*e(k-2(Q15SACHpid_out_reg1;u(k=u(k-1+K0*e(k-K1*e(k-1+K2*e(k-2(Q15;Ifu(k>u_max,u(k=u_max.Ifu(k<u_min,u(k=u_min.LACCpid_out_reg1;ACC=u(k(Q15SUB#PID_OUT_MAX_;ACC=u(k-u_max(Q15BCNDSAT_MAX,GT;BranchifsaturatedatmaxLACCpid_out_reg1;ACC=u(k(Q15SUB#PID_OUT_MIN_;ACC=u(k-u_min(Q15BCNDSAT_MIN,LT;BrnchifsaturatedatminBREG1_ENDSAT_MINSPLK#PID_OUT_MIN_,pid_out_reg1;u(k=u_min(Q15BREG1_ENDSAT_MAXSPLK#PID_OUT_MAX_,pid_out_reg1;