;===========================================================================
; 文件名:	rampgen.asm
;
; 模塊名: 	RAMP_GEN				      
;
; 初始化程序名: RAMP_GEN_INIT
;
; 公司:	達盛科技
;
; 功能描述:	產生頻率、增益、偏移量可變的斜坡信號，模擬轉子磁場
;
;				            |~~~~~~~~~~~~~~~~~|
;	   rmp_gain	    o------>|	 	          |
;	   rmp_offset 	o------>|    RAMP_GEN	  |----->o  rmp_out
;	   rmp_freq	    o------>|		          |
;				            |_________________|
;
;
;=====================================================================================
; 更改紀錄:
;-------------------------------------------------------------------------------------
; 最后更新日期:2005.5.19    版本號: Ver 1.0
;===========================================================================
;(要調用模塊，就將下面聲明語句復制到主程序代碼中相應位置)
;		.ref	RAMP_GEN, RAMP_GEN_INIT			;子程序調用
;		.ref	rmp_gain, rmp_offset, rmp_freq	;輸入
;		.ref	step_angle_max					;輸入
;		.ref	rmp_out							;輸出
;===========================================================================
;變量定義
;===========================================================================
		.def	RAMP_GEN, RAMP_GEN_INIT			;子程序調用
		.def	rmp_gain, rmp_offset, rmp_freq	;輸入
		.def	step_angle_max					;輸入
		.def	rmp_out							;輸出
;===========================================================================
		.include 	"x24x_app.h"  

STEP_ANGLE_RG_MAX	.set	1000	;305.2Hz 對應 fs=20kHz
								
alpha_rg		.usect "rampgen",1
step_angle_rg	.usect "rampgen",1
step_angle_max	.usect "rampgen",1
rmp_gain		.usect "rampgen",1
rmp_offset		.usect "rampgen",1
rmp_freq		.usect "rampgen",1
rmp_out			.usect "rampgen",1
rmp_out_abs		.usect "rampgen",1
rmp_freq_p  	.usect "rampgen",1
;===========================================================================
;初始化
;===========================================================================
RAMP_GEN_INIT:
		LDP	#alpha_rg
		SPLK	#0, alpha_rg  				
		SPLK	#STEP_ANGLE_RG_MAX, step_angle_max

		SPLK	#3FFFh, rmp_gain	 		
		SPLK	#3FFFh, rmp_offset 		
		SPLK	#3FFFh, rmp_freq	 		
		RET
;===========================================================================
;控制程序
;===========================================================================
RAMP_GEN:
		SPM 0
		LDP		#rmp_freq
		LACC	rmp_freq,16		; ACC = rmp_freq  (Q15)
		ABS						; ACC = |rmp_freq|  (Q15)
		SACH	rmp_freq_p 		; rmp_freq_p = |rmp_freq|  (Q15)
	
		LT	rmp_freq_p			;規格化頻率參考值 rmp_freq 
								;Q15
		MPY	step_angle_max		;Q15 x Q0
		PAC						;Q0 x Q15 = Q15 (32bit)
		SACH	step_angle_rg,1	;Q0 

		LACC	alpha_rg		;Q0
		ADD	step_angle_rg		;Q0+Q0
		SACL	alpha_rg		;Q0

	;根據設定值改變輸出增益
		LT	alpha_rg			;Q0
		MPY	rmp_gain			;Q0 x Q15
		PAC						;P = rmp_gain * alpha_rg
		SACH	rmp_out_abs,1	;Q0
		LACC	rmp_out_abs		;Q0
		SACL	rmp_out			;Q15**
**說明
;rmp_out = rmp_out_abs/7FFFh. 

		SETC	SXM				; 允許符號擴展
		SETC	OVM				; 設置溢出保護
		LACC	rmp_freq		; ACC = rmp_freq  (Q15)
		BCND	RMP_FREQ_POS, GT; 轉到 RMP_FREQ_POS，當 rmp_freq > 0  	
		LACC	rmp_out,16		; ACC = rmp_out  (Q15)
		NEG						; ACC = -rmp_out  (Q15)
		SACH	rmp_out			; rmp_out = -rmp_out  (Q15) 
RMP_FREQ_POS		

    ;加偏移量
		LACC	rmp_out			;Q15
		ADD		rmp_offset		;Q15+Q15
		SACL	rmp_out			;Q15
		RET