-
Notifications
You must be signed in to change notification settings - Fork 0
/
tgy.asm
3381 lines (3177 loc) · 103 KB
/
tgy.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
;**** **** **** **** ****
;
;Die Benutzung der Software ist mit folgenden Bedingungen verbunden:
;
;1. Da ich alles kostenlos zur Verfügung stelle, gebe ich keinerlei Garantie
; und übernehme auch keinerlei Haftung für die Folgen der Benutzung.
;
;2. Die Software ist ausschließlich zur privaten Nutzung bestimmt. Ich
; habe nicht geprüft, ob bei gewerblicher Nutzung irgendwelche Patentrechte
; verletzt werden oder sonstige rechtliche Einschränkungen vorliegen.
;
;3. Jeder darf Änderungen vornehmen, z.B. um die Funktion seinen Bedürfnissen
; anzupassen oder zu erweitern. Ich würde mich freuen, wenn ich weiterhin als
; Co-Autor in den Unterlagen erscheine und mir ein Link zur entprechenden Seite
; (falls vorhanden) mitgeteilt wird.
;
;4. Auch nach den Änderungen sollen die Software weiterhin frei sein, d.h. kostenlos bleiben.
;
;!! Wer mit den Nutzungbedingungen nicht einverstanden ist, darf die Software nicht nutzen !!
;
; tp-18a
; October 2004
; autor: Bernhard Konze
; email: bernhard.konze@versanet.de
;--
; Based on upon Bernhard's "tp-18a" and others; see
; http://home.versanet.de/~b-konze/blc_18a/blc_18a.htm
; Copyright (C) 2004 Bernhard Konze
; Copyright (C) 2011-2012 Simon Kirby and other contributors
; NO WARRANTY EXPRESSED OR IMPLIED. USE AT YOUR OWN RISK. Always test
; without propellers! Please respect Bernhard Konze's license above.
;--
; WARNING: I have blown FETs on Turnigy Plush 18A ESCs in previous versions
; of this code with my modifications. Some bugs have since been fixed, such
; as leaving PWM enabled while busy-looping forever outside of ISR code.
; However, this does run with higher PWM frequency than most original code,
; so higher FET temperatures may occur! USE AT YOUR OWN RISK, and maybe see
; how it compares and let me know!
;
; WARNING: This does not check temperature or voltage ADC inputs.
;
; NOTE: We do 16-bit PWM on timer2 at full CPU clock rate resolution, using
; tcnt2h to simulate the high byte. An input FULL to STOP range of 800 plus
; a MIN_DUTY of 56 (a POWER_RANGE of 856) gives 800 unique PWM steps at an
; about 18kHz on a 16MHz CPU clock. The output frequency is slightly lower
; than F_CPU / POWER_RANGE due to cycles used in the interrupt as TCNT2 is
; reloaded.
;
; Simon Kirby <sim@simulated.ca>
;
;-- Device ----------------------------------------------------------------
;
.include "m8def.inc"
;
; 8K Bytes of In-System Self-Programmable Flash
; 512 Bytes EEPROM
; 1K Byte Internal SRAM
;
;-- Fuses -----------------------------------------------------------------
;
; Old fuses for internal RC oscillator at 8MHz were lfuse=0xa4 hfuse=0xdf,
; but since we now set OSCCAL to 0xff (about 16MHz), running under 4.5V is
; officially out of spec. We'd better set the brown-out detection to 4.0V.
; The resulting code works with or without external 16MHz oscillators.
; Boards with external oscillators can use lfuse=0x3f.
;
; If the boot loader is enabled, the last nibble of the hfuse should be set
; to 'a' or '2' to also enable EESAVE - save EEPROM on chip erase. This is
; a 512-word boot flash section (0xe00), and enable BOOTRST to jump to it.
; Setting these fuses actually has no harm even without the boot loader,
; since 0xffff is nop, and it will just nop-sled around into normal code.
;
; Suggested fuses with 4.0V brown-out voltage:
; Without external oscillator: avrdude -U lfuse:w:0x24:m -U hfuse:w:0xda:m
; With external oscillator: avrdude -U lfuse:w:0x3f:m -U hfuse:w:0xca:m
;
; Don't set WDTON if using the boot loader. We will enable it on start.
;
;-- Board -----------------------------------------------------------------
;
; The following only works with avra or avrasm2.
; For avrasm32, just comment out all but the include you need.
#if defined(afro_esc)
#include "afro.inc" ; AfroESC (ICP PWM, I2C, UART)
#elif defined(afro2_esc)
#include "afro2.inc" ; AfroESC 2 (ICP PWM, I2C, UART)
#elif defined(afro_hv_esc)
#include "afro_hv.inc" ; AfroESC HV with drivers (ICP PWM, I2C, UART)
#elif defined(afro_nfet_esc)
#include "afro_nfet.inc" ; AfroESC 3 with all nFETs (ICP PWM, I2C, UART)
#elif defined(arctictiger_esc)
#include "arctictiger.inc" ; Arctic Tiger 30A ESC with all nFETs (ICP PWM)
#elif defined(birdie70a_esc)
#include "birdie70a.inc" ; Birdie 70A with all nFETs (INT0 PWM)
#elif defined(mkblctrl1_esc)
#include "mkblctrl1.inc" ; MK BL-Ctrl v1.2 (ICP PWM, I2C, UART, high side PWM, sense hack)
#elif defined(bs_esc)
#include "bs.inc" ; HobbyKing BlueSeries / Mystery (INT0 PWM)
#elif defined(bs_nfet_esc)
#include "bs_nfet.inc" ; HobbyKing BlueSeries / Mystery with all nFETs (INT0 PWM)
#elif defined(bs40a_esc)
#include "bs40a.inc" ; HobbyKing BlueSeries / Mystery 40A (INT0 PWM)
#elif defined(dlu40a_esc)
#include "dlu40a.inc" ; Pulso Advance Plus 40A DLU40A inverted-PWM-opto (INT0 PWM)
#elif defined(dlux_esc)
#include "dlux.inc" ; HobbyKing Dlux Turnigy ESC 20A
#elif defined(diy0_esc)
#include "diy0.inc" ; HobbyKing DIY Open ESC (unreleased rev 0)
#elif defined(dys_nfet_esc)
#include "dys_nfet.inc" ; DYS 30A ESC with all nFETs (ICP PWM, I2C, UART)
#elif defined(hk200a_esc)
#include "hk200a.inc" ; HobbyKing SS Series 190-200A with all nFETs (INT0 PWM)
#elif defined(hm135a_esc)
#include "hm135a.inc" ; Hacker/Jeti Master 135-O-F5B 135A inverted-PWM-opto (INT0 PWM)
#elif defined(kda_esc)
#include "kda.inc" ; Keda/Multistar 12A, 20A, 30A (original) (inverted INT0 PWM)
#elif defined(kda_8khz_esc)
#include "kda_8khz.inc" ; Keda/Multistar 30A (early 2014) (inverted INT0 PWM)
#elif defined(kda_nfet_esc)
#include "kda_nfet.inc" ; Keda/Multistar 30A with all nFETs (inverted INT0 PWM)
#elif defined(kda_nfet_ni_esc)
#include "kda_nfet_ni.inc" ; Keda/Multistar/Sunrise ~30A with all nFETs (INT0 PWM)
#elif defined(rb50a_esc)
#include "rb50a.inc" ; Red Brick 50A with all nFETs (INT0 PWM)
#elif defined(rb70a_esc)
#include "rb70a.inc" ; Red Brick 70A with all nFETs (INT0 PWM)
#elif defined(rct50a_esc)
#include "rct50a.inc" ; RCTimer 50A (MLF version) with all nFETs (INT0 PWM)
#elif defined(tbs_esc)
#include "tbs.inc" ; TBS 30A ESC (Team BlackSheep) with all nFETs (ICP PWM, UART)
#elif defined(tbs_hv_esc)
#include "tbs_hv.inc" ; TBS high voltage ESC (Team BlackSheep) with all nFETs (ICP PWM, UART)
#elif defined(tp_esc)
#include "tp.inc" ; TowerPro 25A/HobbyKing 18A "type 1" (INT0 PWM)
#elif defined(tp_8khz_esc)
#include "tp_8khz.inc" ; TowerPro 25A/HobbyKing 18A "type 1" (INT0 PWM) at 8kHz PWM
#elif defined(tp_i2c_esc)
#include "tp_i2c.inc" ; TowerPro 25A/HobbyKing 18A "type 1" (I2C)
#elif defined(tp_nfet_esc)
#include "tp_nfet.inc" ; TowerPro 25A with all nFETs "type 3" (INT0 PWM)
#elif defined(tp70a_esc)
#include "tp70a.inc" ; TowerPro 70A with BL8003 FET drivers (INT0 PWM)
#elif defined(tgy6a_esc)
#include "tgy6a.inc" ; Turnigy Plush 6A (INT0 PWM)
#elif defined(tgy_esc)
#include "tgy.inc" ; TowerPro/Turnigy Basic/Plush "type 2" (INT0 PWM)
#else
#error "Unrecognized board type."
#endif
.equ CPU_MHZ = F_CPU / 1000000
.equ BOOT_LOADER = 1 ; Include Turnigy USB linker STK500v2 boot loader on PWM input pin
.equ BOOT_JUMP = 1 ; Jump to any boot loader when PWM input stays high
.equ BOOT_START = THIRDBOOTSTART
.if !defined(COMP_PWM)
.equ COMP_PWM = 0 ; During PWM off, switch high side on (unsafe on some boards!)
.endif
.if !defined(DEAD_LOW_NS)
.equ DEAD_LOW_NS = 300 ; Low-side dead time w/COMP_PWM (62.5ns steps @ 16MHz, max 2437ns)
.equ DEAD_HIGH_NS = 300 ; High-side dead time w/COMP_PWM (62.5ns steps @ 16MHz, max roughly PWM period)
.endif
.equ DEAD_TIME_LOW = DEAD_LOW_NS * CPU_MHZ / 1000
.equ DEAD_TIME_HIGH = DEAD_HIGH_NS * CPU_MHZ / 1000
.if !defined(MOTOR_ADVANCE)
.equ MOTOR_ADVANCE = 18 ; Degrees of timing advance (0 - 30, 30 meaning no delay)
.endif
.if !defined(TIMING_OFFSET)
.equ TIMING_OFFSET = 0 ; Motor timing offset in microseconds
.endif
.equ MOTOR_BRAKE = 0 ; Enable brake during neutral/idle ("motor drag" brake)
.equ LOW_BRAKE = 0 ; Enable brake on very short RC pulse ("thumb" brake like on Airtronics XL2P)
.if !defined(MOTOR_REVERSE)
.equ MOTOR_REVERSE = 0 ; Reverse normal commutation direction
.endif
.equ RC_PULS_REVERSE = 0 ; Enable RC-car style forward/reverse throttle
.equ RC_CALIBRATION = 1 ; Support run-time calibration of min/max pulse lengths
.equ SLOW_THROTTLE = 0 ; Limit maximum throttle jump to try to prevent overcurrent
.equ BEACON = 1 ; Beep periodically when RC signal is lost
.if !defined(CHECK_HARDWARE)
.equ CHECK_HARDWARE = 0 ; Check for correct pin configuration, sense inputs, and functioning MOSFETs
.endif
.equ CELL_MAX_DV = 43 ; Maximum battery cell deciV
.equ CELL_MIN_DV = 35 ; Minimum battery cell deciV
.equ CELL_COUNT = 0 ; 0: auto, >0: hard-coded number of cells (for reliable LVC > ~4S)
.equ BLIP_CELL_COUNT = 0 ; Blip out cell count before arming
.equ DEBUG_ADC_DUMP = 0 ; Output an endless loop of all ADC values (no normal operation)
.equ MOTOR_DEBUG = 0 ; Output sync pulses on MOSI or SCK, debug flag on MISO
.equ I2C_ADDR = 0x50 ; MK-style I2C address
.equ MOTOR_ID = 1 ; MK-style I2C motor ID, or UART motor number
.equ RCP_TOT = 2 ; Number of 65536us periods before considering rc pulse lost
; These are now defaults which can be adjusted via throttle calibration
; (stick high, stick low, (stick neutral) at start).
; These might be a bit wide for most radios, but lines up with POWER_RANGE.
.equ STOP_RC_PULS = 1060 ; Stop motor at or below this pulse length
.equ FULL_RC_PULS = 1860 ; Full speed at or above this pulse length
.equ MAX_RC_PULS = 2400 ; Throw away any pulses longer than this
.equ MIN_RC_PULS = 100 ; Throw away any pulses shorter than this
.equ MID_RC_PULS = (STOP_RC_PULS + FULL_RC_PULS) / 2 ; Neutral when RC_PULS_REVERSE = 1
.if RC_PULS_REVERSE
.equ RCP_DEADBAND = 50 ; Do not start until this much above or below neutral
.equ PROGRAM_RC_PULS = (STOP_RC_PULS + FULL_RC_PULS * 3) / 4 ; Normally 1660
.else
.equ RCP_DEADBAND = 0
.equ PROGRAM_RC_PULS = (STOP_RC_PULS + FULL_RC_PULS) / 2 ; Normally 1460
.endif
.if LOW_BRAKE
.equ RCP_LOW_DBAND = 60 ; Brake at this many microseconds below low pulse
.endif
.equ MAX_DRIFT_PULS = 10 ; Maximum jitter/drift microseconds during programming
; Minimum PWM on-time (too low and FETs won't turn on, hard starting)
.if !defined(MIN_DUTY)
.equ MIN_DUTY = 56 * CPU_MHZ / 16
.endif
; Number of PWM steps (too high and PWM frequency drops into audible range)
.if !defined(POWER_RANGE)
.equ POWER_RANGE = 800 * CPU_MHZ / 16 + MIN_DUTY
.endif
.equ MAX_POWER = (POWER_RANGE-1)
.equ PWR_COOL_START = (POWER_RANGE/24) ; Power limit while starting to reduce heating
.equ PWR_MIN_START = (POWER_RANGE/6) ; Power limit while starting (to start)
.equ PWR_MAX_START = (POWER_RANGE/4) ; Power limit while starting (if still not running)
.equ PWR_MAX_RPM1 = (POWER_RANGE/4) ; Power limit when running slower than TIMING_RANGE1
.equ PWR_MAX_RPM2 = (POWER_RANGE/2) ; Power limit when running slower than TIMING_RANGE2
.equ BRAKE_POWER = MAX_POWER*2/3 ; Brake force is exponential, so start fairly high
.equ BRAKE_SPEED = 3 ; Speed to reach MAX_POWER, 0 (slowest) - 8 (fastest)
.equ LOW_BRAKE_POWER = MAX_POWER*2/3
.equ LOW_BRAKE_SPEED = 5
.equ TIMING_MIN = 0x8000 ; 8192us per commutation
.equ TIMING_RANGE1 = 0x4000 ; 4096us per commutation
.equ TIMING_RANGE2 = 0x2000 ; 2048us per commutation
.equ TIMING_RANGE3 = 0x1000 ; 1024us per commutation
.equ TIMING_MAX = 0x00e0 ; 56us per commutation
.equ TIMEOUT_START = 48000 ; Timeout per commutation for ZC during starting
.if !defined(START_DELAY_US)
.equ START_DELAY_US = 0 ; Initial post-commutation wait during starting
.endif
.equ START_DSTEP_US = 8 ; Microseconds per start delay step
.equ START_DELAY_INC = 15 ; Wait step count increase (wraps in a byte)
.equ START_MOD_INC = 4 ; Start power modulation step count increase (wraps in a byte)
.equ START_MOD_LIMIT = 48 ; Value at which power is reduced to avoid overheating
.equ START_FAIL_INC = 16 ; start_tries step count increase (wraps in a byte, upon which we disarm)
.equ ENOUGH_GOODIES = 12 ; This many start cycles without timeout will transition to running mode
.equ ZC_CHECK_FAST = 12 ; Number of ZC check loops under which PWM noise should not matter
.equ ZC_CHECK_MAX = POWER_RANGE / 32 ; Limit ZC checking to about 1/2 PWM interval
.equ ZC_CHECK_MIN = 3
.equ T0CLK = (1<<CS01) ; clk/8 == 2MHz
.equ T1CLK = (1<<CS10)+(USE_ICP<<ICES1)+(USE_ICP<<ICNC1) ; clk/1 == 16MHz
.equ T2CLK = (1<<CS20) ; clk/1 == 16MHz
.equ EEPROM_SIGN = 31337 ; Random 16-bit value
.equ EEPROM_OFFSET = 0x80 ; Offset into 512-byte space (why not)
; Conditional code inclusion
.set DEBUG_TX = 0 ; Output debugging on UART TX pin
.set ADC_READ_NEEDED = 0 ; Reading from ADCs
;**** **** **** **** ****
; Register Definitions
.def temp5 = r0 ; aux temporary (L) (limited operations)
.def temp6 = r1 ; aux temporary (H) (limited operations)
.def duty_l = r2 ; on duty cycle low, one's complement
.def duty_h = r3 ; on duty cycle high
.def off_duty_l = r4 ; off duty cycle low, one's complement
.def off_duty_h = r5 ; off duty cycle high
.def rx_l = r6 ; received throttle low
.def rx_h = r7 ; received throttle high
.def tcnt2h = r8 ; timer2 high byte
.def i_sreg = r9 ; status register save in interrupts
.def temp7 = r10 ; really aux temporary (limited operations)
.def rc_timeout = r11
.def sys_control_l = r12 ; duty limit low (word register aligned)
.def sys_control_h = r13 ; duty limit high
.def timing_duty_l = r14 ; timing duty limit low
.def timing_duty_h = r15 ; timing duty limit high
.def flags0 = r16 ; state flags
.equ OCT1_PENDING = 0 ; if set, output compare interrupt is pending
.equ SET_DUTY = 1 ; if set when armed, set duty during evaluate_rc
.equ RCP_ERROR = 2 ; if set, corrupted PWM input was seen
; .equ GET_STATE = 3 ; set if state is to be send
.equ EEPROM_RESET = 4 ; if set, reset EEPROM
.equ EEPROM_WRITE = 5 ; if set, save settings to EEPROM
.equ UART_SYNC = 6 ; if set, we are waiting for our serial throttle byte
.equ NO_CALIBRATION = 7 ; if set, disallow calibration (unsafe reset cause)
.def flags1 = r17 ; state flags
.equ POWER_ON = 0 ; if set, switching fets is enabled
.equ FULL_POWER = 1 ; 100% on - don't switch off, but do OFF_CYCLE working
.equ I2C_MODE = 2 ; if receiving updates via I2C
.equ UART_MODE = 3 ; if receiving updates via UART
.equ EVAL_RC = 4 ; if set, evaluate rc command while waiting for OCT1
.equ ACO_EDGE_HIGH = 5 ; if set, looking for ACO high - same bit position as ACO
.equ STARTUP = 6 ; if set, startup-phase is active
.equ REVERSE = 7 ; if set, do reverse commutation
.def flags2 = r18
.equ A_FET = 0 ; if set, A FET is being PWMed
.equ B_FET = 1 ; if set, B FET is being PWMed
.equ C_FET = 2 ; if set, C FET is being PWMed
.equ ALL_FETS = (1<<A_FET)+(1<<B_FET)+(1<<C_FET)
.equ SKIP_CPWM = 7 ; if set, skip complementary PWM (for short off period)
;.def = r19
.def i_temp1 = r20 ; interrupt temporary
.def i_temp2 = r21 ; interrupt temporary
.def temp3 = r22 ; main temporary (L)
.def temp4 = r23 ; main temporary (H)
.def temp1 = r24 ; main temporary (L), adiw-capable
.def temp2 = r25 ; main temporary (H), adiw-capable
; XL: general temporary
; XH: general temporary
; YL: general temporary
; YH: general temporary
; ZL: Next PWM interrupt vector (low)
; ZH: Next PWM interrupt vector (high, stays at zero) -- used as "zero" register
;**** **** **** **** ****
; RAM Definitions
.dseg ; DATA segment
.org SRAM_START
orig_osccal: .byte 1 ; original OSCCAL value
goodies: .byte 1 ; Number of rounds without timeout
powerskip: .byte 1 ; Skip power through this number of steps
ocr1ax: .byte 1 ; 3rd byte of OCR1A
tcnt1x: .byte 1 ; 3rd byte of TCNT1
pwm_on_ptr: .byte 1 ; Next PWM ON vector
rct_boot: .byte 1 ; Counter which increments while rc_timeout is 0 to jump to boot loader
rct_beacon: .byte 1 ; Counter which increments while rc_timeout is 0 to disarm and beep occasionally
last_tcnt1_l: .byte 1 ; last timer1 value
last_tcnt1_h: .byte 1
last_tcnt1_x: .byte 1
l2_tcnt1_l: .byte 1 ; last last timer1 value
l2_tcnt1_h: .byte 1
l2_tcnt1_x: .byte 1
timing_l: .byte 1 ; interval of 2 commutations
timing_h: .byte 1
timing_x: .byte 1
com_time_l: .byte 1 ; time of last commutation
com_time_h: .byte 1
com_time_x: .byte 1
start_delay: .byte 1 ; delay count after starting commutations before checking back-EMF
start_modulate: .byte 1 ; Start modulation counter (to reduce heating from PWR_MAX_START if stuck)
start_fail: .byte 1 ; Number of start_modulate loops for eventual failure and disarm
rc_duty_l: .byte 1 ; desired duty cycle
rc_duty_h: .byte 1
fwd_scale_l: .byte 1 ; 16.16 multipliers to scale input RC pulse to POWER_RANGE
fwd_scale_h: .byte 1
rev_scale_l: .byte 1
rev_scale_h: .byte 1
neutral_l: .byte 1 ; Offset for neutral throttle (in CPU_MHZ)
neutral_h: .byte 1
.if USE_I2C
i2c_max_pwm: .byte 1 ; MaxPWM for MK (NOTE: 250 while stopped is magic and enables v2)
i2c_rx_state: .byte 1
i2c_blc_offset: .byte 1
.endif
motor_count: .byte 1 ; Motor number for serial control
brake_sub: .byte 1 ; Brake speed subtrahend (power of two)
brake_want: .byte 1 ; Type of brake desired
brake_active: .byte 1 ; Type of brake active
;**** **** **** **** ****
; The following entries are block-copied from/to EEPROM
eeprom_sig_l: .byte 1
eeprom_sig_h: .byte 1
puls_high_l: .byte 1 ; -,
puls_high_h: .byte 1 ; |
puls_low_l: .byte 1 ; |- saved pulse lengths during throttle calibration
puls_low_h: .byte 1 ; | (order used by rc_prog)
puls_neutral_l: .byte 1 ; |
puls_neutral_h: .byte 1 ; -'
.if USE_I2C
blc_revision: .byte 1 ; BLConfig revision
blc_setmask: .byte 1 ; BLConfig settings mask
blc_pwmscaling: .byte 1 ; BLConfig pwm scaling
blc_currlimit: .byte 1 ; BLConfig current limit
blc_templimit: .byte 1 ; BLConfig temperature limit
blc_currscale: .byte 1 ; BLConfig current scaling
blc_bitconfig: .byte 1 ; BLConfig bitconfig (1 == MOTOR_REVERSE)
blc_checksum: .byte 1 ; BLConfig checksum (0xaa + above bytes)
.endif
eeprom_end: .byte 1
;-----bko-----------------------------------------------------------------
;**** **** **** **** ****
.cseg
.org 0
;**** **** **** **** ****
; ATmega8 interrupts
;.equ INT0addr=$001 ; External Interrupt0 Vector Address
;.equ INT1addr=$002 ; External Interrupt1 Vector Address
;.equ OC2addr =$003 ; Output Compare2 Interrupt Vector Address
;.equ OVF2addr=$004 ; Overflow2 Interrupt Vector Address
;.equ ICP1addr=$005 ; Input Capture1 Interrupt Vector Address
;.equ OC1Aaddr=$006 ; Output Compare1A Interrupt Vector Address
;.equ OC1Baddr=$007 ; Output Compare1B Interrupt Vector Address
;.equ OVF1addr=$008 ; Overflow1 Interrupt Vector Address
;.equ OVF0addr=$009 ; Overflow0 Interrupt Vector Address
;.equ SPIaddr =$00a ; SPI Interrupt Vector Address
;.equ URXCaddr=$00b ; USART Receive Complete Interrupt Vector Address
;.equ UDREaddr=$00c ; USART Data Register Empty Interrupt Vector Address
;.equ UTXCaddr=$00d ; USART Transmit Complete Interrupt Vector Address
;.equ ADCCaddr=$00e ; ADC Interrupt Vector Address
;.equ ERDYaddr=$00f ; EEPROM Interrupt Vector Address
;.equ ACIaddr =$010 ; Analog Comparator Interrupt Vector Address
;.equ TWIaddr =$011 ; Irq. vector address for Two-Wire Interface
;.equ SPMaddr =$012 ; SPM complete Interrupt Vector Address
;.equ SPMRaddr =$012 ; SPM complete Interrupt Vector Address
;-----bko-----------------------------------------------------------------
; Reset and interrupt jump table
; When multiple interrupts are pending, the vectors are executed from top
; (ext_int0) to bottom.
rjmp reset ; reset
rjmp rcp_int ; ext_int0
reti ; ext_int1
reti ; t2oc_int
ijmp ; t2ovfl_int
rjmp rcp_int ; icp1_int
rjmp t1oca_int ; t1oca_int
reti ; t1ocb_int
rjmp t1ovfl_int ; t1ovfl_int
reti ; t0ovfl_int
reti ; spi_int
rjmp urxc_int ; urxc
reti ; udre
reti ; utxc
reti ; adc_int
reti ; eep_int
reti ; aci_int
rjmp i2c_int ; twi_int
reti ; spmc_int
eeprom_defaults_w:
.db low(EEPROM_SIGN), high(EEPROM_SIGN)
.db byte1(FULL_RC_PULS * CPU_MHZ), byte2(FULL_RC_PULS * CPU_MHZ)
.db byte1(STOP_RC_PULS * CPU_MHZ), byte2(STOP_RC_PULS * CPU_MHZ)
.db byte1(MID_RC_PULS * CPU_MHZ), byte2(MID_RC_PULS * CPU_MHZ)
.if USE_I2C
.equ BL_REVISION = 2
.db BL_REVISION, 144 ; Revision, SetMask -- Settings mask should encode MOTOR_REVERSE bit
.db 255, 255 ; PwmScaling, CurrentLimit
.db 127, 0 ; TempLimit, CurrentScaling
.db 0, byte1(0xaa + BL_REVISION + 144 + 255 + 255 + 127 + 0 + 0) ; BitConfig, crc (0xaa + sum of above bytes)
.endif
;-- Instruction extension macros -----------------------------------------
; Add any 16-bit immediate to a register pair (@0:@1 += @2), no Z flag
.macro adi2
.if byte1(-@2)
subi @0, byte1(-@2)
sbci @1, byte1(-byte2(@2 + 0xff))
.else
subi @1, byte1(-byte2(@2 + 0xff))
.endif
.endmacro
; Smaller version for r24 and above, Z flag not reliable
.macro adiwx
.if (@2) & ~0x3f
adi2 @0, @1, @2
.else
adiw @0, @2
.endif
.endmacro
; Compare any 16-bit immediate from a register pair (@0:@1 -= @2, maybe clobbering @3)
.macro cpiz2
cpi @0, byte1(@2)
.if byte2(@2)
ldi @3, byte2(@2)
cpc @1, @3
.else
cpc @1, ZH
.endif
.endmacro
; Compare any 16-bit immediate from a register pair (@0:@1 -= @2, maybe clobbering @3), no Z flag
; Do not follow by Z flag tests like breq, brne, brlt, brge, brlo, brsh!
; The idea here is that the low byte being compared with (subtracted by)
; 0 will never set carry, so skipping it and cpi'ing the high byte is the
; same other than the result of the Z flag.
.macro cpi2
.if byte1(@2)
cpiz2 @0, @1, @2, @3
.else
cpi @1, byte2(@2)
.endif
.endmacro
; Compare any 24-bit immediate from a register triplet (@0:@1:@2 -= @3, maybe clobbering @4)
.macro cpiz3
cpi @0, byte1(@3)
.if byte2(@3)
ldi @4, byte2(@3)
cpc @1, @4
.else
cpc @1, ZH
.endif
.if byte3(@3)
ldi @4, byte3(@3)
cpc @2, @4
.else
cpc @2, ZH
.endif
.endmacro
; Compare any 24-bit immediate from a register triplet (@0:@1:@2 -= @3, maybe clobbering @4)
; May not set Z flag, as above.
.macro cpi3
.if byte1(@3)
cpiz3 @0, @1, @2, @3, @4
.else
cpi2 @1, @2, @3 >> 8, @4
.endif
.endmacro
; Subtract any 16-bit immediate from a register pair (@0:@1 -= @2), no Z flag
.macro sbi2
.if byte1(@2)
subi @0, byte1(@2)
sbci @1, byte2(@2)
.else
subi @1, byte2(@2)
.endif
.endmacro
; Smaller version for r24 and above, Z flag not reliable
.macro sbiwx
.if (@2) & ~0x3f
sbi2 @0, @1, @2
.else
sbiw @0, @2
.endif
.endmacro
; Load 2-byte immediate
.macro ldi2
ldi @0, byte1(@2)
ldi @1, byte2(@2)
.endmacro
; Load 3-byte immediate
.macro ldi3
ldi @0, byte1(@3)
ldi @1, byte2(@3)
ldi @2, byte3(@3)
.endmacro
; Register out to any address (memory-mapped if necessary)
.macro outr
.if @0 < 64
out @0, @1
.else
sts @0, @1
.endif
.endmacro
; Register in from any address (memory-mapped if necessary)
.macro inr
.if @1 < 64
in @0, @1
.else
lds @0, @1
.endif
.endmacro
; Immediate out to any port (possibly via @2 as a temporary)
.macro outi
.if @1
ldi @2, @1
outr @0, @2
.else
outr @0, ZH
.endif
.endmacro
;-- FET driving macros ---------------------------------------------------
; Careful: "if" conditions split over multiple lines (with backslashes)
; work with arva, but avrasm2.exe silently produces wrong results.
.macro FET_on
.if (INIT_PB & ((@0 == PORTB) << @1)) | (INIT_PC & ((@0 == PORTC) << @1)) | (INIT_PD & ((@0 == PORTD) << @1))
cbi @0, @1
.else
sbi @0, @1
.endif
.endmacro
.macro FET_off
.if (INIT_PB & ((@0 == PORTB) << @1)) | (INIT_PC & ((@0 == PORTC) << @1)) | (INIT_PD & ((@0 == PORTD) << @1))
sbi @0, @1
.else
cbi @0, @1
.endif
.endmacro
.macro AnFET_on
FET_on AnFET_port, AnFET
.endmacro
.macro AnFET_off
FET_off AnFET_port, AnFET
.endmacro
.macro ApFET_on
FET_on ApFET_port, ApFET
.endmacro
.macro ApFET_off
FET_off ApFET_port, ApFET
.endmacro
.macro BnFET_on
FET_on BnFET_port, BnFET
.endmacro
.macro BnFET_off
FET_off BnFET_port, BnFET
.endmacro
.macro BpFET_on
FET_on BpFET_port, BpFET
.endmacro
.macro BpFET_off
FET_off BpFET_port, BpFET
.endmacro
.macro CnFET_on
FET_on CnFET_port, CnFET
.endmacro
.macro CnFET_off
FET_off CnFET_port, CnFET
.endmacro
.macro CpFET_on
FET_on CpFET_port, CpFET
.endmacro
.macro CpFET_off
FET_off CpFET_port, CpFET
.endmacro
.macro all_pFETs_off
.if ApFET_port != BpFET_port || ApFET_port != CpFET_port
ApFET_off
BpFET_off
CpFET_off
.else
in @0, ApFET_port
.if (INIT_PB & ((ApFET_port == PORTB) << ApFET)) | (INIT_PC & ((ApFET_port == PORTC) << ApFET)) | (INIT_PD & ((ApFET_port == PORTD) << ApFET))
sbr @0, (1<<ApFET)+(1<<BpFET)+(1<<CpFET)
.else
cbr @0, (1<<ApFET)+(1<<BpFET)+(1<<CpFET)
.endif
out ApFET_port, @0
.endif
.endmacro
.macro all_nFETs_off
.if AnFET_port != BnFET_port || AnFET_port != CnFET_port
AnFET_off
BnFET_off
CnFET_off
.else
in @0, AnFET_port
.if (INIT_PB & ((AnFET_port == PORTB) << AnFET)) | (INIT_PC & ((AnFET_port == PORTC) << AnFET)) | (INIT_PD & ((AnFET_port == PORTD) << AnFET))
sbr @0, (1<<AnFET)+(1<<BnFET)+(1<<CnFET)
.else
cbr @0, (1<<AnFET)+(1<<BnFET)+(1<<CnFET)
.endif
out AnFET_port, @0
.endif
.endmacro
.macro nFET_brake
.if AnFET_port != BnFET_port || AnFET_port != CnFET_port
AnFET_on
BnFET_on
CnFET_on
.else
in @0, AnFET_port
.if (INIT_PB & ((AnFET_port == PORTB) << AnFET)) | (INIT_PC & ((AnFET_port == PORTC) << AnFET)) | (INIT_PD & ((AnFET_port == PORTD) << AnFET))
cbr @0, (1<<AnFET)+(1<<BnFET)+(1<<CnFET)
.else
sbr @0, (1<<AnFET)+(1<<BnFET)+(1<<CnFET)
.endif
out AnFET_port, @0
.endif
.endmacro
;-- RC pulse setup and edge handling macros ------------------------------
.if USE_ICP
.macro rcp_int_enable
in @0, TIMSK
sbr @0, (1<<TICIE1) ; enable icp1_int
out TIMSK, @0
.endmacro
.macro rcp_int_disable
in @0, TIMSK
cbr @0, (1<<TICIE1) ; disable icp1_int
out TIMSK, @0
.endmacro
.macro rcp_int_rising_edge
ldi @0, T1CLK
out TCCR1B, @0
.endmacro
.macro rcp_int_falling_edge
ldi @0, T1CLK & ~(1<<ICES1)
out TCCR1B, @0
.endmacro
.elif USE_INT0
.macro rcp_int_enable
ldi @0, (1<<INT0) ; enable ext_int0
out GICR, @0
.endmacro
.macro rcp_int_disable
out GICR, ZH ; disable ext_int0
.endmacro
.if USE_INT0 == 1
.macro rcp_int_rising_edge
ldi @0, (1<<ISC01)+(1<<ISC00)
out MCUCR, @0 ; set next int0 to rising edge
.endmacro
.macro rcp_int_falling_edge
ldi @0, (1<<ISC01)
out MCUCR, @0 ; set next int0 to falling edge
.endmacro
.elif USE_INT0 == 2
.macro rcp_int_rising_edge
ldi @0, (1<<ISC01)
out MCUCR, @0 ; set next int0 to falling edge
.endmacro
.macro rcp_int_falling_edge
ldi @0, (1<<ISC01)+(1<<ISC00)
out MCUCR, @0 ; set next int0 to rising edge
.endmacro
.endif
.endif
;-- Analog comparator sense macros ---------------------------------------
; We enable and disable the ADC to override ACME when one of the sense
; pins is AIN1 instead of an ADC pin. In the future, this will allow
; reading from the ADC at the same time.
.macro comp_init
in @0, SFIOR
sbr @0, (1<<ACME) ; set Analog Comparator Multiplexer Enable
out SFIOR, @0
.if defined(mux_a) && defined(mux_b) && defined(mux_c)
cbi ADCSRA, ADEN ; Disable ADC to make sure ACME works
.endif
.endmacro
.macro comp_adc_disable
.if !defined(mux_a) || !defined(mux_b) || !defined(mux_c)
cbi ADCSRA, ADEN ; Disable ADC if we enabled it to get AIN1
.endif
.endmacro
.macro comp_adc_enable
sbi ADCSRA, ADEN ; Eisable ADC to effectively disable ACME
.endmacro
.macro set_comp_phase_a
.if defined(mux_a)
ldi @0, mux_a ; set comparator multiplexer to phase A
out ADMUX, @0
comp_adc_disable
.else
comp_adc_enable
.endif
.endmacro
.macro set_comp_phase_b
.if defined(mux_b)
ldi @0, mux_b ; set comparator multiplexer to phase B
out ADMUX, @0
comp_adc_disable
.else
comp_adc_enable
.endif
.endmacro
.macro set_comp_phase_c
.if defined(mux_c)
ldi @0, mux_c ; set comparator multiplexer to phase C
out ADMUX, @0
comp_adc_disable
.else
comp_adc_enable
.endif
.endmacro
;-- Timing and motor debugging macros ------------------------------------
.macro flag_on
.if MOTOR_DEBUG && (DIR_PB & (1<<4)) == 0
sbi PORTB, 4
.endif
.endmacro
.macro flag_off
.if MOTOR_DEBUG && (DIR_PB & (1<<4)) == 0
cbi PORTB, 4
.endif
.endmacro
.macro sync_on
.if MOTOR_DEBUG && (DIR_PB & (1<<3)) == 0
sbi PORTB, 3
.elif MOTOR_DEBUG && (DIR_PB & (1<<5)) == 0
sbi PORTB, 5
.endif
.endmacro
.macro sync_off
.if MOTOR_DEBUG && (DIR_PB & (1<<3)) == 0
cbi PORTB, 3
.elif MOTOR_DEBUG && (DIR_PB & (1<<5)) == 0
cbi PORTB, 5
.endif
.endmacro
; Short cycle delay without clobbering flags
.equ MAX_BUSY_WAIT_CYCLES = 32
.macro cycle_delay
.if @0 >= MAX_BUSY_WAIT_CYCLES
.error "cycle_delay too long"
.endif
.if @0 > 0
.if @0 & 1
nop
.endif
.if @0 & 2
rjmp PC + 1
.endif
.if @0 & 4
rjmp PC + 1
rjmp PC + 1
.endif
.if @0 & 8
nop
rcall wait_ret ; 3 cycles to call + 4 to return
.endif
.if @0 & 16
rjmp PC + 1
rcall wait_ret
rcall wait_ret
.endif
.endif
.endmacro
;-----bko-----------------------------------------------------------------
; Timer2 overflow interrupt (output PWM) -- the interrupt vector actually
; "ijmp"s to Z, which should point to one of these entry points.
;
; We try to avoid clobbering (and thus needing to save/restore) flags;
; in, out, mov, ldi, cpse, etc. do not modify any flags, while dec does.
;
; We used to check the comparator (ACSR) here to help starting, since PWM
; switching is what introduces noise that affects the comparator result.
; However, timing of this is very sensitive to FET characteristics, and
; would work well on some boards but not at all on others without waiting
; another 100-200ns, which was enough to break other boards. So, instead,
; we do all of the ACSR sampling outside of the interrupt and do digital
; filtering. The AVR interrupt overhead also helps to shield the noise.
;
; We reload TCNT2 as the very last step so as to reduce PWM dead areas
; between the reti and the next interrupt vector execution, which still
; takes a good 4 (reti) + 4 (interrupt call) + 2 (ijmp) cycles. We also
; try to keep the switch on close to the start of pwm_on and switch off
; close to the end of pwm_aff to minimize the power bump at full power.
;
; pwm_*_high and pwm_again are called when the particular on/off cycle
; is longer than will fit in 8 bits. This is tracked in tcnt2h.
.if MOTOR_BRAKE || LOW_BRAKE
pwm_brake_on:
cpse tcnt2h, ZH
rjmp pwm_again
in i_sreg, SREG
nFET_brake i_temp1
ldi i_temp1, 0xff
cp off_duty_l, i_temp1 ; Check for 0 off-time
cpc off_duty_h, ZH
breq pwm_brake_on1
ldi ZL, pwm_brake_off ; Not full on, so turn it off next
lds i_temp2, brake_sub
sub sys_control_l, i_temp2
brne pwm_brake_on1
neg duty_l ; Increase duty
sbc duty_h, i_temp1 ; i_temp1 is 0xff aka -1
com duty_l
com off_duty_l ; Decrease off duty
sbc off_duty_l, ZH
sbc off_duty_h, ZH
com off_duty_l
pwm_brake_on1: mov tcnt2h, duty_h
out SREG, i_sreg
out TCNT2, duty_l
reti
pwm_brake_off:
cpse tcnt2h, ZH
rjmp pwm_again
in i_sreg, SREG
ldi ZL, pwm_brake_on
mov tcnt2h, off_duty_h
all_nFETs_off i_temp1
out SREG, i_sreg
out TCNT2, off_duty_l
reti
.endif
.if DEAD_TIME_HIGH > 7
.equ EXTRA_DEAD_TIME_HIGH = DEAD_TIME_HIGH - 7
.else
.equ EXTRA_DEAD_TIME_HIGH = 0
.endif
pwm_on_fast_high:
.if COMP_PWM && EXTRA_DEAD_TIME_HIGH > MAX_BUSY_WAIT_CYCLES
in i_sreg, SREG
dec tcnt2h
brne pwm_on_fast_high_again
ldi ZL, pwm_on_fast
pwm_on_fast_high_again:
out SREG, i_sreg
reti
.endif
pwm_on_high:
in i_sreg, SREG
dec tcnt2h
brne pwm_on_again
ldi ZL, pwm_on
pwm_on_again: out SREG, i_sreg
reti
pwm_again:
in i_sreg, SREG
dec tcnt2h
out SREG, i_sreg
reti
pwm_on:
.if COMP_PWM
sbrc flags2, A_FET
ApFET_off
sbrc flags2, B_FET
BpFET_off
sbrc flags2, C_FET
CpFET_off
.if EXTRA_DEAD_TIME_HIGH > MAX_BUSY_WAIT_CYCLES
; Reschedule to interrupt once the dead time has passed
.if high(EXTRA_DEAD_TIME_HIGH)
ldi i_temp1, high(EXTRA_DEAD_TIME_HIGH)
mov tcnt2h, i_temp1
ldi ZL, pwm_on_fast_high
.else
ldi ZL, pwm_on_fast
.endif
ldi i_temp1, 0xff - low(EXTRA_DEAD_TIME_HIGH)
out TCNT2, i_temp1
reti ; Do something else while we wait
.equ CPWM_OVERHEAD_HIGH = 7 + 8 + EXTRA_DEAD_TIME_HIGH
.else
; Waste cycles to wait for the dead time
cycle_delay EXTRA_DEAD_TIME_HIGH
.equ CPWM_OVERHEAD_HIGH = 7 + EXTRA_DEAD_TIME_HIGH
; Fall through
.endif
.endif
pwm_on_fast:
sbrc flags2, A_FET
AnFET_on
sbrc flags2, B_FET
BnFET_on
sbrc flags2, C_FET
CnFET_on
ldi ZL, pwm_off
mov tcnt2h, duty_h
out TCNT2, duty_l
reti
pwm_wdr: ; Just reset watchdog
wdr
reti
pwm_off:
cpse tcnt2h, ZH ; 2 cycles to skip when tcnt2h is 0
rjmp pwm_again
wdr ; 1 cycle: watchdog reset
sbrc flags1, FULL_POWER ; 2 cycles to skip if not full power
rjmp pwm_on ; None of this off stuff if full power
lds ZL, pwm_on_ptr ; 2 cycles
mov tcnt2h, off_duty_h ; 1 cycle
sbrc flags2, A_FET ; 2 cycles if skip, 1 cycle otherwise
AnFET_off ; 2 cycles (off at 12 cycles from entry)
sbrc flags2, B_FET ; Offset by 2 cycles here,