2004 Microchip Technology Inc.
DS00908A-page 37
AN908
;
CORCON.SATDW = 1
;
; Support routines required:
;
None
; Local Stack usage:
;
None
; Registers modified:
;
w3 -> w7
; Timing:
;
20 cycles
;*******************************************************************
;
include "general.inc"
; External references
include "park.inc"
; Register usage
.equ ParmW,
w3
; Ptr to ParkParm structure
.equ Sq3W,
w4
; OneBySq3
.equ SinW,
w4
; replaces Work0W
.equ CosW,
w5
.equ IaW,
w6
; copy of qIa
.equ IalphaW,
w6
; replaces Ia
.equ IbW,
w7
; copy of qIb
.equ IbetaW,
w7
; Ibeta replaces Ib
; 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
