;====================================================================================
; File Name:	compen.asm
;
; Module Name: 	COMPEN	
;
; Initialization Routine: COMPEN_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;
; Description:	
; 		Compensates inverter output voltage for the DC bus
;		ripple according to the following equation:
;		Vactual = (Vcal*ADC_actual)/ADC_ref
;
;			|~~~~~~~~~~~~~~~~~|
;	Vcalo---------->|		  |----->o  Vactual
;	ADC_refo------->|     COMPEN      |
;	ADC_actualo---->|		  |
;			|_________________|
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	COMPEN, COMPEN_INIT			;function call
;		.ref	Vcal, ADC_ref, ADC_actual		;Inputs   
;		.ref	Vactual					;Output 
;		.ref	compensa				;Output (utilized by SINCOSPH.asm)
;===========================================================================
;Module definitions for external reference.
		.def	COMPEN, COMPEN_INIT			;function call
		.def	Vcal, ADC_ref, ADC_actual		;Inputs 
		.def	Vactual					;Output 
		.def	compensa				;Output (utilized by SINCOSPH.asm)
;===========================================================================
		.include	x24x_app.h
		.ref	GPR0

Vcal		.usect	"comp", 1
ADC_ref		.usect	"comp", 1
ADC_actual	.usect	"comp", 1
Vactual		.usect	"comp", 1
RESULT_HI	.usect	"comp", 1
RESULT_LO	.usect	"comp", 1
INV_ADC_actual	.usect	"comp", 1
RESULT_COMP	.usect	"comp", 1  
MAX		.usect	"comp", 1 
compensa	.usect	"comp", 1  
;=========================
COMPEN_INIT:	  
;=========================
		LDP	#Vcal                       ; Variables data page
;		SPLK	#08000h, MAX   
		splk	#2000h, MAX
		SPLK	#0, compensa
	                                 	
		RET
   
;=========================
COMPEN:	  
;=========================  
	CLRC	SXM             ; Sign extension mode off
	SPM	0  
	
	LDP	#ADC_actual
;	SPLK	#1, compensa
;	SPLK	#0, compensa	 
	              
	LDP	#ADC_actual         ; Variables data page
	LACC	#07FFFh
	RPT	#15
	SUBC	ADC_actual
	SACL	RESULT_HI
	XOR		RESULT_HI
	OR	#0FFFFh

	RPT	#15
	SUBC	ADC_actual
	SACL	RESULT_LO
	LACC	RESULT_LO
	ADDH	RESULT_HI 
	
	RPT	#4
	SFR
	AND	#7FFFh	
	SACL	INV_ADC_actual	;(1/ADC_actual) in Q11

	LT	INV_ADC_actual
	MPY	ADC_ref
	PAC
;	SACH	RESULT_COMP,5		; (1/ADC_actual)*ADC_ref
 	SACH	RESULT_COMP,3
 	LACC	RESULT_COMP
 	AND	#7FFFh
 	SACL	RESULT_COMP			; RESULT_COMP in Q13
	LT	RESULT_COMP
	MPY	Vcal
	PAC
	SACH	Vactual,1		; ((1/ADC_actual)*ADC_ref)*Vcal	
;	SACH	Vactual,5	
	LACC	Vactual  
	AND	#6000h
	SUB	MAX
	BCND	SATURATE, geq   
	LACC	Vactual,2
	AND	#7FFFh
	SACL	Vactual	
    	SPM	0                 	; SPM reset
	CLRC	SXM             ; Sign extension mode off
	RET	
SATURATE
	SPLK	#7FFFh, Vactual
    	SPM	0                 	; SPM reset
	CLRC	SXM             ; Sign extension mode off
	RET