speed_fr1.asm

这是实现三相异步电动机FOC的DSP开发源代码

;===========================================================================
; 文件名:	speed_fr1.asm			      
;
; 模块名: 	SPEED_FRQ	
;
; 初始化程序名: SPEED_FRQ_INIT
;
; 公司:	达盛科技
;
; 功能描述: 根据固定时间间隔内的转子角位移计算转子速度。 
;		(时间间隔为SPEED_LP_MAX x 速度采样时间) sec.
;
;				        |~~~~~~~~~~~~~~~|
;	shaft_angle	o------>|		        |----->o  speed_frq
;	  			        | SPEED_FRQ 	|
;	  direction o------>|  		        |----->o  speed_frq_rpm
;				        |_______________|
; 
;
;=====================================================================================
; 更改纪录:
;-------------------------------------------------------------------------------------
; 最后更改日期:2005.7.30	版本号:	Ver 1.0	
;===========================================================================
;(使用子程序就把下面的变量声明文件复制到主程序中)
;		.ref	SPEED_FRQ, SPEED_FRQ_INIT		;子程序调用
;		.ref	shaft_angle, direction			;输入
;		.ref	speed_frq, speed_frq_rpm		;输出
;===========================================================================
;变量声明
		.def	SPEED_FRQ, SPEED_FRQ_INIT		;子程序调用
		.def	shaft_angle, direction			;输入
		.def	speed_frq, speed_frq_rpm		;输出
		.def	SPEED_LP_MAX, rpm_scaler
;===========================================================================

		.include	x24x_app.h

SPEED_LP_MAX_	.set	100		;100 x 100uS = 0.010 sec
								;200 x 50uS = 0.010 sec
RPM_SCALER_	.set	6000		;RPM转速转换系数


shaft_angle		.usect	"speed_fr",1
s_angle_cur		.usect	"speed_fr",1
s_angle_old		.usect	"speed_fr",1
direction		.usect	"speed_fr",1
speed_frq_old	.usect	"speed_fr",1
speed_frq		.usect	"speed_fr",1
speed_frq_rpm	.usect	"speed_fr",1
rpm_scaler		.usect	"speed_fr",1
speed_skp_cnt	.usect	"speed_fr",1
SPEED_LP_MAX	.usect	"speed_fr",1

;===========================================================================
SPEED_FRQ_INIT:
;===========================================================================
		ldp	#rpm_scaler
		SPLK	#RPM_SCALER_, rpm_scaler
		splk	#SPEED_LP_MAX_, SPEED_LP_MAX
		splk	#0,speed_frq_old
		RET

;===========================================================================
SPEED_FRQ:
;===========================================================================
		spm 0
		ldp	#speed_skp_cnt
		LACC	speed_skp_cnt
		ADD	#1
		SACL	speed_skp_cnt
		SUB	SPEED_LP_MAX
		BCND	MS_FRQ_END, LT	;跳过转速测量

MSF_1		LACC	s_angle_cur
		SACL	s_angle_old
		LACC	shaft_angle
		SACL	s_angle_cur
		LACC	direction
		BCND	UP_CNT, NEQ		;如果不等于0,则增计数

;---------------------------
DWN_CNT:
;---------------------------
		LACC	s_angle_cur
		SUB	s_angle_old
		BCND	DNEG_DELTA, LEQ	;If negative

DPOS_DELTA:
		SUB	#7FFFh				;Delta-1
		SACL	speed_frq		;Delta = f(t2) - f(t1) - 1
		B	SM_SCALE

DNEG_DELTA:
		SACL	speed_frq		;Delta = f(t2) - f(t1)
		B	SM_SCALE


;---------------------------
UP_CNT:
;---------------------------
		LACC	s_angle_cur
		SUB	s_angle_old
		BCND	UNEG_DELTA, LT	;If negative

UPOS_DELTA	SACL	speed_frq	;Delta = f(t2) - f(t1)
		B	SM_SCALE

UNEG_DELTA	ADD	#7FFFh			;Delta + 1
		SACL	speed_frq		;Delta = 1 + f(t2) - f(t1)


SM_SCALE:
		lacc 	 speed_frq
		sub speed_frq_old
		abs
		sub #2000h
		bcnd norm_arra,lt
		lacc speed_frq_old
		sacl speed_frq
norm_arra		lacc 	speed_frq
		sacl	speed_frq_old
		LT	speed_frq
		MPY	rpm_scaler
		PAC
		SACH	speed_frq_rpm,1

		SPLK	#0, speed_skp_cnt	;复位计数器

MS_FRQ_END:
		RET