📄 arithmetic.asm
字号:
sub #4624
blz yg2
lacc _current_id_sum
blz no_neg_id
splk #4624,_current_id_sum
b yg2
no_neg_id:
splk #-4624,_current_id_sum
yg2:
lacc _current_id_sum
;b Buck_dd
and #07h
add _current_pi_error3
sacl _current_pi_error3
sub #8
blz Buck_dd
sacl _current_pi_error3
lacc _current_id_sum
rpt #2
sfr
add #1
add _current_udref
sacl _current_udref
b current_pi_id_d
Buck_dd:
lacc _current_id_sum
rpt #2
sfr
add _current_udref
sacl _current_udref
current_pi_id_d:
lacc _current_id_err
sub _current_id_err_old
sacl _pi_temp
lt _pi_temp
mpy _current_kd
spm 3
pac
sfr
sfr
add _current_udref
sacl _current_udref
Buck_pi_limit_id:
lacc _current_udref
abs
sub #578
BLZ Buck_pwm_over_id ;限幅值
lacc _current_uqref
blz Buck_u_limit_id
SPLK #578,_current_udref
B Buck_pwm_over_id
Buck_u_limit_id:
SPLK #-578,_current_udref
Buck_pwm_over_id:
lacc _current_id_err
sacl _current_id_err_old
;================================================
;恢复C语言现场
;================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
;=================================================
;电流电压量坐标变换,包括CLARKE和PARK变换以及反变换
;=================================================
_current_clarke:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;========================================================================
; clarke transformation
; i_alfa=ia;
; i_beta=(1/sqrt(3))*(2*ib+ia);
;========================================================================
setc SXM
ldpk _ia
LACC _ia ;foc_isalfa=foc_ia
sacl _is_alfa ;foc_isalfa,4.12f
LACC _ib,1 ;foc_isbeta=(2*foc_ib+foc_ia)/sqrt(3)
add _ia
sacl _is_temp
lt _is_temp
mpy #sqrt3inv ;foc_sqrt3inv=(1/sqrt(3))=093dh,4.12f
spm 0
pac
sach _is_beta,4 ;foc_isbeta,4.12f
;========================================================================
; park transformation
; isd=foc_isalfa*cosTeta_cm+foc_isbeta*sinTeta_cm
; isq=-foc_isalfa*sinTeta_cm+foc_isbeta*cosTeta_cm
;========================================================================
dq_change:
spm 0
lt _is_beta
mpy _sin_value
lta _is_alfa
mpy _cos_value
mpya _sin_value
sach _isd,1
lacc #0
lt _is_beta
mpys _cos_value
apac
sach _isq,1
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
;===========================================================================
; inverse park transformation
; vsbeta_ref=vsq_ref*cosTeta_cm+vsd_ref*sinTeta_cm
; vsalfa_ref=-vsq_ref*sinTeta_cm+vsd_ref*cosTeta_cm
;===========================================================================
_park_inv:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;==================================================
setc SXM
spm 0
lacc #0
ldpk _current_udref
lt _current_udref
mpy _sin_value
lta _current_uqref
mpy _cos_value
mpya _sin_value
sach _vsbeta_ref,1
lacc #0
lt _current_udref
mpys _cos_value
apac
sach _vsalfa_ref,1
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
_Sincos:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;=================================================
spm 0
setc sxm
ldpk _thet
lacc _thet
bgz onit
add #A360
sacl _thet1
onit:
sacl _thet1
sub #A270
blz cc2
splk #-1,_s_sin
splk #1,_s_cos
sacl _ang_cos
sub #A90
neg
sacl _ang_sin
b cc5
cc2:
add #A270
sub #A180
blz cc3
splk #-1,_s_sin
splk #-1,_s_cos
sacl _ang_sin
sub #A90
neg
sacl _ang_cos
b cc5
cc3:
add #A180
sub #A90
blz cc4
splk #1,_s_sin
splk #-1,_s_cos
sacl _ang_cos
sub #A90
neg
sacl _ang_sin
b cc5
cc4:
add #A90
splk #1,_s_sin
splk #1,_s_cos
sacl _ang_sin
sub #A90
neg
sacl _ang_cos
cc5:
lacc _ang_sin
add #Stable
tblr _sin_temp
lt _s_sin
mpy _sin_temp
spl _sin_value
lacc _ang_cos
add #Stable
tblr _sin_temp
lt _s_cos
mpy _sin_temp
spl _cos_value
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
;===========================================================================
;svpwm modulation
;===========================================================================
_swpwm_modulation:
;=================================================
;C语言调用保护
;=================================================
; POPD *+
; SAR AR0,*+
; SAR AR1,*
; LARK AR0,#1
; LAR AR0,*0+,AR0
;================================================
setc SXM
ldpk _vsbeta_ref
lacc _vsbeta_ref
sacl _vref1
lt _vsalfa_ref
mpy #sqrt32
spm 0
pac
sach _vref4,4
lacc _vsbeta_ref
sfr
neg
sacl _vref5
add _vref4
sacl _vref2
lacc _vref5
sub _vref4
sacl _vref3
lacc _vref1
sacl _x
lacc _vref4
sub _vref5
sacl _y
lacc _vref4
add _vref5
neg
sacl _z
lacc _vref1
bgz ua
splk #0,_a
b uaon
ua:
splk #1,_a
uaon:
lacc _vref2
bgz ub
splk #0,_b
b ubon
ub:
splk #1,_b
ubon:
lacc _vref3
bgz uc
splk #0,_c
b ucon
uc:
splk #1,_c
ucon:
lacc _c,2
add _b,1
add _a
sacl _sector
sub #1
bz f_sector1
sub #1
bz f_sector2
sub #1
bz f_sector3
sub #1
bz f_sector4
sub #1
bz f_sector5
sub #1
bz f_sector6
b cmp_ok
f_sector1:
lacc _z
sacl _t1
lacc _y
sacl _t2
b t1t2out
f_sector2:
lacc _y
sacl _t1
lacc _x
neg
sacl _t2
b t1t2out
f_sector3:
lacc _z
neg
sacl _t1
lacc _x
sacl _t2
b t1t2out
f_sector4:
lacc _x
neg
sacl _t1
lacc _z
sacl _t2
b t1t2out
f_sector5:
lacc _x
sacl _t1
lacc _y
neg
sacl _t2
b t1t2out
f_sector6:
lacc _y
neg
sacl _t1
lacc _z
neg
sacl _t2
t1t2out:
b t1t2_ok
lt _t1
mpy #pwmprd
spm 1
pac
sach _t1,3
lt _t2
mpy #pwmprd
spm 1
pac
sach _t2 ,3
t1t2_ok:
lacc _t1
bgz t1_ok
splk #0,_t1
t1_ok:
lacc _t2
bgz t2_ok
splk #0,_t2
t2_ok:
lacc _t1
add _t2
sacl _tmp
sub #pwmprd
blez nosaturation
lacc #pwmprd,15
rpt #15
subc _tmp
sacl _tmp
lt _tmp
mpy _t1
spm 0
pac
sach _t1,1
mpy _t2
pac
sach _t2,1
nosaturation:
lacc #pwmprd
sub _t1
sub _t2
sfr
sacl _taon
add _t1
sacl _tbon
add _t2
sacl _tcon
lacc _sector
sub #1
bnz nosect1
lacc _tbon
sacl _cmp1
lacc _taon
sacl _cmp2
lacc _tcon
sacl _cmp3
b cmp_ok
nosect1:
sub #1
bnz nosect2
lacc _taon
sacl _cmp1
lacc _tcon
sacl _cmp2
lacc _tbon
sacl _cmp3
b cmp_ok
nosect2:
sub #1
bnz nosect3
lacc _taon
sacl _cmp1
lacc _tbon
sacl _cmp2
lacc _tcon
sacl _cmp3
b cmp_ok
nosect3:
sub #1
bnz nosect4
lacc _tcon
sacl _cmp1
lacc _tbon
sacl _cmp2
lacc _taon
sacl _cmp3
b cmp_ok
nosect4:
sub #1
bnz nosect5
lacc _tcon
sacl _cmp1
lacc _taon
sacl _cmp2
lacc _tbon
sacl _cmp3
b cmp_ok
nosect5:
lacc _tbon
sacl _cmp1
lacc _tcon
sacl _cmp2
lacc _taon
sacl _cmp3
cmp_ok:
lacc #pwmprd ;反向逻辑需要进行以下修正
sub _cmp1
sacl _cmp1
lacc #pwmprd
sub _cmp2
sacl _cmp2
lacc #pwmprd
sub _cmp3
sacl _cmp3
;=================================================
;调用完毕，跳出
;=================================================
; SBRK #2
; LAR AR0,*-
; PSHD *
ret
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -