📄 speed_fr.asm
字号:
;=====================================================================================
; File name: SPEED_FR.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: This file contains source for a speed calculation function.
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 01-14-2002 Release Rev 1.00
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx. (Peripheral Independant).
;================================================================================
; Routine Name: speed_frq Routine Type: C Callable
;
; Description:
;
; C prototype : int speed_frq(SPEED_MEAS *)
;================================================================================
; The definition for the Speed Frequency Object is as follows:
;
;typedef struct { int theta_elec; /* Input: Electrical angle (Q15) */
; int K1_fr; /* Parameter: Constant for differentiator (Q6) */
; int theta_old; /* History: Electrical angle at previous step (Q15) */
; int K2_fr; /* Parameter: Constant for low-pass filter (Q15) */
; int speed_frq; /* Output: Speed in per-unit (Q15) */
; int K3_fr; /* Parameter: Constant for low-pass filter (Q15) */
; int rpm_max; /* Parameter: Base speed in rpm (Q0) */
; int speed_rpm; /* Output : Speed in rpm (Q0) */
; int (*calc)(); /* Pointer to the calulation function */
; } SPEED_MEAS; /* Data type created */
;
;================================================================================
; Frame Usage Details:
; | a | b | c | d
;____________|______________|_______________|__________|____________
; FR0 | tmp1 | | |
;
;
;================================================================================
.def _speed_frq
;================================================================================
DIFF_MAX_LIMIT_FR .set 7333h ;7fffh Maximum limit to differentiate angle (Q15)
DIFF_MIN_LIMIT_FR .set 0CCDh ;0000h Minimum limit to differentiate angle (Q15)
; Important!! Note that the default DIFF_LIMIT are shown, if it is neccessary
; to be changed, the module library (e.g., clib_011.lib) must be rebuilt to take the effect.
;================================================================================
__speed_frq_framesize .set 0001h
;================================================================================
_speed_frq:
; Assume now ARP=AR1
POPD *+ ; Keep return address
SAR AR0,*+ ; Keep old frame pointer (FP)
SAR AR1,* ; Keep old stack pointer (SP)
LARK AR0,__speed_frq_framesize ; Load AR0 with frame size
LAR AR0,*0+,AR0 ; AR0->FP0 (new FP), ARP=AR0
;=================================================================================
SBRK #3 ; ARP=AR0, AR0->FR0-3 (1st argument)
;----------------------------------------------------------------------------------
LAR AR2,* ; ARP=AR0, AR0->dir_QEP, AR2->theta_elec
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SETC SXM ; Turn sign extension mode on
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SETC OVM ; Set overflow mode
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SPM 0 ; Reset product mode
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
LACC #DIFF_MAX_LIMIT_FR ; ACC = DIFF_MAX_LIMIT (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SUB * ; ACC = DIFF_MAX_LIMIT - theta_elec (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
BCND LESS_MAX_FR,GT ; Branch to LESS_MAX if DIFF_MAX_LIMIT > theta_elec
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
B SKIP_DIFF_FR ; ARP=AR2, AR0->FR1, AR2->theta_elec
;----------------------------------------------------------------------------------
LESS_MAX_FR ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
LACC #DIFF_MIN_LIMIT_FR ; ACC = DIFF_MIN_LIMIT (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SUB * ; ACC = DIFF_MIN_LIMIT - theta_elec (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
BCND MORE_MIN_FR,LT ; Branch to MORE_MIN if DIFF_MIN_LIMIT < theta_elec
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
B SKIP_DIFF_FR ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
MORE_MIN_FR ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
LT *+ ; TREG = theta_elec (Q15)
; ARP=AR2, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
MPY *+ ; PREG = K1_fr*theta_elec (Q21)
; ARP=AR2, AR0->FR0, AR2->theta_old
;----------------------------------------------------------------------------------
PAC ; ACC = K1_fr*theta_elec (Q21)
; ARP=AR2, AR0->FR0, AR2->theta_old
;----------------------------------------------------------------------------------
LT *- ; TREG = theta_old (Q15)
; ARP=AR2, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
MPY *,AR0 ; PREG = K1_fr*theta_old (Q21)
; ARP=AR2, AR0->FR0, AR2->K1_fr, ARP=AR0
;----------------------------------------------------------------------------------
SPAC ; ACC = K1_fr*theta_elec-K1_fr*theta_old (Q21)
; ARP=AR0, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
SFL ; ACC = K1_fr*theta_elec-K1_fr*theta_old (Q22)
; ARP=AR0, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
SFL ; ACC = K1_fr*theta_elec-K1_fr*theta_old (Q23)
; ARP=AR0, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
SFL ; ACC = K1_fr*theta_elec-K1_fr*theta_old (Q24)
; ARP=AR0, AR0->FR0, AR2->K1_fr
;----------------------------------------------------------------------------------
SACH *,7,AR2 ; FR0 = tmp1 = K1_fr*theta_elec-K1_fr*theta_old (Q15)
; ARP=AR0, AR0->FR0, AR2->K1_fr, ARP=AR2
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->K2_fr
;----------------------------------------------------------------------------------
LT *+ ; TREG = K2_fr (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_frq
;----------------------------------------------------------------------------------
MPY *+ ; PREG = K2_fr*speed_frq (Q30)
; ARP=AR2, AR0->FR0, AR2->K3_fr
;----------------------------------------------------------------------------------
PAC ; ACC = K2_fr*speed_frq (Q30)
; ARP=AR2, AR0->FR0, AR2->K3_fr
;----------------------------------------------------------------------------------
LT *-,AR0 ; TREG = K3_fr (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_frq, ARP=AR0
;----------------------------------------------------------------------------------
MPY *,AR2 ; PREG = K3_fr*speed_frq (Q30)
; ARP=AR0, AR0->FR0, AR2->speed_frq, ARP=AR2
;----------------------------------------------------------------------------------
APAC ; ACC = K2_fr*speed_frq + K3_fr*speed_frq (Q30)
; ARP=AR2, AR0->FR0, AR2->speed_frq
;----------------------------------------------------------------------------------
SACH *,1 ; speed_frq = K2_fr*speed_frq + K3_fr*speed_frq (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_frq
;----------------------------------------------------------------------------------
SBRK #4 ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
SKIP_DIFF_FR ; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
LACC * ; ACC = theta_r_se (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_elec
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->theta_old
;----------------------------------------------------------------------------------
SACL * ; theta_old = theta_elec (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_old
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->speed_frq
;----------------------------------------------------------------------------------
LT * ; TREG = speed_frq (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_frq
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->rpm_max
;----------------------------------------------------------------------------------
MPY *+ ; PREG = speed_frq*rpm_max (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_rpm
;----------------------------------------------------------------------------------
PAC ; ACC = speed_frq*rpm_max (Q15)
; ARP=AR2, AR0->FR0, AR2->speed_rpm
;----------------------------------------------------------------------------------
SACH *,1,AR1 ; speed_rpm = speed_frq*rpm_max (Q0)
; ARP=AR2, AR0->FR0, AR2->speed_rpm, ARP=AR1
;----------------------------------------------------------------------------------
SPEED_FRQ_END
;----------------------------------------------------------------------------------
__speed_frq_exit:
;;;MAR *,AR1 ; can be removed if this condition is met on
; every path to this code. (i.e., ARP=AR1 here)
CLRC SXM
CLRC OVM
SPM 0
SBRK #(__speed_frq_framesize+1)
LAR AR0,*-
PSHD *
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -