;============================================================
; Filename	:	pid.asm
; Module names:	pid_reg_id, pid_reg_iq
; Initialization routines: pid_reg_id_init, pid_reg_iq_init
; 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
;===================================================================== *************************************************************
* 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