;===========================================================================
; 文件名:	qep_drv.asm
; 模塊名:	QEP_THETA_DRV
;
; 初始化程序名: QEP_THETA_DRV_INIT
;	
; 同步計數重置: QEP_INDEX_ISR_DRV
;
; 公司:	達盛科技
;
;
; 功能描述:	利用增量式編碼器檢測轉子位置和旋轉方向 
;	QEP_THETA_DRV: 依據增量式編碼器輸出脈沖計算轉子位置
;	QEP_INDEX_ISR_DRV: 同步中斷重置計數器T2
;
;				         |~~~~~~~~~~~~~~~~~~~|
; QEP A/B(EV H/W)o------>|			         |----->o  theta_elec
; polepairs      o------>|   QEP_THETA_DRV   |----->o  theta_mech
; cal_angle      o------>|  			     |----->o  dir_QEP
; mech_scale     o------>|  			     |
;			  	         |-------------------|
; QEP Index (EV H/W) o-->| QEP_INDEX_ISR_DRV |----->o  index_sync_flg
;				         |                   |----->o  QEP_cnt_idx
;				         |___________________|
;
; 目標板CPU:		x240/24x/240x
;
;=====================================================================================
; 修改紀錄:
;-------------------------------------------------------------------------------------
; 最后更新日期:2005.7.5 	版本號:	Ver 1.0	
;===========================================================================

;---------------------------------------------------------------------------
; 調用時變量聲明
;---------------------------------------------------------------------------
;		.ref	QEP_THETA_DRV,QEP_THETA_DRV_INIT	; function call
;		.ref	polepairs,cal_angle,mech_scale		; Inputs
;		.ref	theta_elec,theta_mech,dir_QEP		; Outputs
;
;		.ref	QEP_INDEX_ISR_DRV					; function call
;		.ref	index_sync_flg,QEP_cnt_idx			; Output
;---------------------------------------------------------------------------
; 變量聲明
;---------------------------------------------------------------------------
		.def	QEP_THETA_DRV,QEP_THETA_DRV_INIT	; function call
		.def	polepairs,cal_angle,mech_scale		; Inputs
		.def	theta_elec,theta_mech,dir_QEP		; Outputs

		.def	QEP_INDEX_ISR_DRV					; function call
		.def	index_sync_flg,QEP_cnt_idx			; Output

;-------------------------------------------
; 相關變量定義
;-------------------------------------------
		.include	"x24x_app.h"

;---------------------------------------
; 變量定義
;---------------------------------------
theta_elec		.usect "qep_drv",1
theta_mech		.usect "qep_drv",1
dir_QEP			.usect "qep_drv",1
polepairs		.usect "qep_drv",1
QEP_cnt_idx		.usect "qep_drv",1
cal_angle		.usect "qep_drv",1
index_sync_flg	.usect "qep_drv",1
mech_scale		.usect "qep_drv",1
theta_raw		.usect "qep_drv",1

;---------------------------------------------------------------
; 參數設定
;---------------------------------------------------------------
POLEPAIRS_	.set	2;4		 	; 極對數
CAL_ANGLE_	.set	206;333		; 偏移角
MECH_SCALE_	.set	16776;6710	; 總計數值*1/4000 的 Q26 表示(1000線編碼器)

;----------------------------------------------------
; 初始化
;----------------------------------------------------
QEP_THETA_DRV_INIT:
		ldp	#polepairs
		SPLK	#POLEPAIRS_,polepairs
		splk	#CAL_ANGLE_,cal_angle
		SPLK	#MECH_SCALE_,mech_scale
		.if (x240)
		ldp	#OCRC>>7
		lacl	OCRC
		or	#01110000b
		sacl	OCRC
		.endif
		.if (x243|x2407)
		ldp	#OCRA>>7
		lacl	OCRA
		or	#00111000b
		sacl	OCRA
		.endif
		ldp	#T2PER>>7
		SPLK	#0FFFFh,T2PER			; compatible across '240/24x/240x
		SPLK	#1001100001110000b,T2CON	; D-Up/Dn, /1, src=QEP
		SPLK	#1111000000000100b,CAPCON	; Tmr2, CAP3 pos edge, enbl
		RET

;---------------------------------------------------------
; 較位置計算程序
;---------------------------------------------------------
QEP_THETA_DRV
		ldp	#GPTCON>>7
		lacc	GPTCON,2
		and	#1,16		; Get direction bit
		ldp	#dir_QEP	;
		sach	dir_QEP	; save
		ldp	#T2CNT>>7	; read count of pulses
		lacc	T2CNT		;
		ldp	#cal_angle	;
		add	cal_angle	; add offset amount
		sacl	theta_raw	;
		LT	theta_raw	;
		MPY	mech_scale	; Q0*Q26=Q26(32bit)
		PAC			;
		and	#7FFFh,11	; modulo(360 in Q26)
		SACH	theta_mech,5; save as Q(26-16+5)=Q15
		LT	theta_mech	;
		MPY	polepairs	; Q15*Q0=Q15(32bit)
		PAC
		and	#7FFFh	; modulo(360 in Q15)
		SACL	theta_elec	; save as Q15
		RET

;---------------------------------------------------------------------------
; 同步計數重置程序
;---------------------------------------------------------------------------
QEP_INDEX_ISR_DRV:
		ldp	#T2CNT>>7
		bldd	T2CNT,#QEP_cnt_idx	; Read out QEP counter value
		SPLK	#0,T2CNT			; Clear counter
		ldp	#index_sync_flg
		SPLK	#0Fh,index_sync_flg	; Indicate Index mark alignment.
		RET