spmc65_sfr.asm

sunplus e_bike demo

 		cmp		G_Hall_old
		beq		L_MotorStopUp_Relay
		lda		G_Hall_Temp
		sta		G_Hall_old		
		rts
		
L_MotorStopUp_Relay:						; motor stop relay
	  	lda    G_Hall_Temp 
      	sta    G_Hall_old                         
      	tst    G_BrakeError_Buf,CB_MotorStopUp_Flag         
     	beq    L_MotorStopUp_Relay0 
;
      	inc    G_Reverse_Cnt 
      	lda    G_Reverse_Cnt 
      	cmp    #C_Reverse_Times                              
      	bcc    L_NotClear         
;
      	lda    #0 
      	sta    G_Reverse_Cnt         
     	sta    G_SpeedMode 
      	sta    G_5S_Count 
      	clr    G_BrakeError_Buf,CB_MotorStopUp_Flag 
;		TurnOnMotor
      	
L_MotorStopUp_Relay0:  
		lda		#0
		sta		G_Reverse_Cnt
		
L_NotClear:		     
      	rts          	
;



			
			
F_Speed_Cap:		 
		lda  	P_TMR3_Cap	
		sta		G_POS_fifo
		lda 	P_TMR3_CapHi		
		sta		G_POS_fifo+1
		lda		#0
		sta		G_Hall1_Num	
		sta     G_MotorStopUp_Time
		sta		G_MotorStop_Cnt
L_Speed_Cap_End:			 	
		rts
;							
			
F_EBS_Chongdian_Control:

		lda		P_TMR0_1_Ctrl0
		and		#f8h
		sta		P_TMR0_1_Ctrl0				;Close timer0
	
		clr		P_INT_Ctrl1,CB_INT_T0OIE	;disable t0 interrupt
		
		tst		G_BrakeError_Buf,CB_EbsPwmLevel_Flag ;
		beq		L_PWM_Level_0
		
L_PWM_Level_1:	
		lda		#256-160					;10khz
		clc
		adc		G_PWM0_Duty
		sta		P_TMR0_Preload
	
		jsr		F_Open_Time0				;open time0 int
		
		jsr		F_Close_UN_VN_WN			;close vn,vn,wn
		
		clr		G_BrakeError_Buf,CB_EbsPwmLevel_Flag
		jmp		L_PWM0_NEXT_Level
		
L_PWM_Level_0:
		lda		#ffh
		sec
		sbc		G_PWM0_Duty												
		sta		P_TMR0_Preload				;(200) 100u 
																	
		jsr		F_Open_Time0				;open time0 int
				
		jsr		F_Close_UP_VP_WP			;close up,vp,wp
		
		jsr		F_Open_UN_VN_WN				;open un,vn,wn
	
		set		G_BrakeError_Buf,CB_EbsPwmLevel_Flag

L_PWM0_NEXT_Level:
		rts
;
F_HallMotorDrv:	

		lda		G_RunState
;		jmp		L_if_run
		cmp		#C_Phase_Halt	;#$00
		bne		L_if_setup

		TurnOffPWM				;Timer2 is disable task mode
		clr		P_IOB_Data, PWM	;#$03
		
		tst		G_SYS_Control_Flag,CB_ChargeMode_Flag
		bne		L_Not_TurnOffIPM_0		;To charge mode process  
		
		TurnOffIPM 				    	
						
L_Not_TurnOffIPM_0:	
;
		lda		#$FFh					; initial phasecmd
		sta		G_PhaseCmd		
		lda		#$0Fh
		sta		G_PhaseCmd+1
		
		lda		#$FFh
		sta		G_PhaseCtrlFilter		; initial PhaseCtrlFilter
		lda		#$0Fh
		sta		G_PhaseCtrlFilter+1
;
		tst		G_Monitor, CB_MotorRun_Flag
		bne		L_to_setup				;is run? 
		rts
;
L_to_setup:
		lda		#C_Phase_Setup
		sta		G_RunState
		rts	

;			
L_if_setup:
		lda		G_RunState
		cmp		#C_Phase_Setup		;#$1
		bne		L_if_run
;

		lda		#$FE					; set higher 4-bit value of period and duty 													
		sta		P_TMR1_DutyPeriod		

		lda		#$00					; set low byte value of period		
		sta		P_TMR1_PWMPeriod		; PWM frequency = 8.0MHz / (0xFFF - 0xE00) = 15.625 KHz

		lda		#$FF					; initial duty ratio = 0  
		sta		P_TMR1_PWMDuty	 		
;;;hlp 05/31	
		lda		#C_Phase_Run
		sta		G_RunState				; initial complete
;
		rts
;
		
L_if_run:
		tst		G_Monitor, CB_MotorRun_Flag
		beq		L_to_halt		
		jsr		F_ReadHall
		tst		G_Hall_Flag,CB_RollBack
		beq		?skip	
		ldx		G_Hall_Temp								
		lda		HallTransTab1,x 
		sta		G_Hall_Pos
?skip:											
L_Six_Slect1:		
		tst		G_Monitor, CB_HallInvalid_Flag
		beq		L_Six_Slect		
		jmp		L_EndSixStep0	
;
L_Six_Slect: 			
		jsr		F_SixStepWave			; In mos&step File
		
L_EndSixStep0:		
L_HallMotorDrv_End:
		rts	
				
L_to_halt:
		lda		#C_Phase_Halt
		sta		G_RunState
		rts	
;
	
F_Spd_MontCtrl:
		tst		G_Monitor, CB_MotorRun_Flag
		bne		L_Hall_If_Ok
				
		TurnOffPWM		
		clr		P_IOB_Data, PWM
		
		tst		G_Monitor,CB_MotorBrake_Flag   
		bne		L_Not_TurnOffIPM_1
				
		tst		G_SYS_Control_Flag,CB_ChargeMode_Flag
		bne		L_Not_TurnOffIPM_1
		
		TurnOffIPM     	
						
L_Not_TurnOffIPM_1:	     		
		nop		;++++++++++++++++++++
L_Spd_MontCtrl_End:
		rts
;
;
L_Hall_If_Ok:
		tst		G_Monitor,CB_HallInvalid_Flag		
		beq		L_Alarm_If_Ok
		rts
;
L_Alarm_If_Ok:
		tst		G_Monitor,CB_IOverAlarm_Flag
		beq		L_do_spd
		rts
;
L_do_spd:
		tst		G_Monitor,CB_MotorBrake_Flag  
		beq     L_I_Control			   
		jsr		L_MsbCmdIsLarge		   
		jmp		L_Pwm_ReNew
;		
L_I_Control:
		tst		G_SYS_Control_Flag,CB_ChargeMode_Flag
		beq     L_I_Control1
		jsr		L_MsbCmdIsLarge		   ;
		jmp		L_Pwm_ReNew
;		
L_I_Control1:
		lda		G_ADC6_Buf			   
		cmp		#C_Iovre_Data          ;
		bcc		L_do_spd0			   ;	
		sec
		sbc		#C_Iovre_Data			; 
		lsr		A
		sta		G_Iover_Buf
;		
		beq		L_do_spd0				;hlp 05/31
		
		jsr		L_MsbCmdIsLarge12A		
		
		jmp		L_Pwm_ReNew		
;		
L_do_spd0:							    
		lda		G_SpdCtrl_Cnt
		cmp		#0
		bne		L_SpdCtrl_Task1
;

L_SpdCtrl_Task0:
		jsr		F_DutyCmdUpdate
;		
F_Pwm_Renew_Process:
L_Pwm_ReNew:	    
		lda		G_PhaseCtrlFilter+1
		rol		A
		rol		A
		rol		A
		rol		A
		and		#$F0
		sta		G_Temp					
;		
		lda		P_TMR1_DutyPeriod
		and		#$0F					
		clc
		adc		G_Temp
		sta		P_TMR1_DutyPeriod		 
;
		lda		G_PhaseCtrlFilter		
		sta		P_TMR1_PWMDuty	
		TurnOnPWM			
L_PWM_RENEW_End:
		rts
;
L_SpdCtrl_Task1:
		inc		G_SpeedRenew_Time
		lda		G_SpeedRenew_Time
		cmp		#C_SpeedRenew_Time
		bcc		L_SpdCtrl_Task1_End
		lda		#0
		sta		G_SpeedRenew_Time
;
		lda		G_SpeedComand+1				 	
		cmp		G_POS_fifo+1
		bcc		L_upper_duty			
		lda		G_SpeedComand+1
		cmp		G_POS_fifo+1
		bne		L_lower_duty 			
;	
		lda		G_SpeedComand
		cmp		G_POS_fifo
		bcc		L_upper_duty
;
		lda		G_SpeedComand
		cmp		G_POS_fifo
		bne		L_lower_duty
		
L_SpdCtrl_Task1_End:
		jmp     L_SpdCtrl_Task0
;
;
L_lower_duty:							
		lda		G_PhaseCmd+1			
		cmp		#$0Fh
		bcc		L_lower_duty_L1
;
		lda		G_PhaseCmd				
		cmp		#$FFh
		beq		L_SpdCtrl_Task1_End1
;
L_lower_duty_L1:
		lda		G_PhaseCmd
		clc
		adc		#$01					
		sta		G_PhaseCmd
		bcc		L_SpdCtrl_Task1_End1
;
; C = 1, do acc higher bytes					
;
		inc		G_PhaseCmd+1
		jmp		L_SpdCtrl_Task1_End1		
;
;
L_upper_duty:							
		lda		G_PhaseCmd+1
		cmp		#$0Eh
		bne		L_upper_duty_L1
;
		lda		G_PhaseCmd
		cmp		#0
		beq		L_SpdCtrl_Task1_End1
		
L_upper_duty_L1:			
		lda		G_PhaseCmd		
		sec
		sbc		#$01					; dec value = 0x01
		sta		G_PhaseCmd
		bcs		L_SpdCtrl_Task1_End1	
;
; C = 0, do dec higher bytes
;		
		dec		G_PhaseCmd+1		   	; borrowed happed
;
L_SpdCtrl_Task1_End1:	
		jmp		L_DutyCmdUpdate1
		
		
F_DutyCmdUpdate:			
		inc		G_PwmRenew_Time		
;		
		lda		G_SpdCtrl_Cnt
		bne		L_SPEED_PWM_RENEW
;		
		lda		G_PwmRenew_Time
		cmp		#C_PwmRenew_Time		; renew pwm time
		bcc		L_DutyCmdUpdate_End
;		
		lda		#0
		sta		G_PwmRenew_Time
		jmp		L_DutyCmdUpdate1
		
L_SPEED_PWM_RENEW:						;
		lda		G_PwmRenew_Time
		cmp		#C_SpeedPwmRenew_Time
;		
		bcc		L_DutyCmdUpdate_End
		lda		#0
		sta		G_PwmRenew_Time
;		
L_DutyCmdUpdate1:		
		lda		G_PhaseCmd+1
		cmp		G_PhaseCtrlFilter+1
		bcc		L_MsbCmdIsLess			
;		
		lda		G_PhaseCmd+1
		cmp		G_PhaseCtrlFilter+1
		bne		L_MsbCmdIsLarge			
;		
		lda		G_PhaseCmd
		cmp		G_PhaseCtrlFilter
		bcc		L_MsbCmdIsLess			
;		
		lda		G_PhaseCmd
		cmp		G_PhaseCtrlFilter
		bne		L_MsbCmdIsLarge			
;		
L_DutyCmdUpdate_End:		
		rts								
;												
;		
;========================================
; C = 0, MSB G_PhaseCmd < G_PhaseCtrlFilter
; G_PhaseCtrlFilter + #C_PwmUp_Step
;If G_PhaseCtrlFilter < 0xe00 then G_PhaseCtrlFilter = 0xe00
;========================================
L_MsbCmdIsLess:	
		tst		G_BrakeError_Buf,CB_IOver_Flag
		beq		L_PWM_UP
		jmp		L_Pwm_ReNew	
L_PWM_UP:
		lda		G_PhaseCtrlFilter		
		sec
		sbc		#C_PwmUp_Step		
		sta		G_PhaseCtrlFilter
		bcs		L_DoMinSature
;
; C = 0, do dec higher bytes
;		
		dec		G_PhaseCtrlFilter+1		

;========================================
; Filter output is less/equal to min; 
; If G_PhaseCtrlFilter < 0xe00 then G_PhaseCtrlFilter = 0xe00
;========================================
L_DoMinSature:
		lda		G_PhaseCtrlFilter+1
		cmp		#$0Eh					
		bcs		L_MsbCmdIsLess_End		
;
		lda		#$0Eh
		sta		G_PhaseCtrlFilter+1
;
		lda		#0		
		sta		G_PhaseCtrlFilter
		
L_MsbCmdIsLess_End:
		jmp		L_DutyCmdUpdate_End
;		
;========================================
; C = 1, MSB G_PhaseCmd >= G_PhaseCtrlFilter
; G_PhaseCtrlFilter - #C_PwmLow_Step
; If G_PhaseCtrlFilter > 0xFFF then G_PhaseCtrlFilter = 0xFFF
;========================================
L_MsbCmdIsLarge:
		lda		G_PhaseCtrlFilter
		clc
		adc		#C_PwmLow_Step					
		sta		G_PhaseCtrlFilter
		bcc		L_DoMaxSature
;
; C = 1, do acc higher bytes					
;
		inc		G_PhaseCtrlFilter+1

;========================================
; Filter output is less/equal to max
; If G_PhaseCtrlFilter > 0xFFF then G_PhaseCtrlFilter = 0xFFF
;========================================
L_DoMaxSature:	
		lda		G_PhaseCtrlFilter+1
		cmp		#0fh									
		bcc		L_MsbCmdIsLarge_End		
;
; Z = 1, C = 1, MSB is equal, so compare LSB
;		
		beq		L_checklower2			
;
; Z = 0, C = 1, so filter > cmd, sature MSB and LSB
;
		lda		#0fh
		sta		G_PhaseCtrlFilter+1
		lda		#ffh
		sta		G_PhaseCtrlFilter
		jmp		L_MsbCmdIsLarge_End

L_checklower2:
		lda		G_PhaseCtrlFilter	
		cmp		#ffh					
		bcc		L_MsbCmdIsLarge_End		
;
		lda		#ffh
		sta		G_PhaseCtrlFilter
L_MsbCmdIsLarge_End:		
		jmp		L_DutyCmdUpdate_End	
;
;
;========================================
; Current is large than 12A
;========================================		
L_MsbCmdIsLarge12A:
		lda		G_PhaseCtrlFilter
		clc
		adc		G_Iover_Buf			
		sta		G_PhaseCtrlFilter
		bcc		L_DoMaxSature12A
;
; C = 1, do acc higher bytes					
;
		inc		G_PhaseCtrlFilter+1

;========================================
; Filter output is less/equal to max
;========================================
L_DoMaxSature12A:	
		lda		G_PhaseCtrlFilter+1
		cmp		#0fh								
		bcc		L_MsbCmdIsLarge12A_End	
;
; Z = 1, C = 1, MSB is equal, so compare LSB
;		
		beq		L_checklower12A			
;
; Z = 0, C = 1, so filter > cmd, sature MSB and LSB
;
		lda		#0fh
		sta		G_PhaseCtrlFilter+1
		lda		#ffh
		sta		G_PhaseCtrlFilter
		jmp		L_MsbCmdIsLarge12A_End

L_checklower12A:
		lda		G_PhaseCtrlFilter	
		cmp		#ffh					
		bcc		L_MsbCmdIsLarge12A_End	
;
		lda		#ffh
		sta		G_PhaseCtrlFilter
L_MsbCmdIsLarge12A_End:		
		rts

T_IAD_TABLE:							;used for debug current AD value 
		DB   129,133,137,141,145,148,152;5A,5,5A,6A,6.5A,7A,7.5A,8A;
		DB	 156,160,164,168,172,176,180;8.5A,9A,9.5A,10A,10.5A,11A,11.5A;
		DB   184,187,191,195,199,203,207;12A,12.5A,13A,13.5A,14A,14.5A,15A;
		DB	 211,215,219,223,226,230,234;15.5A,16A,16.5A,17A,17.5A,18A,18.5A;
		DB	 238,242,246,250,254		;,19A,19.5A,20A,20.5A

;****************************************
;	Applied Body	: SPMC65P2404A
;	Firmware version: V1.0
;	Programer		: 
;	Date			: 2006/09/15	
;	Description		: NMI Interrupt Service Routine
;	Hardware Connect: SPMC65 BLDC demo board	 
;	IDE Version		: v1.6.8	
;	BODY Version	: v1.0.2A
;****************************************	
V_NMI:
		sei
		pha								
		txa								
		pha								
		tya
		pha
; IRQ4 interrupt ? 
;	
		tst		P_INT_Flag0,CB_INT_IRQ0IF		
		beq		L_EndNMI				; no, exit NMI
;
;		
		lda     G_Under_Test			; Under doing Mosfet Check 
		bne		L_Mos_Check				
		
		TurnOffIPM						
		
		TurnOffPWM						
		clr		P_IOB_Data, PWM
;
		set		G_Monitor,CB_IOverAlarm_Flag		
										; alarm happened		
		TurnOffMotor					; stop the motor immediately
	
		lda		#C_INT_IRQ0IF
		sta		P_INT_Flag0
		
		jmp		L_EndNMI
;		
L_Mos_Check:							; Mosfet Fail occurred	;MOSFET Check
		lda		#C_INT_IRQ0IF
		sta		P_INT_Flag0
;
		lda     G_Test_Num				; MOSFET Check
		sta		G_Mos_Num				; MOSFET Check
;		
L_EndNMI:
		pla
		tay	
		pla								
		tax								
		pla								
		cli
		rti

;****************************************
;	Applied Body	: SPMC65P2404A
;	Firmware version: V1.0
;	Programer		: 
;	Date			: 2006/09/15	
;	Description		: IRQ Interrupt Service Routine
;	Hardware Connect: SPMC65 BLDC demo board	 
;	IDE Version		: v1.6.8	
;	BODY Version	: v1.0.2A
;****************************************		
V_IRQ:
		pha	
		txa	
		pha	
;		tya
;		pha
L_T2IRQ:
		;(INT use 50us)
		tst	P_INT_Flag1, CB_INT_T2OIF
		beq	L_T3IRQ
		
		tst	G_Monitor,CB_Alarm_Flag
		bne	?skip

		jsr	F_HallMotorDrv	 
?skip:	
		lda	#C_INT_T2OIF	
		sta	P_INT_Flag1

L_T3IRQ:
		tst	P_INT_Flag0, CB_INT_CAP3IF	
		beq	L_T0IRQ	
		jsr	F_Speed_Cap	
		lda	#C_INT_CAP3IF
		sta	P_INT_Flag0		
		
		
L_T0IRQ:
		tst P_INT_Flag1, CB_INT_T0OIF
		beq	L_EndIRQ
		jsr	F_EBS_Chongdian_Control
		lda	#C_INT_T0OIF
		sta	P_INT_Flag1
;
L_EndIRQ:
;		pla
;		tay
		pla	; pop A register
		tax	; transfer A to X
		pla	; pop A register (ie. pop X)
		rti
;		
		.END