current_pi.asm

利用2407编写的PWM整流器控制程序

**************************************
;              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