sp_rv_pr.asm

无刷直流电机的无传感器控制TI程序

;===========================================================================
; File Name:	Sp_rv_pr.asm
;
; Module Name: 	SPEED_REV_PRD			     
;
; Initialization Routine: SPEED_REV_PRD_INIT
;
; Originator:	Digital Control Systems Group
;			Texas Instruments
;	
; Description:  This module calculates the motor speed based on a signal's 
;		period measurement. Such a signal, for which the period is 
;		measured, can be the periodic output pulses from a motor 
;		speed sensor.
;			
;				|~~~~~~~~~~~~~~~~~|
;				|		  |----->o  speed_prd
;	   event_period o------>|  SPEED_REV_PRD  |
;				|		  |----->o  speed_rpm
;				|_________________|
;
;
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0	
;===========================================================================
;(To use this Module, copy this section to main system file)
;		.ref	SPEED_REV_PRD, SPEED_REV_PRD_INIT		;function call
;		.ref	event_period				;Input
;		.ref	rpm_max, speed_scaler,shift		;parameter
;		.ref	speed_prd, speed_rpm			;Outputs
;===========================================================================
;Module definitions for external reference.
		.def	SPEED_REV_PRD, SPEED_REV_PRD_INIT		;function call
		.def	event_period				;Input
		.def	rpm_max, speed_scaler,shift		;parameter
		.def	speed_prd, speed_rpm			;Outputs
;===========================================================================
		.include	x24x_app.h   

;For 4 pole motor,40kHz sampling loop and Normalization speed = 15000 RPM
;--------------------------------------------------------------------------------
SPEED_SCALER_	.set	80	;Scaling constant(see related document for details)
RPM_MAX_	.set	15000	;Max RPM value. Base RPM for normalization.
SHIFT_		.set	5	;Shift parameter for max accuracy of 32bit/16bit division

;For 4 pole motor,40kHz sampling loop and Normalization speed = 10000 RPM
;--------------------------------------------------------------------------------
;SPEED_SCALER_	.set	120	;Scaling constant(see related document for details)
;RPM_MAX_	.set	10000	;Max RPM value. Base RPM for normalization.
;SHIFT_		.set	5	;Shift parameter for max accuracy of 32bit/16bit division

;For 4 pole motor,40kHz sampling loop and Normalization speed = 6000 RPM
;--------------------------------------------------------------------------------
;SPEED_SCALER_	.set	200	;Scaling constant(see related document for details)
;RPM_MAX_	.set	6000	;Max RPM value. Base RPM for normalization.
;SHIFT_		.set	6	;Shift parameter for max accuracy of 32bit/16bit division				


;For 4 pole motor,20kHz sampling loop and Normalization speed = 6000 RPM
;--------------------------------------------------------------------------------
;SPEED_SCALER_	.set	100	;Scaling constant(see related document for details)
;RPM_MAX_	.set	6000	;Max RPM value. Base RPM for normalization.
;SHIFT_		.set	5	;Shift parameter for max accuracy of 32bit/16bit division		


SHIFT_TOTAL	.set	14 	;Total shift


event_period	.usect "spdrvprd" ,1
speed_hi	.usect "spdrvprd" ,1
speed_lo	.usect "spdrvprd" ,1
speed_prd_max	.usect "spdrvprd" ,1
speed_prd	.usect "spdrvprd" ,1
speed_rpm	.usect "spdrvprd" ,1
speed_scaler	.usect "spdrvprd" ,1		
rpm_max		.usect "spdrvprd" ,1
shift		.usect "spdrvprd" ,1
shift2		.usect "spdrvprd" ,1
						 		
		

SPEED_REV_PRD_INIT:
		LDP	#rpm_max
		SPLK	#RPM_MAX_, rpm_max     		;Q0
		SPLK	#SPEED_SCALER_, speed_scaler 	;Q0
		SPLK	#SHIFT_, shift		 	;Q0
		RET


SPEED_REV_PRD:
        	LDP	#rpm_max
        	CLRC    SXM


;Calculate Speed, i.e. speed = 1/period
;Numerator (i.e. 1) is treated as a Q31 value, speed in Q31(=Q31/Q0)
;Phase 1   

CALC_SPEED:	LACC	#07FFFh 	  	;Load Numerator Hi
		RPT	#15
		SUBC	event_period
		SACL	speed_hi
		XOR	speed_hi
		OR	#0FFFFh 	  	;Load Numerator Lo
;Phase 2
		RPT	#15
		SUBC	event_period
		SACL	speed_lo		

	
		LACC	speed_lo
		ADDH	speed_hi        	;Result in ACC is in Q31(32 bit) format  
  
		rpt	shift			;The following Q numbers are for shift=6.
						;For other values of shift these will
						;change accordingly.
		SFL				;Q38
		SACH	speed_prd_max		;Q22

		LACC	#SHIFT_TOTAL  
		SUB	shift
		SACL	shift2			;shift2=14-shift

		SPM	0
		LT    	speed_prd_max		;Q22
		MPY	speed_scaler		;Q0*Q22 
		PAC   				;Q22, 32 bit format
		RPT	shift2 			;shift2=14-shift
		SFL 				;Q31,32 bit format
		SACH	speed_prd		;Q15, speed_prd = speed_scaler*speed_prd_max
	

	;Scale to Q0 for direct RPM display 

		LT	speed_prd    		;Q15
		MPY	rpm_max  		;Q0*Q15
		PAC                             ;Q15, 32 bit format
		SACH	speed_rpm,1		;Q0                

MSP_EXIT	RET