;
; 开始时：必须调用 InitMeasCompCurr。
; 进入时： MeasCurrParm结构必须包含 qKa和 qKb。 ; ADC通道 1和 2必须包括有符号小数值。 ; 退出时： ParkParm将包含 qIa和 qIb。 ;
;参数：
;输入参数： ;无
;返回值： ; Void
;所需的 SFR设定： ; CORCON.SATA = 0
;如果累加器可能溢出，必须设定： ; CORCON.SATDW = 1 ;
;所需的支持子程序： ;无
;局部堆栈使用： ;无
;修改的寄存器： ; w0,w1,w4,w5 ;执行时间： ; 29个周期
;*******************************************************************
global _MeasCompCurr global MeasCompCurr
_MeasCompCurr: MeasCompCurr:
;; CorrADC1 = ADCBUF1 - iOffsetHa/2^16
;; qIa = 2 * qKa * CorrADC1
mov.w _MeasCurrParm+ADC_iOffsetHa,w0 sub.w _ADCBUF1,WREG ; w0 = ADC - Offset clr.w w1
btsc w0,#15 setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKa,w4 mpy w4*w5,A sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIa
;; iOffset += (ADC-Offset)
add _MeasCurrParm+ADC_iOffsetLa mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHa
;; CorrADC2 = ADCBUF2 - iOffsetHb/2^16 ;; qIb = 2 * qKb * CorrADC2
mov.w _MeasCurrParm+ADC_iOffsetHb,w0
sub.w _ADCBUF2,WREG ; w0 = ADC -Offset clr.w w1 btsc w0,#15 setm w1
mov.w w0,w5
mov.w _MeasCurrParm+ADC_qKb,w4 mpy w4*w5,A sac A,#-1,w4
mov.w w4,_ParkParm+Park_qIb
;; iOffset += (ADC-Offset)
add _MeasCurrParm+ADC_iOffsetLb mov.w w1,w0
addc _MeasCurrParm+ADC_iOffsetHb
return
ClarkePark.s
;******************************************************************* ; ClarkePark ;
; 说明：
;计算 Clarke和 Park变换。
;假定 Cos和 Sin值在 qSin和 qCos中。 ;
; Ialpha = Ia
; Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3; ;其中 Ia+Ib+Ic = 0 ;
; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle) ; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle) ;
;该子程序同样适用于整数定标和 1.15定标格式。;
; 函数原型： ;
; void ClarkePark( void ) ;
;进入时： ParkParm结构必须包含 qSin、 qCos、;退出时： ParkParm将包含 qId和 qIq。 ;
; 参数： ;输入参数： ;返回值： ; Void
;所需的 SFR设定： ; CORCON.SATA = 0
;如果 (Ia+2*Ib)/sqrt(3)可能出现溢出，必须设定
; CORCON.SATDW = 1 ;
; 所需的支持子程序： ;无
; 局部堆栈使用： ;无
; 修改的寄存器： ; w3 -> w7 ; 执行时间：
qIa和 qIb。
; 20个周期
;******************************************************************* ;
include \ ; 外部引用
include \
; 寄存器使用
.equ ParmW, w3 ; 指向 ParkParm结构的指针 .equ Sq3W, w4 ; OneBySq3 .equ SinW, w4 ;替代 Work0W .equ CosW, w5
.equ IaW, w6 ; qIa的拷贝 .equ IalphaW, w6 ; 替代 Ia .equ IbW, w7 ; qIb的拷贝
.equ IbetaW, w7 ; 用 Ibeta替代 Ib ; 常量
equ OneBySq3, 0x49E7 ; 1/sqrt(3)，采用 1.15格式
;=================== 代码 ===================== section .text
global _ClarkePark global ClarkePark
_ClarkePark:
ClarkePark:
;; Ibeta = Ia*OneBySq3 + 2*Ib*OneBySq3;
mov.w #OneBySq3,Sq3W ; 1/sqrt(3)，采用 1.15格式 mov.w _ParkParm+Park_qIa,IaW mpy Sq3W*IaW,A
mov.w _ParkParm+Park_qIb,IbW mac Sq3W*IbW,A mac Sq3W*IbW,A
mov.w _ParkParm+Park_qIa,IalphaW mov.w IalphaW,_ParkParm+Park_qIalpha sac A,IbetaW
mov.w IbetaW,_ParkParm+Park_qIbeta
;; 已经计算 Ialpha 和 Ibeta。现在进行旋转。