diff --git a/src/Bluejay.asm b/src/Bluejay.asm index 36793de..eea1251 100644 --- a/src/Bluejay.asm +++ b/src/Bluejay.asm @@ -753,21 +753,17 @@ wait_for_start_nonzero: ;**** **** **** **** **** **** **** **** **** **** **** **** **** motor_start: clr IE_EA ; Disable interrupts + call switch_power_off - setb IE_EA ; Enable interrupts clr A - mov Flags0, A ; Clear run time flags - mov Flags1, A - mov Demag_Detected_Metric, A ; Clear demag metric - mov Demag_Detected_Metric_Max, A ; Clear demag metric max - - call wait1ms - + mov Flags0, #0 ; Clear run time flags + mov Flags1, #0 + mov Demag_Detected_Metric, #0 ; Clear demag metric + mov Demag_Detected_Metric_Max, #0 ; Clear demag metric max mov Ext_Telemetry_H, #0 ; Clear extended telemetry data ; Set up start operating conditions - clr IE_EA ; Disable interrupts mov Temp2, #Pgm_Startup_Power_Max mov Pwm_Limit_Beg, @Temp2 ; Set initial pwm limit mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg @@ -797,7 +793,6 @@ IF MCU_TYPE == MCU_BB2 or MCU_TYPE == MCU_BB51 mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY_48 ENDIF - setb IE_EA ; Enable interrupts mov C, Flag_Pgm_Dir_Rev ; Read spin direction setting mov Flag_Motor_Dir_Rev, C @@ -811,10 +806,13 @@ ENDIF ; Motor start beginning ;**** **** **** **** **** **** **** **** **** **** **** **** **** motor_start_bidir_done: + ; Set initial motor state setb Flag_Startup_Phase ; Set startup phase flags setb Flag_Initial_Run_Phase mov Startup_Cnt, #0 ; Reset startup phase run counter mov Initial_Run_Rot_Cntd, #12 ; Set initial run rotation countdown + + ; Initialize commutation call comm5_comm6 ; Initialize commutation call comm6_comm1 call initialize_timing ; Initialize timing @@ -823,6 +821,8 @@ motor_start_bidir_done: call calc_next_comm_period call initialize_timing ; Initialize timing + setb IE_EA ; Enable interrupts + ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Run entry point @@ -1030,11 +1030,13 @@ exit_run_mode_on_timeout: inc Startup_Stall_Cnt ; Increment stall count if motors did not properly start exit_run_mode: - clr IE_EA ; Disable all interrupts - clr Flag_Ext_Tele ; Clear extended DSHOT telemetry flag + ; Disable all interrupts (they will be disabled for a while, be aware) + clr IE_EA + call switch_power_off mov Flags0, #0 ; Clear run time flags (in case they are used in interrupts) mov Flags1, #0 + clr Flag_Ext_Tele ; Clear extended DSHOT telemetry flag IF MCU_TYPE == MCU_BB2 or MCU_TYPE == MCU_BB51 Set_MCU_Clk_24MHz @@ -1057,33 +1059,42 @@ IF MCU_TYPE == MCU_BB2 or MCU_TYPE == MCU_BB51 mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY ENDIF - setb IE_EA ; Enable all interrupts - ; Check if RCP is zero, then it is a normal stop or signal timeout jb Flag_Rcp_Stop, exit_run_mode_no_stall + ; It is a stall! ; Signal stall setb Flag_Stall_Notify - clr C ; Otherwise - it's a stall + ; Check max consecutive stalls and exit if stall counter > 3 + clr C mov A, Startup_Stall_Cnt - subb A, #4 ; Maximum consecutive stalls - jnc exit_run_mode_stall_done + subb A, #3 + jnc exit_run_mode_is_stall + + ; At this point there was a desync event, and a new try is to be done. + ; The program will jump to motor_start. Interrupts are disabled at this + ; point so it is safe to jump to motor start, where a new initial state + ; will be set call wait100ms ; Wait for a bit between stall restarts + ljmp motor_start ; Go back and try starting motors again -exit_run_mode_stall_done: +exit_run_mode_is_stall: + ; Enable all interrupts (disabled above, in exit_run_mode) + setb IE_EA + ; Clear extended DSHOT telemetry flag if turtle mode is not active ; This flag is also used for EDT safety arm flag ; We don't want to deactivate extended telemetry during turtle mode ; Extended telemetry flag is important because it is involved in ; EDT safety feature. We don't want to disable EDT arming during ; turtle mode. - jb Flag_User_Reverse_Requested, exit_run_mode_stall_done_beep + jb Flag_User_Reverse_Requested, exit_run_mode_is_stall_beep clr Flag_Ext_Tele -exit_run_mode_stall_done_beep: +exit_run_mode_is_stall_beep: ; Stalled too many times clr IE_EA call beep_motor_stalled @@ -1092,16 +1103,19 @@ exit_run_mode_stall_done_beep: ljmp arming_begin ; Go back and wait for arming exit_run_mode_no_stall: + ; Enable all interrupts (disabled above, in exit_run_mode) + setb IE_EA + ; Clear extended DSHOT telemetry flag if turtle mode is not active ; This flag is also used for EDT safety arm flag ; We don't want to deactivate extended telemetry during turtle mode ; Extended telemetry flag is important because it is involved in ; EDT safety feature. We don't want to disable EDT arming during ; turtle mode. - jb Flag_User_Reverse_Requested, exit_run_mode_no_stall_beep + jb Flag_User_Reverse_Requested, exit_run_mode_no_stall_no_beep clr Flag_Ext_Tele -exit_run_mode_no_stall_beep: +exit_run_mode_no_stall_no_beep: ; Clear stall counter mov Startup_Stall_Cnt, #0 diff --git a/src/Modules/Power.asm b/src/Modules/Power.asm index 91d9a62..d597698 100644 --- a/src/Modules/Power.asm +++ b/src/Modules/Power.asm @@ -50,10 +50,17 @@ switch_power_off: set_pwm_limit: jb Flag_High_Rpm, set_pwm_limit_high_rpm ; If high rpm,limit pwm by rpm instead - ;set_pwm_limit_low_rpm: - ; Set pwm limit +set_pwm_limit_low_rpm: + ; Set pwm limit for startup phase to avoid burning the esc/motor during startup + ; (Startup can happen after a desync caused by a crash, if that is the case it + ; will be better to avoid burning esc/motor) + mov Temp1, Pwm_Limit_Beg + + ; Exit if startup phase is set + jb Flag_Initial_Run_Phase, set_pwm_limit_low_rpm_exit + + ; Set default pwm limit for other phases mov Temp1, #0FFh ; Default full power - jb Flag_Startup_Phase, set_pwm_limit_low_rpm_exit ; Exit if startup phase set mov A, Low_Rpm_Pwr_Slope ; Check if low RPM power protection is enabled jz set_pwm_limit_low_rpm_exit ; Exit if disabled (zero) diff --git a/src/Modules/Timing.asm b/src/Modules/Timing.asm index d12dea6..19eba76 100644 --- a/src/Modules/Timing.asm +++ b/src/Modules/Timing.asm @@ -776,9 +776,16 @@ evaluate_comparator_integrity: jb Flag_Dir_Change_Brake, eval_comp_exit ; Do not exit run mode if braking jb Flag_Demag_Detected, eval_comp_exit ; Do not exit run mode if it is a demag situation - dec SP ; Routine exit without "ret" command + ; Disable all interrupts and cut power ASAP. They will be enabled in exit_run_mode_on_timeout + clr IE_EA + call switch_power_off + + ; Routine exit without "ret" command dec SP - ljmp exit_run_mode_on_timeout ; Exit run mode if timeout has elapsed + dec SP + + ; Go to exit run mode if timeout has elapsed + ljmp exit_run_mode_on_timeout eval_comp_startup: inc Startup_Cnt ; Increment startup counter