Implement deadtime pwm correction.

The correction will be made with a LUT. Previously this correction had to be made in timer1 interrupt that was stuffed, so it penalized commutation. Now the load is moved to a new state machine the like of dshot_tlm_create_packet. dshot_tlm_create_packet has been demistified removing obstructing macros. Fixed bugs Reoptimized t1_int Refactorings Updated comments Updated comment Fixed telemetry state machine to produce frames only if telemetry is enabled Always run at least 1 state of rcpulse state machine so firmware cannot get stuck at max pwm Fixed many bugs Added pwm deadtime lookup table Implemented pwm deadtime adjustment Fixed some bugs Fixed Infinite loop bug and EDT Disable interrupts while applying new pwm config and duty cycle Moved cleaning Flag_Ext_Tele to the place the esc is disarmed to avoid disabling telemetry when desync events are produced Disable telemetry when stall is done Updated deadtime correction LUTs and algorithm to allow being more precise Updated LUTs Fixed LUTs Fixed LUTs Refactored some tags and updated some comments Reset PCA before commutating Reworked commutation to erradicate shoot throughs
bird-sanctuary · Mar 4, 2023 · 24d490f · 24d490f
1 parent e3b0bae
commit 24d490f
Show file tree

Hide file tree

Showing 7 changed files with 854 additions and 545 deletions.
diff --git a/Bluejay.asm b/Bluejay.asm
@@ -24,15 +24,14 @@
 ;
 ; Bluejay is a fork of BLHeli_S <https://github.com/bitdump/BLHeli> by Steffen Skaug.
 ;
-; The input signal can be DShot with rates: DShot150, DShot300 and DShot600.
+; The input signal can be DShot with rates: DShot300 and DShot600.
 ;
 ; This file is best viewed with tab width set to 5.
 ;
 ;**** **** **** **** ****
 ; Master clock is internal 24MHz oscillator (or 48MHz, for which the times below are halved)
 ; Although 24/48 are used in the code, the exact clock frequencies are 24.5MHz or 49.0 MHz
 ; Timer0 (41.67ns counts) always counts up and is used for
-; - RC pulse measurement
 ; - DShot telemetry pulse timing
 ; Timer1 (41.67ns counts) always counts up and is used for
 ; - DShot frame sync detection
@@ -290,6 +289,12 @@ Ext_Telemetry_H:			DS	1
 Scheduler_Counter:			DS	1	; Scheduler Heartbeat
 PwmBitsCount:				DS  1	; 0 = 8 bit pwm, 1 = 9 bit pwm, 2 = 10 bit pwm, 3 = 11 bit pwm
 
+DShot_rcpulse_stm_state:    DS  1   ; RC pulse state machine state or step
+DShot_rcpulse_stm_pwm_t2:   DS  1   ; RC pulse state machine temp2
+DShot_rcpulse_stm_pwm_t3:   DS  1   ; RC pulse state machine temp3
+DShot_rcpulse_stm_pwm_t4:   DS  1   ; RC pulse state machine temp4
+DShot_rcpulse_stm_pwm_t5:   DS  1   ; RC pulse state machine temp5
+
 ;**** **** **** **** ****
 ; Indirect addressing data segments
 ISEG AT 080h						; The variables below must be in this sequence
@@ -336,10 +341,10 @@ Pgm_Var_PWM_lo_thres:		DS	1	; Variable PWM low rcpulse threshold
 Pgm_Var_PWM_hi_thres:		DS	1	; Variable PWM high rcpulse threshold
 
 ISEG AT 0B0h
-Stack:					DS	16	; Reserved stack space
+Stack:					    DS	16	; Reserved stack space
 
 ISEG AT 0C0h
-Dithering_Patterns:			DS	16	; Bit patterns for pwm dithering
+Dithering_Patterns:			DS	8	; Unified bit patterns for pwm dithering
 
 ISEG AT 0D0h
 Temp_Storage:				DS	48	; Temporary storage (internal memory)
@@ -552,7 +557,7 @@ bootloader_done:
 setup_dshot:
 	; Setup timers for DShot
 	mov	TCON, #51h				; Timer0/1 run and Int0 edge triggered
-	mov	CKCON0, #01h				; Timer0/1 clock is system clock divided by 4 (for DShot150)
+	mov	CKCON0, #01h				; Timer0/1 clock is system clock divided by 4 (for DShot150/300)
 	mov	TMOD, #0AAh				; Timer0/1 set to 8-bits auto reload and gated by Int0/1
 	mov	TH0, #0					; Auto reload value zero
 	mov	TH1, #0
@@ -588,6 +593,9 @@ setup_dshot:
 
 	setb	IE_EA					; Enable all interrupts
 
+	; Set Flag_Timer3_Pending so dshot rcpulse stm can run all states in a single run
+	setb	Flag_Timer3_Pending
+
 	; Setup variables for DShot150 (Only on 24MHz because frame length threshold cannot be scaled up)
 IF MCU_TYPE == 0
 	mov	DShot_Timer_Preset, #-64		; Load DShot sync timer preset (for DShot150)
@@ -598,7 +606,15 @@ IF MCU_TYPE == 0
 
 	; Test whether signal is DShot150
 	mov	Rcp_Outside_Range_Cnt, #10	; Set out of range counter
-	call	wait100ms					; Wait for new RC pulse
+	mov Temp6, #100
+
+setup_dshot_150_wait:
+	; Wait for new RC pulse
+	call	wait1ms
+	call	dshot_rcpulse_stm;
+	djnz	Temp6, setup_dshot_150_wait
+
+	; Check rcpulse
 	mov	A, Rcp_Outside_Range_Cnt		; Check if pulses were accepted
 	jz	arming_begin
 ENDIF
@@ -614,7 +630,15 @@ ENDIF
 
 	; Test whether signal is DShot300
 	mov	Rcp_Outside_Range_Cnt, #10	; Set out of range counter
-	call	wait100ms					; Wait for new RC pulse
+	mov Temp6, #100
+
+setup_dshot_300_wait:
+	; Wait for new RC pulse
+	call	wait1ms
+	call	dshot_rcpulse_stm;
+	djnz	Temp6, setup_dshot_300_wait
+
+	; Check rcpulse
 	mov	A, Rcp_Outside_Range_Cnt		; Check if pulses were accepted
 	jz	arming_begin
 
@@ -628,7 +652,15 @@ IF MCU_TYPE >= 1
 
 	; Test whether signal is DShot600
 	mov	Rcp_Outside_Range_Cnt, #10	; Set out of range counter
-	call	wait100ms					; Wait for new RC pulse
+	mov Temp6, #100
+
+setup_dshot_600_wait:
+	; Wait for new RC pulse
+	call	wait1ms
+	call	dshot_rcpulse_stm;
+	djnz	Temp6, setup_dshot_600_wait
+
+	; Check rcpulse
 	mov	A, Rcp_Outside_Range_Cnt		; Check if pulses were accepted
 	jz	arming_begin
 ENDIF
@@ -711,22 +743,30 @@ beep_delay_set:
 	setb	IE_EA					; Enable all interrupts
 
 wait_for_start_no_beep:
+	call 	scheduler_run			; Run scheduler
+	call	dshot_rcpulse_stm		; Process rcpulses
+	call	dshot_cmd_check			; Check and process DShot command
 	jb	Flag_Telemetry_Pending, wait_for_start_check_rcp
-	call	dshot_tlm_create_packet		; Create telemetry packet (0 rpm)
-	call 	scheduler_run
+	call	dshot_tlmpacket_stm		; Create telemetry packets
 
 wait_for_start_check_rcp:
 	jnb	Flag_Rcp_Stop, wait_for_start_nonzero	; Higher than stop, Yes - proceed
 
 	mov	A, Rcp_Timeout_Cntd			; Load RC pulse timeout counter value
 	ljz	init_no_signal				; If pulses are missing - go back to detect input signal
-
-	call	dshot_cmd_check			; Check and process DShot command
-
 	sjmp	wait_for_start_loop			; Go back to beginning of wait loop
 
 wait_for_start_nonzero:
-	call	wait100ms					; Wait to see if start pulse was glitch
+    ; Wait to see if start pulse was glitch
+    mov     Temp6, #100
+
+wait_for_start_nonzero_wait:
+	call	wait1ms
+    call    scheduler_run           ; Run scheduler
+    call    dshot_rcpulse_stm       ; Process rcpulses
+    jb  Flag_Telemetry_Pending, wait_for_start_check_rcp
+    call    dshot_tlmpacket_stm     ; Create telemetry packets
+    djnz    Temp6, wait_for_start_nonzero_wait
 
 	; If Rcp returned to stop - start over
 	jb	Flag_Rcp_Stop, wait_for_start_loop
@@ -1021,7 +1061,6 @@ exit_run_mode_on_timeout:
 
 exit_run_mode:
 	clr	IE_EA					; Disable all interrupts
-	clr Flag_Ext_Tele			; Clear extended DSHOT telemetry flag
 	call	switch_power_off
 	mov	Flags0, #0				; Clear run time flags (in case they are used in interrupts)
 	mov	Flags1, #0
@@ -1064,6 +1103,9 @@ ENDIF
 	ljmp	motor_start				; Go back and try starting motors again
 
 exit_run_mode_stall_done:
+    ; Clear extended DSHOT telemetry flag
+    clr Flag_Ext_Tele
+
 	; Stalled too many times
 	clr	IE_EA
 	call	beep_motor_stalled
@@ -1072,6 +1114,10 @@ exit_run_mode_stall_done:
 	ljmp	arming_begin				; Go back and wait for arming
 
 exit_run_mode_no_stall:
+    ; Clear extended DSHOT telemetry flag
+    clr Flag_Ext_Tele
+
+    ; Clear startup stall counter
 	mov	Startup_Stall_Cnt, #0
 
 	mov	Temp1, #Pgm_Brake_On_Stop	; Check if using brake on stop

diff --git a/Bluejay_Common.asm b/Bluejay_Common.asm
@@ -200,6 +200,19 @@ ELSE
 ENDIF
 ENDM
 
+Clear_PCA MACRO
+	; Clear PWM outputs
+	mov	PCA0CLR, #87h				; Do clear enable
+	mov	PCA0CLR, #07h				; Do clear enable inverted
+	mov	PCA0CLR, #00h				; Disable clear enable
+
+	; Restart timer
+	mov	PCA0CN0, #00h				; Disable PCA
+	mov PCA0L, #00h
+	mov PCA0H, #00h
+	mov	PCA0CN0, #40h				; Enable PCA again
+ENDM
+
 Set_MCU_Clk_24MHz MACRO
 	mov	CLKSEL, #13h				;; Set clock to 24MHz (Oscillator 1 divided by 2)
 
@@ -269,39 +282,27 @@ GCR_Add_Time MACRO reg
 ENDM
 
 ; Prepare telemetry packet while waiting for Timer3 to wrap
+; Uses temp1:2:3:4:5
 Wait_For_Timer3 MACRO
 LOCAL wait_for_t3 done_waiting
-    jb  Flag_Telemetry_Pending, wait_for_t3
+    ; Run at least 1 state of rcpulse stm, so firmware cannot get
+    ; stuck at max PWM
+    call    dshot_rcpulse_stm
 
+    ; Run telemetry packet state machine only if telemetry is
+    ; pending and timer3 is pending
+    jb  Flag_Telemetry_Pending, wait_for_t3
     jnb Flag_Timer3_Pending, done_waiting
-    call    dshot_tlm_create_packet
+    call    dshot_tlmpacket_stm
 
+    ; Now wait until timer3 overflows
 wait_for_t3:
     jnb Flag_Timer3_Pending, done_waiting
     sjmp    wait_for_t3
 
 done_waiting:
 ENDM
 
-; Used for subdividing the DShot telemetry routine into chunks,
-; that will return if Timer3 has wrapped
-Early_Return_Packet_Stage MACRO num
-    Early_Return_Packet_Stage_ num, %(num + 1)
-ENDM
-
-Early_Return_Packet_Stage_ MACRO num next
-IF num > 0
-    inc Temp7                               ;; Increment current packet stage
-    jb  Flag_Timer3_Pending, dshot_packet_stage_&num    ;; Return early if Timer3 has wrapped
-    pop PSW
-    ret
-dshot_packet_stage_&num:
-ENDIF
-IF num < 5
-    cjne    Temp7, #(num), dshot_packet_stage_&next     ;; If this is not current stage, skip to next
-ENDIF
-ENDM
-
 Decode_DShot_2Bit MACRO dest, decode_fail
     movx    A, @Temp1
     mov Temp7, A

diff --git a/Bluejay_Commutation.asm b/Bluejay_Commutation.asm
@@ -10,6 +10,7 @@ comm1_comm2:                        ; C->A
     jb  Flag_Motor_Dir_Rev, comm1_comm2_rev
 
     clr IE_EA
+    Clear_PCA
     B_Com_Fet_Off
     A_Com_Fet_On
     Set_Pwm_Phase_C             ; Reapply power after a demag cut
@@ -19,6 +20,7 @@ comm1_comm2:                        ; C->A
 
 comm1_comm2_rev:                    ; A->C
     clr IE_EA
+    Clear_PCA
     B_Com_Fet_Off
     C_Com_Fet_On
     Set_Pwm_Phase_A             ; Reapply power after a demag cut
@@ -31,6 +33,7 @@ comm2_comm3:                        ; B->A
     jb  Flag_Motor_Dir_Rev, comm2_comm3_rev
 
     clr IE_EA
+    Clear_PCA
     C_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_B
     A_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)
@@ -40,6 +43,7 @@ comm2_comm3:                        ; B->A
 
 comm2_comm3_rev:                    ; B->C
     clr IE_EA
+    Clear_PCA
     A_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_B
     C_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)
@@ -52,6 +56,7 @@ comm3_comm4:                        ; B->C
     jb  Flag_Motor_Dir_Rev, comm3_comm4_rev
 
     clr IE_EA
+    Clear_PCA
     A_Com_Fet_Off
     C_Com_Fet_On
     Set_Pwm_Phase_B             ; Reapply power after a demag cut
@@ -61,6 +66,7 @@ comm3_comm4:                        ; B->C
 
 comm3_comm4_rev:                    ; B->A
     clr IE_EA
+    Clear_PCA
     C_Com_Fet_Off
     A_Com_Fet_On
     Set_Pwm_Phase_B             ; Reapply power after a demag cut
@@ -73,6 +79,7 @@ comm4_comm5:                        ; A->C
     jb  Flag_Motor_Dir_Rev, comm4_comm5_rev
 
     clr IE_EA
+    Clear_PCA
     B_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_A
     C_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)
@@ -82,6 +89,7 @@ comm4_comm5:                        ; A->C
 
 comm4_comm5_rev:                    ; C->A
     clr IE_EA
+    Clear_PCA
     B_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_C
     A_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)
@@ -94,6 +102,7 @@ comm5_comm6:                        ; A->B
     jb  Flag_Motor_Dir_Rev, comm5_comm6_rev
 
     clr IE_EA
+    Clear_PCA
     C_Com_Fet_Off
     B_Com_Fet_On
     Set_Pwm_Phase_A             ; Reapply power after a demag cut
@@ -103,6 +112,7 @@ comm5_comm6:                        ; A->B
 
 comm5_comm6_rev:                    ; C->B
     clr IE_EA
+    Clear_PCA
     A_Com_Fet_Off
     B_Com_Fet_On
     Set_Pwm_Phase_C             ; Reapply power after a demag cut
@@ -115,6 +125,7 @@ comm6_comm1:                        ; C->B
     jb  Flag_Motor_Dir_Rev, comm6_comm1_rev
 
     clr IE_EA
+    Clear_PCA
     A_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_C
     B_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)
@@ -124,6 +135,7 @@ comm6_comm1:                        ; C->B
 
 comm6_comm1_rev:                    ; A->B
     clr IE_EA
+    Clear_PCA
     C_Pwm_Fet_Off                   ; Turn off pwm FET (Necessary for EN/PWM driver)
     Set_Pwm_Phase_A
     B_Com_Fet_On                    ; Reapply power after a demag cut (Necessary for EN/PWM driver)