-
-
Notifications
You must be signed in to change notification settings - Fork 46
/
BLHeli_S.asm
4405 lines (3832 loc) · 113 KB
/
BLHeli_S.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
$NOMOD51
;**** **** **** **** ****
;
; BLHeli program for controlling brushless motors in multirotors
;
; Copyright 2011, 2012 Steffen Skaug
; This program is distributed under the terms of the GNU General Public License
;
; This file is part of BLHeli.
;
; BLHeli 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.
;
; BLHeli 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 BLHeli. If not, see <http://www.gnu.org/licenses/>.
;
;**** **** **** **** ****
;
; This software was initially designed for use with Eflite mCP X, but is now adapted to copters/planes in general
;
; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/blc_6a/blc_6a.htm
; And also Simon Kirby's TGY: https://github.com/sim-/tgy
;
; This file is best viewed with tab width set to 5
;
; The code is designed for multirotor applications, running damped light mode
;
; The input signal can be Normal (1-2ms), OneShot125 (125-250us), OneShot42 (41.7-83.3us) or Multishot (5-25us) at rates as high as allowed by the format.
; Three Dshot signal rates are also supported, Dshot150, Dshot300 and Dshot600. A 48MHz MCU is required for Dshot600.
; The code autodetects normal, OneShot125, Oneshot42, Multishot or Dshot.
;
; The first lines of the software must be modified according to the chosen environment:
; ESCNO EQU "ESC"
; MCU_48MHZ EQU "N"
; FETON_DELAY EQU "N"
;
;**** **** **** **** ****
; Revision history:
; - Rev16.0 Started. Built upon rev 14.5 of base code
; Using hardware pwm for very smooth throttle response, silent running and support of very high rpms
; Implemented reverse bidirectional mode
; Implemented separate throttle gains fwd and rev in bidirectional mode
; Implemented support for Oneshot42 and Multishot
; - Rev16.1 Made low rpm power limiting programmable through the startup power parameter
; - Rev16.2 Fixed bug that prevented temperature protection
; Improved robustness to very high input signal rates
; Beeps can be turned off by programming beep strength to 1
; Throttle cal difference is checked to be above required minimum before storing. Throttle cal max is not stored until successful min throttle cal
; - Rev16.3 Implemented programmable temperature protection
; Improved protection of bootloader and generally reduced risk of flash corruption
; Some small changes for improved sync hold
; - Rev16.4 Fixed bug where bootloader operation could be blocked by a defective "eeprom" signature
; - Rev16.5 Added support for DShot150, DShot300 and DShot600
; - Rev16.6 Fixed signal detection issue of multishot at 32kHz
; Improved bidirectional mode for high input signal rates
; - Rev16.7 Addition of Dshot commands for beeps and temporary reverse direction (largely by brycedjohnson)
;
;
;**** **** **** **** ****
; Minimum 8K Bytes of In-System Self-Programmable Flash
; Minimum 512 Bytes Internal SRAM
;
;**** **** **** **** ****
; 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
; Timer 0 (41.67ns counts) always counts up and is used for
; - RC pulse measurement
; Timer 1 (41.67ns counts) always counts up and is used for
; - DShot frame sync detection
; Timer 2 (500ns counts) always counts up and is used for
; - RC pulse timeout counts and commutation times
; Timer 3 (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
;
;**** **** **** **** ****
; Interrupt handling
; The C8051 does not disable interrupts when entering an interrupt routine.
; Also some interrupt flags need to be cleared by software
; The code disables interrupts in some interrupt routines
; - Interrupts are disabled during beeps, to avoid audible interference from interrupts
;
;**** **** **** **** ****
; 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
A_ EQU 1 ; X X RC X MC MB MA CC X X Cc Cp Bc Bp Ac Ap
B_ EQU 2 ; X X RC X MC MB MA CC X X Ap Ac Bp Bc Cp Cc
C_ EQU 3 ; Ac Ap MC MB MA CC X RC X X X X Cc Cp Bc Bp
D_ EQU 4 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Com fets inverted
E_ EQU 5 ; L1 L0 RC X MC MB MA CC X L2 Cc Cp Bc Bp Ac Ap A with LEDs
F_ EQU 6 ; X X RC X MA MB MC CC X X Cc Cp Bc Bp Ac Ap
G_ EQU 7 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like D, but noninverted com fets
H_ EQU 8 ; RC X X X MA MB CC MC X Ap Bp Cp X Ac Bc Cc
I_ EQU 9 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp
J_ EQU 10 ; L2 L1 L0 RC CC MB MC MA X X Cc Bc Ac Cp Bp Ap LEDs
K_ EQU 11 ; X X MC X MB CC MA RC X X Ap Bp Cp Cc Bc Ac Com fets inverted
L_ EQU 12 ; X X RC X CC MA MB MC X X Ac Bc Cc Ap Bp Cp
M_ EQU 13 ; MA MC CC MB RC L0 X X X Cc Bc Ac Cp Bp Ap X LED
N_ EQU 14 ; X X RC X MC MB MA CC X X Cp Cc Bp Bc Ap Ac
O_ EQU 15 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like D, but low side pwm
P_ EQU 16 ; X X RC MA CC MB MC X X Cc Bc Ac Cp Bp Ap X
Q_ EQU 17 ; Cp Bp Ap L1 L0 X RC X X MA MB MC CC Cc Bc Ac LEDs
R_ EQU 18 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp
S_ EQU 19 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like O, but com fets inverted
T_ EQU 20 ; RC X MA X MB CC MC X X X Cp Bp Ap Ac Bc Cc
U_ EQU 21 ; MA MC CC MB RC L0 L1 L2 X Cc Bc Ac Cp Bp Ap X Like M, but with 3 LEDs
V_ EQU 22 ; Cc X RC X MC CC MB MA X Ap Ac Bp X X Bc Cp
W_ EQU 23 ; RC MC MB X CC MA X X X Ap Bp Cp X X X X Tristate gate driver
;**** **** **** **** ****
; Select the port mapping to use (or unselect all for use with external batch compile file)
;ESCNO EQU A_
;ESCNO EQU B_
;ESCNO EQU C_
;ESCNO EQU D_
;ESCNO EQU E_
;ESCNO EQU F_
;ESCNO EQU G_
;ESCNO EQU H_
;ESCNO EQU I_
;ESCNO EQU J_
;ESCNO EQU K_
;ESCNO EQU L_
;ESCNO EQU M_
;ESCNO EQU N_
;ESCNO EQU O_
;ESCNO EQU P_
;ESCNO EQU Q_
;ESCNO EQU R_
;ESCNO EQU S_
;ESCNO EQU T_
;ESCNO EQU U_
;ESCNO EQU V_
;ESCNO EQU W_
;**** **** **** **** ****
; Select the MCU type (or unselect for use with external batch compile file)
;MCU_48MHZ EQU 0
;**** **** **** **** ****
; Select the fet deadtime (or unselect for use with external batch compile file)
;FETON_DELAY EQU 15 ; 20.4ns per step
$include (Common.inc) ; Include common source code for EFM8BBx based ESCs
;**** **** **** **** ****
; Programming defaults
;
DEFAULT_PGM_STARTUP_PWR EQU 9 ; 1=0.031 2=0.047 3=0.063 4=0.094 5=0.125 6=0.188 7=0.25 8=0.38 9=0.50 10=0.75 11=1.00 12=1.25 13=1.50
DEFAULT_PGM_COMM_TIMING EQU 3 ; 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 ; Beep strength
DEFAULT_PGM_BEACON_STRENGTH EQU 80 ; Beacon strength
DEFAULT_PGM_BEACON_DELAY EQU 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
; COMMON
DEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=Disabled
DEFAULT_PGM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148
DEFAULT_PGM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832
DEFAULT_PGM_CENTER_THROTTLE EQU 122 ; 4*122+1000=1488 (used in bidirectional mode)
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_ENABLE_POWER_PROT EQU 1 ; 1=Enabled 0=Disabled
DEFAULT_PGM_BRAKE_ON_STOP EQU 0 ; 1=Enabled 0=Disabled
DEFAULT_PGM_LED_CONTROL EQU 0 ; Byte for LED control. 2bits per LED, 0=Off, 1=On
;**** **** **** **** ****
; 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
;**** **** **** **** ****
; Register definitions
DSEG AT 20h ; Variables segment
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)
Rcp_Outside_Range_Cnt: DS 1 ; RC pulse outside range counter (incrementing)
Rcp_Timeout_Cntd: DS 1 ; RC pulse timeout counter (decrementing)
Flags0: DS 1 ; State flags. Reset upon init_start
T3_PENDING EQU 0 ; Timer 3 pending flag
DEMAG_DETECTED EQU 1 ; Set when excessive demag time is detected
COMP_TIMED_OUT EQU 2 ; Set when comparator reading timed out
; EQU 3
; EQU 4
; EQU 5
; EQU 6
; EQU 7
Flags1: DS 1 ; State flags. Reset upon init_start
STARTUP_PHASE EQU 0 ; Set when in startup phase
INITIAL_RUN_PHASE EQU 1 ; Set when in initial run phase, before synchronized run is achieved
MOTOR_STARTED EQU 2 ; Set when motor is started
DIR_CHANGE_BRAKE EQU 3 ; Set when braking before direction change
HIGH_RPM EQU 4 ; Set when motor rpm is high (Comm_Period4x_H less than 2)
; EQU 5
; EQU 6
; EQU 7
Flags2: DS 1 ; State flags. NOT reset upon init_start
RCP_UPDATED EQU 0 ; New RC pulse length value available
RCP_ONESHOT125 EQU 1 ; RC pulse input is OneShot125 (125-250us)
RCP_ONESHOT42 EQU 2 ; RC pulse input is OneShot42 (41.67-83us)
RCP_MULTISHOT EQU 3 ; RC pulse input is Multishot (5-25us)
RCP_DSHOT EQU 4 ; RC pulse input is digital shot
RCP_DIR_REV EQU 5 ; RC pulse direction in bidirectional mode
RCP_FULL_RANGE EQU 6 ; When set full input signal range is used (1000-2000us) and stored calibration values are ignored
; EQU 7
Flags3: DS 1 ; State flags. NOT reset upon init_start
PGM_DIR_REV EQU 0 ; Programmed direction. 0=normal, 1=reversed
PGM_BIDIR_REV EQU 1 ; Programmed bidirectional direction. 0=normal, 1=reversed
PGM_BIDIR EQU 2 ; Programmed bidirectional operation. 0=normal, 1=bidirectional
SKIP_T2_INT EQU 3 ; Set for 48MHz MCUs when timer 2 interrupt shall be ignored
CLOCK_SET_AT_48MHZ EQU 4 ; Set if 48MHz MCUs run at 48MHz
; EQU 5
; EQU 6
; EQU 7
;**** **** **** **** ****
; RAM definitions
DSEG AT 30h ; Ram data segment, direct addressing
Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on
Min_Throttle_L: DS 1 ; Minimum throttle scaled (lo byte)
Min_Throttle_H: DS 1 ; Minimum throttle scaled (hi byte)
Center_Throttle_L: DS 1 ; Center throttle scaled (lo byte)
Center_Throttle_H: DS 1 ; Center throttle scaled (hi byte)
Max_Throttle_L: DS 1 ; Maximum throttle scaled (lo byte)
Max_Throttle_H: DS 1 ; Maximum throttle scaled (hi byte)
Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte)
Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte)
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)
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 rpms
Timer0_X: DS 1 ; Timer 0 extended byte
Timer2_X: DS 1 ; Timer 2 extended byte
Prev_Comm_L: DS 1 ; Previous commutation timer 3 timestamp (lo byte)
Prev_Comm_H: DS 1 ; Previous commutation timer 3 timestamp (hi byte)
Prev_Comm_X: DS 1 ; Previous commutation timer 3 timestamp (ext byte)
Prev_Prev_Comm_L: DS 1 ; Pre-previous commutation timer 3 timestamp (lo byte)
Prev_Prev_Comm_H: DS 1 ; Pre-previous commutation timer 3 timestamp (hi byte)
Comm_Period4x_L: DS 1 ; Timer 3 counts between the last 4 commutations (lo byte)
Comm_Period4x_H: DS 1 ; Timer 3 counts between the last 4 commutations (hi byte)
Comparator_Read_Cnt: DS 1 ; Number of comparator reads done
Wt_Adv_Start_L: DS 1 ; Timer 3 start point for commutation advance timing (lo byte)
Wt_Adv_Start_H: DS 1 ; Timer 3 start point for commutation advance timing (hi byte)
Wt_Zc_Scan_Start_L: DS 1 ; Timer 3 start point from commutation to zero cross scan (lo byte)
Wt_Zc_Scan_Start_H: DS 1 ; Timer 3 start point from commutation to zero cross scan (hi byte)
Wt_Zc_Tout_Start_L: DS 1 ; Timer 3 start point for zero cross scan timeout (lo byte)
Wt_Zc_Tout_Start_H: DS 1 ; Timer 3 start point for zero cross scan timeout (hi byte)
Wt_Comm_Start_L: DS 1 ; Timer 3 start point from zero cross to commutation (lo byte)
Wt_Comm_Start_H: DS 1 ; Timer 3 start point from zero cross to commutation (hi byte)
Dshot_Cmd: DS 1 ; Dshot command
Dshot_Cmd_Cnt: DS 1 ; Dshot command count
New_Rcp: DS 1 ; New RC pulse value in pca counts
Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value
Power_Pwm_Reg_L: DS 1 ; Power pwm register setting (lo byte)
Power_Pwm_Reg_H: DS 1 ; Power pwm register setting (hi byte). 0x3F is minimum power
Damp_Pwm_Reg_L: DS 1 ; Damping pwm register setting (lo byte)
Damp_Pwm_Reg_H: DS 1 ; Damping pwm register setting (hi byte)
Current_Power_Pwm_Reg_H: DS 1 ; Current power pwm register setting that is loaded in the PCA register (hi byte)
Pwm_Limit: DS 1 ; Maximum allowed pwm
Pwm_Limit_By_Rpm: DS 1 ; Maximum allowed pwm for low or high rpms
Pwm_Limit_Beg: DS 1 ; Initial pwm limit
Adc_Conversion_Cnt: DS 1 ; Adc conversion counter
Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1)
Throttle_Gain: DS 1 ; Gain to be applied to RCP value
Throttle_Gain_M: DS 1 ; Gain to be applied to RCP value (multiplier 0=1x, 1=2x, 2=4x etc))
Throttle_Gain_BD_Rev: DS 1 ; Gain to be applied to RCP value for reverse direction in bidirectional mode
Throttle_Gain_BD_Rev_M: DS 1 ; Gain to be applied to RCP value for reverse direction in bidirectional mode (multiplier 0=1x, 1=2x, 2=4x etc)
Beep_Strength: DS 1 ; Strength of beeps
Flash_Key_1: DS 1 ; Flash key one
Flash_Key_2: DS 1 ; Flash key two
Temp_Prot_Limit: DS 1 ; Temperature protection limit
DShot_Pwm_Thr: DS 1 ; DShot pulse width threshold value
DShot_Timer_Preset: DS 1 ; DShot timer preset for frame sync detection
DShot_Frame_Start_L: DS 1 ; DShot frame start timestamp (lo byte)
DShot_Frame_Start_H: DS 1 ; DShot frame start timestamp (hi byte)
DShot_Frame_Length_Thr: DS 1 ; DShot frame length criteria (in units of 4 timer 2 ticks)
; Indirect addressing data segment. The variables below must be in this sequence
ISEG AT 080h
_Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gain
_Pgm_Gov_I_Gain: DS 1 ; Programmed governor I gain
_Pgm_Gov_Mode: DS 1 ; Programmed governor mode
_Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limit
_Pgm_Motor_Gain: DS 1 ; Programmed motor gain
_Pgm_Motor_Idle: DS 1 ; Programmed motor idle speed
Pgm_Startup_Pwr: DS 1 ; Programmed startup power
_Pgm_Pwm_Freq: DS 1 ; Programmed pwm frequency
Pgm_Direction: DS 1 ; Programmed rotation direction
Pgm_Input_Pol: DS 1 ; Programmed input pwm polarity
Initialized_L_Dummy: DS 1 ; Place holder
Initialized_H_Dummy: DS 1 ; Place holder
Pgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programming
_Pgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start
_Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup target
_Pgm_Startup_Rpm: DS 1 ; Programmed startup rpm (unused - place holder)
_Pgm_Startup_Accel: DS 1 ; Programmed startup acceleration (unused - place holder)
_Pgm_Volt_Comp: DS 1 ; Place holder
Pgm_Comm_Timing: DS 1 ; Programmed commutation timing
_Pgm_Damping_Force: DS 1 ; Programmed damping force (unused - place holder)
_Pgm_Gov_Range: DS 1 ; Programmed governor range
_Pgm_Startup_Method: DS 1 ; Programmed startup method (unused - place holder)
Pgm_Min_Throttle: DS 1 ; Programmed throttle minimum
Pgm_Max_Throttle: DS 1 ; Programmed throttle maximum
Pgm_Beep_Strength: DS 1 ; Programmed beep strength
Pgm_Beacon_Strength: DS 1 ; Programmed beacon strength
Pgm_Beacon_Delay: DS 1 ; Programmed beacon delay
_Pgm_Throttle_Rate: DS 1 ; Programmed throttle rate (unused - place holder)
Pgm_Demag_Comp: DS 1 ; Programmed demag compensation
_Pgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage
Pgm_Center_Throttle: DS 1 ; Programmed throttle center (in bidirectional mode)
_Pgm_Main_Spoolup_Time: DS 1 ; Programmed main spoolup time
Pgm_Enable_Temp_Prot: DS 1 ; Programmed temperature protection enable
Pgm_Enable_Power_Prot: DS 1 ; Programmed low rpm power protection enable
_Pgm_Enable_Pwm_Input: DS 1 ; Programmed PWM input signal enable
_Pgm_Pwm_Dither: DS 1 ; Programmed output PWM dither
Pgm_Brake_On_Stop: DS 1 ; Programmed braking when throttle is zero
Pgm_LED_Control: DS 1 ; Programmed LED control
; The sequence of the variables below is no longer of importance
Pgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decoded
; Indirect addressing data segment
ISEG AT 0D0h
Temp_Storage: DS 48 ; Temporary storage
;**** **** **** **** ****
CSEG AT 1A00h ; "Eeprom" segment
EEPROM_FW_MAIN_REVISION EQU 16 ; Main revision of the firmware
EEPROM_FW_SUB_REVISION EQU 7 ; Sub revision of the firmware
EEPROM_LAYOUT_REVISION EQU 33 ; 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_Gov_I_Gain: DB 0FFh
_Eep_Pgm_Gov_Mode: DB 0FFh
_Eep_Pgm_Low_Voltage_Lim: DB 0FFh
_Eep_Pgm_Motor_Gain: DB 0FFh
_Eep_Pgm_Motor_Idle: DB 0FFh
Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_STARTUP_PWR ; EEPROM copy of programmed startup power
_Eep_Pgm_Pwm_Freq: DB 0FFh
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 low byte
Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byte
Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable
_Eep_Main_Rearm_Start: DB 0FFh
_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 DEFAULT_PGM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle
Eep_Pgm_Max_Throttle: DB DEFAULT_PGM_MAX_THROTTLE ; 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 DEFAULT_PGM_CENTER_THROTTLE ; 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 DEFAULT_PGM_ENABLE_POWER_PROT ; 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 " " ; Name tag (16 Bytes)
;**** **** **** **** ****
Interrupt_Table_Definition ; SiLabs interrupts
CSEG AT 80h ; Code segment after interrupt vectors
;**** **** **** **** ****
; Table definitions
STARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0h
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer 0 interrupt routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
IF MCU_48MHZ == 1
t0_int:
inc Timer0_X
reti
ENDIF
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer 1 interrupt routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
t1_int:
clr IE_EA
clr IE_EX0 ; Disable int0 interrupts
anl EIE1, #0EFh ; Disable pca interrupts
clr TCON_TR1 ; Stop timer 1
mov TL1, DShot_Timer_Preset ; Reset sync timer
push PSW
setb PSW.3 ; Select register bank 1 for this interrupt
push ACC
push B ; Will be pop'ed by int0 exit
clr TMR2CN0_TR2 ; Timer 2 disabled
mov Temp1, TMR2L ; Read timer value
mov Temp2, TMR2H
setb TMR2CN0_TR2 ; Timer 2 enabled
setb IE_EA
; Reset timer 0
mov TL0, #0
; Check frame time length
clr C
mov A, Temp1
subb A, DShot_Frame_Start_L
mov Temp1, A
mov A, Temp2
subb A, DShot_Frame_Start_H
jnz t1_int_frame_fail ; Frame too long
clr C ; todo: was not cleared?
mov A, Temp1
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
; todo: try using registers instead?
; Check that correct number of pulses is received
mov A, DPL ; Read current pointer
cjne A, #16, t1_int_frame_fail
; Decode transmitted data
mov Temp5, #0 ; Reset timestamp
mov DPTR, #0 ; Set pointer
mov Temp1, DShot_Pwm_Thr ; DShot pulse width criteria
; Decode DShot data Msb. Use more code space to save time (by not using loop)
Decode_DShot_2Bit Temp4, t1_int_frame_fail
Decode_DShot_2Bit Temp4, t1_int_frame_fail
ajmp t1_int_decode_lsb
t1_int_frame_fail:
mov DPTR, #0 ; Set pointer to start
setb IE_EX0 ; Enable int0 interrupts
setb IE_EX1 ; Enable int1 interrupts
ajmp int0_int_outside_range
t1_int_decode_lsb:
; Decode DShot data Lsb
Decode_DShot_2Bit Temp3, t1_int_decode_fail
Decode_DShot_2Bit Temp3, t1_int_decode_fail
Decode_DShot_2Bit Temp3, t1_int_decode_fail
Decode_DShot_2Bit Temp3, t1_int_decode_fail
ajmp t1_int_decode_checksum
t1_int_decode_fail:
mov DPTR, #0 ; Set pointer to start
setb IE_EX0 ; Enable int0 interrupts
setb IE_EX1 ; Enable int1 interrupts
ajmp int0_int_outside_range
t1_int_decode_checksum:
; Decode DShot data checksum
Decode_DShot_2Bit Temp2, t1_int_decode_fail
Decode_DShot_2Bit Temp2, t1_int_decode_fail
; XOR check (in inverted data, which is ok)
mov A, Temp3
swap A
xrl A, Temp3
xrl A, Temp4
xrl A, Temp2
anl A, #0Fh
jnz t1_int_decode_fail ; XOR check
; Invert DShot data and subtract 96 (still 12 bits)
clr C
mov A, Temp3
cpl A
mov Temp2, A
subb A, #96
mov Temp3, A
mov A, Temp4
cpl A
anl A, #0Fh
subb A, #0
mov Temp4, A
jnc t1_normal_range
mov A, Temp2 ; Check for 0 or dshot command
mov Temp4, #0
mov Temp3, #0
jz t1_normal_range
mov Temp2, #0
clr C ; We are in the special dshot range
rrc A ; Divide by 2
jnc t1_dshot_set_cmd ; Check for tlm bit set (if not telemetry, Temp2 will be zero and result in invalid command)
mov Temp2, A
cjne A, Dshot_Cmd, t1_dshot_set_cmd
inc Dshot_Cmd_Cnt
jmp t1_normal_range
t1_dshot_set_cmd:
mov Dshot_Cmd, Temp2
mov Dshot_Cmd_Cnt, #0
t1_normal_range:
; Check for bidirectional operation (0=stop, 96-2095->fwd, 2096-4095->rev)
jnb Flags3.PGM_BIDIR, t1_int_not_bidir ; If not bidirectional operation - branch
; Subtract 2000 (still 12 bits)
clr C
mov A, Temp3
subb A, #0D0h
mov Temp1, A
mov A, Temp4
subb A, #07h
mov Temp2, A
jc t1_int_bidir_fwd ; If result is negative - branch
mov A, Temp1
mov Temp3, A
mov A, Temp2
mov Temp4, A
jb Flags2.RCP_DIR_REV, t1_int_bidir_rev_chk ; If same direction - branch
setb Flags2.RCP_DIR_REV
ajmp t1_int_bidir_rev_chk
t1_int_bidir_fwd:
jnb Flags2.RCP_DIR_REV, t1_int_bidir_rev_chk ; If same direction - branch
clr Flags2.RCP_DIR_REV
t1_int_bidir_rev_chk:
jb Flags3.PGM_BIDIR_REV, ($+5)
cpl Flags2.RCP_DIR_REV
clr C ; Multiply throttle value by 2
mov A, Temp3
rlc A
mov Temp3, A
mov A, Temp4
rlc A
mov Temp4, A
t1_int_not_bidir:
; Generate 4/256
mov A, Temp4
add A, Temp4
addc A, Temp4
addc A, Temp4
mov Temp2, A
; Align to 11 bits
clr C
mov A, Temp4
rrc A
mov Temp4, A
mov A, Temp3
rrc A
mov Temp3, A
; Scale from 2000 to 2048
mov A, Temp3
add A, Temp2 ; Holds 4/128
mov Temp3, A
mov A, Temp4
addc A, #0
mov Temp4, A
jnb ACC.3, ($+7)
mov Temp3, #0FFh
mov Temp4, #0FFh
; Boost pwm during direct start
mov A, Flags1
anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE))
jz t1_int_startup_boosted
jb Flags1.MOTOR_STARTED, t1_int_startup_boosted ; Do not boost when changing direction in bidirectional mode
mov A, Pwm_Limit_Beg ; Set 25% of max startup power as minimum power
rlc A
mov Temp2, A
mov A, Temp4
jnz t1_int_startup_boost_stall
clr C
mov A, Temp2
subb A, Temp3
jc t1_int_startup_boost_stall
mov A, Temp2
mov Temp3, A
t1_int_startup_boost_stall:
mov A, Stall_Cnt ; Add an extra power boost during start
swap A
rlc A
add A, Temp3
mov Temp3, A
mov A, Temp4
addc A, #0
mov Temp4, A
t1_int_startup_boosted:
; Set 8bit value
clr C
mov A, Temp3
rlc A
swap A
anl A, #0Fh
mov Temp1, A
mov A, Temp4
rlc A
swap A
anl A, #0F0h
orl A, Temp1
mov Temp1, A
jnz t1_int_zero_rcp_checked ; New_Rcp (Temp1) is only zero if all 11 bits are zero
mov A, Temp3
jz t1_int_zero_rcp_checked
mov Temp1, #1
t1_int_zero_rcp_checked:
; Align to 10 bits for 24MHz MCU
IF MCU_48MHZ == 0
clr C
mov A, Temp4
rrc A
mov Temp4, A
mov A, Temp3
rrc A
mov Temp3, A
ENDIF
mov DPTR, #0 ; Set pointer to start
setb IE_EX0 ; Enable int0 interrupts
setb IE_EX1 ; Enable int1 interrupts
; Decrement outside range counter
mov A, Rcp_Outside_Range_Cnt
jz ($+4)
dec Rcp_Outside_Range_Cnt
ajmp int0_int_pulse_ready
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer 2 interrupt routine
;
; No assumptions
; Requirements: Temp variables can NOT be used since PSW.x is not set
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
t2_int: ; Happens every 32ms
push PSW ; Preserve registers through interrupt
push ACC
clr TMR2CN0_TF2H ; Clear interrupt flag
inc Timer2_X
IF MCU_48MHZ == 1
jnb Flags3.CLOCK_SET_AT_48MHZ, t2_int_start
; Check skip variable
jnb Flags3.SKIP_T2_INT, t2_int_start ; Execute this interrupt
clr Flags3.SKIP_T2_INT
ajmp t2_int_exit
t2_int_start:
setb Flags3.SKIP_T2_INT ; Skip next interrupt
ENDIF
; Update RC pulse timeout counter
mov A, Rcp_Timeout_Cntd ; RC pulse timeout count zero?
jz ($+4) ; Yes - do not decrement
dec Rcp_Timeout_Cntd ; No decrement
; Check RC pulse against stop value
clr C
mov A, New_Rcp ; Load new pulse value
jz t2_int_rcp_stop ; Check if pulse is below stop value
; RC pulse higher than stop value, reset stop counter
mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter
ajmp t2_int_exit
t2_int_rcp_stop:
; RC pulse less than stop value
mov A, Rcp_Stop_Cnt ; Increment stop counter
add A, #1
mov Rcp_Stop_Cnt, A
jnc ($+5) ; Branch if counter has not wrapped
mov Rcp_Stop_Cnt, #0FFh ; Set stop counter to max
t2_int_exit:
pop ACC ; Restore preserved registers
pop PSW
reti
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Timer 3 interrupt routine
;
; No assumptions
; Requirements: Temp variables can NOT be used since PSW.x is not set
; ACC can not be used, as it is not pushed to stack
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
t3_int: ; Used for commutation timing
clr IE_EA ; Disable all interrupts
anl EIE1, #7Fh ; Disable timer 3 interrupts
mov TMR3RLL, #0FAh ; Set a short delay before next interrupt
mov TMR3RLH, #0FFh
clr Flags0.T3_PENDING ; Flag that timer has wrapped
anl TMR3CN0, #07Fh ; Timer 3 interrupt flag cleared
setb IE_EA ; Enable all interrupts
reti
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Int0 interrupt routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
int0_int: ; Used for RC pulse timing
push ACC
mov A, TL0 ; Read pwm for DShot immediately
; Test for DShot
jnb Flags2.RCP_DSHOT, int0_int_not_dshot
mov TL1, DShot_Timer_Preset ; Reset sync timer
movx @DPTR, A ; Store pwm
inc DPTR
pop ACC
reti
; Not DShot
int0_int_not_dshot:
pop ACC
clr IE_EA
anl EIE1, #0EFh ; Disable pca interrupts
push PSW ; Preserve registers through interrupt
push ACC
push B
setb PSW.3 ; Select register bank 1 for this interrupt
setb IE_EA
; Get the counter values
Get_Rcp_Capture_Values
; Scale down to 10 bits (for 24MHz, and 11 bits for 48MHz)
jnb Flags2.RCP_MULTISHOT, int0_int_fall_not_multishot
; Multishot - Multiply by 2 and add 1/16 and 1/32
mov A, Temp1 ; Divide by 16
swap A
anl A, #0Fh
mov Temp3, A
mov A, Temp2
swap A
anl A, #0F0h
orl A, Temp3
mov Temp3, A
clr C ; Make divided by 32
rrc A
add A, Temp3 ; Add 1/16 to 1/32
mov Temp3, A
clr C ; Multiply by 2
mov A, Temp1
rlc A
mov Temp1, A
mov A, Temp2
rlc A
mov Temp2, A
mov A, Temp1 ; Add 1/16 and 1/32
add A, Temp3
mov Temp3, A
mov A, Temp2
IF MCU_48MHZ == 0
addc A, #03h ; Add to low end, to make signal look like 20-40us
ELSE
addc A, #06h
ENDIF
mov Temp4, A
ajmp int0_int_fall_gain_done
int0_int_fall_not_multishot:
jnb Flags2.RCP_ONESHOT42, int0_int_fall_not_oneshot_42
; Oneshot42 - Add 2/256
clr C
mov A, Temp1
rlc A
mov A, Temp2
rlc A
mov Temp3, A
mov A, Temp1
add A, Temp3
mov Temp3, A
mov A, Temp2
addc A, #0
mov Temp4, A
ajmp int0_int_fall_gain_done
int0_int_fall_not_oneshot_42:
jnb Flags2.RCP_ONESHOT125, int0_int_fall_not_oneshot_125
; Oneshot125 - multiply by 86/256
mov A, Temp1 ; Multiply by 86 and divide by 256
mov B, #56h
mul AB
mov Temp3, B
mov A, Temp2
mov B, #56h
mul AB
add A, Temp3
mov Temp3, A
xch A, B
addc A, #0
mov Temp4, A
ajmp int0_int_fall_gain_done
int0_int_fall_not_oneshot_125:
; Regular signal - multiply by 43/1024
IF MCU_48MHZ == 1
clr C
mov A, Temp3 ; Divide by 2
rrc A
mov Temp3, A
mov A, Temp2
rrc A
mov Temp2, A
mov A, Temp1
rrc A
mov Temp1, A
ENDIF
mov A, Temp1 ; Multiply by 43 and divide by 1024
IF MCU_48MHZ == 0
mov B, #2Bh
ELSE
mov B, #56h ; Multiply by 86
ENDIF
mul AB
mov Temp3, B
mov A, Temp2
IF MCU_48MHZ == 0
mov B, #2Bh
ELSE
mov B, #56h ; Multiply by 86
ENDIF
mul AB
add A, Temp3
mov Temp3, A
xch A, B
addc A, #0
clr C
rrc A ; Divide by 2 for total 512
mov Temp4, A
mov A, Temp3
rrc A
mov Temp3, A
clr C
mov A, Temp4 ; Divide by 2 for total 1024
rrc A
mov Temp4, A
mov A, Temp3
rrc A
mov Temp3, A
int0_int_fall_gain_done:
; Check if 2235us or above (in order to ignore false pulses)
clr C
mov A, Temp4 ; Is pulse 2235us or higher?
IF MCU_48MHZ == 0
subb A, #09h
ELSE
subb A, #12h
ENDIF
jnc int0_int_outside_range ; Yes - ignore pulse
; Check if below 900us (in order to ignore false pulses)
clr C
mov A, Temp3
IF MCU_48MHZ == 0
subb A, #9Ah
ELSE
subb A, #34h
ENDIF
mov A, Temp4
IF MCU_48MHZ == 0
subb A, #03h
ELSE
subb A, #07h
ENDIF
jnc int0_int_check_full_range ; No - proceed
int0_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
jnc ($+4)
ajmp int0_int_set_timeout ; If outside limits - ignore first pulses
mov New_Rcp, #0 ; Set pulse length to zero
ajmp int0_int_exit ; Exit without reseting timeout
int0_int_check_full_range:
; Decrement outside range counter
mov A, Rcp_Outside_Range_Cnt
jz ($+4)
dec Rcp_Outside_Range_Cnt