📄 bldc3_21.asm
字号:
splk #09000h, pid_min_reg2 ;Q15
SPLK #0080h,K0_reg2 ;Q9
SPLK #0140h,K1_reg2 ;Q13
SPLK #0506h,Kc_reg2 ;Q13
;------------------------------------------------------------------------------------------
;SYSTEM INCREMENTAL BUILD OPTIONS - Initialization
;------------------------------------------------------------------------------------------
.if (phase1_inc_build)
;DAC pointer init
ldp #DAC_IPTR0
SPLK #ig_out, DAC_IPTR0
.endif
;-------------------------------------------------------------------------------------------
.if (phase2_inc_build)
;ADC gain & offset adjustment
ldp #C1_gain
SPLK #0800h, C1_gain ;gain = 0.125 (Q13)
SPLK #0800h, C2_gain ;gain = 0.125 (Q13)
SPLK #0800h, C3_gain ;gain = 0.125 (Q13)
SPLK #1FFFh, C4_gain ;gain = 1.0 (Q13)
;DAC pointer init
ldp #DAC_IPTR0
SPLK #C1_out, DAC_IPTR0
SPLK #C2_out, DAC_IPTR1
SPLK #C3_out, DAC_IPTR2
SPLK #C4_out, DAC_IPTR3
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase3_inc_build)
;ADC gain & offset adjustment
ldp #C1_gain
SPLK #0800h, C1_gain ;gain = 0.125 (Q13)
SPLK #0800h, C2_gain ;gain = 0.125 (Q13)
SPLK #0800h, C3_gain ;gain = 0.125 (Q13)
SPLK #1FFFh, C4_gain ;gain = 1.0 (Q13)
;DAC pointer init
ldp #DAC_IPTR0
SPLK #cmtn_trig, DAC_IPTR0
SPLK #neutral, DAC_IPTR1
SPLK #zc_trig, DAC_IPTR2
SPLK #debug_Bemf, DAC_IPTR3
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase4_inc_build)
;ADC gain & offset adjustment
ldp #C1_gain
SPLK #0800h, C1_gain ;gain = 0.125 (Q13)
SPLK #0800h, C2_gain ;gain = 0.125 (Q13)
SPLK #0800h, C3_gain ;gain = 0.125 (Q13)
SPLK #1FFFh, C4_gain ;gain = 1.0 (Q13)
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase5_inc_build)
;DAC pointer init
ldp #DAC_IPTR0
SPLK #cmtn_trig, DAC_IPTR0
SPLK #debug_Bemf, DAC_IPTR1
;ADC gain & offset adjustment
ldp #C1_gain
SPLK #0800h, C1_gain ;gain = 0.125 (Q13)
SPLK #0800h, C2_gain ;gain = 0.125 (Q13)
SPLK #0800h, C3_gain ;gain = 0.125 (Q13)
SPLK #1FFFh, C4_gain ;gain = 1.0 (Q13)
POINT_B0
SPLK #0065h, current_set
.endif
;-------------------------------------------------------------------------------------------
;===========================================================================================
MAIN: ;Main system background loop
;===========================================================================================
M_1 B M_1
;===========================================================================================
;-------------------------------------------------------------------------------------------
;T2PR Interrupt Service Routine
;-------------------------------------------------------------------------------------------
T2_PERIOD_ISR:
;Context save regs
MAR *,AR1 ;AR1 is stack pointer
MAR *+ ;skip one position
SST #1, *+ ;save ST1
SST #0, *+ ;save ST0
SACH *+ ;save acc high
SACL *+ ;save acc low
SAR AR6,*+ ;save AR6
SAR AR5,* ;save AR5
POINT_EV
SPLK #0FFFFh,IFRB ; Clear all Group B interrupt flags (T2 ISR)
;Start main section of ISR
POINT_B0
; verifying the ISR
LACC isr_ticker
ADD #1
SACL isr_ticker
;-------------------------------------------------------------------------------------------
;Rotor alignment phase
;----------------------------------------
POINT_B0
lacc align_flag
bcnd RUN_MODE, EQ
ldp #m6_cntr
SPLK #0, m6_cntr
ldp #cmtn_ptr_bd
SPLK #0, cmtn_ptr_bd
CALL BLDC_3PWM_DRV
POINT_B0
lacc v_timer
sub #07FFEh ;wait for a "while"
bcnd updat_v_timer, LT ;If not time out, stay in alignment mode
lacc loop_cnt
sub #LOOP_CNT_MAX
bcnd CLR_ALGN_FLAG,EQ
lacc loop_cnt
add #1
sacl loop_cnt
splk #0, v_timer
B updat_v_timer
CLR_ALGN_FLAG
splk #0, align_flag
RUN_MODE
;SYSTEM INCREMENTAL BUILD OPTIONS
;-------------------------------------------------------------------------------------------
.if (phase1_inc_build)
;Open loop commutation control
;-----------------------------
CALL RMP3CNTL
POINT_B0
bldd #rmp3_out, cmtn_period_setpt
ldp #ig_period
bldd #rmp3_out, ig_period
CALL IMPULSE
ldp #m6_trig_in
bldd #ig_out, m6_trig_in
CALL MOD6_CNT
ldp #cmtn_ptr_bd
bldd #m6_cntr, cmtn_ptr_bd ;Input to BLDC_PWM_DRV
ldp #cmtn_ptr_ct
bldd #m6_cntr, cmtn_ptr_ct ;Input to COMMUTATION_TRIGGER
CALL BLDC_3PWM_DRV
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase2_inc_build)
;Open loop commutation control
;-----------------------------
CALL RMP3CNTL
POINT_B0
bldd #rmp3_out, cmtn_period_setpt
ldp #ig_period
bldd #rmp3_out, ig_period
CALL IMPULSE
ldp #m6_trig_in
bldd #ig_out, m6_trig_in
CALL MOD6_CNT
ldp #cmtn_ptr_bd
bldd #m6_cntr, cmtn_ptr_bd ;Input to BLDC_PWM_DRV
ldp #cmtn_ptr_ct
bldd #m6_cntr, cmtn_ptr_ct ;Input to COMMUTATION_TRIGGER
CALL BLDC_3PWM_DRV
CALL ADC04U_DRV
ldp #Va
bldd #C1_out, Va
bldd #C2_out, Vb
bldd #C3_out, Vc
CALL COMTN_TRIG
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase3_inc_build)
;Open loop commutation control
;-----------------------------
CALL RMP3CNTL
POINT_B0
bldd #rmp3_out, cmtn_period_setpt
ldp #ig_period
bldd #rmp3_out, ig_period
CALL IMPULSE
ldp #m6_trig_in
bldd #ig_out, m6_trig_in
CALL MOD6_CNT
ldp #cmtn_ptr_bd
bldd #m6_cntr, cmtn_ptr_bd ;Input to BLDC_PWM_DRV
ldp #cmtn_ptr_ct
bldd #m6_cntr, cmtn_ptr_ct ;Input to COMMUTATION_TRIGGER
CALL BLDC_3PWM_DRV
CALL ADC04U_DRV
ldp #Va
bldd #C1_out, Va
bldd #C2_out, Vb
bldd #C3_out, Vc
CALL COMTN_TRIG
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase4_inc_build)
;Closed loop commutation control
;-----------------------------
lacc sp_up_done_flg
bcnd CLSD_LOOP, NEQ ;If set go into closed loop mode
CALL RMP3CNTL
POINT_B0
bldd #rmp3_done_flg, sp_up_done_flg
bldd #rmp3_out, cmtn_period_setpt
ldp #ig_period
bldd #rmp3_out, ig_period
CALL IMPULSE
ldp #m6_trig_in
bldd #ig_out, m6_trig_in
B OPEN_LOOP
CLSD_LOOP
ldp #m6_trig_in
bldd #cmtn_trig, m6_trig_in
OPEN_LOOP
CALL MOD6_CNT
ldp #cmtn_ptr_bd
bldd #m6_cntr, cmtn_ptr_bd ;Input to BLDC_PWM_DRV
ldp #cmtn_ptr_ct
bldd #m6_cntr, cmtn_ptr_ct ;Input to COMMUTATION_TRIGGER
;------------------------------------------------------------------
;Implements ramp control of D_func during open current loop control
ldp #rmp2_desired
bldd #D_func_desired, rmp2_desired
CALL RMP2CNTL
ldp #D_func
bldd #rmp2_out, D_func
;------------------------------------------------------------------
CALL BLDC_3PWM_DRV
CALL ADC04U_DRV
ldp #Va
bldd #C1_out, Va
bldd #C2_out, Vb
bldd #C3_out, Vc
CALL COMTN_TRIG
.endif
;-------------------------------------------------------------------------------------------
;-------------------------------------------------------------------------------------------
.if (phase5_inc_build)
;Closed loop commutation control
;-----------------------------
lacc sp_up_done_flg
bcnd CLSD_LOOP, NEQ ;If set go into closed loop mode
CALL RMP3CNTL
POINT_B0
bldd #rmp3_done_flg, sp_up_done_flg
bldd #rmp3_out, cmtn_period_setpt
ldp #ig_period
bldd #rmp3_out, ig_period
CALL IMPULSE
ldp #m6_trig_in
bldd #ig_out, m6_trig_in
B OPEN_LOOP
CLSD_LOOP
ldp #m6_trig_in
bldd #cmtn_trig, m6_trig_in
OPEN_LOOP
CALL MOD6_CNT
ldp #cmtn_ptr_bd
bldd #m6_cntr, cmtn_ptr_bd ;Input to BLDC_PWM_DRV
ldp #cmtn_ptr_ct
bldd #m6_cntr, cmtn_ptr_ct ;Input to COMMUTATION_TRIGGER
ldp #pid_out_reg2
bldd #C4_out, pid_fb_reg2
bldd #current_set, pid_ref_reg2
CALL PID_REG2
;------------------------------------------------------------------
;Uses PID result when the flag is set, i.e., I_loop_flg=1
;Implements ramp control of D_func during open current loop
;control, i.e., I_loop_flg=0
POINT_B0
lacc I_loop_flg
bnz SKIP_D_FUNC_RMP
ldp #rmp2_desired
bldd #D_func_desired, rmp2_desired
CALL RMP2CNTL
ldp #D_func
bldd #rmp2_out, D_func
B SKIP_PID_DATA
;------------------------------------------------------------------
SKIP_D_FUNC_RMP
ldp #D_func
bldd #pid_out_reg2, D_func
SKIP_PID_DATA
CALL BLDC_3PWM_DRV
CALL ADC04U_DRV
ldp #Va
bldd #C1_out, Va
bldd #C2_out, Vb
bldd #C3_out, Vc
CALL COMTN_TRIG
ldp #BC_IN
bldd #rev_period, BC_IN
CALL BC_CALC
ldp #event_period
bldd #BC_OUT,event_period
CALL SPEED_REV_PRD
.endif
;-------------------------------------------------------------------------------------------
;Update Virtual Timer
;--------------------------
updat_v_timer
POINT_B0
LACC v_timer ;Inc virtual timer
ADD #1
AND #07FFFh ;Force 15 bit wrap around
SACL v_timer ;Save
; CALL DAC_VIEW_DRV
CALL DATA_LOG
;-------------------------------------------------------------------------------------------
;End main section of ISR
;-------------------------------------------------------------------------------------------
;Restore Context
END_ISR:
MAR *, AR1 ;make stack pointer active
LAR AR5,*- ;Restore AR5
LAR AR6,*- ;Restore AR6
LACL *- ;Restore Acc low
ADDH *- ;Restore Acc high
LST #0, *- ;load ST0
LST #1, *- ;load ST1
CLRC INTM
RET
;===========================================================================================
; I S R - PHANTOM
;
; Description: Dummy ISR, used to trap spurious interrupts.
;
;===========================================================================================
PHANTOM B PHANTOM
PHANTOM1 B PHANTOM1
PHANTOM2 B PHANTOM2
PHANTOM3 B PHANTOM3
PHANTOM4 B PHANTOM4
PHANTOM5 B PHANTOM5
PHANTOM6 B PHANTOM6
;===========================================================================================
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -