.sym	_Encoder_CalcPosition,_Encoder_CalcPosition,32,2,0
	.globl	_Encoder_CalcPosition

	.func	212
;>>>> 	void Encoder_CalcPosition(unsigned qep_incr_sample)
******************************************************
* FUNCTION DEF : _Encoder_CalcPosition
******************************************************
_Encoder_CalcPosition:

LF4	.set	0

	POPD	*+
	SAR	AR0,*+
	SAR	AR1,*
	LARK	AR0,5
	LAR	AR0,*0+,AR2

	.sym	_qep_incr_sample,-3+LF4,14,9,16
	.sym	_sin_sample,1,4,1,16
	.sym	_ncos_sample,2,4,1,16
	.sym	_incr,3,14,1,16
	.sym	_buffer,4,14,1,16
	.line	2
;>>>> 	    volatile int sin_sample;
;>>>> 	    volatile int ncos_sample;
;>>>> 	    volatile unsigned incr;
;>>>> 	    volatile unsigned buffer;
*** 222	-----------------------    incr = qep_incr_sample;
	.line	11
;>>>> 	    incr = qep_incr_sample;
	LARK	AR2,-3+LF4
	MAR	*0+
	LAC	* 
	ADRK	6-LF4
	SACL	* ,AR3
L8:
***	-----------------------g2:
*** 227	-----------------------    if ( !(*(unsigned*)0x7032&0x100) ) goto g2;
	.line	16
;>>>> 	    ADC_READ2(sin_sample,ncos_sample);
	LARK	AR3,28722
	BIT	* ,7
	BBZ	L8
*** 227	-----------------------    sin_sample = *(unsigned*)0x7036;
	ADRK	4
	LAC	* ,AR2
	LARK	AR2,1
	MAR	*0+
	SACL	* ,AR3
*** 227	-----------------------    ncos_sample = *(unsigned*)0x7038;
	ADRK	2
	LAC	* ,AR2
	MAR	*+
	SACL	* ,AR3
*** 227	-----------------------    *(unsigned*)0x7032 |= 0x100u;
	SBRK	6
	LACK	256
	OR	* 
	SACL	* ,AR2
*** 230	-----------------------    sin_sample = (unsigned)sin_sample-0x8000u;
	.line	19
;>>>> 	    sin_sample  = sin_sample - ENC_U0_OFFSET; 
	MAR	*-
	LAC	* 
	ADLK	65535,15
	SACL	*+
*** 231	-----------------------    ncos_sample = (unsigned)ncos_sample+32468u;
	.line	20
;>>>> 	    ncos_sample = ncos_sample - ENC_U90_OFFSET;
	LAC	* 
	ADDK	32468
	SACL	* 
*** 236	-----------------------    encoder_position[0] = Encoder_CalcPhase(sin_sample, ncos_sample);
	.line	25
;>>>> 	    encoder_position[0] = Encoder_CalcPhase(sin_sample, ncos_sample);   
	LAC	*-,AR1
	SACL	*+,AR2
	LAC	* ,AR1
	SACL	*+
	CALL	_Encoder_CalcPhase
	SBRK	2
	LDPK	_encoder_position
	SACL	_encoder_position
*** 241	-----------------------    buffer = encoder_position[0]>>14;
	.line	30
;>>>> 	    buffer = ((encoder_position[0] >> 14) & 0x0003);
;>>>> 	    switch (buffer)
;>>>> 	       case 0:  if ((incr & 0x0003) == 3)
;>>>> 	                   incr = (incr + 1) & (qep_rollover-1);
;>>>> 	                break;  
	RSXM
	LAC	_encoder_position,1
	MAR	* ,AR2
	LARK	AR2,4
	MAR	*0+
	SACH	* ,1
***  	-----------------------    switch ( buffer ) {...};
	ZALS	* 
	BZ	L12
	SUBK	3
	BNZ	L14
***	-----------------------g4:
*** 248	-----------------------    if ( incr&3u ) goto g8;
	.line	37
;>>>> 	       case 3:  if ((incr & 0x0003) == 0)
	LACK	3
	MAR	*-
	AND	* 
	BNZ	L14
*** 249	-----------------------    incr = incr-1u&qep_rollover-1u;
	.line	38
;>>>> 	                   incr = (incr - 1) & (qep_rollover-1);
;>>>> 	                break;  
	LDPK	_qep_rollover
	LAC	_qep_rollover
	SUBK	1
	MAR	* ,AR1
	SACL	* ,AR2
	LAC	* ,AR1
	SUBK	1
	AND	* ,AR2
	SACL	* 
*** 249	-----------------------    goto g8;
	B	L14
L12:
***	-----------------------g6:
*** 244	-----------------------    if ( (incr&3u) != 3u ) goto g8;
	.line	33
	LACK	3
	MAR	*-
	AND	* 
	SUBK	3
	BNZ	L14
*** 245	-----------------------    incr = incr+1u&qep_rollover-1u;
	.line	34
	LDPK	_qep_rollover
	LAC	_qep_rollover
	SUBK	1
	MAR	* ,AR1
	SACL	* ,AR2
	LAC	* ,AR1
	ADDK	1
	AND	* ,AR2
	SACL	* 
L14:
***	-----------------------g8:
*** 254	-----------------------    encoder_position[1] = incr>>2;
	.line	43
;>>>> 	    encoder_position[1] = (incr >> 2);
	LARK	AR2,3
	MAR	*0+
	LAC	* ,13
	LDPK	_encoder_position+1
	SACH	_encoder_position+1,1
***  	-----------------------    return;
EPI0_4:
	.line	44
	MAR	* ,AR1
	SBRK	6
	LAR	AR0,*-
	PSHD	*
	RET

	.endfunc	255,000000000H,5

	.sym	_Encoder_CalcPhase,_Encoder_CalcPhase,36,2,0
	.globl	_Encoder_CalcPhase

	.func	268
;>>>> 	int  Encoder_CalcPhase(int qep_sin, int qep_negcos)  
******************************************************
* FUNCTION DEF : _Encoder_CalcPhase
******************************************************
_Encoder_CalcPhase:

LF5	.set	0

	POPD	*+
	SAR	AR0,*+
	SAR	AR1,*
	LARK	AR0,3
	LAR	AR0,*0+,AR2

	.sym	_qep_sin,-3+LF5,4,9,16
	.sym	_qep_negcos,-4+LF5,4,9,16
*
*  ---  buffer shares AUTO storage with phase
*  ---  buffer shares AUTO storage with U$2
*
	.sym	U$3,1,4,1,16
	.sym	_buffer,2,4,1,16
	.line	2
;>>>> 	    int  phase;
;>>>> 	    int  buffer;
*** 276	-----------------------    U$3 = ABS(qep_negcos);
	.line	9
;>>>> 	   if (abs(qep_sin) == abs(qep_negcos))
;>>>> 	      phase = (PI/4);               
	SSXM
	LARK	AR2,-4+LF5
	MAR	*0+
	LAC	* 
	ABS
	ADRK	5-LF5
	SACL	* 
*** 276	-----------------------    U$2 = ABS(qep_sin);
	SBRK	4-LF5
	LAC	* 
	ABS
	ADRK	5-LF5
	SACL	* 
*** 276	-----------------------    if ( U$2 == U$3 ) goto g4;
	LAC	*-
	SUB	* 
	BZ	L18
*** 278	-----------------------    if ( U$2 < U$3 ) goto g3;
	.line	11
;>>>> 	   else if (abs(qep_sin) < abs(qep_negcos))
;>>>> 	      buffer = q15_div(abs(qep_sin),abs(qep_negcos)); 
;>>>> 	      phase  = q15p_atan(buffer);   
;>>>> 	   else
	MAR	*+
	LAC	*-
	SUB	* 
	BLZ	L17
*** 286	-----------------------    buffer = q15_div((unsigned)U$3, (unsigned)U$2);
	.line	19
;>>>> 	      buffer = q15_div(abs(qep_negcos),abs(qep_sin)); 
	MAR	*+
	LAC	*-,AR1
	SACL	*+,AR2
	LAC	* ,AR1
	SACL	*+
	CALL	_q15_div
	SBRK	2
	MAR	* ,AR2
	LARK	AR2,2
	MAR	*0+
	SACL	* ,AR1
*** 288	-----------------------    phase = 16384L-(long)q15p_atan((unsigned)buffer);
	.line	21
;>>>> 	      phase  = (PI/2) - q15p_atan(buffer);   
	SACL	*+
	CALL	_q15p_atan
	MAR	*-,AR2
	NEG
	ADDK	16384
	LARK	AR2,2
	MAR	*0+
	SACL	* 
*** 288	-----------------------    goto g5;
	B	L19
L17:
***	-----------------------g3:
*** 280	-----------------------    buffer = q15_div((unsigned)U$2, (unsigned)U$3);
	.line	13
	LAC	*+,AR1
	SACL	*+,AR2
	LAC	* ,AR1
	SACL	*+
	CALL	_q15_div
	SBRK	2
	MAR	* ,AR2
	LARK	AR2,2
	MAR	*0+
	SACL	* ,AR1
*** 282	-----------------------    phase = q15p_atan((unsigned)buffer);
	.line	15
	SACL	*+
	CALL	_q15p_atan
	MAR	*-,AR2
	LARK	AR2,2
	MAR	*0+
	SACL	* 
***  	-----------------------    goto g5;
	B	L19
L18:
***	-----------------------g4:
*** 277	-----------------------    phase = 8192;
	.line	10
	MAR	*+
	SPLK	#8192,* 
L19:
***	-----------------------g5:
*** 294	-----------------------    if ( qep_sin >= 0 ) goto g9;
	.line	27
;>>>> 	   if (qep_sin >= 0)   
;>>>> 	      if (qep_negcos > 0)
;>>>> 	         phase = PI - phase;    /* 2nd quadrant */ 
;>>>> 	   else 
	SSXM
	SBRK	5-LF5
	LAC	* 
	BGEZ	L23
*** 302	-----------------------    if ( qep_negcos <= 0 ) goto g8;
	.line	35
;>>>> 	      if ( qep_negcos > 0 )
	MAR	*-
	LAC	* 
	BLEZ	L22
*** 303	-----------------------    phase = (long)phase-32768L;
	.line	36
;>>>> 	         phase = PI + phase;    /* 3rd quadrant */ 
;>>>> 	      else
	ADRK	6-LF5
	LAC	* 
	ADLK	65535,15
	SACL	* 
***  	-----------------------    return phase;
	LAC	* 
	B	EPI0_5
L22:
***	-----------------------g8:
*** 305	-----------------------    phase = -phase;
	.line	38
;>>>> 	         phase = -phase;        /* 4th quadrant */ 
	ADRK	6-LF5
	LAC	* 
	NEG
	SACL	* 
*** 305	-----------------------    return phase;
	LAC	* 
	B	EPI0_5
L23:
***	-----------------------g9:
*** 296	-----------------------    if ( qep_negcos <= 0 ) goto g11;
	.line	29
	MAR	*-
	LAC	* 
	BLEZ	L25
*** 297	-----------------------    phase = 32768L-(long)phase;
	.line	30
	LALK	1,15
	ADRK	6-LF5
	SUB	* 
	SACL	* 
L25:
***	-----------------------g11:
*** 308	-----------------------    return phase;
	.line	41
;>>>> 	   return  phase; 
	LARK	AR2,2
	MAR	*0+
	LAC	* 
EPI0_5:
	.line	42
	MAR	* ,AR1
	SBRK	4
	LAR	AR0,*-
	PSHD	*
	RET

	.endfunc	309,000000000H,3

	.sym	_encoder_position,_encoder_position,62,2,32,,2
	.globl	_encoder_position
	.end