📄 speed_pr.asm
字号:
;=====================================================================================
; File name: SPEED_PR.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description:
; This file contains source for a speed calculation function.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000 Release Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx. (Peripheral Independant).
;================================================================================
; Routine Name: speed_prd Routine Type: C Callable
;
; Description:
;
; C prototype : int speed_prd(SPEED_MEAS *)
;================================================================================
; The definition for the Speed Period Object is as follows:
;
; typedef struct {
; int time_stamp_new; /* Variable : New 'Timestamp' corresponding to a capture event */
; int time_stamp_old; /* Variable : Old 'Timestamp' corresponding to a capture event */
; int time_stamp; /* Input : Current 'Timestamp' corresponding to a capture event */
; int shift; /* Parameter : For maximum accuracy of 32bit/16bit division */
; int speed_scaler; /* Parameter : Scaler converting 1/N cycles to a Q15 speed */
; int speed_prd; /* Output : speed in per-unit */
; int rpm_max; /* Parameter : Scaler converting Q15 speed to rpm (Q0)speed */
; int speed_rpm; /* Output : speed in r.p.m. */
; int (*calc)(); /* Pointer to the calulation function */
; } SPEED_MEAS; /* Data type created */
;
;================================================================================
; Frame Usage Details:
; | a | b | c d
;____________|______________|_______________|_______________________
; FR0 | event_period | |
; FR1 | speed_hi | |
; FR2 | speed_lo | |
; FR3 | speed_prd_max| |
; FR4 | shift2 | |
;================================================================================
.def _speed_prd
;================================================================================
SHIFT_TOTAL .set 14
;================================================================================
__speed_prd_framesize .set 0005h
;================================================================================
_speed_prd:
; 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_prd_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->time_stamp_new, AR2->time_stamp_new
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
CLRC SXM ; Turn sign extension mode off
; ARP=AR2, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
CLRC OVM ; Set overflow mode
; ARP=AR2, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
SPM 0 ; Reset product mode
; ARP=AR2, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
LACC *+ ; ACC = time_stamp_new
; ARP=AR2, AR0->FR0, AR2->time_stamp_old
;----------------------------------------------------------------------------------
SACL *+ ; time_stamp_old = time_stamp_new
; ARP=AR2, AR0->FR0, AR2->time_stamp
;----------------------------------------------------------------------------------
LACC * ; ACC = time_stamp
; ARP=AR2, AR0->FR0, AR2->time_stamp
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR2, AR0->FR0, AR2->time_stamp_new
;----------------------------------------------------------------------------------
SACL *+ ; time_stamp_new = time_stamp
; ARP=AR2, AR0->FR0, AR2->time_stamp_old
;----------------------------------------------------------------------------------
SUB *+ ; ACC = time_stamp - time_stamp_old
; ARP=AR2, AR0->FR0, AR2->time_stamp
;----------------------------------------------------------------------------------
MAR *+,AR0 ; ARP=AR2, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
BCND NEG_DELTA, LT ; Branch to NEG_DELTA if ACC < 0 (allow "wrapping")
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
POS_DELTA ; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
SACL * ; FR0 = event_period = time_stamp - time_stamp_old
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
B CALC_SPEED ; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
NEG_DELTA ; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
ADD #07FFFh ; ACC = 7FFFh + time_stamp - time_stamp_old
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
SACL * ; FR0 = event_period = 7FFFh + time_stamp - time_stamp_old
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
; Calculate speed, i.e., speed = 1/period
; Numeraor (i.e., 1) is treated as a Q31 value, speed in Q31 (=Q31/Q0)
; Phase 1
;----------------------------------------------------------------------------------
CALC_SPEED ; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
LACC #07FFFh ; ACC = 7FFFh (load numerator high)
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
RPT #15 ; Repeat next instuction 16 times
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
SUBC * ; Division operation (FR0 = event_period)
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
ADRK #1 ; ARP=AR0, AR0->FR1, AR2->shift
;----------------------------------------------------------------------------------
SACL * ; FR1 = speed_hi = ACC low
; ARP=AR0, AR0->FR1, AR2->shift
;----------------------------------------------------------------------------------
XOR *- ; XOR with speed_hi
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
OR #0FFFFh ; load numerator low
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
; Phase 2
;----------------------------------------------------------------------------------
RPT #15 ; Repeat next instuction 16 times
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
SUBC * ; Division operation (FR0 = event_period)
; ARP=AR0, AR0->FR0, AR2->shift
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR0, AR0->FR2, AR2->shift
;----------------------------------------------------------------------------------
SACL * ; FR2 = speed_lo = ACC low
; ARP=AR0, AR0->FR2, AR2->shift
;----------------------------------------------------------------------------------
LACC *- ; ACC = speed_lo
; ARP=AR0, AR0->FR1, AR2->shift
;----------------------------------------------------------------------------------
ADDH *,AR2 ; ACC = speed_hi + speed_lo
; ARP=AR0, AR0->FR1, AR2->shift, ARP=AR2
;----------------------------------------------------------------------------------
RPT *,AR0 ; Repeat next instuction "shift" times
; ARP=AR2, AR0->FR1, AR2->shift, ARP=AR0
;----------------------------------------------------------------------------------
SFL ; Left shift in ACC
; ARP=AR0, AR0->FR1, AR2->shift
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR0, AR0->FR3, AR2->shift
;----------------------------------------------------------------------------------
SACH *+,AR2 ; FR3 = speed_prd_max
; ARP=AR0, AR0->FR4, AR2->shift, ARP=AR2
;----------------------------------------------------------------------------------
LACC #SHIFT_TOTAL ; ACC = SHIFT_TOTAL
; ARP=AR2, AR0->FR4, AR2->shift
;----------------------------------------------------------------------------------
SUB *+,AR0 ; ACC = SHIFT_TOTAL - shift
; ARP=AR2, AR0->FR4, AR2->speed_scaler, ARP=AR0
;----------------------------------------------------------------------------------
SACL *- ; FR4 = shift2 = SHIFT_TOTAL - shift
; ARP=AR0, AR0->FR3, AR2->speed_scaler
;----------------------------------------------------------------------------------
LT *+,AR2 ; TREG = speed_prd_max
; ARP=AR0, AR0->FR4, AR2->speed_scaler, ARP=AR2
;----------------------------------------------------------------------------------
MPY *+,AR0 ; PREG = speed_prd_max*speed_scaler
; ARP=AR2, AR0->FR4, AR2->speed_prd, ARP=AR0
;----------------------------------------------------------------------------------
PAC ; ACC = speed_prd_max*speed_scaler
; ARP=AR0, AR0->FR4, AR2->speed_prd
;----------------------------------------------------------------------------------
RPT *,AR2 ; Repeat next instuction "shift2" times
; ARP=AR0, AR0->FR4, AR2->speed_prd, ARP=AR2
;----------------------------------------------------------------------------------
SFL ; Left shift in ACC
; ARP=AR2, AR0->FR4, AR2->speed_prd
;----------------------------------------------------------------------------------
SACH * ; speed_prd = ACC high
; ARP=AR2, AR0->FR4, AR2->speed_prd
;----------------------------------------------------------------------------------
LT *+ ; TREG = speed_prd (Q15)
; ARP=AR2, AR0->FR4, AR2->rpm_max
;----------------------------------------------------------------------------------
MPY *+ ; PREG = speed_prd*rpm_max (Q15)
; ARP=AR2, AR0->FR4, AR2->speed_rpm
;----------------------------------------------------------------------------------
PAC ; ACC = speed_prd*rpm_max (Q15)
; ARP=AR2, AR0->FR4, AR2->speed_rpm
;----------------------------------------------------------------------------------
SACH *,1,AR1 ; speed_rpm = speed_prd*rpm_max (Q0)
; ARP=AR2, AR0->FR4, AR2->speed_rpm, ARP=AR1
;----------------------------------------------------------------------------------
__speed_prd_exit:
;; MAR *,AR1 ; can be removed if this condition is met on
; every path to this code. (i.e., ARP=AR1 here)
SBRK #(__speed_prd_framesize+1)
LAR AR0,*-
PSHD *
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -