📄 aci_mras.asm
字号:
; update imbeta(k-1)
LACC imbeta_high,16 ; ACC high = imbeta(k)_high
ADDS imbeta_low ; ACC = imbeta(k)
SACL imbeta_old_low ; imbeta(k-1)_low = imbeta(k)_low
SACH imbeta_old_high ; imbeta(k-1)_high = imbeta(k)_high
; update imalfa(k-1)
LACC imalfa_high,16 ; ACC high = imalfa(k)_high
ADDS imalfa_low ; ACC = imalfa(k)
SACL imalfa_old_low ; imalfa(k-1)_low = imalfa(k)_low
SACH imalfa_old_high ; imalfa(k-1)_high = imalfa(k)_high
E_ALFA
; ialfa(k) - imalfa(k) = tmp1_low => Q15 - Q31 = Q15
LACC ialfa_mras,16 ; ACC high = ialfa
SUBS imalfa_low ; ACC low = ialfa_low-imalfa_low
SUBH imalfa_high ; ACC high = ialfa_high-imalfa_high
SACH tmp1_low ; tmp1_low = ialfa_high-imalfa_high (H 16-bit)
; ibeta(k) - imbeta(k) = tmp1_high => Q15 - Q31 = Q15
LACC ibeta_mras,16 ; ACC high = ibeta
SUBS imbeta_low ; ACC low = ibeta_low-imbeta_low
SUBH imbeta_high ; ACC high = ibeta_high-imbeta_high
SACH tmp1_high ; tmp1_high = ibeta_high-imbeta_high (H 16-bit)
; wr_hat(k)*imbeta(k) = tmp4 => Q15*Q31 = Q15
; check sign for "wr_hat" and "imbeta" kept in sign1
LACC wr_hat_mras ; ACC = wr_hat
XOR imbeta_high ;
SACL sign1 ; Sign (0=+,1=-) for wr_hat*imbeta(k)
; take absolute for "imbeta" and "wr_hat" (tmp5 = |wr_hat|)
LACC wr_hat_mras ; ACC = wr_hat
ABS ; ACC = |wr_hat|
SACL tmp5 ; tmp5 = |wr_hat|
LACC imbeta_high,16 ; ACC high= imbeta(k)_high
ADDS imbeta_low ; ACC = imbeta(k)
ABS
SACL imbeta_low ; imbeta(k)_low = ACC low
SACH imbeta_high ; imbeta(k)_high = ACC high
; now they're positive
LT tmp5 ; TREG = |wr_hat(k)| (Q15)
MPYU imbeta_low ; PREG = wr_hat(k)*imbeta(k)_low
SPH tmp4 ; tmp4 = PREG high
MPYU imbeta_high ; PREG = wr_hat(k)*imbeta(k)_high
PAC ; ACC = wr_hat(k)*imbeta(k)_high
ADDS tmp4 ; ACC = wr_hat(k)*imbeta(k)_high+tmp4
SACH tmp4,1 ; tmp4 = wr_hat(k)*imbeta(k) (Q15)
; check the sign condition
LACC sign1 ; ACC = sign1
BCND DONE5, GT ; Check sign1 = positive ?
LACC tmp4 ; ACC = tmp4
NEG ; Make the result negative
SACL tmp4 ; tmp4 = ACC low (Q15)
DONE5
; K3*tmp4 = tmp4 => Q8*Q15 = Q9
LT K3 ; TREG = K3 (Q8)
MPY tmp4 ; PREG = tmp4*K3 (Q23)
PAC ; ACC = tmp4*K3 (Q23)
SACH tmp4,2 ; tmp4 = tmp4*K3 (Q9)
; tmp1_low - tmp4 = tmp4 => Q15 - Q9 = Q9
LACC tmp1_low,10 ; ACC high = tmp1_low (Q9)
SUBH tmp4 ; ACC = tmp1_low-tmp4 (Q9)
SACH tmp4 ; tmp4 = tmp1_low-tmp4 (Q9)
; ealfa(k) = tmp4*K2 => Q15 = Q9*Q15
LT K2 ; TREG = K2 (Q15)
MPY tmp4 ; PREG = tmp4*K2 (Q24)
PAC ; ACC = tmp4*K2 (Q24)
SACH ealfa,7 ; ealfa = tmp4*K2 (Q15)
E_BETA
; wr_hat(k)*imalfa(k) = tmp4 => Q15*Q31 = Q15
; already took absolute for "wr_hat" kept in tmp5
; check sign for "wr_hat" and "imalfa" kept in sign2
LACC wr_hat_mras ; ACC = wr_hat
XOR imalfa_high ;
SACL sign2 ; Sign (0=+,1=-) for wr_hat*imalfa(k)
; take absolute for "imalfa" (tmp5 = |wr_hat|)
LACC imalfa_high,16 ; ACC high= imalfa(k)_high
ADDS imalfa_low ; ACC = imalfa(k)
ABS
SACL imalfa_low ; imalfa(k)_low = ACC low
SACH imalfa_high ; imalfa(k)_high = ACC high
; now they're positive
LT tmp5 ; TREG = |wr_hat(k)| (Q15)
MPYU imalfa_low ; PREG = wr_hat(k)*imalfa(k)_low
SPH tmp4 ; tmp4 = PREG high
MPYU imalfa_high ; PREG = wr_hat(k)*imalfa(k)_high
PAC ; ACC = wr_hat(k)*imalfa(k)_high
ADDS tmp4 ; ACC = wr_hat(k)*imalfa(k)_high+tmp4
SACH tmp4,1 ; tmp4 = wr_hat(k)*imalfa(k) (Q15)
; check the sign condition
LACC sign2 ; ACC = sign2
BCND DONE6, GT ; Check sign2 = positive ?
LACC tmp4 ; ACC = tmp4
NEG ; Make the result negative
SACL tmp4 ; tmp4 = ACC low (Q15)
DONE6
; K3*tmp4 = tmp4 => Q8*Q15 = Q9
LT K3 ; TREG = K3 (Q8)
MPY tmp4 ; PREG = tmp4*K3 (Q23)
PAC ; ACC = tmp4*K3 (Q23)
SACH tmp4,2 ; tmp4 = tmp4*K3 (Q9)
; tmp1_high + tmp4 = tmp4 => Q15 + Q9 = Q9
LACC tmp1_high,10 ; ACC high = tmp1_high (Q9)
ADDH tmp4 ; ACC = tmp1_high+tmp4 (Q9)
SACH tmp4 ; tmp4 = tmp1_high+tmp4 (Q9)
; ebeta(k) = tmp4*K2 => Q15 = Q9*Q15
LT K2 ; TREG = K2 (Q15)
MPY tmp4 ; PREG = tmp4*K2 (Q23)
PAC ; ACC = tmp4*K2 (Q23)
SACH ebeta,7 ; ebeta = tmp4*K2 (Q15)
Q_HAT
; q_hat(k) = ialfa(k)*ebeta(k)-ibeta(k)*ealfa(k) => Q15 = Q15*Q15-Q15*Q15
LT ibeta_mras ; TREG = ibeta(k) (Q15)
MPY ealfa ; PREG = ibeta(k)*ealfa(k) (Q30)
PAC ; ACC = ibeta(k)*ealfa(k) (Q30)
NEG ; Make result negative
LT ialfa_mras ; TREG = ialfa(k) (Q15)
MPY ebeta ; PREG = ialfa(k)*ebeta(k) (Q30)
APAC ; ACC = ialfa(k)*ebeta(k)-ibeta(k)*ealfa(k) (Q30)
SACH q_hat,1 ; q_hat = ialfa(k)*ebeta(k)-ibeta(k)*ealfa(k) (Q15)
; ------------------------ End adaptive model section ----------------------------
.endif
.if (pi_mras_)
; --------------- Start conventional PI controller section -----------------------
;=========================
;PI_MRAS:
;=========================
; tmp1 = Kp*error(k-1) => Q31 = Q15*Q15
LT Kp ; TREG = Kp (Q15)
MPY error ; PREG = Kp*error(k-1) (Q30)
PAC ; ACC = Kp*error(k-1) (Q30)
SACH tmp1_high,1 ; tmp1_high = Kp*error(k-1) (Q31)
SACL tmp1_low,1 ; tmp1_low = Kp*error(k-1) (Q31)
; error(k) = q - q_hat => Q15 = Q15 - Q15
LACC q ; ACC = q (Q15)
SUB q_hat ; ACC = q - q_hat (Q15)
SACL error ; error = q - q_hat (Q15)
; Kp + Ki = tmp2 => Q15 + Q31 = Q31
LACC Ki_high,16 ; ACC high = Ki_high
ADDS Ki_low ; ACC = Ki_low (Q31)
ADDH Kp ; ACC high = Ki_high + Kp (Q31)
SACH tmp2_high ; tmp2_high = Ki_high + Kp (Q31)
SACL tmp2_low ; tmp2_low = Ki_low + Kp (Q31)
; tmp2*error = tmp2 => Q31*Q15 = Q31
; check sign for "error" only
LACC error ; ACC = error
SACL sign1 ; Sign (0=+,1=-) for (Kp+Ki)*error
; take absolute for "error" only because "tmp2" is always positive
ABS ; ACC = |error|
SACL error ; error = ACC low
; now they're positive.
LT error ; TREG = error
MPYU tmp2_low ; PREG = tmp2_low*error
SPH tmp2_low ; tmp2_low = PREG high
MPYU tmp2_high ; PREG = tmp2_high*error
PAC ; ACC = tmp2_high*error
ADDS tmp2_low ; ACC = tmp2_high*error + tmp2_low
SACH tmp2_high,1 ; tmp2_high = ACC high (Q31)
SACL tmp2_low,1 ; tmp2_low = ACC low (Q31)
; check the sign condition
LACC sign1 ; ACC = sign1
BCND DONE7, GT ; Check sign1 = positive ?
LACC tmp2_high,16 ; ACC high= tmp2_high
ADDS tmp2_low ; ACC low = tmp2_low
NEG ; Make the result negative
SACH tmp2_high ; tmp2_high = ACC high
SACL tmp2_low ; tmp2_low = ACC low
DONE7
; tmp2 - tmp1 = tmp1_high => Q31 - Q31 = Q15
LACC tmp2_high,16 ; ACC high = tmp2_high (Q31)
ADDS tmp2_low ; ACC low = tmp2_low (Q31)
SUBS tmp1_low ; ACC = tmp2_low-tmp1_low (Q31)
SUBH tmp1_high ; ACC = tmp2_high-tmp1_high (Q31)
SACH tmp1_high ; tmp1_high = tmp2-tmp1 (Q15)
; wr_hat(k) = wr_hat(k-1) + tmp1_high => Q15 = Q15 + Q15 (update wr_hat)
LACC wr_hat_mras ; ACC = wr_hat(k-1) (Q15)
ADD tmp1_high ; ACC = wr_hat(k-1)+tmp1_high (Q15)
SACL wr_hat_mras ; wr_hat(k) = wr_hat(k-1)+tmp1_high (Q15)
.endif
; --------------- End conventional PI controller section -----------------------
; Change motor speed from pu value to rpm value (Q15 -> Q0 signed)
LT base_rpm ; TREG = base_rpm (Q3)
MPY wr_hat_mras ; PREG = base_rpm*wr_hat_mras (Q18)
PAC ; ACC = base_rpm*wr_hat_mras (Q18)
SFR ; ACC = base_rpm*wr_hat_mras (Q17)
SFR ; ACC = base_rpm*wr_hat_mras (Q16)
SACH wr_hat_rpm_mras ; wr_hat_rpm_mras = base_rpm*wr_hat_mras (Q0)
RET
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -