From 5727cfe57bc8beac001377f0c5ddb5931848a248 Mon Sep 17 00:00:00 2001 From: Mathias Rasmussen Date: Mon, 4 Jan 2021 05:35:15 +0100 Subject: [PATCH] refactor: Reorganize function routines --- Bluejay.asm | 2170 +++++++++++++++++++++++++-------------------------- 1 file changed, 1085 insertions(+), 1085 deletions(-) diff --git a/Bluejay.asm b/Bluejay.asm index 15d2c83..fa13f59 100644 --- a/Bluejay.asm +++ b/Bluejay.asm @@ -1130,379 +1130,174 @@ ENDIF ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; DShot GCR encode -; -; GCR encode e-period data for DShot telemetry -; -; Input -; - Temp1: Data pointer for storing pulse timings -; - A: 4-bit value to GCR encode -; - B: Time that must be added to transition -; Output -; - B: Time remaining to be added to next transition +; Wait a number of milliseconds (Multiple entry points) ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -dshot_gcr_encode: - anl A, #0Fh - rl A ; Multiply by 2 to match jump offsets - mov DPTR, #dshot_gcr_encode_jump_table - jmp @A+DPTR - -dshot_gcr_encode_jump_table: - ajmp dshot_gcr_encode_0_11001 - ajmp dshot_gcr_encode_1_11011 - ajmp dshot_gcr_encode_2_10010 - ajmp dshot_gcr_encode_3_10011 - ajmp dshot_gcr_encode_4_11101 - ajmp dshot_gcr_encode_5_10101 - ajmp dshot_gcr_encode_6_10110 - ajmp dshot_gcr_encode_7_10111 - ajmp dshot_gcr_encode_8_11010 - ajmp dshot_gcr_encode_9_01001 - ajmp dshot_gcr_encode_A_01010 - ajmp dshot_gcr_encode_B_01011 - ajmp dshot_gcr_encode_C_11110 - ajmp dshot_gcr_encode_D_01101 - ajmp dshot_gcr_encode_E_01110 - ajmp dshot_gcr_encode_F_01111 - -; GCR encoding is ordered by least significant bit first, -; and represented as pulse durations. -dshot_gcr_encode_0_11001: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_3 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +wait1ms: + mov Temp2, #1 + sjmp waitxms_o -dshot_gcr_encode_1_11011: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +wait10ms: + mov Temp2, #10 + sjmp waitxms_o -dshot_gcr_encode_2_10010: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_3 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +wait100ms: + mov Temp2, #100 + sjmp waitxms_o -dshot_gcr_encode_3_10011: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_3 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +wait200ms: + mov Temp2, #200 + sjmp waitxms_o -dshot_gcr_encode_4_11101: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_1 +waitxms_o: ; Outer loop + mov Temp1, #23 +waitxms_m: ; Middle loop + clr A + djnz ACC, $ ; Inner loop (41.8us - 1024 cycles) + djnz Temp1, waitxms_m + djnz Temp2, waitxms_o ret -dshot_gcr_encode_5_10101: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret -dshot_gcr_encode_6_10110: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Beeper routines (Multiple entry points) +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +beep_f1: + mov Temp3, #66 ; Off wait loop length (Tone) + mov Temp4, #(3500 / 66) ; Number of beep pulses (Duration) + sjmp beep -dshot_gcr_encode_7_10111: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +beep_f2: + mov Temp3, #45 + mov Temp4, #(3500 / 45) + sjmp beep -dshot_gcr_encode_8_11010: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +beep_f3: + mov Temp3, #38 + mov Temp4, #(3500 / 38) + sjmp beep -dshot_gcr_encode_9_01001: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_3 - mov Tmp_B, DShot_GCR_Pulse_Time_2 - ret +beep_f4: + mov Temp3, #25 + mov Temp4, #(3500 / 25) + sjmp beep -dshot_gcr_encode_A_01010: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - mov Tmp_B, DShot_GCR_Pulse_Time_2 - ret +beep_f5: + mov Temp3, #20 + mov Temp4, #(3500 / 25) + sjmp beep -dshot_gcr_encode_B_01011: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - mov Tmp_B, DShot_GCR_Pulse_Time_2 - ret +beep_f1_short: + mov Temp3, #66 + mov Temp4, #(2000 / 66) + sjmp beep -dshot_gcr_encode_C_11110: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_1 - ret +beep_f2_short: + mov Temp3, #45 + mov Temp4, #(2000 / 45) + sjmp beep -dshot_gcr_encode_D_01101: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_2 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_2 +beep: + mov A, Beep_Strength + djnz ACC, beep_start ; Start if beep strength is not 1 ret -dshot_gcr_encode_E_01110: - DShot_GCR_Get_Time - Push_Mem Temp1, A - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_2 - ret +beep_start: + mov Temp2, #2 +beep_onoff: + clr A + BcomFET_off ; BcomFET off + djnz ACC, $ ; Allow some time after comfet is turned off + BpwmFET_on ; BpwmFET on (in order to charge the driver of the BcomFET) + djnz ACC, $ ; Let the pwmfet be turned on a while + BpwmFET_off ; BpwmFET off again + djnz ACC, $ ; Allow some time after pwmfet is turned off + BcomFET_on ; BcomFET on + djnz ACC, $ ; Allow some time after comfet is turned on + ; Turn on pwmfet + mov A, Temp2 + jb ACC.0, beep_apwmfet_on + ApwmFET_on +beep_apwmfet_on: + jnb ACC.0, beep_cpwmfet_on + CpwmFET_on +beep_cpwmfet_on: + mov A, Beep_Strength + djnz ACC, $ + ; Turn off pwmfet + mov A, Temp2 + jb ACC.0, beep_apwmfet_off + ApwmFET_off +beep_apwmfet_off: + jnb ACC.0, beep_cpwmfet_off + CpwmFET_off +beep_cpwmfet_off: + mov A, #150 ; 25us off + djnz ACC, $ + djnz Temp2, beep_onoff + ; Copy variable + mov A, Temp3 + mov Temp1, A +beep_off: ; Fets off loop + mov A, #150 + djnz ACC, $ + djnz Temp1, beep_off + djnz Temp4, beep_start -dshot_gcr_encode_F_01111: - Push_Mem Temp1, Tmp_B - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - Push_Mem Temp1, DShot_GCR_Pulse_Time_1 - mov Tmp_B, DShot_GCR_Pulse_Time_2 + BcomFET_off ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; DShot 12-bit encode -; -; Encodes 16-bit e-period as a 12-bit value of the form: -; where M SHL E ~ e-period [us] +; LED control ; -; Note: Not callable to improve performance +; Controls LEDs ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -dshot_12bit_7: - ;mov A, Tlm_Data_H - mov C, Tlm_Data_L.7 - rlc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #0fh - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_6: - ;mov A, Tlm_Data_H - mov C, Tlm_Data_L.7 - rlc A - mov C, Tlm_Data_L.6 - rlc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #0dh - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_5: - ;mov A, Tlm_Data_H - mov C, Tlm_Data_L.7 - rlc A - mov C, Tlm_Data_L.6 - rlc A - mov C, Tlm_Data_L.5 - rlc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #0bh - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_4: - mov A, Tlm_Data_L - anl A, #0f0h - clr Tlm_Data_H.4 - orl A, Tlm_Data_H - swap A - mov Tlm_Data_L, A - mov Tlm_Data_H, #09h - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_3: - mov A, Tlm_Data_L - mov C, Tlm_Data_H.0 - rrc A - mov C, Tlm_Data_H.1 - rrc A - mov C, Tlm_Data_H.2 - rrc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #07h - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_2: - mov A, Tlm_Data_L - mov C, Tlm_Data_H.0 - rrc A - mov C, Tlm_Data_H.1 - rrc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #05h - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_1: - mov A, Tlm_Data_L - mov C, Tlm_Data_H.0 - rrc A - mov Tlm_Data_L, A - mov Tlm_Data_H, #03h - ajmp dshot_tlm_12bit_encoded - -dshot_12bit_encode: - ; Encode 16-bit e-period as a 12-bit value - jb ACC.7, dshot_12bit_7 ; ACC = Tlm_Data_H - jb ACC.6, dshot_12bit_6 - jb ACC.5, dshot_12bit_5 - jb ACC.4, dshot_12bit_4 - jb ACC.3, dshot_12bit_3 - jb ACC.2, dshot_12bit_2 - jb ACC.1, dshot_12bit_1 - mov A, Tlm_Data_L ; Already 12-bit (E=0) - ajmp dshot_tlm_12bit_encoded - - -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; DShot telemetry create packet -; -; Create DShot telemetry packet and prepare it for being sent -; The routine is divided into 6 sections that can return early -; in order to reduce commutation interference -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -dshot_tlm_create_packet: - push PSW - mov PSW, #10h ; Select register bank 2 - - Early_Return_Packet_Stage 0 - - ; Read commutation period - clr IE_EA - mov Tlm_Data_L, Comm_Period4x_L - mov Tlm_Data_H, Comm_Period4x_H - setb IE_EA - - ; Multiply period by 3/4 (1/2 + 1/4) - mov A, Tlm_Data_L - mov C, Tlm_Data_H.0 - rrc A - mov Temp2, A - mov C, Tlm_Data_H.1 - rrc A - add A, Temp2 - mov Tlm_Data_L, A - - mov A, Tlm_Data_H - rr A - clr ACC.7 +led_control: + mov Temp1, #Pgm_LED_Control + mov A, @Temp1 mov Temp2, A - rr A - clr ACC.7 - addc A, Temp2 - mov Tlm_Data_H, A - - Early_Return_Packet_Stage 1 - mov A, Tlm_Data_H - - ; 12-bit encode telemetry data - jnz dshot_12bit_encode - mov A, Tlm_Data_L ; Already 12-bit - jnz dshot_tlm_12bit_encoded - - ; If period is zero then reset to FFFFh (FFFh for 12-bit) - mov Tlm_Data_H, #0Fh - mov Tlm_Data_L, #0FFh - -dshot_tlm_12bit_encoded: - Early_Return_Packet_Stage 2 - mov A, Tlm_Data_L - - ; Compute inverted xor checksum (4-bit) - swap A - xrl A, Tlm_Data_L - xrl A, Tlm_Data_H - cpl A - - ; GCR encode the telemetry data (16-bit) - mov Temp1, #Temp_Storage ; Store pulse timings in Temp_Storage - mov Tmp_B, DShot_GCR_Pulse_Time_1 ; Final transition time - - call dshot_gcr_encode ; GCR encode lowest 4-bit of A (store through Temp1) - - Early_Return_Packet_Stage 3 - - mov A, Tlm_Data_L - call dshot_gcr_encode - - Early_Return_Packet_Stage 4 - - mov A, Tlm_Data_L - swap A - call dshot_gcr_encode - - Early_Return_Packet_Stage 5 - - mov A, Tlm_Data_H - call dshot_gcr_encode - - Push_Mem Temp1, Tmp_B ; Initial transition time - - mov Temp5, #0 - setb Flag_PACKET_PENDING - - pop PSW + anl A, #03h + Set_LED_0 + jnz led_0_done + Clear_LED_0 +led_0_done: + mov A, Temp2 + anl A, #0Ch + Set_LED_1 + jnz led_1_done + Clear_LED_1 +led_1_done: + mov A, Temp2 + anl A, #030h + Set_LED_2 + jnz led_2_done + Clear_LED_2 +led_2_done: + mov A, Temp2 + anl A, #0C0h + Set_LED_3 + jnz led_3_done + Clear_LED_3 +led_3_done: ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Wait a number of milliseconds (Multiple entry points) +; Switch power off routine +; +; Switches all fets off ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -wait1ms: - mov Temp2, #1 - sjmp waitxms_o - -wait10ms: - mov Temp2, #10 - sjmp waitxms_o - -wait100ms: - mov Temp2, #100 - sjmp waitxms_o - -wait200ms: - mov Temp2, #200 - sjmp waitxms_o - -waitxms_o: ; Outer loop - mov Temp1, #23 -waitxms_m: ; Middle loop - clr A - djnz ACC, $ ; Inner loop (41.8us - 1024 cycles) - djnz Temp1, waitxms_m - djnz Temp2, waitxms_o +switch_power_off: + All_pwmFETs_Off ; Turn off all pwm fets + All_comFETs_Off ; Turn off all commutation fets + Set_Pwms_Off ret @@ -2245,520 +2040,1006 @@ setup_zc_scan_timeout_startup_done: orl EIE1, #80h ; Enable timer 3 interrupts setb IE_EA -wait_before_zc_scan_exit: +wait_before_zc_scan_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for comparator to go low/high routines +; +; Waits for the zero cross scan wait time to elapse +; Then scans for comparator going low/high +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comp_out_low: + setb Flag_DEMAG_DETECTED ; Set demag detected flag as default + mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads + mov Bit_Access, #00h ; Desired comparator output + jnb Flag_DIR_CHANGE_BRAKE, ($+6) + mov Bit_Access, #40h + sjmp wait_for_comp_out_start + +wait_for_comp_out_high: + setb Flag_DEMAG_DETECTED ; Set demag detected flag as default + mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads + mov Bit_Access, #40h ; Desired comparator output + jnb Flag_DIR_CHANGE_BRAKE, ($+6) + mov Bit_Access, #00h + +wait_for_comp_out_start: + ; Set number of comparator readings + mov Temp1, #1 ; Number of OK readings required + mov Temp2, #1 ; Max number of readings required + jb Flag_HIGH_RPM, comp_scale_samples ; Branch if high rpm + + mov A, Flags_Startup ; Clear demag detected flag if start phases + jz ($+4) + + clr Flag_DEMAG_DETECTED + + ; Too low value (~<15) causes rough running at pwm harmonics. + ; Too high a value (~>35) causes the RCT4215 630 to run rough on full throttle + mov Temp2, #20 + mov A, Comm_Period4x_H ; Set number of readings higher for lower speeds + clr C + rrc A + jnz ($+3) + inc A + mov Temp1, A + clr C + subb A, #20 + jc ($+4) + + mov Temp1, #20 + + jnb Flag_STARTUP_PHASE, comp_scale_samples + + mov Temp1, #27 ; Set many samples during startup, approximately one pwm period + mov Temp2, #27 + +comp_scale_samples: +IF MCU_48MHZ == 1 + clr C + mov A, Temp1 + rlc A + mov Temp1, A + clr C + mov A, Temp2 + rlc A + mov Temp2, A +ENDIF +comp_check_timeout: + jb Flag_T3_PENDING, comp_check_timeout_not_timed_out ; Has zero cross scan timeout elapsed? + + mov A, Comparator_Read_Cnt ; Check that comparator has been read + jz comp_check_timeout_not_timed_out ; If not read - branch + + jnb Flag_STARTUP_PHASE, comp_check_timeout_timeout_extended ; Extend timeout during startup + + djnz Startup_Zc_Timeout_Cntd, comp_check_timeout_extend_timeout + +comp_check_timeout_timeout_extended: + setb Flag_COMP_TIMED_OUT + sjmp setup_comm_wait + +comp_check_timeout_extend_timeout: + call setup_zc_scan_timeout +comp_check_timeout_not_timed_out: + inc Comparator_Read_Cnt ; Increment comparator read count + Read_Comp_Out ; Read comparator output + anl A, #40h + cjne A, Bit_Access, comp_read_wrong + + ; Comp read ok + mov A, Startup_Cnt ; Force a timeout for the first commutation + jz wait_for_comp_out_start + + jb Flag_DEMAG_DETECTED, wait_for_comp_out_start ; Do not accept correct comparator output if it is demag + + djnz Temp1, comp_check_timeout ; Decrement readings counter - repeat comparator reading if not zero + + clr Flag_COMP_TIMED_OUT + + sjmp setup_comm_wait + +comp_read_wrong: + jnb Flag_STARTUP_PHASE, comp_read_wrong_not_startup + + inc Temp1 ; Increment number of OK readings required + clr C + mov A, Temp1 + subb A, Temp2 ; If above initial requirement - do not increment further + jc ($+3) + dec Temp1 + + sjmp comp_check_timeout ; Continue to look for good ones + +comp_read_wrong_not_startup: + jb Flag_DEMAG_DETECTED, comp_read_wrong_extend_timeout + + inc Temp1 ; Increment number of OK readings required + clr C + mov A, Temp1 + subb A, Temp2 + jc comp_check_timeout ; If below initial requirement - take another reading + sjmp wait_for_comp_out_start ; Otherwise - go back and restart + +comp_read_wrong_extend_timeout: + clr Flag_DEMAG_DETECTED ; Clear demag detected flag + anl EIE1, #7Fh ; Disable timer 3 interrupts + mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared + jnb Flag_HIGH_RPM, comp_read_wrong_low_rpm ; Branch if not high rpm + + mov TMR3L, #00h ; Set timeout to ~1ms +IF MCU_48MHZ == 1 + mov TMR3H, #0F0h +ELSE + mov TMR3H, #0F8h +ENDIF +comp_read_wrong_timeout_set: + mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared + setb Flag_T3_PENDING + orl EIE1, #80h ; Enable timer 3 interrupts + jmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart + +comp_read_wrong_low_rpm: + mov A, Comm_Period4x_H ; Set timeout to ~4x comm period 4x value + mov Temp7, #0FFh ; Default to long +IF MCU_48MHZ == 1 + clr C + rlc A + jc comp_read_wrong_load_timeout + +ENDIF + clr C + rlc A + jc comp_read_wrong_load_timeout + + clr C + rlc A + jc comp_read_wrong_load_timeout + + mov Temp7, A + +comp_read_wrong_load_timeout: + clr C + clr A + subb A, Temp7 + mov TMR3L, #0 + mov TMR3H, A + sjmp comp_read_wrong_timeout_set + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Setup commutation timing routine +; +; Sets up and starts wait from commutation to zero cross +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +setup_comm_wait: + clr IE_EA + anl EIE1, #7Fh ; Disable timer 3 interrupts + mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared + mov TMR3L, Wt_Comm_Start_L + mov TMR3H, Wt_Comm_Start_H + mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared + ; Setup next wait time + mov TMR3RLL, Wt_Adv_Start_L + mov TMR3RLH, Wt_Adv_Start_H + setb Flag_T3_PENDING + orl EIE1, #80h ; Enable timer 3 interrupts + setb IE_EA ; Enable interrupts again + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Evaluate comparator integrity +; +; Checks comparator signal behavior versus expected behavior +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +evaluate_comparator_integrity: + mov A, Flags_Startup + jz eval_comp_check_timeout + + jb Flag_INITIAL_RUN_PHASE, ($+5) ; Do not increment beyond startup phase + inc Startup_Cnt ; Increment counter + sjmp eval_comp_exit + +eval_comp_check_timeout: + jnb Flag_COMP_TIMED_OUT, eval_comp_exit ; Has timeout elapsed? + jb Flag_DIR_CHANGE_BRAKE, eval_comp_exit ; Do not exit run mode if it is 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 + dec SP + ljmp run_to_wait_for_power_on_fail ; Yes - exit run mode + +eval_comp_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for commutation routine +; +; Waits from zero cross to commutation +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comm: + ; Update demag metric + mov A, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120 + mov B, #7 + mul AB ; Multiply by 7 + + jnb Flag_DEMAG_DETECTED, ($+4) ; Add new value for current demag status + inc B + + mov C, B.0 ; Divide by 8 + rrc A + mov C, B.1 + rrc A + mov C, B.2 + rrc A + mov Demag_Detected_Metric, A + clr C + subb A, #120 ; Limit to minimum 120 + jnc ($+5) + mov Demag_Detected_Metric, #120 + + clr C + mov A, Demag_Detected_Metric ; Check demag metric + subb A, Demag_Pwr_Off_Thresh + jc wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations + + All_pwmFETs_off + Set_Pwms_Off + +wait_for_comm_wait: + Wait_For_Timer3 + + ; Setup next wait time + mov TMR3RLL, Wt_Zc_Scan_Start_L + mov TMR3RLH, Wt_Zc_Scan_Start_H + setb Flag_T3_PENDING + orl EIE1, #80h ; Enable timer 3 interrupts + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Commutation routines +; +; Performs commutation switching +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; Comm phase 1 to comm phase 2 +comm1comm2: + Set_RPM_Out + jb Flag_PGM_DIR_REV, comm12_rev + + clr IE_EA ; Disable all interrupts + BcomFET_off ; Turn off comfet + AcomFET_on ; Turn on comfet + Set_Pwm_C ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_B ; Set comparator phase + ret + +comm12_rev: + clr IE_EA ; Disable all interrupts + BcomFET_off ; Turn off comfet + CcomFET_on ; Turn on comfet (reverse) + Set_Pwm_A ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_B ; Set comparator phase + ret + + +; Comm phase 2 to comm phase 3 +comm2comm3: + Clear_RPM_Out + jb Flag_PGM_DIR_REV, comm23_rev + + clr IE_EA ; Disable all interrupts + CpwmFET_off ; Turn off pwmfet + Set_Pwm_B ; To reapply power after a demag cut + AcomFET_on + setb IE_EA + Set_Comp_Phase_C ; Set comparator phase + ret + +comm23_rev: + clr IE_EA ; Disable all interrupts + ApwmFET_off ; Turn off pwmfet (reverse) + Set_Pwm_B ; To reapply power after a demag cut + CcomFET_on + setb IE_EA + Set_Comp_Phase_A ; Set comparator phase (reverse) + ret + + +; Comm phase 3 to comm phase 4 +comm3comm4: + Set_RPM_Out + jb Flag_PGM_DIR_REV, comm34_rev + + clr IE_EA ; Disable all interrupts + AcomFET_off ; Turn off comfet + CcomFET_on ; Turn on comfet + Set_Pwm_B ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_A ; Set comparator phase + ret + +comm34_rev: + clr IE_EA ; Disable all interrupts + CcomFET_off ; Turn off comfet (reverse) + AcomFET_on ; Turn on comfet (reverse) + Set_Pwm_B ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_C ; Set comparator phase (reverse) + ret + + +; Comm phase 4 to comm phase 5 +comm4comm5: + Clear_RPM_Out + jb Flag_PGM_DIR_REV, comm45_rev + + clr IE_EA ; Disable all interrupts + BpwmFET_off ; Turn off pwmfet + Set_Pwm_A ; To reapply power after a demag cut + CcomFET_on + setb IE_EA + Set_Comp_Phase_B ; Set comparator phase + ret + +comm45_rev: + clr IE_EA ; Disable all interrupts + BpwmFET_off ; Turn off pwmfet + Set_Pwm_C + AcomFET_on ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_B ; Set comparator phase + ret + + +; Comm phase 5 to comm phase 6 +comm5comm6: + Set_RPM_Out + jb Flag_PGM_DIR_REV, comm56_rev + + clr IE_EA ; Disable all interrupts + CcomFET_off ; Turn off comfet + BcomFET_on ; Turn on comfet + Set_Pwm_A ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_C ; Set comparator phase + ret + +comm56_rev: + clr IE_EA ; Disable all interrupts + AcomFET_off ; Turn off comfet (reverse) + BcomFET_on ; Turn on comfet + Set_Pwm_C ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_A ; Set comparator phase (reverse) + ret + + +; Comm phase 6 to comm phase 1 +comm6comm1: + Clear_RPM_Out + jb Flag_PGM_DIR_REV, comm61_rev + + clr IE_EA ; Disable all interrupts + ApwmFET_off ; Turn off pwmfet + Set_Pwm_C + BcomFET_on ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_A ; Set comparator phase + ret + +comm61_rev: + clr IE_EA ; Disable all interrupts + CpwmFET_off ; Turn off pwmfet (reverse) + Set_Pwm_A + BcomFET_on ; To reapply power after a demag cut + setb IE_EA + Set_Comp_Phase_C ; Set comparator phase (reverse) + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Detect DShot RCP level +; +; Determine if RCP signal level is normal or inverted DShot +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +detect_rcp_level: + mov A, #100 ; Must repeat the same level 100 times in a row + mov C, RTX_PORT.RTX_PIN + +detect_rcp_level_read: + jc ($+5) + jb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from low to high - start over + jnc ($+5) + jnb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from high to low - start over + djnz ACC, detect_rcp_level_read + + mov Flag_RCP_DSHOT_INVERTED, C ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Wait for comparator to go low/high routines +; Check DShot command ; -; Waits for the zero cross scan wait time to elapse -; Then scans for comparator going low/high +; Determine received DShot command and perform action ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -wait_for_comp_out_low: - setb Flag_DEMAG_DETECTED ; Set demag detected flag as default - mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads - mov Bit_Access, #00h ; Desired comparator output - jnb Flag_DIR_CHANGE_BRAKE, ($+6) - mov Bit_Access, #40h - sjmp wait_for_comp_out_start - -wait_for_comp_out_high: - setb Flag_DEMAG_DETECTED ; Set demag detected flag as default - mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads - mov Bit_Access, #40h ; Desired comparator output - jnb Flag_DIR_CHANGE_BRAKE, ($+6) - mov Bit_Access, #00h +dshot_cmd_check: + mov A, Dshot_Cmd + jz dshot_cmd_exit_no_clear -wait_for_comp_out_start: - ; Set number of comparator readings - mov Temp1, #1 ; Number of OK readings required - mov Temp2, #1 ; Max number of readings required - jb Flag_HIGH_RPM, comp_scale_samples ; Branch if high rpm + mov Temp1, A + clr C + subb A, #6 ; Beacon beeps for command 1-5 + jnc dshot_cmd_direction_1 - mov A, Flags_Startup ; Clear demag detected flag if start phases - jz ($+4) + call beacon_beep + sjmp dshot_cmd_exit - clr Flag_DEMAG_DETECTED +dshot_cmd_direction_1: + ; Change programmed motor direction to normal + cjne Temp1, #7, dshot_cmd_direction_2 - ; Too low value (~<15) causes rough running at pwm harmonics. - ; Too high a value (~>35) causes the RCT4215 630 to run rough on full throttle - mov Temp2, #20 - mov A, Comm_Period4x_H ; Set number of readings higher for lower speeds - clr C - rrc A - jnz ($+3) - inc A - mov Temp1, A clr C - subb A, #20 - jc ($+4) - - mov Temp1, #20 + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear - jnb Flag_STARTUP_PHASE, comp_scale_samples + mov A, #1 + jnb Flag_PGM_BIDIR, ($+5) + mov A, #3 + mov Temp1, #Pgm_Direction + mov @Temp1, A + clr Flag_PGM_DIR_REV + clr Flag_PGM_BIDIR_REV + sjmp dshot_cmd_exit - mov Temp1, #27 ; Set many samples during startup, approximately one pwm period - mov Temp2, #27 +dshot_cmd_direction_2: + ; Change programmed motor direction to reversed + cjne Temp1, #8, dshot_cmd_direction_bidir_off -comp_scale_samples: -IF MCU_48MHZ == 1 - clr C - mov A, Temp1 - rlc A - mov Temp1, A clr C - mov A, Temp2 - rlc A - mov Temp2, A -ENDIF -comp_check_timeout: - jb Flag_T3_PENDING, comp_check_timeout_not_timed_out ; Has zero cross scan timeout elapsed? + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear - mov A, Comparator_Read_Cnt ; Check that comparator has been read - jz comp_check_timeout_not_timed_out ; If not read - branch + mov A, #2 + jnb Flag_PGM_BIDIR, ($+5) + mov A, #4 + mov Temp1, #Pgm_Direction + mov @Temp1, A + setb Flag_PGM_DIR_REV + setb Flag_PGM_BIDIR_REV + sjmp dshot_cmd_exit - jnb Flag_STARTUP_PHASE, comp_check_timeout_timeout_extended ; Extend timeout during startup +dshot_cmd_direction_bidir_off: + ; Change programmed motor mode to normal (not bidirectional) + cjne Temp1, #9, dshot_cmd_direction_bidir_on - djnz Startup_Zc_Timeout_Cntd, comp_check_timeout_extend_timeout + clr C + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear -comp_check_timeout_timeout_extended: - setb Flag_COMP_TIMED_OUT - sjmp setup_comm_wait + jnb Flag_PGM_BIDIR, dshot_cmd_exit -comp_check_timeout_extend_timeout: - call setup_zc_scan_timeout -comp_check_timeout_not_timed_out: - inc Comparator_Read_Cnt ; Increment comparator read count - Read_Comp_Out ; Read comparator output - anl A, #40h - cjne A, Bit_Access, comp_read_wrong + clr C + mov Temp1, #Pgm_Direction + mov A, @Temp1 + subb A, #2 + mov @Temp1, A + clr Flag_PGM_BIDIR + sjmp dshot_cmd_exit - ; Comp read ok - mov A, Startup_Cnt ; Force a timeout for the first commutation - jz wait_for_comp_out_start +dshot_cmd_direction_bidir_on: + ; Change programmed motor mode to bidirectional + cjne Temp1, #10, dshot_cmd_direction_normal - jb Flag_DEMAG_DETECTED, wait_for_comp_out_start ; Do not accept correct comparator output if it is demag + clr C + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear - djnz Temp1, comp_check_timeout ; Decrement readings counter - repeat comparator reading if not zero + jb Flag_PGM_BIDIR, dshot_cmd_exit - clr Flag_COMP_TIMED_OUT + mov Temp1, #Pgm_Direction + mov A, @Temp1 + add A, #2 + mov @Temp1, A + setb Flag_PGM_BIDIR - sjmp setup_comm_wait +dshot_cmd_exit: + mov Dshot_Cmd, #0 + mov Dshot_Cmd_Cnt, #0 -comp_read_wrong: - jnb Flag_STARTUP_PHASE, comp_read_wrong_not_startup +dshot_cmd_exit_no_clear: + ret + +dshot_cmd_direction_normal: + ; Change programmed motor direction to that stored in eeprom + cjne Temp1, #20, dshot_cmd_direction_reverse - inc Temp1 ; Increment number of OK readings required clr C - mov A, Temp1 - subb A, Temp2 ; If above initial requirement - do not increment further - jc ($+3) - dec Temp1 + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear - sjmp comp_check_timeout ; Continue to look for good ones + clr IE_EA ; DPTR used in interrupts + mov DPTR, #Eep_Pgm_Direction ; Read from flash + mov A, #0 + movc A, @A+DPTR + setb IE_EA + mov Temp1, #Pgm_Direction + mov @Temp1, A + rrc A ; Lsb to carry + clr Flag_PGM_DIR_REV + clr Flag_PGM_BIDIR_REV + jc ($+4) + setb Flag_PGM_DIR_REV + jc ($+4) + setb Flag_PGM_BIDIR_REV + sjmp dshot_cmd_exit -comp_read_wrong_not_startup: - jb Flag_DEMAG_DETECTED, comp_read_wrong_extend_timeout +dshot_cmd_direction_reverse: ; Temporary reverse + ; Change programmed motor direction to the reverse of what is stored in eeprom + cjne Temp1, #21, dshot_cmd_save_settings - inc Temp1 ; Increment number of OK readings required clr C - mov A, Temp1 - subb A, Temp2 - jc comp_check_timeout ; If below initial requirement - take another reading - sjmp wait_for_comp_out_start ; Otherwise - go back and restart + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear -comp_read_wrong_extend_timeout: - clr Flag_DEMAG_DETECTED ; Clear demag detected flag - anl EIE1, #7Fh ; Disable timer 3 interrupts - mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared - jnb Flag_HIGH_RPM, comp_read_wrong_low_rpm ; Branch if not high rpm + clr IE_EA ; DPTR used in interrupts + mov DPTR, #Eep_Pgm_Direction ; Read from flash + mov A, #0 + movc A, @A+DPTR + setb IE_EA + mov Temp1, A + cjne Temp1, #1, ($+5) + mov A, #2 + cjne Temp1, #2, ($+5) + mov A, #1 + cjne Temp1, #3, ($+5) + mov A, #4 + cjne Temp1, #4, ($+5) + mov A, #3 + mov Temp1, #Pgm_Direction + mov @Temp1, A + rrc A ; Lsb to carry + clr Flag_PGM_DIR_REV + clr Flag_PGM_BIDIR_REV + jc ($+4) + setb Flag_PGM_DIR_REV + jc ($+4) + setb Flag_PGM_BIDIR_REV + sjmp dshot_cmd_exit - mov TMR3L, #00h ; Set timeout to ~1ms -IF MCU_48MHZ == 1 - mov TMR3H, #0F0h -ELSE - mov TMR3H, #0F8h -ENDIF -comp_read_wrong_timeout_set: - mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared - setb Flag_T3_PENDING - orl EIE1, #80h ; Enable timer 3 interrupts - ajmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart +dshot_cmd_save_settings: + cjne Temp1, #12, dshot_cmd_exit -comp_read_wrong_low_rpm: - mov A, Comm_Period4x_H ; Set timeout to ~4x comm period 4x value - mov Temp7, #0FFh ; Default to long -IF MCU_48MHZ == 1 clr C - rlc A - jc comp_read_wrong_load_timeout + mov A, Dshot_Cmd_Cnt + subb A, #6 ; Needs to receive it 6 times in a row + jc dshot_cmd_exit_no_clear + + mov Flash_Key_1, #0A5h ; Initialize flash keys to valid values + mov Flash_Key_2, #0F1h -ENDIF - clr C - rlc A - jc comp_read_wrong_load_timeout + call erase_and_store_all_in_eeprom - clr C - rlc A - jc comp_read_wrong_load_timeout + mov Flash_Key_1, #0 ; Initialize flash keys to invalid values + mov Flash_Key_2, #0 - mov Temp7, A + setb IE_EA -comp_read_wrong_load_timeout: - clr C - clr A - subb A, Temp7 - mov TMR3L, #0 - mov TMR3H, A - sjmp comp_read_wrong_timeout_set + jmp dshot_cmd_exit ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Setup commutation timing routine +; DShot beacon beep ; -; Sets up and starts wait from commutation to zero cross +; Beep with beacon strength +; Beep type 1-5 in Temp1 ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -setup_comm_wait: +beacon_beep: clr IE_EA - anl EIE1, #7Fh ; Disable timer 3 interrupts - mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared - mov TMR3L, Wt_Comm_Start_L - mov TMR3H, Wt_Comm_Start_H - mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared - ; Setup next wait time - mov TMR3RLL, Wt_Adv_Start_L - mov TMR3RLH, Wt_Adv_Start_H - setb Flag_T3_PENDING - orl EIE1, #80h ; Enable timer 3 interrupts - setb IE_EA ; Enable interrupts again + call switch_power_off ; Switch power off in case braking is set + mov Temp2, #Pgm_Beacon_Strength + mov Beep_Strength, @Temp2 + + cjne Temp1, #1, beacon_beep2 + call beep_f1 + sjmp beacon_beep_exit + +beacon_beep2: + cjne Temp1, #2, beacon_beep3 + call beep_f2 + sjmp beacon_beep_exit + +beacon_beep3: + cjne Temp1, #3, beacon_beep4 + call beep_f3 + sjmp beacon_beep_exit + +beacon_beep4: + cjne Temp1, #4, beacon_beep5 + call beep_f4 + sjmp beacon_beep_exit + +beacon_beep5: + call beep_f5 + +beacon_beep_exit: + mov Temp2, #Pgm_Beep_Strength + mov Beep_Strength, @Temp2 + setb IE_EA + call wait100ms + ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Evaluate comparator integrity +; DShot telemetry create packet ; -; Checks comparator signal behavior versus expected behavior +; Create DShot telemetry packet and prepare it for being sent +; The routine is divided into 6 sections that can return early +; in order to reduce commutation interference ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -evaluate_comparator_integrity: - mov A, Flags_Startup - jz eval_comp_check_timeout +dshot_tlm_create_packet: + push PSW + mov PSW, #10h ; Select register bank 2 - jb Flag_INITIAL_RUN_PHASE, ($+5) ; Do not increment beyond startup phase - inc Startup_Cnt ; Increment counter - sjmp eval_comp_exit + Early_Return_Packet_Stage 0 -eval_comp_check_timeout: - jnb Flag_COMP_TIMED_OUT, eval_comp_exit ; Has timeout elapsed? - jb Flag_DIR_CHANGE_BRAKE, eval_comp_exit ; Do not exit run mode if it is 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 - dec SP - ljmp run_to_wait_for_power_on_fail ; Yes - exit run mode + ; Read commutation period + clr IE_EA + mov Tlm_Data_L, Comm_Period4x_L + mov Tlm_Data_H, Comm_Period4x_H + setb IE_EA -eval_comp_exit: + ; Multiply period by 3/4 (1/2 + 1/4) + mov A, Tlm_Data_L + mov C, Tlm_Data_H.0 + rrc A + mov Temp2, A + mov C, Tlm_Data_H.1 + rrc A + add A, Temp2 + mov Tlm_Data_L, A + + mov A, Tlm_Data_H + rr A + clr ACC.7 + mov Temp2, A + rr A + clr ACC.7 + addc A, Temp2 + mov Tlm_Data_H, A + + Early_Return_Packet_Stage 1 + mov A, Tlm_Data_H + + ; 12-bit encode telemetry data + jnz dshot_12bit_encode + mov A, Tlm_Data_L ; Already 12-bit + jnz dshot_tlm_12bit_encoded + + ; If period is zero then reset to FFFFh (FFFh for 12-bit) + mov Tlm_Data_H, #0Fh + mov Tlm_Data_L, #0FFh + +dshot_tlm_12bit_encoded: + Early_Return_Packet_Stage 2 + mov A, Tlm_Data_L + + ; Compute inverted xor checksum (4-bit) + swap A + xrl A, Tlm_Data_L + xrl A, Tlm_Data_H + cpl A + + ; GCR encode the telemetry data (16-bit) + mov Temp1, #Temp_Storage ; Store pulse timings in Temp_Storage + mov Tmp_B, DShot_GCR_Pulse_Time_1 ; Final transition time + + call dshot_gcr_encode ; GCR encode lowest 4-bit of A (store through Temp1) + + Early_Return_Packet_Stage 3 + + mov A, Tlm_Data_L + call dshot_gcr_encode + + Early_Return_Packet_Stage 4 + + mov A, Tlm_Data_L + swap A + call dshot_gcr_encode + + Early_Return_Packet_Stage 5 + + mov A, Tlm_Data_H + call dshot_gcr_encode + + Push_Mem Temp1, Tmp_B ; Initial transition time + + mov Temp5, #0 + setb Flag_PACKET_PENDING + + pop PSW ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Wait for commutation routine +; DShot 12-bit encode ; -; Waits from zero cross to commutation +; Encodes 16-bit e-period as a 12-bit value of the form: +; where M SHL E ~ e-period [us] +; +; Note: Not callable to improve performance ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -wait_for_comm: - ; Update demag metric - mov A, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120 - mov B, #7 - mul AB ; Multiply by 7 +dshot_12bit_encode: + ; Encode 16-bit e-period as a 12-bit value + jb ACC.7, dshot_12bit_7 ; ACC = Tlm_Data_H + jb ACC.6, dshot_12bit_6 + jb ACC.5, dshot_12bit_5 + jb ACC.4, dshot_12bit_4 + jb ACC.3, dshot_12bit_3 + jb ACC.2, dshot_12bit_2 + jb ACC.1, dshot_12bit_1 + mov A, Tlm_Data_L ; Already 12-bit (E=0) + ajmp dshot_tlm_12bit_encoded - jnb Flag_DEMAG_DETECTED, ($+4) ; Add new value for current demag status - inc B +dshot_12bit_7: + ;mov A, Tlm_Data_H + mov C, Tlm_Data_L.7 + rlc A + mov Tlm_Data_L, A + mov Tlm_Data_H, #0fh + ajmp dshot_tlm_12bit_encoded - mov C, B.0 ; Divide by 8 +dshot_12bit_6: + ;mov A, Tlm_Data_H + mov C, Tlm_Data_L.7 + rlc A + mov C, Tlm_Data_L.6 + rlc A + mov Tlm_Data_L, A + mov Tlm_Data_H, #0dh + ajmp dshot_tlm_12bit_encoded + +dshot_12bit_5: + ;mov A, Tlm_Data_H + mov C, Tlm_Data_L.7 + rlc A + mov C, Tlm_Data_L.6 + rlc A + mov C, Tlm_Data_L.5 + rlc A + mov Tlm_Data_L, A + mov Tlm_Data_H, #0bh + ajmp dshot_tlm_12bit_encoded + +dshot_12bit_4: + mov A, Tlm_Data_L + anl A, #0f0h + clr Tlm_Data_H.4 + orl A, Tlm_Data_H + swap A + mov Tlm_Data_L, A + mov Tlm_Data_H, #09h + ajmp dshot_tlm_12bit_encoded + +dshot_12bit_3: + mov A, Tlm_Data_L + mov C, Tlm_Data_H.0 rrc A - mov C, B.1 + mov C, Tlm_Data_H.1 rrc A - mov C, B.2 + mov C, Tlm_Data_H.2 rrc A - mov Demag_Detected_Metric, A - clr C - subb A, #120 ; Limit to minimum 120 - jnc ($+5) - mov Demag_Detected_Metric, #120 - - clr C - mov A, Demag_Detected_Metric ; Check demag metric - subb A, Demag_Pwr_Off_Thresh - jc wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations - - All_pwmFETs_off - Set_Pwms_Off + mov Tlm_Data_L, A + mov Tlm_Data_H, #07h + ajmp dshot_tlm_12bit_encoded -wait_for_comm_wait: - Wait_For_Timer3 +dshot_12bit_2: + mov A, Tlm_Data_L + mov C, Tlm_Data_H.0 + rrc A + mov C, Tlm_Data_H.1 + rrc A + mov Tlm_Data_L, A + mov Tlm_Data_H, #05h + ajmp dshot_tlm_12bit_encoded - ; Setup next wait time - mov TMR3RLL, Wt_Zc_Scan_Start_L - mov TMR3RLH, Wt_Zc_Scan_Start_H - setb Flag_T3_PENDING - orl EIE1, #80h ; Enable timer 3 interrupts - ret +dshot_12bit_1: + mov A, Tlm_Data_L + mov C, Tlm_Data_H.0 + rrc A + mov Tlm_Data_L, A + mov Tlm_Data_H, #03h + ajmp dshot_tlm_12bit_encoded ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Commutation routines +; DShot GCR encode ; -; Performs commutation switching +; GCR encode e-period data for DShot telemetry +; +; Input +; - Temp1: Data pointer for storing pulse timings +; - A: 4-bit value to GCR encode +; - B: Time that must be added to transition +; Output +; - B: Time remaining to be added to next transition ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -; Comm phase 1 to comm phase 2 -comm1comm2: - Set_RPM_Out - jb Flag_PGM_DIR_REV, comm12_rev - - clr IE_EA ; Disable all interrupts - BcomFET_off ; Turn off comfet - AcomFET_on ; Turn on comfet - Set_Pwm_C ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_B ; Set comparator phase - ret - -comm12_rev: - clr IE_EA ; Disable all interrupts - BcomFET_off ; Turn off comfet - CcomFET_on ; Turn on comfet (reverse) - Set_Pwm_A ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_B ; Set comparator phase - ret - - -; Comm phase 2 to comm phase 3 -comm2comm3: - Clear_RPM_Out - jb Flag_PGM_DIR_REV, comm23_rev - - clr IE_EA ; Disable all interrupts - CpwmFET_off ; Turn off pwmfet - Set_Pwm_B ; To reapply power after a demag cut - AcomFET_on - setb IE_EA - Set_Comp_Phase_C ; Set comparator phase - ret - -comm23_rev: - clr IE_EA ; Disable all interrupts - ApwmFET_off ; Turn off pwmfet (reverse) - Set_Pwm_B ; To reapply power after a demag cut - CcomFET_on - setb IE_EA - Set_Comp_Phase_A ; Set comparator phase (reverse) - ret - +dshot_gcr_encode: + anl A, #0Fh + rl A ; Multiply by 2 to match jump offsets + mov DPTR, #dshot_gcr_encode_jump_table + jmp @A+DPTR -; Comm phase 3 to comm phase 4 -comm3comm4: - Set_RPM_Out - jb Flag_PGM_DIR_REV, comm34_rev +dshot_gcr_encode_jump_table: + ajmp dshot_gcr_encode_0_11001 + ajmp dshot_gcr_encode_1_11011 + ajmp dshot_gcr_encode_2_10010 + ajmp dshot_gcr_encode_3_10011 + ajmp dshot_gcr_encode_4_11101 + ajmp dshot_gcr_encode_5_10101 + ajmp dshot_gcr_encode_6_10110 + ajmp dshot_gcr_encode_7_10111 + ajmp dshot_gcr_encode_8_11010 + ajmp dshot_gcr_encode_9_01001 + ajmp dshot_gcr_encode_A_01010 + ajmp dshot_gcr_encode_B_01011 + ajmp dshot_gcr_encode_C_11110 + ajmp dshot_gcr_encode_D_01101 + ajmp dshot_gcr_encode_E_01110 + ajmp dshot_gcr_encode_F_01111 - clr IE_EA ; Disable all interrupts - AcomFET_off ; Turn off comfet - CcomFET_on ; Turn on comfet - Set_Pwm_B ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_A ; Set comparator phase +; GCR encoding is ordered by least significant bit first, +; and represented as pulse durations. +dshot_gcr_encode_0_11001: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_3 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret -comm34_rev: - clr IE_EA ; Disable all interrupts - CcomFET_off ; Turn off comfet (reverse) - AcomFET_on ; Turn on comfet (reverse) - Set_Pwm_B ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_C ; Set comparator phase (reverse) +dshot_gcr_encode_1_11011: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret - -; Comm phase 4 to comm phase 5 -comm4comm5: - Clear_RPM_Out - jb Flag_PGM_DIR_REV, comm45_rev - - clr IE_EA ; Disable all interrupts - BpwmFET_off ; Turn off pwmfet - Set_Pwm_A ; To reapply power after a demag cut - CcomFET_on - setb IE_EA - Set_Comp_Phase_B ; Set comparator phase +dshot_gcr_encode_2_10010: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_3 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret -comm45_rev: - clr IE_EA ; Disable all interrupts - BpwmFET_off ; Turn off pwmfet - Set_Pwm_C - AcomFET_on ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_B ; Set comparator phase +dshot_gcr_encode_3_10011: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_3 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret - -; Comm phase 5 to comm phase 6 -comm5comm6: - Set_RPM_Out - jb Flag_PGM_DIR_REV, comm56_rev - - clr IE_EA ; Disable all interrupts - CcomFET_off ; Turn off comfet - BcomFET_on ; Turn on comfet - Set_Pwm_A ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_C ; Set comparator phase +dshot_gcr_encode_4_11101: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret -comm56_rev: - clr IE_EA ; Disable all interrupts - AcomFET_off ; Turn off comfet (reverse) - BcomFET_on ; Turn on comfet - Set_Pwm_C ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_A ; Set comparator phase (reverse) +dshot_gcr_encode_5_10101: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret - -; Comm phase 6 to comm phase 1 -comm6comm1: - Clear_RPM_Out - jb Flag_PGM_DIR_REV, comm61_rev - - clr IE_EA ; Disable all interrupts - ApwmFET_off ; Turn off pwmfet - Set_Pwm_C - BcomFET_on ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_A ; Set comparator phase +dshot_gcr_encode_6_10110: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret -comm61_rev: - clr IE_EA ; Disable all interrupts - CpwmFET_off ; Turn off pwmfet (reverse) - Set_Pwm_A - BcomFET_on ; To reapply power after a demag cut - setb IE_EA - Set_Comp_Phase_C ; Set comparator phase (reverse) +dshot_gcr_encode_7_10111: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + mov Tmp_B, DShot_GCR_Pulse_Time_1 ret +dshot_gcr_encode_8_11010: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_1 + ret -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; Beeper routines (Multiple entry points) -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -beep_f1: - mov Temp3, #66 ; Off wait loop length (Tone) - mov Temp4, #(3500 / 66) ; Number of beep pulses (Duration) - sjmp beep - -beep_f2: - mov Temp3, #45 - mov Temp4, #(3500 / 45) - sjmp beep - -beep_f3: - mov Temp3, #38 - mov Temp4, #(3500 / 38) - sjmp beep - -beep_f4: - mov Temp3, #25 - mov Temp4, #(3500 / 25) - sjmp beep - -beep_f5: - mov Temp3, #20 - mov Temp4, #(3500 / 25) - sjmp beep - -beep_f1_short: - mov Temp3, #66 - mov Temp4, #(2000 / 66) - sjmp beep +dshot_gcr_encode_9_01001: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_3 + mov Tmp_B, DShot_GCR_Pulse_Time_2 + ret -beep_f2_short: - mov Temp3, #45 - mov Temp4, #(2000 / 45) - sjmp beep +dshot_gcr_encode_A_01010: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + mov Tmp_B, DShot_GCR_Pulse_Time_2 + ret -beep: - mov A, Beep_Strength - djnz ACC, beep_start ; Start if beep strength is not 1 +dshot_gcr_encode_B_01011: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + mov Tmp_B, DShot_GCR_Pulse_Time_2 ret -beep_start: - mov Temp2, #2 -beep_onoff: - clr A - BcomFET_off ; BcomFET off - djnz ACC, $ ; Allow some time after comfet is turned off - BpwmFET_on ; BpwmFET on (in order to charge the driver of the BcomFET) - djnz ACC, $ ; Let the pwmfet be turned on a while - BpwmFET_off ; BpwmFET off again - djnz ACC, $ ; Allow some time after pwmfet is turned off - BcomFET_on ; BcomFET on - djnz ACC, $ ; Allow some time after comfet is turned on - ; Turn on pwmfet - mov A, Temp2 - jb ACC.0, beep_apwmfet_on - ApwmFET_on -beep_apwmfet_on: - jnb ACC.0, beep_cpwmfet_on - CpwmFET_on -beep_cpwmfet_on: - mov A, Beep_Strength - djnz ACC, $ - ; Turn off pwmfet - mov A, Temp2 - jb ACC.0, beep_apwmfet_off - ApwmFET_off -beep_apwmfet_off: - jnb ACC.0, beep_cpwmfet_off - CpwmFET_off -beep_cpwmfet_off: - mov A, #150 ; 25us off - djnz ACC, $ - djnz Temp2, beep_onoff - ; Copy variable - mov A, Temp3 - mov Temp1, A -beep_off: ; Fets off loop - mov A, #150 - djnz ACC, $ - djnz Temp1, beep_off - djnz Temp4, beep_start +dshot_gcr_encode_C_11110: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_1 + ret - BcomFET_off +dshot_gcr_encode_D_01101: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_2 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_2 ret +dshot_gcr_encode_E_01110: + DShot_GCR_Get_Time + Push_Mem Temp1, A + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_2 + ret -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; Switch power off routine -; -; Switches all fets off -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -switch_power_off: - All_pwmFETs_Off ; Turn off all pwm fets - All_comFETs_Off ; Turn off all commutation fets - Set_Pwms_Off +dshot_gcr_encode_F_01111: + Push_Mem Temp1, Tmp_B + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + Push_Mem Temp1, DShot_GCR_Pulse_Time_1 + mov Tmp_B, DShot_GCR_Pulse_Time_2 ret @@ -2808,375 +3089,94 @@ set_default_parameters: Push_Mem Temp1, #DEFAULT_PGM_ENABLE_TEMP_PROT ; Pgm_Enable_Temp_Prot Push_Mem Temp1, #DEFAULT_PGM_ENABLE_POWER_PROT ; Pgm_Enable_Power_Prot Push_Mem Temp1, #0FFh ; _Pgm_Enable_Pwm_Input - Push_Mem Temp1, #0FFh ; _Pgm_Pwm_Dither - Push_Mem Temp1, #DEFAULT_PGM_BRAKE_ON_STOP ; Pgm_Brake_On_Stop - Push_Mem Temp1, #DEFAULT_PGM_LED_CONTROL ; Pgm_LED_Control - - ret - - -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; Decode settings -; -; Decodes various settings -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -decode_settings: - ; Load programmed direction - mov Temp1, #Pgm_Direction - mov A, @Temp1 - clr C - subb A, #3 - setb Flag_PGM_BIDIR - jnc ($+4) - - clr Flag_PGM_BIDIR - - clr Flag_PGM_DIR_REV - mov A, @Temp1 - jnb ACC.1, ($+5) - setb Flag_PGM_DIR_REV - mov C, Flag_PGM_DIR_REV - mov Flag_PGM_BIDIR_REV, C - ; Decode startup power - mov Temp1, #Pgm_Startup_Pwr - mov A, @Temp1 - dec A - mov DPTR, #STARTUP_POWER_TABLE - movc A, @A+DPTR - mov Temp1, #Pgm_Startup_Pwr_Decoded - mov @Temp1, A - ; Decode low rpm power slope - mov Temp1, #Pgm_Startup_Pwr - mov A, @Temp1 - mov Low_Rpm_Pwr_Slope, A - clr C - subb A, #2 - jnc ($+5) - mov Low_Rpm_Pwr_Slope, #2 - ; Decode demag compensation - mov Temp1, #Pgm_Demag_Comp - mov A, @Temp1 - mov Demag_Pwr_Off_Thresh, #255 ; Set default - - cjne A, #2, decode_demag_high - - mov Demag_Pwr_Off_Thresh, #160 ; Settings for demag comp low - -decode_demag_high: - cjne A, #3, decode_demag_done - - mov Demag_Pwr_Off_Thresh, #130 ; Settings for demag comp high - -decode_demag_done: - ; Decode temperature protection limit - mov Temp1, #Pgm_Enable_Temp_Prot - mov A, @Temp1 - mov Temp1, A - jz decode_temp_done - - mov A, #(TEMP_LIMIT-TEMP_LIMIT_STEP) -decode_temp_step: - add A, #TEMP_LIMIT_STEP - djnz Temp1, decode_temp_step - -decode_temp_done: - mov Temp_Prot_Limit, A - - ; Dithering -IF PWM_BITS_H == 1 - mov Temp1, #Dithering_Patterns - Push_Mem Temp1, #00h - Push_Mem Temp1, #55h -ELSEIF PWM_BITS_H == 0 - mov Temp1, #Dithering_Patterns - Push_Mem Temp1, #00h - Push_Mem Temp1, #11h - Push_Mem Temp1, #55h - Push_Mem Temp1, #77h -ENDIF - ret - - -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; Detect DShot RCP level -; -; Determine if RCP signal level is normal or inverted DShot -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -detect_rcp_level: - mov A, #100 ; Must repeat the same level 100 times in a row - mov C, RTX_PORT.RTX_PIN - -detect_rcp_level_read: - jc ($+5) - jb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from low to high - start over - jnc ($+5) - jnb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from high to low - start over - djnz ACC, detect_rcp_level_read - - mov Flag_RCP_DSHOT_INVERTED, C - ret - - -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; LED control -; -; Controls LEDs -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -led_control: - mov Temp1, #Pgm_LED_Control - mov A, @Temp1 - mov Temp2, A - anl A, #03h - Set_LED_0 - jnz led_0_done - Clear_LED_0 -led_0_done: - mov A, Temp2 - anl A, #0Ch - Set_LED_1 - jnz led_1_done - Clear_LED_1 -led_1_done: - mov A, Temp2 - anl A, #030h - Set_LED_2 - jnz led_2_done - Clear_LED_2 -led_2_done: - mov A, Temp2 - anl A, #0C0h - Set_LED_3 - jnz led_3_done - Clear_LED_3 -led_3_done: + Push_Mem Temp1, #0FFh ; _Pgm_Pwm_Dither + Push_Mem Temp1, #DEFAULT_PGM_BRAKE_ON_STOP ; Pgm_Brake_On_Stop + Push_Mem Temp1, #DEFAULT_PGM_LED_CONTROL ; Pgm_LED_Control + ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; -; Check DShot command +; Decode settings ; -; Determine received DShot command and perform action +; Decodes various settings ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** -dshot_cmd_check: - mov A, Dshot_Cmd - jz dshot_cmd_exit_no_clear - - mov Temp1, A - clr C - subb A, #6 ; Beacon beeps for command 1-5 - jnc dshot_cmd_direction_1 - - call beacon_beep - sjmp dshot_cmd_exit - -dshot_cmd_direction_1: - ; Change programmed motor direction to normal - cjne Temp1, #7, dshot_cmd_direction_2 - - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - mov A, #1 - jnb Flag_PGM_BIDIR, ($+5) - mov A, #3 - mov Temp1, #Pgm_Direction - mov @Temp1, A - clr Flag_PGM_DIR_REV - clr Flag_PGM_BIDIR_REV - sjmp dshot_cmd_exit - -dshot_cmd_direction_2: - ; Change programmed motor direction to reversed - cjne Temp1, #8, dshot_cmd_direction_bidir_off - - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - mov A, #2 - jnb Flag_PGM_BIDIR, ($+5) - mov A, #4 - mov Temp1, #Pgm_Direction - mov @Temp1, A - setb Flag_PGM_DIR_REV - setb Flag_PGM_BIDIR_REV - sjmp dshot_cmd_exit - -dshot_cmd_direction_bidir_off: - ; Change programmed motor mode to normal (not bidirectional) - cjne Temp1, #9, dshot_cmd_direction_bidir_on - - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - jnb Flag_PGM_BIDIR, dshot_cmd_exit - - clr C +decode_settings: + ; Load programmed direction mov Temp1, #Pgm_Direction mov A, @Temp1 - subb A, #2 - mov @Temp1, A - clr Flag_PGM_BIDIR - sjmp dshot_cmd_exit - -dshot_cmd_direction_bidir_on: - ; Change programmed motor mode to bidirectional - cjne Temp1, #10, dshot_cmd_direction_normal - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - jb Flag_PGM_BIDIR, dshot_cmd_exit - - mov Temp1, #Pgm_Direction - mov A, @Temp1 - add A, #2 - mov @Temp1, A + subb A, #3 setb Flag_PGM_BIDIR + jnc ($+4) -dshot_cmd_exit: - mov Dshot_Cmd, #0 - mov Dshot_Cmd_Cnt, #0 - -dshot_cmd_exit_no_clear: - ret - -dshot_cmd_direction_normal: - ; Change programmed motor direction to that stored in eeprom - cjne Temp1, #20, dshot_cmd_direction_reverse - - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear + clr Flag_PGM_BIDIR - clr IE_EA ; DPTR used in interrupts - mov DPTR, #Eep_Pgm_Direction ; Read from flash - mov A, #0 - movc A, @A+DPTR - setb IE_EA - mov Temp1, #Pgm_Direction - mov @Temp1, A - rrc A ; Lsb to carry clr Flag_PGM_DIR_REV - clr Flag_PGM_BIDIR_REV - jc ($+4) + mov A, @Temp1 + jnb ACC.1, ($+5) setb Flag_PGM_DIR_REV - jc ($+4) - setb Flag_PGM_BIDIR_REV - sjmp dshot_cmd_exit - -dshot_cmd_direction_reverse: ; Temporary reverse - ; Change programmed motor direction to the reverse of what is stored in eeprom - cjne Temp1, #21, dshot_cmd_save_settings - - clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - clr IE_EA ; DPTR used in interrupts - mov DPTR, #Eep_Pgm_Direction ; Read from flash - mov A, #0 + mov C, Flag_PGM_DIR_REV + mov Flag_PGM_BIDIR_REV, C + ; Decode startup power + mov Temp1, #Pgm_Startup_Pwr + mov A, @Temp1 + dec A + mov DPTR, #STARTUP_POWER_TABLE movc A, @A+DPTR - setb IE_EA - mov Temp1, A - cjne Temp1, #1, ($+5) - mov A, #2 - cjne Temp1, #2, ($+5) - mov A, #1 - cjne Temp1, #3, ($+5) - mov A, #4 - cjne Temp1, #4, ($+5) - mov A, #3 - mov Temp1, #Pgm_Direction + mov Temp1, #Pgm_Startup_Pwr_Decoded mov @Temp1, A - rrc A ; Lsb to carry - clr Flag_PGM_DIR_REV - clr Flag_PGM_BIDIR_REV - jc ($+4) - setb Flag_PGM_DIR_REV - jc ($+4) - setb Flag_PGM_BIDIR_REV - sjmp dshot_cmd_exit - -dshot_cmd_save_settings: - cjne Temp1, #12, dshot_cmd_exit - + ; Decode low rpm power slope + mov Temp1, #Pgm_Startup_Pwr + mov A, @Temp1 + mov Low_Rpm_Pwr_Slope, A clr C - mov A, Dshot_Cmd_Cnt - subb A, #6 ; Needs to receive it 6 times in a row - jc dshot_cmd_exit_no_clear - - mov Flash_Key_1, #0A5h ; Initialize flash keys to valid values - mov Flash_Key_2, #0F1h - - call erase_and_store_all_in_eeprom - - mov Flash_Key_1, #0 ; Initialize flash keys to invalid values - mov Flash_Key_2, #0 - - setb IE_EA - - jmp dshot_cmd_exit + subb A, #2 + jnc ($+5) + mov Low_Rpm_Pwr_Slope, #2 + ; Decode demag compensation + mov Temp1, #Pgm_Demag_Comp + mov A, @Temp1 + mov Demag_Pwr_Off_Thresh, #255 ; Set default + cjne A, #2, decode_demag_high -;**** **** **** **** **** **** **** **** **** **** **** **** **** -; -; DShot beacon beep -; -; Beep with beacon strength -; Beep type 1-5 in Temp1 -; -;**** **** **** **** **** **** **** **** **** **** **** **** **** -beacon_beep: - clr IE_EA - call switch_power_off ; Switch power off in case braking is set - mov Temp2, #Pgm_Beacon_Strength - mov Beep_Strength, @Temp2 + mov Demag_Pwr_Off_Thresh, #160 ; Settings for demag comp low - cjne Temp1, #1, beacon_beep2 - call beep_f1 - sjmp beacon_beep_exit +decode_demag_high: + cjne A, #3, decode_demag_done -beacon_beep2: - cjne Temp1, #2, beacon_beep3 - call beep_f2 - sjmp beacon_beep_exit + mov Demag_Pwr_Off_Thresh, #130 ; Settings for demag comp high -beacon_beep3: - cjne Temp1, #3, beacon_beep4 - call beep_f3 - sjmp beacon_beep_exit +decode_demag_done: + ; Decode temperature protection limit + mov Temp1, #Pgm_Enable_Temp_Prot + mov A, @Temp1 + mov Temp1, A + jz decode_temp_done -beacon_beep4: - cjne Temp1, #4, beacon_beep5 - call beep_f4 - sjmp beacon_beep_exit + mov A, #(TEMP_LIMIT-TEMP_LIMIT_STEP) +decode_temp_step: + add A, #TEMP_LIMIT_STEP + djnz Temp1, decode_temp_step -beacon_beep5: - call beep_f5 +decode_temp_done: + mov Temp_Prot_Limit, A -beacon_beep_exit: - mov Temp2, #Pgm_Beep_Strength - mov Beep_Strength, @Temp2 - setb IE_EA - call wait100ms + ; Dithering +IF PWM_BITS_H == 1 + mov Temp1, #Dithering_Patterns + Push_Mem Temp1, #00h + Push_Mem Temp1, #55h +ELSEIF PWM_BITS_H == 0 + mov Temp1, #Dithering_Patterns + Push_Mem Temp1, #00h + Push_Mem Temp1, #11h + Push_Mem Temp1, #55h + Push_Mem Temp1, #77h +ENDIF ret