forked from mathiasvr/bluejay
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBluejay.asm
4384 lines (3621 loc) · 120 KB
/
Bluejay.asm
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
;**** **** **** **** ****
;
; Bluejay digital ESC firmware for controlling brushless motors in multirotors
;
; Copyright 2020-2022 Mathias Rasmussen
; Copyright 2011-2017 Steffen Skaug
;
; This file is part of Bluejay.
;
; Bluejay is free software: you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation, either version 3 of the License, or
; (at your option) any later version.
;
; Bluejay is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License
; along with Bluejay. If not, see <http://www.gnu.org/licenses/>.
;
;**** **** **** **** ****
;
; Bluejay is a fork of BLHeli_S <https://github.com/bitdump/BLHeli> by Steffen Skaug.
;
; The input signal can be DShot with rates: DShot150, DShot300 and DShot600.
;
; This file is best viewed with tab width set to 5.
;
;**** **** **** **** ****
; Master clock is internal 24MHz oscillator (or 48MHz, for which the times below are halved)
; Although 24/48 are used in the code, the exact clock frequencies are 24.5MHz or 49.0 MHz
; Timer0 (41.67ns counts) always counts up and is used for
; - RC pulse measurement
; - DShot telemetry pulse timing
; Timer1 (41.67ns counts) always counts up and is used for
; - DShot frame sync detection
; Timer2 (500ns counts) always counts up and is used for
; - RC pulse timeout counts and commutation times
; Timer3 (500ns counts) always counts up and is used for
; - Commutation timeouts
; PCA0 (41.67ns counts) always counts up and is used for
; - Hardware PWM generation
;
;**** **** **** **** ****
; Motor control:
; - Brushless motor control with 6 states for each electrical 360 degrees
; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next
; - Timing advance in this implementation is set to 15deg nominally
; - Motor pwm is always damped light (aka complementary pwm, regenerative braking)
; Motor sequence starting from zero crossing:
; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation
; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this
; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross
; - Scan for zero cross 22.5deg ; Nominal, with some motor variations
;
; Motor startup:
; There is a startup phase and an initial run phase, before normal bemf commutation run begins.
;
;**** **** **** **** ****
; List of enumerated supported ESCs
; PORT 0 PORT 1 PWM COM PWM LED
; P0 P1 P2 P3 P4 P5 P6 P7 P0 P1 P2 P3 P4 P5 P6 P7 inv inv side n
; ----------------------- ----------------------- ------------------
A_ EQU 1 ; Vn Am Bm Cm __ RX __ __ Ap Ac Bp Bc Cp Cc __ __ no no high _
B_ EQU 2 ; Vn Am Bm Cm __ RX __ __ Cc Cp Bc Bp Ac Ap __ __ no no high _
C_ EQU 3 ; RX __ Vn Am Bm Cm Ap Ac Bp Bc Cp Cc __ __ __ __ no no high _
D_ EQU 4 ; Bm Cm Am Vn __ RX __ __ Ap Ac Bp Bc Cp Cc __ __ no yes high _
E_ EQU 5 ; Vn Am Bm Cm __ RX L0 L1 Ap Ac Bp Bc Cp Cc L2 __ no no high 3 Pinout like A, with LEDs
F_ EQU 6 ; Vn Cm Bm Am __ RX __ __ Ap Ac Bp Bc Cp Cc __ __ no no high _
G_ EQU 7 ; Bm Cm Am Vn __ RX __ __ Ap Ac Bp Bc Cp Cc __ __ no no high _ Pinout like D, but non-inverted com FETs
H_ EQU 8 ; Cm Vn Bm Am __ __ __ RX Cc Bc Ac __ Cp Bp Ap __ no no high _
I_ EQU 9 ; Vn Am Bm Cm __ RX __ __ Cp Bp Ap Cc Bc Ac __ __ no no high _
J_ EQU 10 ; Am Cm Bm Vn RX L0 L1 L2 Ap Bp Cp Ac Bc Cc __ __ no no high 3
K_ EQU 11 ; RX Am Vn Bm __ Cm __ __ Ac Bc Cc Cp Bp Ap __ __ no yes high _
L_ EQU 12 ; Cm Bm Am Vn __ RX __ __ Cp Bp Ap Cc Bc Ac __ __ no no high _
M_ EQU 13 ; __ __ L0 RX Bm Vn Cm Am __ Ap Bp Cp Ac Bc Cc __ no no high 1
N_ EQU 14 ; Vn Am Bm Cm __ RX __ __ Ac Ap Bc Bp Cc Cp __ __ no no high _
O_ EQU 15 ; Bm Cm Am Vn __ RX __ __ Ap Ac Bp Bc Cp Cc __ __ no yes low _ Pinout Like D, but low side pwm
P_ EQU 16 ; __ Cm Bm Vn Am RX __ __ __ Ap Bp Cp Ac Bc Cc __ no no high _
Q_ EQU 17 ; __ RX __ L0 L1 Ap Bp Cp Ac Bc Cc Vn Cm Bm Am __ no no high 2
R_ EQU 18 ; Vn Am Bm Cm __ RX __ __ Cp Bp Ap Cc Bc Ac __ __ no no high _ Same as I
S_ EQU 19 ; Bm Cm Am Vn __ RX __ __ Ac Ap Bc Bp Cc Cp __ __ no no high _
T_ EQU 20 ; __ Cm Vn Bm __ Am __ RX Cc Bc Ac Ap Bp Cp __ __ no no high _
U_ EQU 21 ; L2 L1 L0 RX Bm Vn Cm Am __ Ap Bp Cp Ac Bc Cc __ no no high 3 Pinout like M, with 3 LEDs
V_ EQU 22 ; Am Bm Vn Cm __ RX __ Cc Cp Bc __ __ Bp Ac Ap __ no no high _
W_ EQU 23 ; __ __ Am Vn __ Bm Cm RX __ __ __ __ Cp Bp Ap __ n/a n/a high _ Tristate gate driver
X_ EQU 24
Y_ EQU 25
Z_ EQU 26 ; Bm Cm Am Vn __ RX __ __ Ac Ap Bc Bp Cc Cp __ __ yes no high - Pinout like S, but inverted pwm FETs
;**** **** **** **** ****
; Select the port mapping to use (or unselect all for use with external batch compile file)
;ESCNO EQU A_
;**** **** **** **** ****
; Select the MCU type (or unselect for use with external batch compile file)
;MCU_48MHZ EQU 0
;**** **** **** **** ****
; Select the FET dead time (or unselect for use with external batch compile file)
;DEADTIME EQU 15 ; 20.4ns per step
;**** **** **** **** ****
; Select the pwm frequency (or unselect for use with external batch compile file)
;PWM_FREQ EQU 0 ; 0=24, 1=48, 2=96 kHz
PWM_CENTERED EQU DEADTIME > 0 ; Use center aligned pwm on ESCs with dead time
IF MCU_48MHZ < 2 AND PWM_FREQ < 3
; Number of bits in pwm high byte
PWM_BITS_H EQU (2 + MCU_48MHZ - PWM_CENTERED - PWM_FREQ)
ENDIF
$include (Common.inc) ; Include common source code for EFM8BBx based ESCs
;**** **** **** **** ****
; Programming defaults
DEFAULT_PGM_RPM_POWER_SLOPE EQU 9 ; 0=Off, 1..13 (Power limit factor in relation to rpm)
DEFAULT_PGM_COMM_TIMING EQU 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
DEFAULT_PGM_DEMAG_COMP EQU 2 ; 1=Disabled 2=Low 3=High
DEFAULT_PGM_DIRECTION EQU 1 ; 1=Normal 2=Reversed 3=Bidir 4=Bidir rev
DEFAULT_PGM_BEEP_STRENGTH EQU 40 ; 0..255 (BLHeli_S is 1..255)
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_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
DEFAULT_PGM_STARTUP_POWER_MIN EQU 51 ; 0..255 => (1000..1125 Throttle): value * (1000 / 2047) + 1000
DEFAULT_PGM_STARTUP_BEEP EQU 1 ; 0=Short beep, 1=Melody
DEFAULT_PGM_DITHERING EQU 1 ; 0=Disabled, 1=Enabled
DEFAULT_PGM_STARTUP_POWER_MAX EQU 25 ; 0..255 => (1000..2000 Throttle): Maximum startup power
DEFAULT_PGM_BRAKING_STRENGTH EQU 255 ; 0..255 => 0..100 % Braking
;**** **** **** **** ****
; Temporary register definitions
Temp1 EQU R0
Temp2 EQU R1
Temp3 EQU R2
Temp4 EQU R3
Temp5 EQU R4
Temp6 EQU R5
Temp7 EQU R6
Temp8 EQU R7
;**** **** **** **** ****
; RAM definitions
; Bit-addressable data segment
DSEG AT 20h
Bit_Access: DS 1 ; MUST BE AT THIS ADDRESS. Variable at bit accessible address (for non interrupt routines)
Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts)
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
Flags1: DS 1 ; State flags. Reset upon motor_start
Flag_Timer3_Pending BIT Flags1.0 ; Timer3 pending flag
Flag_Demag_Detected BIT Flags1.1 ; Set when excessive demag time is detected
Flag_Comp_Timed_Out BIT Flags1.2 ; Set when comparator reading timed out
Flag_Motor_Running BIT Flags1.3
Flag_Motor_Started BIT Flags1.4 ; Set when motor is started
Flag_Dir_Change_Brake BIT Flags1.5 ; Set when braking before direction change in case of bidirectional operation
Flag_High_Rpm BIT Flags1.6 ; Set when motor rpm is high (Comm_Period4x_H less than 2)
Flags2: DS 1 ; State flags. NOT reset upon motor_start
Flag_Temperature_Exceeded BIT Flags2.0 ; Set if temperature is above the configured one
Flag_Pgm_Dir_Rev BIT Flags2.1 ; Set if the programmed direction is reversed
Flag_Pgm_Bidir BIT Flags2.2 ; Set if the programmed control mode is bidirectional operation
Flag_Skip_Timer2_Int BIT Flags2.3 ; Set for 48MHz MCUs when Timer2 interrupt shall be ignored
Flag_Clock_At_48MHz BIT Flags2.4 ; Set if 48MHz MCUs run at 48MHz
Flag_Rcp_Stop BIT Flags2.5 ; Set if the RC pulse value is zero or if timeout occurs
Flag_Rcp_Dir_Rev BIT Flags2.6 ; RC pulse direction in bidirectional mode
Flag_Rcp_DShot_Inverted BIT Flags2.7 ; DShot RC pulse input is inverted (and supports telemetry)
Flags3: DS 1 ; State flags. NOT reset upon motor_start
Flag_Telemetry_Pending BIT Flags3.0 ; DShot telemetry data packet is ready to be sent
Flag_Dithering BIT Flags3.1 ; PWM dithering enabled
Flag_Had_Signal BIT Flags3.2 ; Used to detect reset after having had a valid signal
Tlm_Data_L: DS 1 ; DShot telemetry data (lo byte)
Tlm_Data_H: DS 1 ; DShot telemetry data (hi byte)
;**** **** **** **** ****
; Direct addressing data segment
DSEG AT 30h
Rcp_Outside_Range_Cnt: DS 1 ; RC pulse outside range counter (incrementing)
Rcp_Timeout_Cntd: DS 1 ; RC pulse timeout counter (decrementing)
Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value
Beacon_Delay_Cnt: DS 1 ; Counter to trigger beacon during wait for start
Startup_Cnt: DS 1 ; Startup phase commutations counter (incrementing)
Startup_Zc_Timeout_Cntd: DS 1 ; Startup zero cross timeout counter (decrementing)
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_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
Timer2_X: DS 1 ; Timer2 extended byte
Prev_Comm_L: DS 1 ; Previous commutation Timer2 timestamp (lo byte)
Prev_Comm_H: DS 1 ; Previous commutation Timer2 timestamp (hi byte)
Prev_Comm_X: DS 1 ; Previous commutation Timer2 timestamp (ext byte)
Prev_Prev_Comm_L: DS 1 ; Pre-previous commutation Timer2 timestamp (lo byte)
Prev_Prev_Comm_H: DS 1 ; Pre-previous commutation Timer2 timestamp (hi byte)
Comm_Period4x_L: DS 1 ; Timer2 ticks between the last 4 commutations (lo byte)
Comm_Period4x_H: DS 1 ; Timer2 ticks between the last 4 commutations (hi byte)
Comparator_Read_Cnt: DS 1 ; Number of comparator reads done
Wt_Adv_Start_L: DS 1 ; Timer3 start point for commutation advance timing (lo byte)
Wt_Adv_Start_H: DS 1 ; Timer3 start point for commutation advance timing (hi byte)
Wt_Zc_Scan_Start_L: DS 1 ; Timer3 start point from commutation to zero cross scan (lo byte)
Wt_Zc_Scan_Start_H: DS 1 ; Timer3 start point from commutation to zero cross scan (hi byte)
Wt_Zc_Tout_Start_L: DS 1 ; Timer3 start point for zero cross scan timeout (lo byte)
Wt_Zc_Tout_Start_H: DS 1 ; Timer3 start point for zero cross scan timeout (hi byte)
Wt_Comm_Start_L: DS 1 ; Timer3 start point from zero cross to commutation (lo byte)
Wt_Comm_Start_H: DS 1 ; Timer3 start point from zero cross to commutation (hi byte)
Pwm_Limit: DS 1 ; Maximum allowed pwm (8-bit)
Pwm_Limit_By_Rpm: DS 1 ; Maximum allowed pwm for low or high rpm (8-bit)
Pwm_Limit_Beg: DS 1 ; Initial pwm limit (8-bit)
Pwm_Braking_L: DS 1 ; Max Braking pwm (lo byte)
Pwm_Braking_H: DS 1 ; Max Braking pwm (hi byte)
Adc_Conversion_Cnt: DS 1 ; Adc conversion counter
Temp_Level: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1)
Temp_Prot_Limit: DS 1 ; Temperature protection limit
Temp_Pwm_Level_Setpoint: DS 1 ; PWM level setpoint
Beep_Strength: DS 1 ; Strength of beeps
Flash_Key_1: DS 1 ; Flash key one
Flash_Key_2: DS 1 ; Flash key two
DShot_Pwm_Thr: DS 1 ; DShot pulse width threshold value (Timer0 ticks)
DShot_Timer_Preset: DS 1 ; DShot timer preset for frame sync detection (Timer1 lo byte)
DShot_Frame_Start_L: DS 1 ; DShot frame start timestamp (Timer2 lo byte)
DShot_Frame_Start_H: DS 1 ; DShot frame start timestamp (Timer2 hi byte)
DShot_Frame_Length_Thr: DS 1 ; DShot frame length criteria (Timer2 ticks)
DShot_Cmd: DS 1 ; DShot command
DShot_Cmd_Cnt: DS 1 ; DShot command count
; Pulse durations for GCR encoding DShot telemetry data
DShot_GCR_Pulse_Time_1: DS 1 ; Encodes binary: 1
DShot_GCR_Pulse_Time_2: DS 1 ; Encodes binary: 01
DShot_GCR_Pulse_Time_3: DS 1 ; Encodes binary: 001
DShot_GCR_Pulse_Time_1_Tmp: DS 1
DShot_GCR_Pulse_Time_2_Tmp: DS 1
DShot_GCR_Pulse_Time_3_Tmp: DS 1
DShot_GCR_Start_Delay: DS 1
;**** **** **** **** ****
; Indirect addressing data segments
ISEG AT 080h ; The variables below must be in this sequence
_Pgm_Gov_P_Gain: DS 1 ;
Pgm_Startup_Power_Min: DS 1 ; Minimum power during startup phase
Pgm_Startup_Beep: DS 1 ; Startup beep melody on/off
Pgm_Dithering: DS 1 ; Enable PWM dithering
Pgm_Startup_Power_Max: DS 1 ; Maximum power (limit) during startup (and starting initial run phase)
_Pgm_Rampup_Slope: DS 1 ;
Pgm_Rpm_Power_Slope: DS 1 ; Low RPM power protection slope (factor)
Pgm_Pwm_Freq: DS 1 ; PWM frequency (temporary method for display)
Pgm_Direction: DS 1 ; Rotation direction
_Pgm_Input_Pol: DS 1 ; Input PWM polarity
Initialized_L_Dummy: DS 1 ; Place holder
Initialized_H_Dummy: DS 1 ; Place holder
_Pgm_Enable_TX_Program: DS 1 ; Enable/disable value for TX programming
Pgm_Braking_Strength: DS 1 ; Set maximum braking strength (complementary pwm)
_Pgm_Gov_Setup_Target: DS 1 ; Main governor setup target
_Pgm_Startup_Rpm: DS 1 ; Startup RPM
_Pgm_Startup_Accel: DS 1 ; Startup acceleration
_Pgm_Volt_Comp: DS 1 ; Voltage comp
Pgm_Comm_Timing: DS 1 ; Commutation timing
_Pgm_Damping_Force: DS 1 ; Damping force
_Pgm_Gov_Range: DS 1 ; Governor range
_Pgm_Startup_Method: DS 1 ; Startup method
_Pgm_Min_Throttle: DS 1 ; Minimum throttle
_Pgm_Max_Throttle: DS 1 ; Maximum throttle
Pgm_Beep_Strength: DS 1 ; Beep strength
Pgm_Beacon_Strength: DS 1 ; Beacon strength
Pgm_Beacon_Delay: DS 1 ; Beacon delay
_Pgm_Throttle_Rate: DS 1 ; Throttle rate
Pgm_Demag_Comp: DS 1 ; Demag compensation
_Pgm_BEC_Voltage_High: DS 1 ; BEC voltage
_Pgm_Center_Throttle: DS 1 ; Center throttle (in bidirectional mode)
_Pgm_Main_Spoolup_Time: DS 1 ; Main spoolup time
Pgm_Enable_Temp_Prot: DS 1 ; Temperature protection enable
_Pgm_Enable_Power_Prot: DS 1 ; Low RPM power protection enable
_Pgm_Enable_Pwm_Input: DS 1 ; Enable PWM input signal
_Pgm_Pwm_Dither: DS 1 ; Output PWM dither
Pgm_Brake_On_Stop: DS 1 ; Braking when throttle is zero
Pgm_LED_Control: DS 1 ; LED control
ISEG AT 0B0h
Stack: DS 16 ; Reserved stack space
ISEG AT 0C0h
Dithering_Patterns: DS 16 ; Bit patterns for pwm dithering
ISEG AT 0D0h
Temp_Storage: DS 48 ; Temporary storage (internal memory)
;**** **** **** **** ****
; EEPROM code segments
; A segment of the flash is used as "EEPROM", which is not available in SiLabs MCUs
CSEG AT 1A00h
EEPROM_FW_MAIN_REVISION EQU 0 ; Main revision of the firmware
EEPROM_FW_SUB_REVISION EQU 16 ; Sub revision of the firmware
EEPROM_LAYOUT_REVISION EQU 204 ; Revision of the EEPROM layout
Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number
Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number
Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number
_Eep_Pgm_Gov_P_Gain: DB 0FFh
Eep_Pgm_Startup_Power_Min: DB DEFAULT_PGM_STARTUP_POWER_MIN
Eep_Pgm_Startup_Beep: DB DEFAULT_PGM_STARTUP_BEEP
Eep_Pgm_Dithering: DB DEFAULT_PGM_DITHERING
Eep_Pgm_Startup_Power_Max: DB DEFAULT_PGM_STARTUP_POWER_MAX
_Eep_Pgm_Rampup_Slope: DB 0FFh
Eep_Pgm_Rpm_Power_Slope: DB DEFAULT_PGM_RPM_POWER_SLOPE ; EEPROM copy of programmed rpm power slope (formerly startup power)
Eep_Pgm_Pwm_Freq: DB (24 SHL PWM_FREQ) ; Temporary method for display
Eep_Pgm_Direction: DB DEFAULT_PGM_DIRECTION ; EEPROM copy of programmed rotation direction
_Eep__Pgm_Input_Pol: DB 0FFh
Eep_Initialized_L: DB 055h ; EEPROM initialized signature (lo byte)
Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature (hi byte)
_Eep_Enable_TX_Program: DB 0FFh ; EEPROM TX programming enable
Eep_Pgm_Braking_Strength: DB DEFAULT_PGM_BRAKING_STRENGTH
_Eep_Pgm_Gov_Setup_Target: DB 0FFh
_Eep_Pgm_Startup_Rpm: DB 0FFh
_Eep_Pgm_Startup_Accel: DB 0FFh
_Eep_Pgm_Volt_Comp: DB 0FFh
Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_COMM_TIMING ; EEPROM copy of programmed commutation timing
_Eep_Pgm_Damping_Force: DB 0FFh
_Eep_Pgm_Gov_Range: DB 0FFh
_Eep_Pgm_Startup_Method: DB 0FFh
_Eep_Pgm_Min_Throttle: DB 0FFh ; EEPROM copy of programmed minimum throttle
_Eep_Pgm_Max_Throttle: DB 0FFh ; EEPROM copy of programmed minimum throttle
Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_BEACON_DELAY ; EEPROM copy of programmed beacon delay
_Eep_Pgm_Throttle_Rate: DB 0FFh
Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_DEMAG_COMP ; EEPROM copy of programmed demag compensation
_Eep_Pgm_BEC_Voltage_High: DB 0FFh
_Eep_Pgm_Center_Throttle: DB 0FFh ; EEPROM copy of programmed center throttle
_Eep_Pgm_Main_Spoolup_Time: DB 0FFh
Eep_Pgm_Temp_Prot_Enable: DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
_Eep_Pgm_Enable_Power_Prot: DB 0FFh ; EEPROM copy of programmed low rpm power protection enable
_Eep_Pgm_Enable_Pwm_Input: DB 0FFh
_Eep_Pgm_Pwm_Dither: DB 0FFh
Eep_Pgm_Brake_On_Stop: DB DEFAULT_PGM_BRAKE_ON_STOP ; EEPROM copy of programmed braking when throttle is zero
Eep_Pgm_LED_Control: DB DEFAULT_PGM_LED_CONTROL ; EEPROM copy of programmed LED control
Eep_Dummy: DB 0FFh ; EEPROM address for safety reason
CSEG AT 1A60h
Eep_Name: DB "Bluejay " ; Name tag (16 Bytes)
CSEG AT 1A70h
Eep_Pgm_Beep_Melody: DB 2, 58, 4, 32, 52, 66, 13, 0, 69, 45, 13, 0, 52, 66, 13, 0, 78, 39, 211, 0, 69, 45, 208, 25, 52, 25, 0
;**** **** **** **** ****
Interrupt_Table_Definition ; SiLabs interrupts
CSEG AT 80h ; Code segment after interrupt vectors
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Macros
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
DSHOT_TLM_CLOCK EQU 24500000 ; 24.5MHz
DSHOT_TLM_START_DELAY EQU -(5 * 25 / 4) ; Start telemetry after 5 us (~30 us after receiving DShot cmd)
IF MCU_48MHZ == 0
DSHOT_TLM_PREDELAY EQU 9 ; 9 Timer0 ticks inherent delay
ELSE
DSHOT_TLM_PREDELAY EQU 7 ; 7 Timer0 ticks inherent delay
ENDIF
IF MCU_48MHZ == 1
DSHOT_TLM_CLOCK_48 EQU 49000000 ; 49MHz
DSHOT_TLM_START_DELAY_48 EQU -(16 * 49 / 4) ; Start telemetry after 16 us (~30 us after receiving DShot cmd)
DSHOT_TLM_PREDELAY_48 EQU 11 ; 11 Timer0 ticks inherent delay
ENDIF
Set_DShot_Tlm_Bitrate MACRO rate
mov DShot_GCR_Pulse_Time_1, #(DSHOT_TLM_PREDELAY - (1 * DSHOT_TLM_CLOCK / 4 / rate))
mov DShot_GCR_Pulse_Time_2, #(DSHOT_TLM_PREDELAY - (2 * DSHOT_TLM_CLOCK / 4 / rate))
mov DShot_GCR_Pulse_Time_3, #(DSHOT_TLM_PREDELAY - (3 * DSHOT_TLM_CLOCK / 4 / rate))
mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY
IF MCU_48MHZ == 1
mov DShot_GCR_Pulse_Time_1_Tmp, #(DSHOT_TLM_PREDELAY_48 - (1 * DSHOT_TLM_CLOCK_48 / 4 / rate))
mov DShot_GCR_Pulse_Time_2_Tmp, #(DSHOT_TLM_PREDELAY_48 - (2 * DSHOT_TLM_CLOCK_48 / 4 / rate))
mov DShot_GCR_Pulse_Time_3_Tmp, #(DSHOT_TLM_PREDELAY_48 - (3 * DSHOT_TLM_CLOCK_48 / 4 / rate))
ENDIF
ENDM
; DShot GCR encoding, adjust time by adding to previous item
GCR_Add_Time MACRO reg
mov B, @reg
mov A, DShot_GCR_Pulse_Time_2
cjne A, B, ($+5)
mov A, DShot_GCR_Pulse_Time_3
mov @reg, A
ENDM
; Prepare telemetry packet while waiting for Timer3 to wrap
Wait_For_Timer3 MACRO
LOCAL wait_for_t3 done_waiting
jb Flag_Telemetry_Pending, wait_for_t3
jnb Flag_Timer3_Pending, done_waiting
call dshot_tlm_create_packet
wait_for_t3:
jnb Flag_Timer3_Pending, done_waiting
Wdt_Kick_Enable ; Kick watchdog
sjmp wait_for_t3
done_waiting:
ENDM
; Used for subdividing the DShot telemetry routine into chunks,
; that will return if Timer3 has wrapped
Early_Return_Packet_Stage MACRO num
Early_Return_Packet_Stage_ num, %(num + 1)
ENDM
Early_Return_Packet_Stage_ MACRO num next
IF num > 0
inc Temp7 ;; Increment current packet stage
jb Flag_Timer3_Pending, dshot_packet_stage_&num ;; Return early if Timer3 has wrapped
pop PSW
ret
dshot_packet_stage_&num:
ENDIF
IF num < 5
cjne Temp7, #(num), dshot_packet_stage_&next ;; If this is not current stage, skip to next
ENDIF
ENDM
Decode_DShot_2Bit MACRO dest, decode_fail
movx A, @Temp1
mov Temp7, A
clr C
subb A, Temp6 ;; Subtract previous timestamp
clr C
subb A, Temp2
jc decode_fail ;; Check that bit is longer than minimum
subb A, Temp2 ;; Check if bit is zero or one
rlca dest ;; Shift bit into data byte
inc Temp1 ;; Next bit
movx A, @Temp1
mov Temp6, A
clr C
subb A, Temp7
clr C
subb A, Temp2
jc decode_fail
subb A, Temp2
rlca dest
inc Temp1
ENDM
;**** **** **** **** ****
; Compound instructions for convenience
xcha MACRO var1, var2 ;; Exchange via accumulator
mov A, var1
xch A, var2
mov var1, A
ENDM
rrca MACRO var ;; Rotate right through carry via accumulator
mov A, var
rrc A
mov var, A
ENDM
rlca MACRO var ;; Rotate left through carry via accumulator
mov A, var
rlc A
mov var, A
ENDM
rla MACRO var ;; Rotate left via accumulator
mov A, var
rl A
mov var, A
ENDM
ljc MACRO label ;; Long jump if carry set
LOCAL skip
jnc skip
jmp label
skip:
ENDM
ljz MACRO label ;; Long jump if accumulator is zero
LOCAL skip
jnz skip
jmp label
skip:
ENDM
imov MACRO reg, val ;; Increment pointer register and move
inc reg
mov @reg, val ;; Write value to memory address pointed to by register
ENDM
;**** **** **** **** ****
; Division
;
; ih, il: input (hi byte, lo byte)
; oh, ol: output (hi byte, lo byte)
;
Divide_By_16 MACRO ih, il, oh, ol
mov A, ih
swap A
mov ol, A
anl A, #00Fh
mov oh, A
mov A, ol
anl A, #0F0h
mov ol, A
mov A, il
swap A
anl A, #00Fh
orl A, ol
mov ol, A
ENDM
Divide_12Bit_By_16 MACRO ih, il, ol ;; Only if ih < 16
mov A, ih
swap A
mov ol, A
mov A, il
swap A
anl A, #00Fh
orl A, ol
mov ol, A
ENDM
Divide_By_8 MACRO ih, il, oh, ol
mov A, ih
swap A
rl A
mov ol, A
anl A, #01Fh
mov oh, A
mov A, ol
anl A, #0E0h
mov ol, A
mov A, il
swap A
rl A
anl A, #01Fh
orl A, ol
mov ol, A
ENDM
Divide_11Bit_By_8 MACRO ih, il, ol ;; Only if ih < 8
mov A, ih
swap A
rl A
mov ol, A
mov A, il
swap A
rl A
anl A, #01Fh
orl A, ol
mov ol, A
ENDM
Divide_By_4 MACRO ih, il, oh, ol
clr C
mov A, ih
rrc A
mov oh, A
mov A, il
rrc A
mov ol, A
clr C
mov A, oh
rrc A
mov oh, A
mov A, ol
rrc A
mov ol, A
ENDM
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Interrupt handlers
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer0 interrupt routine (High priority)
;
; Generate DShot telemetry signal
;
; Requirements:
; - Must NOT be called while Flag_Telemetry_Pending is cleared
; - Must NOT write to Temp7, Temp8
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
t0_int:
push PSW
mov PSW, #10h ; Select register bank 2 for this interrupt
dec Temp1
cjne Temp1, #(Temp_Storage - 1), t0_int_dshot_tlm_transition
inc Temp1 ; Set pointer to uncritical position
; If last pulse is high, telemetry is finished,
; otherwise wait for it to return to high
jb RTX_BIT, t0_int_dshot_tlm_finish
t0_int_dshot_tlm_transition:
cpl RTX_BIT ; Invert signal level
mov TL0, @Temp1 ; Schedule next update
pop PSW
reti
t0_int_dshot_tlm_finish:
; Configure RTX_PIN for digital input
anl RTX_MDOUT, #(NOT (1 SHL RTX_PIN)) ; Set RTX_PIN output mode to open-drain
setb RTX_BIT ; Float high
clr IE_ET0 ; Disable Timer0 interrupts
mov CKCON0, Temp8 ; Restore regular DShot Timer0/1 clock settings
mov TMOD, #0AAh ; Timer0/1 gated by Int0/1
clr TCON_IE0 ; Clear Int0 pending flag
clr TCON_IE1 ; Clear Int1 pending flag
mov TL0, #0 ; Reset Timer0 count
setb IE_EX0 ; Enable Int0 interrupts
setb IE_EX1 ; Enable Int1 interrupts
clr Flag_Telemetry_Pending ; Mark that new telemetry packet may be created
pop PSW
reti
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer1 interrupt routine
;
; Decode DShot frame
; Process new throttle value and update pwm registers
; Schedule DShot telemetry
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
t1_int:
clr IE_EX0 ; Disable Int0 interrupts
clr TCON_TR1 ; Stop Timer1
mov TL1, DShot_Timer_Preset ; Reset sync timer
push PSW
mov PSW, #8h ; Select register bank 1 for this interrupt
push ACC
push B
; Note: Interrupts are not explicitly disabled
; Assume higher priority interrupts (Int0, Timer0) to be disabled at this point
clr TMR2CN0_TR2 ; Timer2 disabled
mov Temp2, TMR2L ; Read timer value
mov Temp3, TMR2H
setb TMR2CN0_TR2 ; Timer2 enabled
; Check frame time length
clr C
mov A, Temp2
subb A, DShot_Frame_Start_L
mov Temp2, A
mov A, Temp3
subb A, DShot_Frame_Start_H
jnz t1_int_frame_fail ; Frame too long
clr C
mov A, Temp2
subb A, DShot_Frame_Length_Thr
jc t1_int_frame_fail ; Frame too short
subb A, DShot_Frame_Length_Thr
jnc t1_int_frame_fail ; Frame too long
; Check that correct number of pulses is received
cjne Temp1, #16, t1_int_frame_fail ; Read current pointer
; Decode transmitted data
mov Temp1, #0 ; Set pointer
mov Temp2, DShot_Pwm_Thr ; DShot pulse width criteria
mov Temp6, #0 ; Reset timestamp
; Decode DShot data Msb. Use more code space to save time (by not using loop)
Decode_DShot_2Bit Temp5, t1_int_frame_fail
Decode_DShot_2Bit Temp5, t1_int_frame_fail
sjmp t1_int_decode_lsb
t1_int_frame_fail:
sjmp t1_int_outside_range
t1_int_decode_lsb:
; Decode DShot data Lsb
Decode_DShot_2Bit Temp4, t1_int_outside_range
Decode_DShot_2Bit Temp4, t1_int_outside_range
Decode_DShot_2Bit Temp4, t1_int_outside_range
Decode_DShot_2Bit Temp4, t1_int_outside_range
sjmp t1_int_decode_checksum
t1_int_outside_range:
inc Rcp_Outside_Range_Cnt
mov A, Rcp_Outside_Range_Cnt
jnz ($+4)
dec Rcp_Outside_Range_Cnt
clr C
mov A, Rcp_Outside_Range_Cnt
subb A, #50 ; Allow a given number of outside pulses
jc t1_int_exit_timeout ; If outside limits - ignore first pulses
; RCP signal has not timed out, but pulses are not recognized as DShot
setb Flag_Rcp_Stop ; Set pulse length to zero
mov DShot_Cmd, #0 ; Reset DShot command
mov DShot_Cmd_Cnt, #0
ajmp t1_int_exit_no_tlm ; Exit without resetting timeout
t1_int_exit_timeout:
mov Rcp_Timeout_Cntd, #10 ; Set timeout count
ajmp t1_int_exit_no_tlm
t1_int_decode_checksum:
; Decode DShot data checksum
Decode_DShot_2Bit Temp3, t1_int_outside_range
Decode_DShot_2Bit Temp3, t1_int_outside_range
; XOR check (in inverted data, which is ok), only low nibble is considered
mov A, Temp4
swap A
xrl A, Temp4
xrl A, Temp5
xrl A, Temp3
jnb Flag_Rcp_DShot_Inverted, ($+4)
cpl A ; Invert checksum if using inverted DShot
anl A, #0Fh
jnz t1_int_outside_range ; XOR check
; Invert DShot data and subtract 96 (still 12 bits)
clr C
mov A, Temp4
cpl A
mov Temp3, A ; Store in case it is a DShot command
subb A, #96
mov Temp4, A
mov A, Temp5
cpl A
anl A, #0Fh
subb A, #0
mov Temp5, A
jnc t1_int_normal_range
mov A, Temp3 ; Check for 0 or DShot command
mov Temp5, #0
mov Temp4, #0
jz t1_int_dshot_set_cmd ; Clear DShot command when RCP is zero
clr C ; We are in the special DShot range
rrc A ; Shift tlm bit into carry
jnc t1_int_dshot_clear_cmd ; Check for tlm bit set (if not telemetry, invalid command)
cjne A, DShot_Cmd, t1_int_dshot_set_cmd
inc DShot_Cmd_Cnt
sjmp t1_int_normal_range
t1_int_dshot_clear_cmd:
clr A
t1_int_dshot_set_cmd:
mov DShot_Cmd, A
mov DShot_Cmd_Cnt, #0
t1_int_normal_range:
; Check for bidirectional operation (0=stop, 96-2095->fwd, 2096-4095->rev)
jnb Flag_Pgm_Bidir, t1_int_not_bidir ; If not bidirectional operation - branch
; Subtract 2000 (still 12 bits)
clr C
mov A, Temp4
subb A, #0D0h
mov B, A
mov A, Temp5
subb A, #07h
jc t1_int_bidir_set ; Is result is positive?
mov Temp4, B ; Yes - Use the subtracted value
mov Temp5, A
t1_int_bidir_set:
jnb Flag_Pgm_Dir_Rev, ($+4) ; Check programmed direction
cpl C ; Reverse direction
mov Flag_Rcp_Dir_Rev, C ; Set rcp direction
clr C ; Multiply throttle value by 2
rlca Temp4
rlca Temp5
t1_int_not_bidir:
; From here Temp5/Temp4 should be at most 3999 (4095-96)
mov A, Temp4 ; Divide by 16 (12 to 8-bit)
anl A, #0F0h
orl A, Temp5 ; Note: Assumes Temp5 to be 4-bit
swap A
mov B, #5 ; Divide by 5 (80 in total)
div AB
mov Temp3, A
; Align to 11 bits
;clr C ; Note: Cleared by div
rrca Temp5
mov A, Temp4
rrc A
; Scale from 2000 to 2048
add A, Temp3
mov Temp4, A
mov A, Temp5
addc A, #0
mov Temp5, A
jnb ACC.3, ($+7) ; Limit to 11-bit maximum
mov Temp4, #0FFh
mov Temp5, #07h
; Do not boost when changing direction in bidirectional mode
jb Flag_Motor_Started, t1_int_startup_boosted
; Boost pwm during direct start
jnb Flag_Initial_Run_Phase, t1_int_startup_boosted
mov A, Temp5
jnz t1_int_stall_boost ; Already more power than minimum at startup
mov Temp2, #Pgm_Startup_Power_Min ; Read minimum startup power setting
mov B, @Temp2
clr C ; Set power to at least be minimum startup power
mov A, Temp4
subb A, B
jnc t1_int_stall_boost
mov Temp4, B
t1_int_stall_boost:
mov A, Startup_Stall_Cnt ; Check stall count
jz t1_int_startup_boosted
mov B, #40 ; Note: Stall count should be less than 6
mul AB
add A, Temp4 ; Add more power when failing to start motor (stalling)
mov Temp4, A
mov A, Temp5
addc A, #0
mov Temp5, A
jnb ACC.3, ($+7) ; Limit to 11-bit maximum
mov Temp4, #0FFh
mov Temp5, #07h
t1_int_startup_boosted:
; Set 8-bit value
mov A, Temp4
anl A, #0F8h
orl A, Temp5 ; Assumes Temp5 to be 3-bit (11-bit rcp)
swap A
rl A
mov Temp2, A
jnz t1_int_rcp_not_zero
mov A, Temp4 ; Only set Rcp_Stop if all all 11 bits are zero
jnz t1_int_rcp_not_zero
setb Flag_Rcp_Stop
sjmp t1_int_zero_rcp_checked
t1_int_rcp_not_zero:
mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter
clr Flag_Rcp_Stop ; Pulse ready
t1_int_zero_rcp_checked:
; Decrement outside range counter
mov A, Rcp_Outside_Range_Cnt
jz ($+4)
dec Rcp_Outside_Range_Cnt
; Set pwm limit
clr C
mov A, Pwm_Limit ; Limit to the smallest
mov Temp6, A ; Store limit in Temp6
subb A, Pwm_Limit_By_Rpm
jc ($+4)
mov Temp6, Pwm_Limit_By_Rpm
; Check against limit
clr C
mov A, Temp6
subb A, Temp2 ; 8-bit rc pulse
jnc t1_int_scale_pwm_resolution
IF PWM_BITS_H == 0 ; 8-bit pwm
mov A, Temp6
mov Temp2, A
ELSE
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
mov A, Temp5
cpl A
anl A, #7
mov Temp3, A
mov A, Temp4
cpl A
mov Temp2, A
ELSEIF PWM_BITS_H == 2 ; 10-bit pwm
clr C
mov A, Temp5
rrc A
cpl A
anl A, #3
mov Temp3, A
mov A, Temp4
rrc A
cpl A
mov Temp2, A
ELSEIF PWM_BITS_H == 1 ; 9-bit pwm
mov B, Temp5
mov A, Temp4
mov C, B.0
rrc A
mov C, B.1
rrc A
cpl A
mov Temp2, A
mov A, Temp5
rr A
rr A
cpl A
anl A, #1
mov Temp3, A
ELSEIF PWM_BITS_H == 0 ; 8-bit pwm
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
mov A, Temp4 ; 11-bit low byte
cpl A
anl A, #((1 SHL (3 - PWM_BITS_H)) - 1); Get index into dithering pattern table