Microchip Technology AN908, Using Manual

Page: 53 / 76


2004 Microchip Technology Inc. DS00908A-page 53
AN908
jCurMod1:
;; if( ImagW < 0 ) => toggle SignW and set ImagW = -ImagW
cp0 ImagW
bra NN,jCurMod2
neg ImagW,ImagW
com SignW,SignW ; toggle sign
jCurMod2:
;; Calculate Kslip*|IqW| in Acc A to maintain 1.31
mov.w _CurMoCurMod_qKslip,KslipW
mpy IqW*KslipW,A
;; Make sure denominator is > numerator else skip term
sac A,w0 ; temporary
cp ImagW,w0 ; |qdImag| - |Kslip*qIq|
bra LEU,jCurModSkip ; skip term: |qdImag| <= |Kslip*qIq|
;; This will not be required for later releases of the 6010 <SILICON_ERR>
clr.w ShiftW
;; Calculate how many places ImagW can be shifted without putting
;; a one in the msb location (preserves sign)
ff1l ImagW,ShiftW
sub.w ShiftW,#2,ShiftW ; # shifts necessary to put 1 in bit 14
;; Shift: ImagW = ImagW << ShiftW
sl ImagW,ShiftW,ImagW
;; Shift AccA, Requires (-ShiftW) to shift left.
neg ShiftW,ShiftW
;; |Kslip*qIq| = |Kslip*qIq| << ShiftW
sftac A,ShiftW
;; Do divide of |qKslip*qIq|/|ImagW|. We know at this point that the
;; results will be positive and < 1.0. We also know that we have maximum
;; precision.
sac A,w6
repeat #17
divf w6,ImagW ; w0 = KslipW*IqW/ImagW, w1 = remainder
;; Limit maximum slip speed
mov.w _CurMoCurMod_qMaxSlipVel,w1
cp w1,w0 ; qMaxSlipSpeed - | Kslip*qIq/qdImag |
bra NN,jCurMod4
;; result too large: replace it with qMaxSlipSpeed
mov.w w1,w0
bra jCurMod4
jCurModSkip:
;; term skipped entirely - set it = 0
clr.w w0