📄 aci_fe.asm
字号:
;=====================================================================================
; File name: ACI_FE.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: Flux Estimator of Induction Motor
;
; Note 0000h < theta_r_fe < 7FFFh
; (i.e. equivalent to 0 < theta_r_fe < 360 deg)
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 02-08-2001 Release Rev 3.00
; 12-15-2005 Release Rev 3.11: Change the sign in current model equation
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx. (Peripheral Independent).
;================================================================================
; Routine Name: aci_fe_calc Type: C Callable
;
; C prototype : void aci_fe_calc(struct ACIFE *p);
;
; The struct object is defined in the header file "aci_fe.h" as follows:
;
; typedef struct { int theta_r_fe; /* Output: Rotor flux angle (Q15) */
; int i_qs_fe; /* Input: Stationary q-axis stator current (Q15) */
; int i_ds_fe; /* Input: Stationary d-axis stator current (Q15) */
; int K1_fe; /* Parameter: Constant using in current model (Q15) */
; int flx_dr_e /* Variable: Rotating d-axis rotor flux (current model) (Q15) */
; int K2_fe; /* Parameter: Constant using in current model (Q15) */
; int flx_qr_s; /* Variable: Stationary q-axis rotor flux (current model) (Q15) */
; int flx_dr_s; /* Variable: Stationary d-axis rotor flux (current model) (Q15) */
; int K3_fe; /* Parameter: Constant using in stator flux computation (Q15) */
; int K4_fe; /* Parameter: Constant using in stator flux computation (Q15) */
; int flx_ds_s; /* Variable: Stationary d-axis stator flux (current model) (Q15) */
; int flx_qs_s; /* Variable: Stationary q-axis stator flux (current model) (Q15) */
; int psi_ds_fe; /* Variable: Stationary d-axis stator flux (voltage model) (Q31) */
; int Kp_fe; /* Parameter: PI proportionnal gain (Q15) */
; int ui_lo_ds; /* Variable: Stationary d-axis integral term (Q30) */
; int ui_hi_ds; /* Variable: Stationary d-axis integral term (Q30) */
; int ucomp_ds; /* Variable: Stationary d-axis compensated voltage (Q15) */
; int Ki_fe; /* Parameter: PI integral gain (Q31-16bit) */
; int psi_qs_fe; /* Variable: Stationary q-axis stator flux (voltage model) (Q31) */
; int ui_lo_qs; /* Variable: Stationary q-axis integral term (Q30) */
; int ui_hi_qs; /* Variable: Stationary q-axis integral term (Q30) */
; int ucomp_qs; /* Variable: Stationary q-axis compensated voltage (Q15) */
; int emf_ds /* Variable: Stationary d-axis back emf (Q15) */
; int u_ds_fe; /* Input: Stationary d-axis stator voltage (Q15) */
; int K5_fe; /* Parameter: Constant using in back emf computation (Q15) */
; int K6_fe; /* Parameter: Constant using in back emf computation (Q15) */
; int psi_ds_lo; /* Variable: Stationary d-axis stator flux (voltage model) (Q31) */
; int emf_qs /* Variable: Stationary q-axis back emf (Q15) */
; int u_qs_fe; /* Input: Stationary q-axis stator voltage (Q15) */
; int psi_qs_lo; /* Variable: Stationary q-axis stator flux (voltage model) (Q31) */
; int K8_fe; /* Parameter: Constant using in rotor flux computation (Q15) */
; int K7_fe; /* Parameter: Constant using in rotor flux computation (Q14) */
; int psi_dr_fe; /* Output: Stationary d-axis estimated rotor flux (Q15) */
; int psi_qr_fe; /* Output: Stationary q-axis estimated rotor flux (Q15) */
; int (*calc)(); /* Pointer to calculation function */
; } ACIFE;
;
;
; Frame Usage Details:
; step | a | b | c | d
;____________|_____________|______________|______________|_____________
; AR0 | tmp1_fe(1) | ptr_fe(1) | |
; AR1 | cos_fe(1,3) | error_ds(5) | error_qs(5) | ptr_fe(8)
; AR2 | sin_fe(1,3) | tmp1_fe(5) | tmp1_fe(5) | psi_qdr(8)
; AR3 | i_ds_e(1) | tmp1_fe(6) | psi_dr_p(8) |
; AR4 | i_qs_e(1) | psi_qr_p(8) | |
;
;================================================================================
.def _aci_fe_calc
.ref SINTAB_360 ; Sine look-up table (256 entries)
.ref ATANTAB_45 ; ArcTan look-up table (256 entries)
;================================================================================
__aci_fe_calc_framesize .set 0005h
;================================================================================
_aci_fe_calc:
; Assume now ARP=AR1
POPD *+ ; Keep return address
SAR AR0,*+ ; Keep old frame pointer (FP)
SAR AR1,* ; Keep old stack pointer (SP)
LARK AR0,__aci_fe_calc_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->theta_r_fe, AR2->theta_r_fe
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SETC SXM ; Turn sign extension mode on
; ARP=AR2, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SETC OVM ; Set overflow mode
; ARP=AR2, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SPM 0 ; Reset product mode
; ARP=AR2, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
; (1) Park transformation on the measured stator current
;----------------------------------------------------------------------------------
LACC *,AR0 ; ACC = theta_r_fe (Q15)
; ARP=AR2, AR0->FR0, AR2->theta_r_fe, ARP=AR0
;----------------------------------------------------------------------------------
ADD #8192 ; ACC = theta_r_fe + 90 deg (cos(A)=sin(A+90)) (Q15)
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
AND #07FFFh ; Force positive wrap-around
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACL * ; FR0 = tmp1_fe = theta_r_fe + 90 deg
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
LACC *,9 ; ACC = (theta_r_fe+90 deg)/128
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
SACH * ; FR0 = ptr_fe = Table pointer for cos
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
LACC #SINTAB_360 ; ACC = &SINTAB_360
; ARP=AR0, AR0->FR0, AR2->theta_r_fe
;----------------------------------------------------------------------------------
ADD *+ ; ACC = &SINTAB_360 + ptr_fe
; ARP=AR0, AR0->FR1, AR2->theta_r_fe
;----------------------------------------------------------------------------------
TBLR *-,AR2 ; FR1 = cos_fe = cos(theta_e_fe) (Q15)
; ARP=AR0, AR0->FR0, AR2->theta_r_fe, ARP=AR2
;----------------------------------------------------------------------------------
LACC *+,9,AR0 ; ACC = (theta_r_fe)/128
; ARP=AR2, AR0->FR0, AR2->i_qs_fe, ARP=AR0
;----------------------------------------------------------------------------------
SACH * ; FR0 = ptr_fe = Table pointer for sin
; ARP=AR0, AR0->FR0, AR2->i_qs_fe
;----------------------------------------------------------------------------------
LACC #SINTAB_360 ; ACC = &SINTAB_360
; ARP=AR0, AR0->FR0, AR2->i_qs_fe
;----------------------------------------------------------------------------------
ADD * ; ACC = &SINTAB_360 + ptr_fe
; ARP=AR0, AR0->FR0, AR2->i_qs_fe
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR0, AR0->FR2, AR2->i_qs_fe
;----------------------------------------------------------------------------------
TBLR *,AR2 ; FR2 = sin_fe = sin(theta_e_fe) (Q15)
; ARP=AR0, AR0->FR2, AR2->i_qs_fe, ARP=AR2
;----------------------------------------------------------------------------------
; ***** d-axis *****
SPM 1 ; ARP=AR2, AR0->FR2, AR2->i_qs_fe
;----------------------------------------------------------------------------------
ZAC ; Reset ACC
; ARP=AR2, AR0->FR2, AR2->i_qs_fe
;----------------------------------------------------------------------------------
LT *+,AR0 ; TREG = i_qs_fe (Q15)
; ARP=AR2, AR0->FR2, AR2->i_ds_fe, ARP=AR0
;----------------------------------------------------------------------------------
MPY *-,AR2 ; PREG = i_qs_fe*sin_fe (Q30)
; ARP=AR0, AR0->FR1, AR2->i_ds_fe, ARP=AR2
;----------------------------------------------------------------------------------
LTA *-,AR0 ; ACC = i_qs_fe*sin_fe (Q31), TREG = i_ds_fe (Q15)
; ARP=AR2, AR0->FR1, AR2->i_qs_fe, ARP=AR0
;----------------------------------------------------------------------------------
MPY *+ ; PREG = i_ds_fe*cos_fe (Q30)
; ARP=AR0, AR0->FR2, AR2->i_qs_fe
;----------------------------------------------------------------------------------
MPYA *+ ; ACC = i_qs_fe*sin_fe+i_ds_fe*cos_fe (Q31)
; PREG = i_ds_fe*sin_fe (Q30)
; ARP=AR0, AR0->FR3, AR2->i_qs_fe
;----------------------------------------------------------------------------------
SACH *,AR2 ; FR3 = i_ds_e = i_qs_fe*sin_fe+i_ds_fe*cos_fe (Q15)
; ARP=AR0, AR0->FR3, AR2->i_qs_fe, ARP=AR2
;----------------------------------------------------------------------------------
; ***** q-axis *****
LACC #0 ; Clear ACC
; ARP=AR2, AR0->FR3, AR2->i_qs_fe
;----------------------------------------------------------------------------------
LT *,AR0 ; TREG = i_qs_fe (Q15)
; ARP=AR2, AR0->FR3, AR2->i_qs_fe, ARP=AR0
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR0, AR0->FR1, AR2->i_qs_fe
;----------------------------------------------------------------------------------
MPYS * ; ACC = -i_ds_fe*sin_fe (Q31), PREG = i_qs_fe*cos_fe (Q30)
; ARP=AR0, AR0->FR1, AR2->i_qs_fe
;----------------------------------------------------------------------------------
APAC ; ACC = i_qs_fe*cos_fe-i_ds_fe*sin_fe (Q31)
; ARP=AR0, AR0->FR1, AR2->i_qs_fe
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR4, AR2->i_qs_fe
;----------------------------------------------------------------------------------
SACH *-,AR2 ; FR4 = i_qs_e = i_qs_fe*cos_fe-i_ds_fe*sin_fe (Q15)
; ARP=AR0, AR0->FR3, AR2->i_qs_fe, ARP=AR2
;----------------------------------------------------------------------------------
; (2) The current model section (Classical Rotor Flux Vector Control Eq.)
;----------------------------------------------------------------------------------
SPM 0 ; Reset Product mode
; ARP=AR2, AR0->FR3, AR2->i_qs_fe
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR3, AR2->K1_fe
;----------------------------------------------------------------------------------
LT *+ ; TREG = K1_fe (Q15)
; ARP=AR2, AR0->FR3, AR2->flx_dr_e
;----------------------------------------------------------------------------------
MPY *+ ; PREG = K1_fe*flx_dr_e (Q30)
; ARP=AR2, AR0->FR3, AR2->K2_fe
;----------------------------------------------------------------------------------
PAC ; ACC = K1_fe*flx_dr_e (Q30)
; ARP=AR2, AR0->FR3, AR2->K2_fe
;----------------------------------------------------------------------------------
LT *-,AR0 ; TREG = K2_fe (Q15)
; ARP=AR2, AR0->FR3, AR2->flx_dr_e, ARP=AR0
;----------------------------------------------------------------------------------
MPY *-,AR2 ; PREG = K2_fe*i_ds_e (Q30)
; ARP=AR0, AR0->FR2, AR2->flx_dr_e, ARP=AR2
;----------------------------------------------------------------------------------
APAC ; ACC = K1_fe*flx_dr_e + K2_fe*i_ds_e (Q30)
; ARP=AR2, AR0->FR2, AR2->flx_dr_e
;----------------------------------------------------------------------------------
SACH *,1 ; flx_dr_e = K1_fe*flx_dr_e + K2_fe*i_ds_e (Q15)
; ARP=AR2, AR0->FR2, AR2->flx_dr_e
;----------------------------------------------------------------------------------
; (3) Inverse park transformation on the rotor flux from the current model
;----------------------------------------------------------------------------------
; ***** q-axis *****
LT *,AR0 ; TREG = flx_dr_e (Q15)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -