current_pi.asm

关于整流器的程序.这个程序是在TI的DSP上实现

**************************************
;              File name: current_pi.asm
;           Module names: i_pi_reg 
;Initialization routines: init_i_pi_reg
;Description:	PI current regulator with integral correction
;					for d and q axes
;--------------------------------------
;		    	|~~~~~~~~~~~|
; 	i_fdb o------>	|           |
; 	i_ref o------>	|  pi_reg   |----->o  u_out
; 	u_int o------>	|	    |
; 	Kp            	|           |
;	Ki    o------>	|___________|
; 	Kc
;====================================== 
***************************************
* d-Axis PI Current Regulator
***************************************
;--------------------------------------
; Global Definitions
;--------------------------------------
		.def	i_pi_reg
		.def    init_i_pi_reg		; function call
		.def	id_fdb,id_ref		; d-axis Inputs
		.def	ud_out			; d_axis Output
		.def	iq_fdb      		; q-axis Input
		.def	uq_out			; q_axis Output
		
		.ref    D_THETA 
		.ref  DATA_CACHE                 ;FOR TEST

;--------------------------------------
; d-axis Variable Definitions
;--------------------------------------
id_fdb	.usect "pid",1		; Q10,current feedback
id_ref	.usect "pid",1		; Q10,current reference
ud_out	.usect "pid",1		; control voltage output

ud_inth	.usect "pid",1		; error integral
ud_intl	.usect "pid",1

Kp_d	.usect "pid",1		; proportional gain
Ki_d	.usect "pid",1		; integral gain
Kc_d	.usect "pid",1		; integral correction gain

umax_d  .usect "pid",1
umin_d  .usect "pid",1

i_error  .usect "pid",1		; current error
saterr   .usect "pid",1		; saturation error
e_d      .usect "pid",1
;--------------------------------------
; q-axis Variable Definitions
;--------------------------------------
iq_fdb	.usect "pid",1
iq_ref	.usect "pid",1
uq_out	.usect "pid",1

uq_inth	.usect "pid",1		; error integral
uq_intl	.usect "pid",1

Kp_q	.usect "pid",1		; proportional gain
Ki_q	.usect "pid",1		; integral gain
Kc_q	.usect "pid",1		; integral correction gain

umax_q  .usect "pid",1
umin_q  .usect "pid",1
;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------

Umax_d_	.set	07000h		; maximum U
Umin_d_	.set	08FFFh		; minimum U

;------------------------------------------------------------
; Initialization
;------------------------------------------------------------  
init_i_pi_reg:
    NOP
    LDP   #id_fdb
    SPLK  #10,id_fdb
    SPLK  #1400H,Kp_d    ;Q11,2.5*2^11
    SPLK  #0080H,Ki_d    ;Q11,1250*2^11/20000, Sampling frequency is 20k
    SPLK  #0333H,Kc_d    ;Q15,Kc_d=Ki_d/Kp_d=500*2^15/20000 
    SPLK  #1900H,umax_d  ;Q6,100
    SPLK  #0C180H,umin_d ;Q6,-250
    SPLK  #1400H,Kp_q    ;Q11,2.5*2^11
    SPLK  #0080H,Ki_q    ;Q11,1250*2^11/20000, Sampling frequency is 20k
    SPLK  #0333H,Kc_q    ;Q15,Kc_q=Ki_q/Kp_q=500*2^15/20000
    SPLK  #780H,umax_q   ;Q6,30
    SPLK  #0F880H,umin_q ;Q6,-30
    SPLK  #17F8H,e_d      ;Q7 48*2^7 
    SPLK  #0020H,ud_inth ;Q5 1*2^5
    LACL  #0
    SACL  id_fdb
    SACL  i_error
    SACL  ud_intl
    SACL  ud_inth
    SACL  ud_out
    SACL  uq_intl
    SACL  uq_inth
    SACL  iq_ref  
    RET
    
i_pi_reg:
    NOP
*************************************************************
* d-Axis PI Current Regulator
*************************************************************
    	LDP   #id_ref				
    	LACC  id_ref,16		; Use ACCH for OV protection
    	SUB   id_fdb,16		; 
    	SACH  i_error		; Q9 id_ref - id_fdb
    	SPLK  #0400H,i_error 	; Q9 2*2^9 FOR TEST 
    	LACL  i_error        	; for test
	CALL  DATA_CACHE        ;FOR TEST        

	LACC  ud_inth,16			
	ADDS  ud_intl   	; 32-bit Q21
     	SPM	  1		; 
	LT	  i_error	;
  	MPY	  Kp_d	        ; Q9*Q11 -> 32-bit Q20
	APAC			; 32-bit Q21 uint + id_error*Kp_d

	SUB   umin_d,15		;
  	BCND  U_gmind,GEQ	; Continue if tmp_d>=U_min
  	SACH  saterr       	 ; Q5
  	LACC  umin_d,15		; otherwise, Negative saturate
  	B     Nextd            
U_gmind
	ADD   umin_d,15		;		
	SUB   umax_d,15	;
  	BCND  U_lmaxd,LEQ   	; Continue if tmp_d<=U_max
  	SACH  saterr        	; Q5
  	LACC  umax_d,15    	; otherwise,Positive saturate
	B     Nextd

U_lmaxd
       SPLK  #0,saterr
	ADD  umax_d,15		;
Nextd                            	
	SACH  ud_out,2		; Q7 
	LACL  ud_out        	; for test
	CALL  DATA_CACHE        ;FOR TEST        

Int_termd
	LACC  ud_inth,16			
	ADDS  ud_intl   	; 32-bit Q21
	LT    i_error		;
	MPY    Ki_d		; Q9*Q11 -> Q20
	LTA   saterr    	; Q20 -> Q21
	MPY   Kc_d          	; Q5*Q15->Q20
	SPAC                	; Q21
	SACH  ud_inth
	SACL  ud_intl
	
***END d-Axis PI Current Regulator
*************************************************************
* q-Axis PI Current Regulator
*************************************************************
    LDP   #iq_ref				
    LACC  iq_ref,16		; Use ACCH for OV protection
    SUB   iq_fdb,16		;
    SACH  i_error		; Q9 id_ref - id_fdb

	LACC  uq_inth,16			
	ADDS  uq_intl   	; 32-bit Q21
	LT	  i_error	;
  	MPY	  Kp_q	        ; Q9*Q11 -> 32-bit Q20
	APAC			; 32-bit Q21 uint + id_error*Kp_d

	SUB   umin_q,15		;
  	BCND  U_gminq,GEQ	; Continue if tmp_d>=U_min
  	SACH  saterr        	; Q5
  	LACC  umin_q,15		; otherwise, Negative saturate
  	B	  Nextq            
U_gminq
	ADD   umin_q,15		;		
	SUB   umax_q,15		;
  	BCND  U_lmaxq,LEQ   	; Continue if tmp_d<=U_max
  	SACH  saterr        	;Q5
  	LACC  umax_q,15    	; otherwise,Positive saturate
	B     Nextq

U_lmaxq
   	 SPLK  #0,saterr
	ADD	umax_q,15	;
Nextq                            	
	SACH  uq_out,2		; Q7 
	LACL  uq_out        	; for test
	CALL  DATA_CACHE        ;FOR TEST

Int_termq
	LACC  uq_inth,16			
	ADDS  uq_intl   	; 32-bit Q21
	LT    i_error		;
	MPY    Ki_q		; Q9*Q11 -> Q20
	LTA   saterr    	; Q20 -> Q21
	MPY   Kc_q          	; Q5*Q15->Q20
	SPAC                	; Q21
	SACH  uq_inth
	SACL  uq_intl
	
***END q-Axis PI Current Regulator 
*******************************************
****(id,iq) decoupling control

*******************************************
    	SPM   0
    	SETC  OVM
    	LDP   #D_THETA
   	 LT    D_THETA       ;Q13 此处的直、交轴侧解耦系数K=D_THETA
	LDP   #id_fdb
	LACC  uq_out,16     ;Q23      
	MPY   id_fdb        ;Q9, Q13*Q9=Q22
	MPYA  iq_fdb        ;Q23
	NEG                 ;(ACC)*(-1)->ACC
	SACH  uq_out        ;Q7
	PAC                 ;Q23
	SUB   ud_out,16     ;Q23
	ADD   e_d,16
	SACH  ud_out        ;Q7
	CLRC  OVM 
	SPM   1
    RET