From 4835b6baa8bf502afcfca1e0f5c0d4fe91bf15c7 Mon Sep 17 00:00:00 2001 From: iso9660 Date: Sun, 8 Jan 2023 09:51:04 +0100 Subject: [PATCH 1/2] Implemented demag and stall events over extended dshot telemetry Demag and stall events notification flag cleared during dshot configuration Fixed 640ms scheduler step Updated status frame to carry demag metric max, and demag, desync and stall event flags Fixed stall bug Added clear for demag metrix max Scheduler has been rewritten and optimized to allow sending demag metric every 64ms and temperature, status, debug1 and debug2 every 256ms Demag_Detected_Metric_Max scaled to take full advantage of 4bit, small optimization in temperature PWM limit to follow Temp_Pwm_Level_Setpoint Improved comment in stat frame Fixed max demag event load for the case it is below 120 (should happen only when motors are stopped) Clear notification flags after sending status frame Updated .gitignore Fixed review findings Rebased to develop Set default power rating to 2S+ --- Bluejay.asm | 340 +++++++++++++++++++++++++++++++--------------------- 1 file changed, 201 insertions(+), 139 deletions(-) diff --git a/Bluejay.asm b/Bluejay.asm index cef66e8..a682cfe 100644 --- a/Bluejay.asm +++ b/Bluejay.asm @@ -151,7 +151,7 @@ DEFAULT_PGM_BEACON_STRENGTH EQU 80 ; 0..255 DEFAULT_PGM_BEACON_DELAY EQU 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite DEFAULT_PGM_ENABLE_TEMP_PROT EQU 7 ; 0=Disabled 1=80C 2=90C 3=100C 4=110C 5=120C 6=130C 7=140C -DEFAULT_PGM_POWER_RATING EQU 1 ; 1=1S, 2=2S+ +DEFAULT_PGM_POWER_RATING EQU 2 ; 1=1S, 2=2S+ DEFAULT_PGM_BRAKE_ON_STOP EQU 0 ; 1=Enabled 0=Disabled DEFAULT_PGM_LED_CONTROL EQU 0 ; Byte for LED control. 2 bits per LED, 0=Off, 1=On @@ -185,6 +185,9 @@ Flags0: DS 1 ; State flags. Reset upon motor_start Flag_Startup_Phase BIT Flags0.0 ; Set when in startup phase Flag_Initial_Run_Phase BIT Flags0.1 ; Set when in initial run phase (or startup phase), before synchronized run is achieved. Flag_Motor_Dir_Rev BIT Flags0.2 ; Set if the current spinning direction is reversed +Flag_Demag_Notify BIT Flags0.3 ; Set when motor demag has been detected but still not notified +Flag_Desync_Notify BIT Flags0.4 ; Set when motor desync has been detected but still not notified +Flag_Stall_Notify BIT Flags0.5 ; Set when motor stall detected but still not notified Flags1: DS 1 ; State flags. Reset upon motor_start Flag_Timer3_Pending BIT Flags1.0 ; Timer3 pending flag @@ -227,6 +230,7 @@ Startup_Zc_Timeout_Cntd: DS 1 ; Startup zero cross timeout counter (decrementin Initial_Run_Rot_Cntd: DS 1 ; Initial run rotations counter (decrementing) Startup_Stall_Cnt: DS 1 ; Counts start/run attempts that resulted in stall. Reset upon a proper stop Demag_Detected_Metric: DS 1 ; Metric used to gauge demag event frequency +Demag_Detected_Metric_Max: DS 1 ; Metric used to gauge demag event frequency Demag_Pwr_Off_Thresh: DS 1 ; Metric threshold above which power is cut Low_Rpm_Pwr_Slope: DS 1 ; Sets the slope of power increase for low rpm @@ -1706,6 +1710,22 @@ set_pwm_limit_high_rpm_store: ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Scheduler +; Each step 32ms, cycle 256ms (8 steps) +; +; ReqSch00: - Steps even [0, 2, 4, 6] +; ReqSch01: - Update temperature setpoint +; ReqSch02: - [TELEMETRY] Send demag metric frame +; ReqSch03: - Steps odd [1, 3, 5, 7] +; ReqSch04: - Update temperature PWM limit +; ReqSch06: - Case step 1 +; ReqSch07: - [TELEMETRY] Send status frame +; ReqSch08: - Case step 3 +; ReqSch09: - [TELEMETRY] Send debug1 frame +; ReqSch10: - Case step 5 +; ReqSch11: - [TELEMETRY] Send debug2 frame +; ReqSch12: - Case step 7 +; ReqSch13: - [TELEMETRY] Send temperature frame +; ReqSch14: - Start new ADC conversion ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** @@ -1718,22 +1738,19 @@ scheduler_start: ; Increment Scheduler Counter inc Scheduler_Counter -;****************** 128 ms scheduler ******************* -;*********** For PWM temperature limiting ************** -scheduler_128ms_switch_case: - ; Apply 128ms mask to Scheduler_Counter (2 lowest bits) + ; Choose between odd or even steps mov A, Scheduler_Counter - anl A, #03h + jb ACC.0, scheduler_steps_odd -scheduler_128ms_switch_case_32ms: - cjne A, #1, scheduler_128ms_switch_case_64ms +scheduler_steps_even: + ; ********************* UPDATE TEMPERATURE SETPOINT ***************** - ; Check temp protection enabled, and exit when protection is disabled + ; Check temp protection enabled, and skip when protection is disabled mov A, Temp_Prot_Limit - jz scheduler_1024ms_switch_case + jz scheduler_steps_even_telemetry_demag_metric - ; ******* UPDATE TEMPERATURE SETPOINT ******* - mov Temp_Pwm_Level_Setpoint, #255 ; Remove setpoint + ; Set setpoint maximum value + mov Temp_Pwm_Level_Setpoint, #255 ; Check TEMP_LIMIT in Base.inc and make calculations to understand temperature readings ; Is temperature reading below 256? @@ -1743,191 +1760,213 @@ scheduler_128ms_switch_case_32ms: ; On BB51 ; - Using external voltage regulator and internal 1.65V as ADC reference -> ADC 10bit value corresponding to about 0ºC mov A, ADC0H ; Load temp hi - jz scheduler_1024ms_switch_case ; Temperature below 25ºC (on 2S+ (BB1, BB2)) and below 0ºC (on 1S (BB1, BB21), BB51) do not update setpoint + jz scheduler_steps_even_telemetry_demag_metric ; Temperature below 25ºC (on 2S+ (BB1, BB2)) and below 0ºC (on 1S (BB1, BB21), BB51) do not update setpoint mov A, ADC0L ; Load temp lo clr C subb A, Temp_Prot_Limit ; Is temperature below first limit? - jc scheduler_1024ms_switch_case ; Yes - Jump to next scheduler + jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler mov Temp_Pwm_Level_Setpoint, #200 ; No - update pwm limit (about 80%) subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below second limit - jc scheduler_1024ms_switch_case ; Yes - Jump to next scheduler + jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler mov Temp_Pwm_Level_Setpoint, #150 ; No - update pwm limit (about 60%) subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below third limit - jc scheduler_1024ms_switch_case ; Yes - Jump to next scheduler + jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler mov Temp_Pwm_Level_Setpoint, #100 ; No - update pwm limit (about 40% allowing landing) subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below final limit - jc scheduler_1024ms_switch_case ; Yes - Jump to next scheduler + jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler mov Temp_Pwm_Level_Setpoint, #50 ; No - update pwm limit (about 20% forced landing) ; Zero pwm cannot be set because of set_pwm_limit algo restrictions ; Otherwise hard stuttering is produced - ; Jump to next scheduler - sjmp scheduler_1024ms_switch_case +scheduler_steps_even_telemetry_demag_metric: + ; ********************* [TELEMETRY] SEND DEMAG METRIC FRAME ***************** + mov Ext_Telemetry_L, Demag_Detected_Metric ; Set telemetry low value to demag metric data + mov Ext_Telemetry_H, #0Ch ; Set telemetry high value to demag metric frame ID + + ; No more work to do + jmp scheduler_exit -scheduler_128ms_switch_case_64ms: - cjne A, #2, scheduler_128ms_switch_case_96ms +scheduler_steps_odd: + ; ********************* UPDATE TEMPERATURE PWM LIMIT ***************** ; Check temp protection enabled, and exit when protection is disabled mov A, Temp_Prot_Limit - jz scheduler_1024ms_switch_case - - ; Update PWM limit + jz scheduler_steps_odd_choose_step ; pwm limit is updated one unit at a time to avoid abrupt pwm changes - ; resulting in current spikes + ; resulting in current spikes, that may damage motor/esc ; Compare pwm limit to setpoint clr C mov A, Pwm_Limit subb A, Temp_Pwm_Level_Setpoint - jz scheduler_1024ms_switch_case ; pwm limit == setpoint -> next - jc scheduler_128ms_temp_update_pwm_limit_inc ; pwm limit < setpoint -> increase pwm limit + jz scheduler_steps_odd_choose_step ; pwm limit == setpoint -> next + jc scheduler_steps_odd_temp_pwm_limit_inc ; pwm limit < setpoint -> increase pwm limit -scheduler_128ms_temp_update_pwm_limit_dec: +scheduler_steps_odd_temp_pwm_limit_dec: ; Decrease pwm limit - mov A, Pwm_Limit - jz scheduler_1024ms_switch_case ; pwm limit is 0 -> Exit dec Pwm_Limit - ; Jump to next scheduler - sjmp scheduler_1024ms_switch_case + ; Now run speciffic odd scheduler step + sjmp scheduler_steps_odd_choose_step -scheduler_128ms_temp_update_pwm_limit_inc: +scheduler_steps_odd_temp_pwm_limit_inc: ; Increase pwm limit - mov A, Pwm_Limit - inc A - jz ($+4) inc Pwm_Limit - ; Jump to next scheduler - sjmp scheduler_1024ms_switch_case + ; Now run speciffic odd scheduler step -scheduler_128ms_switch_case_96ms: - cjne A, #3, scheduler_1024ms_switch_case - - ; Start a new ADC conversion so after 64ms it will be ready for stage 2 og 128ms scheduler - Stop_Adc - Start_Adc - - ; Continue on next scheduler +scheduler_steps_odd_choose_step: + ; Let A = Scheduler_Counter % 8, so A = [0 - 7] value + mov A, Scheduler_Counter + anl A, #07h +scheduler_steps_odd_step_1: + ; ********************* [TELEMETRY] SEND STATUS FRAME ***************** + cjne A, #1, scheduler_steps_odd_step_3 + ; if (Demag_Detected_Metric_Max >= 120) + ; stat.demagMetricMax = (Demag_Detected_Metric_Max - 120) / 9 + ; else + ; stat.demagMetricMax = 0 + clr C + mov A, Demag_Detected_Metric_Max + subb A, #120 ; 120: substract the minimum + jnc scheduler_steps_odd_demag_metric_max_load + clr A + sjmp scheduler_steps_odd_demag_metric_max_loaded +scheduler_steps_odd_demag_metric_max_load: + mov B, #9 + div AB ; Ranges: [0 - 135] / 9 == [0 - 15] -;****************** 1024 ms scheduler ******************* -;************ For Extended Dshot Telemetry ************** -scheduler_1024ms_switch_case: - ; Return if extended telemetry is disabled - jnb Flag_Ext_Tele, scheduler_exit +scheduler_steps_odd_demag_metric_max_loaded: + ; Load flags + mov C, Flag_Demag_Notify + mov ACC.7, C + mov C, Flag_Desync_Notify + mov ACC.6, C + mov C, Flag_Stall_Notify + mov ACC.5, C - ; Apply 1s mask to Scheduler_Counter (5 lowest bits) - mov A, Scheduler_Counter - anl A, #01Fh + ; Data loaded clear flags + clr Flag_Demag_Notify + clr Flag_Desync_Notify + clr Flag_Stall_Notify + ; Load status frame + mov Ext_Telemetry_L, A ; Set telemetry low value to status data + mov Ext_Telemetry_H, #0Eh ; Set telemetry high value to status frame ID -scheduler_1024ms_switch_case_128ms: - cjne A, #4, scheduler_1024ms_switch_case_256ms + ; Now restart ADC conversion + sjmp scheduler_steps_odd_restart_ADC - ; ****************************************************************** - ; Power rating only applies to BB21 because voltage references behave diferently - ; depending on an external voltage regulator is used or not. - ; For BB51 (MCU_TYPE == 2) 1s power rating code path is mandatory - ; ****************************************************************** -IF MCU_TYPE < 2 - mov Temp1, #Pgm_Power_Rating - cjne @Temp1, #01h, scheduler_1024ms_power_rating_2s -ENDIF +scheduler_steps_odd_step_3: + ; ********************* [TELEMETRY] SEND DEBUG1 FRAME ***************** + cjne A, #3, scheduler_steps_odd_step_5 -scheduler_1024ms_power_rating_1s: - ; ****************************************************************** - ; ON BB51 and BB1, BB2 at 1S, all using internal 1.65V ADC reference - ; ****************************************************************** - mov A, ADC0H - jnz scheduler_1024ms_pr1s_do_extended_telemetry_temp_above_0 - -scheduler_1024ms_pr1s_do_extended_telemetry_temp_below_0: - ; If Hi Byte is not 0x01 we are definetly below 0, thus - ; clamp to 0. - clr A - sjmp scheduler_1024ms_do_extended_telemetry_temp_load - -scheduler_1024ms_pr1s_do_extended_telemetry_temp_above_0: - ; Prepare extended telemetry temperature value for next telemetry transmission - ; On BB51 they hi byte is always 1 if the temperature is above 0ºC. - ; In fact the value is 0x0114 at 0ºC, thus we ignore the hi byte and normalize - ; the low byte to - mov A, ADC0L - subb A, #14h - sjmp scheduler_1024ms_do_extended_telemetry_temp_load - -scheduler_1024ms_power_rating_2s: - ; ***************************************************** - ; ON BB1, BB2 at more than 1S, using vdd V3.3 ADC reference - ; ***************************************************** - ; Prepare extended telemetry temperature value for next telemetry transmission - ; Check value above or below 20ºC - this is an approximation ADCOH having a value - ; of 0x01 equals to around 22.5ºC. - mov A, ADC0H - jnz scheduler_1024ms_pr2s_do_extended_telemetry_temp_above_20 - -scheduler_1024ms_pr2s_do_extended_telemetry_temp_below_20: - ; Value below 20ºC -> to code between 0-20 - mov A, ADC0L - clr C - subb A, #(255 - 20) - jnc scheduler_1024ms_do_extended_telemetry_temp_load - - ; Value below 0ºC -> clamp to 0 - clr A - sjmp scheduler_1024ms_do_extended_telemetry_temp_load - -scheduler_1024ms_pr2s_do_extended_telemetry_temp_above_20: - ; Value above 20ºC -> to code between 20-255 - mov A, ADC0L ; This is an approximation: 9 ADC steps @10 Bit are 10 degrees - add A, #20 - -scheduler_1024ms_do_extended_telemetry_temp_load: - mov Ext_Telemetry_L, A ; Set telemetry low value with temperature data - mov Ext_Telemetry_H, #02h ; Set telemetry high value on first repeated dshot coding partition + ; Stub for debug 1 + mov Ext_Telemetry_L, #088h ; Set telemetry low value + mov Ext_Telemetry_H, #08h ; Set telemetry high value to debug1 frame ID - ; Exit switch case - sjmp scheduler_exit + ; Now restart ADC conversion + sjmp scheduler_steps_odd_restart_ADC +scheduler_steps_odd_step_5: + ; ********************* [TELEMETRY] SEND DEBUG2 FRAME ***************** + cjne A, #5, scheduler_steps_odd_step_7 -scheduler_1024ms_switch_case_256ms: - cjne A, #8, scheduler_1024ms_switch_case_384ms + ; Stub for debug 2 + mov Ext_Telemetry_L, #0AAh ; Set telemetry low value + mov Ext_Telemetry_H, #0Ah ; Set telemetry high value to debug2 frame ID - ; Stub for debug 0 - mov Ext_Telemetry_L, #088h ; Set telemetry low value to dummy value - mov Ext_Telemetry_H, #08h ; Set telemetry high value on first repeated dshot coding partition + ; Now restart ADC conversion + sjmp scheduler_steps_odd_restart_ADC - sjmp scheduler_exit +scheduler_steps_odd_step_7: + ; ********************* [TELEMETRY] SEND TEMPERATURE FRAME ***************** + cjne A, #7, scheduler_steps_odd_restart_ADC -scheduler_1024ms_switch_case_384ms: - cjne A, #12, scheduler_1024ms_switch_case_512ms + ; ****************************************************************** + ; Power rating only applies to BB21 because voltage references behave diferently + ; depending on an external voltage regulator is used or not. + ; For BB51 (MCU_TYPE == 2) 1s power rating code path is mandatory + ; ****************************************************************** +IF MCU_TYPE < 2 + mov Temp1, #Pgm_Power_Rating + cjne @Temp1, #01h, scheduler_steps_odd_step_7_power_rating_2s +ENDIF - ; Stub for debug 1 - mov Ext_Telemetry_L, #0AAh ; Set telemetry low value to dummy value - mov Ext_Telemetry_H, #0Ah ; Set telemetry high value on first repeated dshot coding partition +scheduler_steps_odd_step_7_power_rating_1s: + ; ****************************************************************** + ; ON BB51 and BB1, BB2 at 1S, all using internal 1.65V ADC reference + ; ****************************************************************** + mov A, ADC0H + jnz scheduler_steps_odd_step_7_pr1s_temperature_above_0 + +scheduler_steps_odd_step_7_pr1s_temperature_below_0: + ; If Hi Byte is not 0x01 we are definetly below 0, thus + ; clamp to 0. + clr A + sjmp scheduler_steps_odd_step_7_temp_load + +scheduler_steps_odd_step_7_pr1s_temperature_above_0: + ; Prepare extended telemetry temperature value for next telemetry transmission + ; On BB51 they hi byte is always 1 if the temperature is above 0ºC. + ; In fact the value is 0x0114 at 0ºC, thus we ignore the hi byte and normalize + ; the low byte to + mov A, ADC0L + subb A, #14h + sjmp scheduler_steps_odd_step_7_temp_load + +scheduler_steps_odd_step_7_power_rating_2s: + ; ***************************************************** + ; ON BB1, BB2 at more than 1S, using vdd V3.3 ADC reference + ; ***************************************************** + ; Prepare extended telemetry temperature value for next telemetry transmission + ; Check value above or below 20ºC - this is an approximation ADCOH having a value + ; of 0x01 equals to around 22.5ºC. + mov A, ADC0H + jnz scheduler_steps_odd_step_7_pr2s_temperature_above_20 + +scheduler_steps_odd_step_7_pr2s_temperature_below_20: + ; Value below 20ºC -> to code between 0-20 + mov A, ADC0L + clr C + subb A, #(255 - 20) + jnc scheduler_steps_odd_step_7_temp_load + + ; Value below 0ºC -> clamp to 0 + clr A + sjmp scheduler_steps_odd_step_7_temp_load + +scheduler_steps_odd_step_7_pr2s_temperature_above_20: + ; Value above 20ºC -> to code between 20-255 + mov A, ADC0L ; This is an approximation: 9 ADC steps @10 Bit are 10 degrees + add A, #20 + +scheduler_steps_odd_step_7_temp_load: + mov Ext_Telemetry_L, A ; Set telemetry low value with temperature data + mov Ext_Telemetry_H, #02h ; Set telemetry high value on first repeated dshot coding partition - sjmp scheduler_exit + ; Now restart ADC conversion -scheduler_1024ms_switch_case_512ms: - cjne A, #16, scheduler_exit +scheduler_steps_odd_restart_ADC: + ; ********************* START NEW ADC CONVERSION ***************** - ; Stub for debug 2 - mov Ext_Telemetry_L, #0CCh ; Set telemetry low value with temperature data - mov Ext_Telemetry_H, #0Ch ; Set telemetry high value on first repeated dshot coding partition + ; Start a new ADC conversion so after 64ms it will be ready for stage 2 og 128ms scheduler + Stop_Adc + Start_Adc - ; exit + ; Nothing else to do scheduler_exit: ret @@ -2726,9 +2765,13 @@ wait_for_comm: mov B, #7 mul AB ; Multiply by 7 - jnb Flag_Demag_Detected, ($+4) ; Add new value for current demag status + jnb Flag_Demag_Detected, wait_for_comm_demag_event_added + ; Add new value for current demag status inc B + ; Signal demag + setb Flag_Demag_Notify +wait_for_comm_demag_event_added: mov C, B.0 ; Divide by 8 rrc A mov C, B.1 @@ -2741,11 +2784,23 @@ wait_for_comm: jnc ($+5) mov Demag_Detected_Metric, #120 + ; Update demag metric max clr C - mov A, Demag_Detected_Metric ; Check demag metric + mov A, Demag_Detected_Metric + subb A, Demag_Detected_Metric_Max + jc wait_for_comm_demag_metric_max_updated + mov Demag_Detected_Metric_Max, Demag_Detected_Metric + +wait_for_comm_demag_metric_max_updated: + ; Check demag metric + clr C + mov A, Demag_Detected_Metric subb A, Demag_Pwr_Off_Thresh jc wait_for_comm_wait + ; Signal desync + setb Flag_Desync_Notify; + ; Cut power if many consecutive demags. This will help retain sync during hard accelerations All_Pwm_Fets_Off Set_All_Pwm_Phases_Off @@ -4073,6 +4128,9 @@ setup_dshot: jnb Flag_Rcp_DShot_Inverted, ($+6) mov IT01CF, #(08h + (RTX_PIN SHL 4) + RTX_PIN) ; Route RCP input to Int0/1, with Int0 inverted + clr Flag_Demag_Notify ; Clear motor events + clr Flag_Desync_Notify + clr Flag_Stall_Notify clr Flag_Telemetry_Pending ; Clear DShot telemetry flag clr Flag_Ext_Tele ; Clear extended telemetry enabled flag @@ -4241,6 +4299,7 @@ motor_start: 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 @@ -4544,12 +4603,15 @@ ENDIF ; Check if RCP is zero, then it is a normal stop or signal timeout jb Flag_Rcp_Stop, exit_run_mode_no_stall + ; Signal stall + setb Flag_Stall_Notify + clr C ; Otherwise - it's a stall mov A, Startup_Stall_Cnt subb A, #4 ; Maximum consecutive stalls jnc exit_run_mode_stall_done - call wait100ms ; Wait for a bit between stall restarts + call wait100ms ; Wait for a bit between stall restarts ljmp motor_start ; Go back and try starting motors again exit_run_mode_stall_done: From b0ed7dbe778cb47507f190d8bb9eb94ac87148e7 Mon Sep 17 00:00:00 2001 From: iso9660 Date: Thu, 9 Feb 2023 20:27:40 +0100 Subject: [PATCH 2/2] Implemented pwm configurable by eeprom setting Fixed calculate_pwm_bits Fixed calculate_pwm_bits function Fixed some compiling errors Fixed format Fixed whitespaces Merged latest edt_events changes Optimized t1_int pwm bitN code paths --- Bluejay.asm | 398 +++++++++++++++++++++++++++++++++++++--------------- Common.inc | 4 +- 2 files changed, 291 insertions(+), 111 deletions(-) diff --git a/Bluejay.asm b/Bluejay.asm index a682cfe..d5e75c3 100644 --- a/Bluejay.asm +++ b/Bluejay.asm @@ -133,11 +133,6 @@ ELSE IS_MCU_48MHZ EQU 1 ENDIF -IF MCU_TYPE < 3 AND PWM_FREQ < 3 - ; Number of bits in pwm high byte - PWM_BITS_H EQU (2 + IS_MCU_48MHZ - PWM_CENTERED - PWM_FREQ) -ENDIF - $include (Common.inc) ; Include common source code for EFM8BBx based ESCs ;**** **** **** **** **** @@ -291,6 +286,7 @@ DShot_GCR_Start_Delay: DS 1 Ext_Telemetry_L: DS 1 ; Extended telemetry data to be sent 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 ;**** **** **** **** **** ; Indirect addressing data segments @@ -971,7 +967,7 @@ t1_int_zero_rcp_checked: jz ($+4) dec Rcp_Outside_Range_Cnt - ; Set pwm limit + ; Get minimum pwm limit between pwm rpm limit and pwm temperature limit (Pwm_Limit) clr C mov A, Pwm_Limit ; Limit to the smallest mov Temp6, A ; Store limit in Temp6 @@ -979,26 +975,29 @@ t1_int_zero_rcp_checked: jc ($+4) mov Temp6, Pwm_Limit_By_Rpm - ; Check against limit + + ; Limit PWM and scale pwm resolution and invert (duty cycle is defined inversely) + ; depending on pwm bits count + mov A, PwmBitsCount + +t1_int_pwm_limit_scale_dithering_pwm11bit: + cjne A, #3, t1_int_pwm_limit_scale_dithering_pwm10bit + + ; Check against pwm limit clr C mov A, Temp6 - subb A, Temp2 ; 8-bit rc pulse - jnc t1_int_scale_pwm_resolution + subb A, Temp2 ; Compare against 8-bit rc pulse + jnc t1_int_pwm_limit_scale_dithering_pwm11bit_limited -IF PWM_BITS_H == 0 ; 8-bit pwm - mov A, Temp6 - mov Temp2, A -ELSE + ; Limit pwm to 9, 10, 11 bit pwm limit mov A, Temp6 ; Multiply limit by 8 for 11-bit pwm mov B, #8 mul AB mov Temp4, A mov Temp5, B -ENDIF -t1_int_scale_pwm_resolution: -; Scale pwm resolution and invert (duty cycle is defined inversely) -IF PWM_BITS_H == 3 ; 11-bit pwm +t1_int_pwm_limit_scale_dithering_pwm11bit_limited: + ; 11-bit pwm mov A, Temp5 cpl A anl A, #7 @@ -1006,7 +1005,28 @@ IF PWM_BITS_H == 3 ; 11-bit pwm mov A, Temp4 cpl A mov Temp2, A -ELSEIF PWM_BITS_H == 2 ; 10-bit pwm + + ; 11bit does not need 11bit dithering, only for 10, 9, and 8 bit pwm + jmp t1_int_set_pwm + +t1_int_pwm_limit_scale_dithering_pwm10bit: + cjne A, #2, t1_int_pwm_limit_scale_dithering_pwm9bit + + ; Check against pwm limit + clr C + mov A, Temp6 + subb A, Temp2 ; Compare against 8-bit rc pulse + jnc t1_int_pwm_limit_scale_dithering_pwm10bit_limited + + ; Limit pwm to 9, 10, 11 bit pwm limit + mov A, Temp6 ; Multiply limit by 8 for 11-bit pwm + mov B, #8 + mul AB + mov Temp4, A + mov Temp5, B + +t1_int_pwm_limit_scale_dithering_pwm10bit_limited: + ; 10-bit pwm scaling clr C mov A, Temp5 rrc A @@ -1017,7 +1037,56 @@ ELSEIF PWM_BITS_H == 2 ; 10-bit pwm rrc A cpl A mov Temp2, A -ELSEIF PWM_BITS_H == 1 ; 9-bit pwm + + ; 11-bit effective dithering of 10-bit pwm + jb Flag_Dithering, t1_int_pwm_limit_scale_dithering_pwm10bit_scaled + jmp t1_int_set_pwm ; Long jmp needed here + +t1_int_pwm_limit_scale_dithering_pwm10bit_scaled: + mov A, Temp4 ; 11-bit low byte + cpl A + anl A, #((1 SHL (3 - 2)) - 1) ; Get index into dithering pattern table + + add A, #Dithering_Patterns + mov Temp1, A ; Reuse DShot pwm pointer since it is not currently in use. + mov A, @Temp1 ; Retrieve pattern + rl A ; Rotate pattern + mov @Temp1, A ; Store pattern + + jnb ACC.0, t1_int_set_pwm ; Increment if bit is set + + mov A, Temp2 + add A, #1 + mov Temp2, A + jnz t1_int_set_pwm + + mov A, Temp3 + addc A, #0 + mov Temp3, A + jnb ACC.2, t1_int_set_pwm + dec Temp3 ; Reset on overflow + + dec Temp2 + sjmp t1_int_set_pwm + +t1_int_pwm_limit_scale_dithering_pwm9bit: + cjne A, #1, t1_int_pwm_limit_scale_dithering_pwm8bit + + ; Check against pwm limit + clr C + mov A, Temp6 + subb A, Temp2 ; Compare against 8-bit rc pulse + jnc t1_int_pwm_limit_scale_dithering_pwm9bit_limited + + ; Limit pwm to 9, 10, 11 bit pwm limit + mov A, Temp6 ; Multiply limit by 8 for 11-bit pwm + mov B, #8 + mul AB + mov Temp4, A + mov Temp5, B + +t1_int_pwm_limit_scale_dithering_pwm9bit_limited: + ; 9-bit pwm scaling mov B, Temp5 mov A, Temp4 mov C, B.0 @@ -1032,42 +1101,77 @@ ELSEIF PWM_BITS_H == 1 ; 9-bit pwm cpl A anl A, #1 mov Temp3, A -ELSEIF PWM_BITS_H == 0 ; 8-bit pwm + + ; 11-bit effective dithering of 9-bit pwm + jnb Flag_Dithering, t1_int_set_pwm + + mov A, Temp4 ; 11-bit low byte + cpl A + anl A, #((1 SHL (3 - 1)) - 1) ; Get index into dithering pattern table + + add A, #Dithering_Patterns + mov Temp1, A ; Reuse DShot pwm pointer since it is not currently in use. + mov A, @Temp1 ; Retrieve pattern + rl A ; Rotate pattern + mov @Temp1, A ; Store pattern + + jnb ACC.0, t1_int_set_pwm ; Increment if bit is set + + mov A, Temp2 + add A, #1 + mov Temp2, A + jnz t1_int_set_pwm + + mov A, Temp3 + addc A, #0 + mov Temp3, A + jnb ACC.1, t1_int_set_pwm + dec Temp3 ; Reset on overflow + + dec Temp2 + sjmp t1_int_set_pwm + + +t1_int_pwm_limit_scale_dithering_pwm8bit: + ; Check against pwm limit + clr C + mov A, Temp6 + subb A, Temp2 ; Compare against 8-bit rc pulse + jnc t1_int_pwm_limit_scale_dithering_pwm8bit_limited + + ; Limit pwm to 8-bit pwm limit + mov A, Temp6 + mov Temp2, A + +t1_int_pwm_limit_scale_dithering_pwm8bit_limited: + ; 8-bit pwm scaling mov A, Temp2 ; Temp2 already 8-bit cpl A mov Temp2, A mov Temp3, #0 -ENDIF -; 11-bit effective dithering of 8/9/10-bit pwm -IF PWM_BITS_H < 3 - jnb Flag_Dithering, t1_int_set_pwm + ; 11-bit effective dithering of 8-bit pwm + jnb Flag_Dithering, t1_int_set_pwm - mov A, Temp4 ; 11-bit low byte - cpl A - anl A, #((1 SHL (3 - PWM_BITS_H)) - 1); Get index into dithering pattern table + mov A, Temp4 ; 11-bit low byte + cpl A + anl A, #((1 SHL (3 - 0)) - 1) ; Get index into dithering pattern table - add A, #Dithering_Patterns - mov Temp1, A ; Reuse DShot pwm pointer since it is not currently in use. - mov A, @Temp1 ; Retrieve pattern - rl A ; Rotate pattern - mov @Temp1, A ; Store pattern + add A, #Dithering_Patterns + mov Temp1, A ; Reuse DShot pwm pointer since it is not currently in use. + mov A, @Temp1 ; Retrieve pattern + rl A ; Rotate pattern + mov @Temp1, A ; Store pattern - jnb ACC.0, t1_int_set_pwm ; Increment if bit is set + jnb ACC.0, t1_int_set_pwm ; Increment if bit is set + + mov A, Temp2 + add A, #1 + mov Temp2, A + jnz t1_int_set_pwm + + dec Temp2 - mov A, Temp2 - add A, #1 - mov Temp2, A - jnz t1_int_set_pwm -IF PWM_BITS_H != 0 - mov A, Temp3 - addc A, #0 - mov Temp3, A - jnb ACC.PWM_BITS_H, t1_int_set_pwm - dec Temp3 ; Reset on overflow -ENDIF - dec Temp2 -ENDIF t1_int_set_pwm: ; Set pwm registers @@ -1105,26 +1209,30 @@ t1_int_max_braking_set: t1_int_pwm_braking_set: ENDIF + ; Note: Interrupts are not explicitly disabled ; Assume higher priority interrupts (Int0, Timer0) to be disabled at this point -IF PWM_BITS_H != 0 - ; Set power pwm auto-reload registers + ; Set power and damp pwm auto-reload registers + mov A, PwmBitsCount + jz t1_int_set_power_eq_8bits + +t1_int_set_power_neq_8bits: Set_Power_Pwm_Reg_L Temp2 Set_Power_Pwm_Reg_H Temp3 -ELSE - Set_Power_Pwm_Reg_H Temp2 +IF DEADTIME != 0 + Set_Damp_Pwm_Reg_L Temp4 + Set_Damp_Pwm_Reg_H Temp5 ENDIF + sjmp t1_int_set_power_done +t1_int_set_power_eq_8bits: + Set_Power_Pwm_Reg_H Temp2 IF DEADTIME != 0 - ; Set damp pwm auto-reload registers - IF PWM_BITS_H != 0 - Set_Damp_Pwm_Reg_L Temp4 - Set_Damp_Pwm_Reg_H Temp5 - ELSE - Set_Damp_Pwm_Reg_H Temp4 - ENDIF + Set_Damp_Pwm_Reg_H Temp4 ENDIF + +t1_int_set_power_done: mov Rcp_Timeout_Cntd, #10 ; Set timeout count ; Prepare DShot telemetry @@ -1747,7 +1855,7 @@ scheduler_steps_even: ; Check temp protection enabled, and skip when protection is disabled mov A, Temp_Prot_Limit - jz scheduler_steps_even_telemetry_demag_metric + jz scheduler_steps_even_demag_metric_frame ; Set setpoint maximum value mov Temp_Pwm_Level_Setpoint, #255 @@ -1760,37 +1868,37 @@ scheduler_steps_even: ; On BB51 ; - Using external voltage regulator and internal 1.65V as ADC reference -> ADC 10bit value corresponding to about 0ºC mov A, ADC0H ; Load temp hi - jz scheduler_steps_even_telemetry_demag_metric ; Temperature below 25ºC (on 2S+ (BB1, BB2)) and below 0ºC (on 1S (BB1, BB21), BB51) do not update setpoint + jz scheduler_steps_even_demag_metric_frame ; Temperature below 25ºC (on 2S+ (BB1, BB2)) and below 0ºC (on 1S (BB1, BB21), BB51) do not update setpoint - mov A, ADC0L ; Load temp lo + mov A, ADC0L ; Load temp lo clr C - subb A, Temp_Prot_Limit ; Is temperature below first limit? - jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler + subb A, Temp_Prot_Limit ; Is temperature below first limit? + jc scheduler_steps_even_demag_metric_frame ; Yes - Jump to next scheduler - mov Temp_Pwm_Level_Setpoint, #200 ; No - update pwm limit (about 80%) + mov Temp_Pwm_Level_Setpoint, #200 ; No - update pwm limit (about 80%) - subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below second limit - jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler + subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below second limit + jc scheduler_steps_even_demag_metric_frame ; Yes - Jump to next scheduler - mov Temp_Pwm_Level_Setpoint, #150 ; No - update pwm limit (about 60%) + mov Temp_Pwm_Level_Setpoint, #150 ; No - update pwm limit (about 60%) - subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below third limit - jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler + subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below third limit + jc scheduler_steps_even_demag_metric_frame ; Yes - Jump to next scheduler - mov Temp_Pwm_Level_Setpoint, #100 ; No - update pwm limit (about 40% allowing landing) + mov Temp_Pwm_Level_Setpoint, #100 ; No - update pwm limit (about 40% allowing landing) - subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below final limit - jc scheduler_steps_even_telemetry_demag_metric ; Yes - Jump to next scheduler + subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below final limit + jc scheduler_steps_even_demag_metric_frame ; Yes - Jump to next scheduler - mov Temp_Pwm_Level_Setpoint, #50 ; No - update pwm limit (about 20% forced landing) + mov Temp_Pwm_Level_Setpoint, #50 ; No - update pwm limit (about 20% forced landing) ; Zero pwm cannot be set because of set_pwm_limit algo restrictions ; Otherwise hard stuttering is produced -scheduler_steps_even_telemetry_demag_metric: +scheduler_steps_even_demag_metric_frame: ; ********************* [TELEMETRY] SEND DEMAG METRIC FRAME ***************** - mov Ext_Telemetry_L, Demag_Detected_Metric ; Set telemetry low value to demag metric data - mov Ext_Telemetry_H, #0Ch ; Set telemetry high value to demag metric frame ID + mov Ext_Telemetry_L, Demag_Detected_Metric ; Set telemetry low value to demag metric data + mov Ext_Telemetry_H, #0Ch ; Set telemetry high value to demag metric frame ID ; No more work to do jmp scheduler_exit @@ -1829,9 +1937,9 @@ scheduler_steps_odd_choose_step: mov A, Scheduler_Counter anl A, #07h -scheduler_steps_odd_step_1: +scheduler_steps_odd_status_frame: ; ********************* [TELEMETRY] SEND STATUS FRAME ***************** - cjne A, #1, scheduler_steps_odd_step_3 + cjne A, #1, scheduler_steps_odd_debug1_frame ; if (Demag_Detected_Metric_Max >= 120) ; stat.demagMetricMax = (Demag_Detected_Metric_Max - 120) / 9 @@ -1840,15 +1948,15 @@ scheduler_steps_odd_step_1: clr C mov A, Demag_Detected_Metric_Max subb A, #120 ; 120: substract the minimum - jnc scheduler_steps_odd_demag_metric_max_load + jnc scheduler_steps_odd_status_frame_max_load clr A - sjmp scheduler_steps_odd_demag_metric_max_loaded + sjmp scheduler_steps_odd_status_frame_max_loaded -scheduler_steps_odd_demag_metric_max_load: +scheduler_steps_odd_status_frame_max_load: mov B, #9 div AB ; Ranges: [0 - 135] / 9 == [0 - 15] -scheduler_steps_odd_demag_metric_max_loaded: +scheduler_steps_odd_status_frame_max_loaded: ; Load flags mov C, Flag_Demag_Notify mov ACC.7, C @@ -1869,9 +1977,9 @@ scheduler_steps_odd_demag_metric_max_loaded: ; Now restart ADC conversion sjmp scheduler_steps_odd_restart_ADC -scheduler_steps_odd_step_3: +scheduler_steps_odd_debug1_frame: ; ********************* [TELEMETRY] SEND DEBUG1 FRAME ***************** - cjne A, #3, scheduler_steps_odd_step_5 + cjne A, #3, scheduler_steps_odd_debug2_frame ; Stub for debug 1 mov Ext_Telemetry_L, #088h ; Set telemetry low value @@ -1880,9 +1988,9 @@ scheduler_steps_odd_step_3: ; Now restart ADC conversion sjmp scheduler_steps_odd_restart_ADC -scheduler_steps_odd_step_5: +scheduler_steps_odd_debug2_frame: ; ********************* [TELEMETRY] SEND DEBUG2 FRAME ***************** - cjne A, #5, scheduler_steps_odd_step_7 + cjne A, #5, scheduler_steps_odd_temperature_frame ; Stub for debug 2 mov Ext_Telemetry_L, #0AAh ; Set telemetry low value @@ -1891,7 +1999,7 @@ scheduler_steps_odd_step_5: ; Now restart ADC conversion sjmp scheduler_steps_odd_restart_ADC -scheduler_steps_odd_step_7: +scheduler_steps_odd_temperature_frame: ; ********************* [TELEMETRY] SEND TEMPERATURE FRAME ***************** cjne A, #7, scheduler_steps_odd_restart_ADC @@ -1902,32 +2010,32 @@ scheduler_steps_odd_step_7: ; ****************************************************************** IF MCU_TYPE < 2 mov Temp1, #Pgm_Power_Rating - cjne @Temp1, #01h, scheduler_steps_odd_step_7_power_rating_2s + cjne @Temp1, #01h, scheduler_steps_odd_temperature_frame_power_rating_2s ENDIF -scheduler_steps_odd_step_7_power_rating_1s: +scheduler_steps_odd_temperature_frame_power_rating_1s: ; ****************************************************************** ; ON BB51 and BB1, BB2 at 1S, all using internal 1.65V ADC reference ; ****************************************************************** mov A, ADC0H - jnz scheduler_steps_odd_step_7_pr1s_temperature_above_0 + jnz scheduler_steps_odd_temperature_frame_pr1s_temperature_above_0 -scheduler_steps_odd_step_7_pr1s_temperature_below_0: +scheduler_steps_odd_temperature_frame_pr1s_temperature_below_0: ; If Hi Byte is not 0x01 we are definetly below 0, thus ; clamp to 0. clr A - sjmp scheduler_steps_odd_step_7_temp_load + sjmp scheduler_steps_odd_temperature_frame_temp_load -scheduler_steps_odd_step_7_pr1s_temperature_above_0: +scheduler_steps_odd_temperature_frame_pr1s_temperature_above_0: ; Prepare extended telemetry temperature value for next telemetry transmission ; On BB51 they hi byte is always 1 if the temperature is above 0ºC. ; In fact the value is 0x0114 at 0ºC, thus we ignore the hi byte and normalize ; the low byte to mov A, ADC0L subb A, #14h - sjmp scheduler_steps_odd_step_7_temp_load + sjmp scheduler_steps_odd_temperature_frame_temp_load -scheduler_steps_odd_step_7_power_rating_2s: +scheduler_steps_odd_temperature_frame_power_rating_2s: ; ***************************************************** ; ON BB1, BB2 at more than 1S, using vdd V3.3 ADC reference ; ***************************************************** @@ -1935,25 +2043,25 @@ scheduler_steps_odd_step_7_power_rating_2s: ; Check value above or below 20ºC - this is an approximation ADCOH having a value ; of 0x01 equals to around 22.5ºC. mov A, ADC0H - jnz scheduler_steps_odd_step_7_pr2s_temperature_above_20 + jnz scheduler_steps_odd_temperature_frame_pr2s_temperature_above_20 -scheduler_steps_odd_step_7_pr2s_temperature_below_20: +scheduler_steps_odd_temperature_frame_pr2s_temperature_below_20: ; Value below 20ºC -> to code between 0-20 mov A, ADC0L clr C subb A, #(255 - 20) - jnc scheduler_steps_odd_step_7_temp_load + jnc scheduler_steps_odd_temperature_frame_temp_load ; Value below 0ºC -> clamp to 0 clr A - sjmp scheduler_steps_odd_step_7_temp_load + sjmp scheduler_steps_odd_temperature_frame_temp_load -scheduler_steps_odd_step_7_pr2s_temperature_above_20: +scheduler_steps_odd_temperature_frame_pr2s_temperature_above_20: ; Value above 20ºC -> to code between 20-255 mov A, ADC0L ; This is an approximation: 9 ADC steps @10 Bit are 10 degrees add A, #20 -scheduler_steps_odd_step_7_temp_load: +scheduler_steps_odd_temperature_frame_temp_load: mov Ext_Telemetry_L, A ; Set telemetry low value with temperature data mov Ext_Telemetry_H, #02h ; Set telemetry high value on first repeated dshot coding partition @@ -3915,8 +4023,8 @@ decode_temp_step: add A, #TEMP_LIMIT_STEP djnz Temp2, decode_temp_step - ; Set Temp_Prot_Limit to the temperature limit calculated in A decode_temp_done: + ; Set Temp_Prot_Limit to the temperature limit calculated in A mov Temp_Prot_Limit, A mov Temp1, #Pgm_Beep_Strength ; Read programmed beep strength setting @@ -3924,7 +4032,14 @@ decode_temp_done: mov Temp1, #Pgm_Braking_Strength ; Read programmed braking strength setting mov A, @Temp1 -IF PWM_BITS_H == 3 ; Scale braking strength to pwm resolution + + ; Decode braking strength depending on PwmBitsCount + mov Temp2, PwmBitsCount + +decode_braking_strength_pwm11bits: + cjne Temp2, #3, decode_braking_strength_pwm10bits + + ; Scale braking strength to pwm resolution ; Note: Added for completeness ; Currently 11-bit pwm is only used on targets with built-in dead time insertion rl A @@ -3936,7 +4051,11 @@ IF PWM_BITS_H == 3 ; Scale braking strength to pwm resolution mov A, Temp2 anl A, #0F8h mov Pwm_Braking_L, A -ELSEIF PWM_BITS_H == 2 + sjmp decode_braking_strength_done + +decode_braking_strength_pwm10bits: + cjne Temp2, #2, decode_braking_strength_pwm9bits + rl A rl A mov Temp2, A @@ -3945,7 +4064,11 @@ ELSEIF PWM_BITS_H == 2 mov A, Temp2 anl A, #0FCh mov Pwm_Braking_L, A -ELSEIF PWM_BITS_H == 1 + sjmp decode_braking_strength_done + +decode_braking_strength_pwm9bits: + cjne Temp2, #1, decode_braking_strength_pwm8bits + rl A mov Temp2, A anl A, #01h @@ -3953,10 +4076,13 @@ ELSEIF PWM_BITS_H == 1 mov A, Temp2 anl A, #0FEh mov Pwm_Braking_L, A -ELSEIF PWM_BITS_H == 0 + sjmp decode_braking_strength_done + +decode_braking_strength_pwm8bits: mov Pwm_Braking_H, #0 mov Pwm_Braking_L, A -ENDIF + +decode_braking_strength_done: cjne @Temp1, #0FFh, decode_pwm_dithering mov Pwm_Braking_L, #0FFh ; Apply full braking if setting is max @@ -3966,17 +4092,31 @@ decode_pwm_dithering: add A, #0FFh ; Carry set if A is not zero mov Flag_Dithering, C ; Set dithering enabled -IF PWM_BITS_H == 2 ; Initialize pwm dithering bit patterns + ; Decode dithering pattern depending on PwmBitsCount + mov A, PwmBitsCount + +decode_pwm_dithering_pwm10bit: + cjne A, #2, decode_pwm_dithering_pwm9bit + + ; Initialize pwm dithering bit patterns mov Temp1, #Dithering_Patterns ; 1-bit dithering (10-bit to 11-bit) mov @Temp1, #00h ; 00000000 imov Temp1, #55h ; 01010101 -ELSEIF PWM_BITS_H == 1 + sjmp decode_pwm_dithering_done + +decode_pwm_dithering_pwm9bit: + cjne A, #1, decode_pwm_dithering_pwm8bit + mov Temp1, #Dithering_Patterns ; 2-bit dithering (9-bit to 11-bit) mov @Temp1, #00h ; 00000000 imov Temp1, #11h ; 00010001 imov Temp1, #55h ; 01010101 imov Temp1, #77h ; 01110111 -ELSEIF PWM_BITS_H == 0 + sjmp decode_pwm_dithering_done + +decode_pwm_dithering_pwm8bit: + cjne A, #0, decode_pwm_dithering_done + mov Temp1, #Dithering_Patterns ; 3-bit dithering (8-bit to 11-bit) mov @Temp1, #00h ; 00000000 imov Temp1, #01h ; 00000001 @@ -3986,7 +4126,44 @@ ELSEIF PWM_BITS_H == 0 imov Temp1, #5Bh ; 01011011 imov Temp1, #77h ; 01110111 imov Temp1, #7fh ; 01111111 -ENDIF + +decode_pwm_dithering_done: + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; pwm bits count calculation +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +calculate_pwm_bits: + ; Number of bits in pwm high byte + ; PWM_BITS_H EQU (2 + IS_MCU_48MHZ - PWM_CENTERED - PWM_FREQ) + clr C + mov A, #2 + add A, #IS_MCU_48MHZ + subb A, #PWM_CENTERED + + ; Load and decode Pgm_Pwm_Freq [24, 48, 96] -> [0, 1, 2] + ; Let Temp1 = Pgm_Pwm_Freq / 24 + mov Temp1, #Pgm_Pwm_Freq + +calculate_pwm_bits_pwm96bits: + ; If pwm is 96 khz substract 2 + cjne @Temp1, #96, calculate_pwm_bits_pwm48bits + subb A, #2 + sjmp calculate_pwm_bits_pwm_decoded + +calculate_pwm_bits_pwm48bits: + ; If pwm is 48 khz substract 1, otherwise do not substract (24khz) + cjne @Temp1, #48, calculate_pwm_bits_pwm_decoded + subb A, #1 + +calculate_pwm_bits_pwm_decoded: + ; Clip result to [0-3] and store result + anl A, #03h + mov PwmBitsCount, A + +calculate_pwm_bits_done: ret @@ -4051,6 +4228,7 @@ ENDIF call set_default_parameters ; Set default programmed parameters call read_all_eeprom_parameters ; Read all programmed parameters + call calculate_pwm_bits ; Calculates pwm bits call decode_settings ; Decode programmed settings ; Initializing beeps diff --git a/Common.inc b/Common.inc index d55cde6..5bcb6ed 100644 --- a/Common.inc +++ b/Common.inc @@ -189,7 +189,9 @@ Initialize_PCA MACRO mov PCA0CN0, #40h ;; PCA enabled mov PCA0MD, #08h ;; PCA clock is system clock - mov PCA0PWM, #(80h + PWM_BITS_H) ;; Enable PCA auto-reload registers and set pwm cycle length (8-11 bits) + mov A, #80h + add A, PwmBitsCount + mov PCA0PWM, A ;; Enable PCA auto-reload registers and set pwm cycle length (8-11 bits) IF PWM_CENTERED == 1 mov PCA0CENT, #07h ;; Center aligned pwm