;============================================================
; Filename:		pid.asm
; Module names:	pid_reg_id, pid_reg_iq
; Initialization routines: pid_reg_id_init, pid_reg_iq_init
; Originator:	Digital control systems Group
;			Texas Instruments
;
; Description:	PI current regulator with integral correction
;			for d and q axes
;		    |~~~~~~~~~~~~|
; i_fdb o------>|		     |
; i_ref o------>|  pid_reg   |----->o  u_out
; u_int o------>|		     |
; Kp            |            |
; Ki    o------>|____________|
; Kc
;
; Target dependency: 'c2xx core only
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	

*************************************************************
* D-Axis PI Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;		.ref	pid_reg_id,pid_reg_id_init	; function call
;		.ref	id_fdb,id_ref,Kp_d,Ki_d,Kc_d	; Inputs
;		.ref	ud_int				; Input
;		.ref	ud_out				; Outputs

;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
		.def	pid_reg_id,pid_reg_id_init	; function call
		.def	id_fdb,id_ref,Kp_d,Ki_d,Kc_d	; Inputs
		.def	ud_int				; Input
		.def	ud_out				; Outputs

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

ud_int	.usect "pid",1		; error integral
uintlo_d	.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

id_error	.usect "pid",1		; current error
uprsat_d	.usect "pid",1		; control voltage prio saturation
saterr_d	.usect "pid",1		; saturation error
tmp_d		.usect "pid",1		; temp scrach

;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_d_		.set	388			; Q11, proportional gain
Ki_d_		.set	1271			; Q25, integral gain
Kc_d_		.set	32767			; Q14, saturation correction gain

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

;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
pid_reg_id_init
      ldp	#Kp_d				;
	SPLK	#Kp_d_,Kp_d          	;
      SPLK  #Ki_d_,Ki_d        	;
      SPLK  #Kc_d_,Kc_d       	;
	SPLK	#0,ud_int			; zero integral term
      SPLK	#0,uintlo_d
	RET

;------------------------------------------------------------
; Routine
;------------------------------------------------------------
pid_reg_id
	setc	SXM           		; Allow sign extension
	setc	OVM				; Set overflow protection mode

      ldp	#id_ref			;
      LACC	id_ref,16			; Use ACCH for OV protection
      SUB   id_fdb,16			;
      SACH  id_error			; Q15 id_ref - id_fdb

	lacl	uintlo_d			;
	add	ud_int,16			; 32-bit Q30
      spm	2				; product l/s 4 for accumulation
	LT	id_error			;
      mpy	Kp_d	                	; Q15*Q11 -> 32-bit Q26
	apac					; 32-bit Q30 uint + id_error*Kp_d
      SACH	uprsat_d			; save as Q14 uprsat_d
      sacl	tmp_d				;
      adds	tmp_d				;
	add	uprsat_d,16			; Q30 -> Q31 with OV protection
	sach	tmp_d				; save to tmp_d as Q15
		
	lacc	tmp_d				;
	sub	#Umin_d_			;
      bcnd	U_gmind,GEQ		  	; Continue if tmp_d>=U_min
      lacc	#Umin_d_			; otherwise, saturate
      B	Nextd            
U_gmind
	lacc	tmp_d				;		
	sub	#Umax_d_	        	;
      BCND	U_lmaxd,LEQ     		; Continue if tmp_d<=U_max
      lacc	#Umax_d_		      ; otherwise, saturate
	b	Nextd

U_lmaxd
	lacc	tmp_d				;
Nextd                            	
	sacl	ud_out			;

Int_termd
	lacc	ud_out,15			; Use ACCH for OV protection
	SUB	uprsat_d,16			;
	sach	saterr_d			; save as Q14 saterr_d = ud_out-uprsat_d

	lt	id_error			;
	mpy	Ki_d				; Q15*Q25 -> Q40
	pac					; Q40 -> Q44
	sach	tmp_d				; 
	lacc	tmp_d				; Q44 -> Q28 (r/s 16 bits)
		
	LT	saterr_d           	;
      MPY	Kc_d             	 	; Q14*Q14 -> Q28 saterr_d * Kc_d
	APAC                     	; Q28 Ki_d*id_error + Kc_d*saterr_d

	norm	*				;
	norm	*				; Q28 -> Q30 (with OV protection)

	ADDS	uintlo_d
	ADD	ud_int,16        		; uint + saterr_d*Kc_d + id_error*Ki_d
      SACH  ud_int
	SACL	uintlo_d             	; save as 32-bit Q30

	RET

***END D-Axis PI Current Regulator

		
*************************************************************
* Q-Axis PI Current Regulator
*************************************************************
;------------------------------------------------------------
; Reference/Prototype
;------------------------------------------------------------
;	.ref	pid_reg_iq,pid_reg_iq_init	; function call
;	.ref	iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q	; Inputs
;	.ref	uq_int				; Input
;	.ref	uq_out				; Outputs

;------------------------------------------------------------
; Global Definitions
;------------------------------------------------------------
	.def	pid_reg_iq,pid_reg_iq_init	; function call
	.def	iq_fdb,iq_ref,Kp_q,Ki_q,Kc_q	; Inputs
	.def	uq_int				; Input
	.def	uq_out				; Outputs

;------------------------------------------------------------
; Variable Definitions
;------------------------------------------------------------
iq_fdb	.usect "pid",1		; current feedback
iq_ref	.usect "pid",1		; current reference
uq_out	.usect "pid",1		; control voltage output
                                    
uq_int	.usect "pid",1		; error integral
uintlo_q	.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

iq_error	.usect "pid",1		; current error
uprsat_q	.usect "pid",1		; control voltage prio saturation
saterr_q	.usect "pid",1		; saturation error
tmp_q		.usect "pid",1		; temp scrach

;------------------------------------------------------------
; Default parameters
; Parameter spreadsheet: pid.xls
;------------------------------------------------------------
Kp_q_		.set	776			; Q11, proportional gain
Ki_q_		.set	1907			; Q25, integral gain
Kc_q_		.set	24576			; Q14, saturation correction gain

Umax_q_	.set	07000h		; maximum U
Umin_q_	.set	08FFFh		; minimum U

;------------------------------------------------------------
; Initialization
;------------------------------------------------------------
pid_reg_iq_init
	ldp	#Kp_q				;
	SPLK	#Kp_q_,Kp_q          	;
      SPLK  #Ki_q_,Ki_q        	;
      SPLK  #Kc_q_,Kc_q       	;
	SPLK	#0,uq_int			; zero integral term
      SPLK	#0,uintlo_q
	RET

;------------------------------------------------------------
; Routine
;------------------------------------------------------------
pid_reg_iq
	setc	SXM           		; Allow sign extension
	setc	OVM				; Set overflow protection mode

      ldp	#iq_ref			;
      LACC	iq_ref,16			; Use ACCH for OV protection
      SUB   iq_fdb,16			;
      SACH  iq_error			; Q15 iq_ref - iq_fdb

	lacl	uintlo_q			;
	add	uq_int,16			; 32-bit Q30
      spm	2				; product l/s 4 before accumulation
	LT	iq_error			;
      mpy	Kp_q	                	; Q15*Q11 -> 32-bit Q26
	apac					; 32-bit Q30 uint + iq_error*Kp_q
      SACH	uprsat_q			; save as Q14 uprsat_q
      sacl	tmp_q				;
      adds	tmp_q				;
	add	uprsat_q,16			; Q30 -> Q31 with OV protection
	sach	tmp_q				; save to tmp_q as Q15
		
	lacc	tmp_q				;
	sub	#Umin_q_			;
      bcnd	U_gminq,GEQ		  	; Continue if tmp_q>=U_min
      lacc	#Umin_q_			; otherwise, saturate
      B	Nextq            
U_gminq
	lacc	tmp_q				;		
	sub	#Umax_q_	        	;
      BCND	U_lmaxq,LEQ     		; Continue if tmp_q<=U_max
      lacc	#Umax_q_		      ; otherwise, saturate
      b	Nextq

U_lmaxq
	lacc	tmp_q				;
Nextq                            	
	sacl	uq_out			;

Int_termq
	lacc	uq_out,15			; Use ACCH for OV protection
	SUB	uprsat_q,16			;
      sach	saterr_q			; save as Q14 saterr_q = uq_out-uprsat_q

	lt	iq_error			;
	mpy	Ki_q				; Q15*Q26 -> Q40
	pac					; Q40 -> Q44
	sach	tmp_q				;
	lacc	tmp_q				; Q44 -> Q28 (r/s 16 bits)
		
	LT	saterr_q           	;
      MPY	Kc_q             	 	; Q14*Q14 -> Q28 saterr_q * Kc_q
	APAC                     	; Q28 Ki_q*iq_error + Kc_q*saterr_q

	norm	*				;
	norm	*				; Q28 -> Q30 (with OV protection)

	ADDS	uintlo_q
	ADD	uq_int,16        		; uint + saterr_q*Kc_q + iq_error*Ki_q
      SACH 	uq_int
	SACL	uintlo_q             	; save as 32-bit Q30

	RET

***END Q-Axis PI Current Regulator