AN908
DS00908A-page 50
2004 Microchip Technology Inc.
; Constants
.equ OneBySq3,0x49E7
; 1/sqrt(3) in 1.15 format
;=================== CODE =====================
.section
.text
.global
_ClarkePark
.global
ClarkePark
_ClarkePark:
ClarkePark:
;; Ibeta = Ia*On 2*Ib*OneBySq3;
mov.w
#OneBySq3,Sq3W
; 1/sqrt(3) in 1.15 format
mov.w _PPark_qIa,IaW
mpy
Sq3W*IaW,A
mov.w
_PPark_qIb,IbW
mac
Sq3W*IbW,A
mac
Sq3W*IbW,A
mov.w
_PPark_qIa,IalphaW
mov.w
IalphaW,_PPark_qIalpha
sac
A,IbetaW
mov.w
IbetaW,_PPark_qIbeta
;; Ialpha and Ibeta have been calculated. Now do rotation.
;; Get qSin, qCos from ParkParm structure
mov.w
_PPark_qSin,SinW
mov.w
_PPark_qCos,CosW
;; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle)
mpy
SinW*IbetaW,A
; Ibeta*qSin -> A
mac
CosW*IalphaW,A
; add Ialpha*qCos to A
mov.w
#_PPark_qId,ParmW
sac
A,[ParmW++]
; store to qId, inc ptr to qIq
;; Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
mpy
CosW*IbetaW,A
; Ibeta*qCos -> A
msc
SinW*IalphaW,A
; sub Ialpha*qSin from A
sac
A,[ParmW]
; store to qIq
return
.end
