v_pi_reg.asm

师姐给我留下的项目的源代码

************************************
*   File name   : v_pi_reg.asm
*   Module names: INIT_VOLTAGE_PI
;   Description :	PI DC voltage regulator 
;                   with integral correction
					
************************************

;----------------------------------------------------
;		    			|~~~~~~~~~~	|
; 	Udc_fdb o------>	|		    |
; 	Udc_ref o------>	|  voltage 	|----->o  Udc_out(Id_ref)
; 	Id_int  o------>	|   pi_reg  |
; 	Kp_u            	|           |
;	 	Ki_u    o------>|___________|
; 	Kc_u
;====================================================
    
    .include "F2407REGS.H"
    .def     INIT_VOLTAGE_PI_REG  ;Function call
    .def     VOLTAGE_PI_REG       ;Function call
    .def     Udc_fdb              ;input
    .def     Udc_out              ;output 
    
    ;.ref     DATA_CACHE

;-----------------------------------
; Variable Definitions
;-----------------------------------
   
Udc_ref    .usect "voltage",1  ; Q6,直流侧电压给定
Udc_fdb    .usect "voltage",1  ; Q6,直流侧电压反馈
Udc_error  .usect "voltage",1  ; Q6,直流电压偏差
Id_intl    .usect "voltage",1  ; Q18,error integral
Id_inth    .usect "voltage",1
Udc_out    .usect "voltage",1  ; Output of voltage regulator 
Kp_u       .usect "voltage",1  ; Q11, proportional gain
Ki_u       .usect "voltage",1  ; Q15, integral gain
Kc_u       .usect "voltage",1  ; Q15, saturation correction gain
SATerr_u   .usect "voltage",1
Imax_d     .usect "voltage",1
Imin_d     .usect "voltage",1

*****************************************
**The initiating function of voltage PI regulator
**Name:INIT_VOLTAGE_PI_REG

*****************************************

INIT_VOLTAGE_PI_REG:
    NOP
    LDP   #Udc_ref
    SPLK  #47FFH,Udc_fdb    ;Q6
    SPLK  #3700H,Udc_ref    ;Q6,Udc_ref=220*2^6v
    SPLK  #0266H,Kp_u       ;Q11,0.3*2^11
    SPLK  #0003H,Ki_u       ;Q11,30*2^11/20000, Sampling frequency is 20k
    SPLK  #00A4H,Kc_u       ;Q15,Kc_u=Ki_u/Kp_u=100*2^15/20000
    SPLK  #2800H,Imax_d     ; Q9,maximum Id=20*2^9
    SPLK  #0FE00H,Imin_d    ; Q9,minimum Id=-1*2^9
    SPLK  #06H,Id_inth      ; Q2, 1.5*2^2
    LACL  #0
    ;SACL  Udc_fdb
    SACL  Udc_error
    SACL  Id_intl
    ;SACL  Id_inth
    SACL  Udc_out  
    RET 
    
******************************************
**The function of voltage PI regulator  
**Name: VOLTAGE_PI_REG
**Udc_error=Udc_ref-Udc_fdb
**Un=Id_int+Kp_u*Udc_error
**if Un>Imax_d_, Udc_out=Imax_d_
**esle if Un<Imin_d_, Udc_out=Imin_d_
**else Udc_out=Un
**SATerr=Un-Udc_out
                       
******************************************

VOLTAGE_PI_REG:
    NOP
                            
    LDP   #Udc_ref				
    LACC  Udc_ref,16		; Use ACCH for OV protection
    SUB   Udc_fdb,16		;
    SACH  Udc_error			; Q6 Udc_error=Udc_ref-Udc_fdb
    ;SPLK  #0FFE0H,Udc_error    ; Q6 -0.5*2^6 FOR TEST
	
	LACC  Id_inth,16	
	ADDS  Id_intl			; 32-bit Q18,ACC=Id_int
    ;SPM	  1					
	LT	  Udc_error		
  	MPY	  Kp_u	            ; Q6*Q11 -> 32-bit Q17
	APAC					; 32-bit Q18 uint + Udc_error*Kp_d
	SUB   Imin_d,9
	BCND  NO_NEG_SAT,GT
NEG_SAT:
    SACH  SATerr_u          ; Q2, Negative saturation
    LACL  Imin_d
    SACL  Udc_out           ; Q9
    B     LIMITERS
NO_NEG_SAT:
    ADD   Imin_d,9
    SUB   Imax_d,9
    BCND  POS_SAT,GT
    SPLK  #0,SATerr_u
    ADD   Imax_d,9      
    SACH  Udc_out,7         ; Q9
    B     LIMITERS
POS_SAT:
    SACH  SATerr_u          ; Q2, Positive saturation
    LACL  Imax_d
    SACL  Udc_out
            
**************************************************
******Calculate the integral component
**Id_int=Id_int+Ki_u*Udc_error-Kc_u*SATerr_u

**************************************************

LIMITERS:
    
    ;SPM   1 
    ;SPLK  #900H,Udc_out     ; Q9 4.5*2^9 for test
    LT    Ki_u
    MPY   Udc_error         ; Q11*Q6=Q17,Ki_u*Udc_error
    LTP   Kc_u
    MPY   SATerr_u          ; Q15*Q2=Q17 ,Kc_u*SATerr_u
    SPAC                    ; Q18 (32bit),Ki_u*Udc_error-Kc_u*SATerr_u
    ADD   Id_inth,16
    ADDS  Id_intl
    SACH  Id_inth
    SACL  Id_intl	
    RET