;无
;局部堆栈使用： ;0
;修改的寄存器： : w0-w7,AccA ;执行时间： ; 72个指令周期
;******************************************************************* ;
;=================== 代码 =====================
.section .text
;用于 CurModel的寄存器
.equ SignW, w2 ; 跟踪符号变化
.equ ShiftW, w3 ; 执行除法之前的移位位数 .equ IqW, w4 ; Q电流（ 1.15）
.equ KslipW, w5 ; Kslip常数（ 1.15） .equ ImagW, w7 ; 励磁电流（ 1.15）
.global _CurModel
.global CurModel
_CurModel: CurModel:
;; qdImag = qdImag + qKcur * (qId - qdImag) ;; 励磁电流
mov.w _CurModelParm+CurMod_qdImag,w6
mov.w _CurModelParm+CurMod_qdImag+2,w7
lac w7,A
mov.w w6,ACCALL
mov.w _ParkParm+Park_qId,w4 sub.w w4,w7,w4 ; qId-qdImagH
mov.w _CurModelParm+CurMod_qKcur,w5
mac w4*w5,A ; 将 Kcur*(Id-Imag)加到 Imag sac A,w7
mov.w ACCALL,w6
mov.w w6,_CurModelParm+CurMod_qdImag mov.w w7,_CurModelParm+CurMod_qdImag+2
;; qVelSlip = qKslip * qIq/qdImag
;; 首先将 qIqW 和 qdImagW置为正数，并将符号位存放在 SignW中 clr SignW ; 将标志符号设定为正
;; if( IqW < 0 ) => 翻转 SignW并设定 IqW = -IqW
mov.w _ParkParm+Park_qIq,IqW
cp0 IqW
bra Z,jCurModSkip
bra NN,jCurMod1
neg IqW,IqW
com SignW,SignW ;翻转符号位 jCurMod1:
;; if( ImagW < 0 ) => 翻转 SignW并设定 ImagW = -ImagW
cp0 ImagW
bra NN,jCurMod2
neg ImagW,ImagW
com SignW,SignW ;翻转符号位 jCurMod2:
;; 在 Acc A中计算 Kslip*|IqW|以保持 1.31格式
mov.w _CurModelParm+CurMod_qKslip,KslipW
mpy IqW*KslipW,A
;; 确保 denominator > numerator，否则跳过项 sac A,w0 ; 暂时的
cp ImagW,w0 ; |qdImag| - |Kslip*qIq|
bra LEU,jCurModSkip ; 跳过项： |qdImag| <= |Kslip*qIq|
;; 在 6010
;; 计算不将最高有效位直接置 1（保留符号位）的情况下，可将 ImagW移位多少位。 ;;
ff1l ImagW,ShiftW
sub.w ShiftW,#2,ShiftW ; 为将 1放入 bit 14，需要移位的位数
;; 移位：ImagW = ImagW << ShiftW sl ImagW,ShiftW,ImagW
;; 对 AccA进行移位，需要将 (-ShiftW)左移。 neg ShiftW,ShiftW
;; |Kslip*qIq| = |Kslip*qIq| << ShiftW sftac A,ShiftW
;; 执行除法操作 |qKslip*qIq|/|ImagW|。此时结果将为正且 < 1.0，同时具有最高的精度。 ;; ;;
sac A,w6 repeat #17
divf w6,ImagW ; w0 = KslipW*IqW/ImagW, w1 = remainder
;; 限制最大转差速度
mov.w _CurModelParm+CurMod_qMaxSlipVel,w1 cp w1,w0 ; qMaxSlipSpeed - | Kslip*qIq/qdImag | bra NN,jCurMod4
;; 结果太大 :用 qMaxSlipSpeed代替 mov.w w1,w0 bra jCurMod4
jCurModSkip:
;; 整个项被跳过 -将其置为 = 0 clr.w w0
jCurMod4:
;; 设定正确的符号 btsc SignW,#0 neg w0,w0
;; 用于测试
mov.w w0,_CurModelParm+CurMod_qVelSlip
;; 加入机械转速
mov.w _CurModelParm+CurMod_qVelMech,w4 add.w w0,w4,w4
mov.w w4,_CurModelParm+CurMod_qVelFlux