-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtgy.asm
2099 lines (1975 loc) · 63.5 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: [email protected]
;--
; Forked from "TGYP2010_416HzRCIntervalRate_PPM-Mod-only_NoCal" which removed
; OSCCAL setting from EEPROM and tweaked some startup parameters, all which
; have since been replaced.
;
; 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 the full CPU clock rate, using tcnt2h
; to simulate the high byte. An input FULL to STOP range of 800 plus a
; MIN_DUTY of 63 (a POWER_RANGE of 863) 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 before
; reloading TCNT2.
;
; Simon Kirby <[email protected]>
;
;-- Device ----------------------------------------------------------------
;
;.include "m8def.inc"
; we don't use the macros to access registers
#define __SFR_OFFSET 0
#include <avr/io.h>
;
; 8K Bytes of In-System Self-Programmable Flash
; 512 Bytes EEPROM
; 1K Byte Internal SRAM
;
;-- Fuses -----------------------------------------------------------------
;
; Old fuses for internal RC oscillator at 8 MHz were lfuse=0xa4 hfuse=0xdf,
; but since we now set OSCCAL to actually run at about 16 MHz, we'd better
; set brown-out detection to 4.0V. This code should work without changes on
; boards with external 16MHz crystals / resonators; just set lfuse=0x3f.
;
; Suggested fuses with 4.0V brown-out voltage:
; Without external crystal: avrdude -U lfuse:w:0x24:m -U hfuse:w:0xd7:m
; With external crystal: avrdude -U lfuse:w:0x3f:m -U hfuse:w:0xc7:m
;
; Testing fuses with 2.7V brown-out voltage (unsafe at 16MHz):
; Without external crystal: avrdude -U lfuse:w:0xa4:m -U hfuse:w:0xd7:m
; With external crystal: avrdude -U lfuse:w:0xbf:m -U hfuse:w:0xc7:m
;
;-- 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 PPM)
#elif defined(ian_esc)
#include "ian.inc" //; Ian
#elif defined(afro2_esc)
#include "afro2.inc" //; AfroESC 2 (ICP PPM)
#elif defined(bs_esc)
#include "bs.inc" //; HobbyKing BlueSeries / Mystery (INT0 PPM)
#elif defined(bs_nfet_esc)
#include "bs_nfet.inc" //; HobbyKing BlueSeries / Mystery with all nFETs (INT0 PPM)
#elif defined(bs40a_esc)
#include "bs40a.inc" //; HobbyKing BlueSeries / Mystery 40A (INT0 PPM)
#elif defined(rct50a_esc)
#include "rct50a.inc" //; RCTimer 50A with all nFETs (INT0 PPM)
#elif defined(tp_esc)
#include "tp.inc" //; TowerPro 25A/HobbyKing 18A "type 1" (INT0 PPM)
#elif defined(tp_nfet_esc)
#include "tp_nfet.inc" //; TowerPro 25A with all nFETs "type 3" (INT0 PPM)
#elif defined(tgy6a_esc)
#include "tgy6a.inc" //; Turnigy Plush 6A (INT0 PPM)
#elif defined(i2c_tgy_esc)
#include "i2c_tgy.inc" //; TowerPro/Turnigy Basic/Plush "type 2" (I2C)
#else
#include "tgy.inc" //; TowerPro/Turnigy Basic/Plush "type 2" (INT0 PPM)
#endif
; .macro doesn't work in .byte expressions
#define _lo8(v) ((v) & 0xFF)
#define _hi8(v) (((v) >> 8) & 0xFF)
#define _hlo8(v) (((v) >> 16) & 0xFF)
.equ I2C_ADDR , 0x50 ; MK-style I2C address
.equ MOTOR_ID , 0 ; MK-style I2C motor ID, or UART motor number
; const uint8_t addr = (0x50 >> 1) + 1;
.equ TIME_ACCUMULATE , 0 ; Accumulate 4 commutations timing method
.equ TIME_HALFADD , 0 ; Update half timing method
.equ TIME_QUARTERADD , 1 ; Update quarter timing method (original)
.equ MOTOR_BRAKE , 0 ; Enable brake
.equ MOTOR_REVERSE , 0 ; Reverse normal commutation direction
.equ RC_PULS_REVERSE , 0 ; Enable RC-car style forward/reverse throttle
.equ SLOW_THROTTLE , 0 ; Limit maximum throttle jump to try to prevent overcurrent
.equ RCP_TOT , 16 ; Number of 65536us periods before considering rc pulse lost
.equ CPU_MHZ , F_CPU / 1000000
; These are now defaults which can be adjusted via throttle calibration
; (stick high, stick low, (stick neutral) at start).
.ifdef ultrapwm
.equ STOP_RC_PULS , 200 ; Support for http://www.xaircraft.com/wiki/UltraPWM/en
.equ FULL_RC_PULS , 1200 ; which says motors should start at 200us,
.equ MAX_RC_PULS , 1400 ; but does not define min/max pulse width.
.else
; 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
.endif
.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
.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)
.equ MIN_DUTY , 63 * CPU_MHZ / 16
; Number of PWM steps (too high and PWM frequency drops into audible range)
.equ POWER_RANGE , 800 * CPU_MHZ / 16 + MIN_DUTY
.equ MAX_POWER , (POWER_RANGE-1)
.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 TIMING_MIN , 0x8000 ; 8192us per commutation
.equ TIMING_RUN , 0x1000 ; 1024us per commutation
.equ TIMING_RANGE1 , 0x4000 ; 4096us per commutation
.equ TIMING_RANGE2 , 0x2000 ; 2048us per commutation
.equ TIMING_MAX , 0x0050 ; 20us per commutation
.equ timeoutSTART , 48000 ; 48ms per commutation
.equ timeoutMIN , 36000 ; 36ms per commutation
.equ ENOUGH_GOODIES , 12 ; This many start cycles without timeout will transition to running mode
.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)
;**** **** **** **** ****
; Register Definitions
.set temp5 , 0 ; aux temporary (L) (limited operations)
.set temp6 , 1 ; aux temporary (H) (limited operations)
.set duty_l , 2 ; on duty cycle low, one's complement
.set duty_h , 3 ; on duty cycle high
.set off_duty_l , 4 ; off duty cycle low, one's complement
.set off_duty_h , 5 ; off duty cycle high
.set _rx_l , 6 ; received throttle low
.set _rx_h , 7 ; received throttle high
.set tcnt2h , 8 ; time2 high byte
.set i_sreg , 9 ; status register save in interrupts
.set i2c_count , 10 ; which byte to send over i2c
.set _rc_timeout , 11 ;
.set sys_control_l , 12 ; duty limit low (word register aligned)
.set sys_control_h , 13 ; duty limit high
.set temp7 , 14 ; really aux temporary (limited operations)
;.set , 15
.set nfet_on , 18
.set nfet_off , 19
.set i_temp1 , 20 ; interrupt temporary
.set i_temp2 , 21 ; interrupt temporary
.set temp3 , 22 ; main temporary (L)
.set temp4 , 23 ; main temporary (H)
.set temp1 , 24 ; main temporary (L), adiw-capable
.set temp2 , 25 ; main temporary (H), adiw-capable
.set flags0 , 16 ; state flags
.equ OCT1_PENDING , 0 ; if set, output compare interrupt is pending
; .equ OCT1B_PENDING , 1 ; if set, output compare interrupt B is pending
; .equ I_pFET_HIGH , 2 ; set if over-current detect
; .equ GET_STATE , 3 ; set if state is to be send
.equ I2C_FIRST , 4 ; if set, i2c will receive first byte next
.equ I2C_SPACE_LEFT , 5 ; if set, i2c buffer has room
.equ UART_SYNC , 6 ; if set, we are waiting for our serial throttle byte
; .equ I_ON_CYCLE , 7 ; if set, current on cycle is active (optimized as MSB)
.set flags1 , 17 ; state flags
.equ POWER_OFF , 0 ; switch fets on disabled
.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 - conviently located at the same bit position as ACO
.equ STARTUP , 6 ; if set, startup-phase is active
.equ REVERSE , 7 ; if set, do reverse commutation
;.set flags2 , 25
; .equ RPM_RANGE1 , 0 ; if set RPM is lower than 1831 RPM
; .equ RPM_RANGE2 , 1 ; if set RPM is lower than 3662 RPM
; .equ RC_INTERVAL_OK , 2
; .equ POFF_CYCLE , 3 ; if set one commutation cycle is performed without power
; .equ COMP_SAVE , 4 ; if set ACO was high
; .equ COMP_SAVE_READY , 5 ; if acsr_save was set by PWM interrupt
; .equ STARTUP , 6 ; if set startup-phase is active
; .equ SCAN_TIMEOUT , 7 ; if set a startup timeout occurred
.equ T5 , PB5 ; (sck stk200 interface)
.equ T4 , PB4 ; (miso stk200 interface)
.equ T3 , PB3 ; (mosi stk200 interface)
.equ SPI_PORT , PORTB
.equ SPI_PIN , PINB
.equ SPI_DIR , DIR_PB
; disable debugging via SPI pin if the pin is set to input
.macro T_on t:req
.if SPI_DIR & (1 << \t)
sbi SPI_PORT, \t
.endif
.endm
.macro T_off t:req
.if SPI_DIR & (1 << \t)
cbi SPI_PORT, \t
.endif
.endm
.macro T_pulse t:req
.if SPI_DIR & (1 << \t)
T_on \t
nop
T_off \t
.endif
.endm
; here the XYZ registers are placed ( r26-r31)
; XL: I/O address of PWM nFET port
; XH: I/O address of PWM nFET port
; 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
.data ; DATA segment
.org 0
orig_osccal: .byte 1 ; original OSCCAL value
goodies: .byte 1
ocr1ax: .byte 1 ; 3rd byte of OCR1A
tcnt1x: .byte 1 ; 3rd byte of TCNT1
last_tcnt1_l: .byte 1 ; last timer1 value
last_tcnt1_h: .byte 1
last_tcnt1_x: .byte 1
timing_l: .byte 1 ; holds time of 4 commutations
timing_h: .byte 1
timing_x: .byte 1
timing_count: .byte 1
wt_comp_scan_l: .byte 1 ; time from switch to comparator scan (blanking time)
wt_comp_scan_h: .byte 1
wt_comp_scan_x: .byte 1
com_timing_l: .byte 1 ; time from zero-crossing to switch of the appropriate FET
com_timing_h: .byte 1
com_timing_x: .byte 1
wt_OCT1_tot_l: .byte 1 ; time for each startup commutation
wt_OCT1_tot_h: .byte 1
wt_OCT1_tot_x: .byte 1
zero_wt_l: .byte 1 ; time to wait for zero-crossing while running
zero_wt_h: .byte 1
zero_wt_x: .byte 1
zc_filter_time: .byte 1 ; number of times to check zero-crossing
start_rcpuls_l: .byte 1
start_rcpuls_h: .byte 1
start_rcpuls_x: .byte 1
rc_duty_l: .byte 1 ; desired duty cycle
rc_duty_h: .byte 1
timing_duty_l: .byte 1 ; duty cycle limit based on timing
timing_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
max_pwm: .byte 1 ; MaxPWM for MK (NOTE: 250 while stopped is magic and enables v2)
motor_count: .byte 1 ; Motor number for serial control
current_sum: .byte 4
.global got_started
got_started: .byte 1
beep_on_nonzeros: .byte 1
;**** **** **** **** ****
; 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 ; -'
.global twi_address
twi_address: .byte 1
.global reverse
reverse: .byte 1
eeprom_end: .byte 1
;-----bko-----------------------------------------------------------------
;**** **** **** **** ****
.section .vectors,"ax",@progbits
;**** **** **** **** ****
; 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 TWI_vect ; twi_int
reti ; spmc_int
.text
;.org 0
TWI_shim: T_pulse T5
rjmp TWI_vect
eeprom_defaults_w:
.byte _lo8(EEPROM_SIGN), _hi8(EEPROM_SIGN)
.byte _lo8(FULL_RC_PULS * CPU_MHZ), _hi8(FULL_RC_PULS * CPU_MHZ)
.byte _lo8(STOP_RC_PULS * CPU_MHZ), _hi8(STOP_RC_PULS * CPU_MHZ)
.byte _lo8((FULL_RC_PULS + STOP_RC_PULS) * CPU_MHZ / 2), _hi8((FULL_RC_PULS + STOP_RC_PULS) * CPU_MHZ / 2)
.byte I2C_ADDR + MOTOR_ID, 0 ; an even number of bytes is critical, or we get
; "warning: internal error: out of range error"
;-----bko-----------------------------------------------------------------
; .global required due to linker args: --gc-sections,--entry=reset
.global reset
reset: clr r0
out SREG, r0 ; Clear interrupts and flags
; Set up stack
ldi ZH, hi8(RAMEND+1)
ldi ZL, lo8(RAMEND+1)
out SPH, ZH
out SPL, ZL
; Clear RAM and all registers
clear_loop: st -Z, r0
cpi ZL, lo8(RAMSTART)
cpc ZH, r0
brne clear_loop1
ldi ZL, 30 ; Start clearing registers
clear_loop1: cp ZL, r0
cpc ZH, r0
brne clear_loop ; Leaves with all registers (0 through ZH) at 0
; Save original OSCCAL and reset cause
in i_sreg, OSCCAL
sts orig_osccal, i_sreg
in i_sreg, MCUCSR
out MCUCSR, r0
; portB - all FETs off
ldi temp1, INIT_PB
out PORTB, temp1
ldi temp1, DIR_PB
out DDRB, temp1
; portC reads comparator inputs
ldi temp1, INIT_PC
out PORTC, temp1
ldi temp1, DIR_PC
out DDRC, temp1
; portD reads rc-puls + AIN0 ( + RxD, TxD for debug )
ldi temp1, INIT_PD
out PORTD, temp1
ldi temp1, DIR_PD
out DDRD, temp1
; Start timers except output PWM
ldi temp1, T0CLK ; time0: beep control, delays
out TCCR0, temp1
ldi temp1, T1CLK ; time1: commutation timing,
out TCCR1B, temp1 ; RC pulse measurement
;ldi temp1, (1<<COM1A1)|(1<<COM1A0)
;out TCCR1A, temp1
out TCCR2, ZH ; time2: PWM, stopped
; Enable watchdog (WDTON may be set or unset)
ldi temp1, (1<<WDCE)+(1<<WDE)
out WDTCR, temp1
ldi temp1, (1<<WDE) ; Fastest option: ~16.3ms timeout
out WDTCR, temp1
; Read EEPROM block to RAM
rcall wait120ms
clt
rcall eeprom_read_block ; Also calls osccal_set
; Check EEPROM signature
ldi XL, lo8(eeprom_sig_l + 2)
ld temp2, -X
ld temp1, -X ; Leave X at eeprom_sig_l
subi temp1, lo8(EEPROM_SIGN)
sbci temp2, hi8(EEPROM_SIGN)
breq eeprom_good
; CONVERT: the following was:
; ldi ZL, lo8(eeprom_defaults_w << 1)
; But that is avrasm2 syntax; per the link below, avr-as is more intelligent when
; it comes to using the lo8 operators. avr-as also throws an error when << 1 is found.
; http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=54574&start=0
; Signature not good: set defaults in RAM, but do not write
; to the EEPROM until we actually set something non-default
ldi ZL, lo8(eeprom_defaults_w)
eeprom_default: lpm temp1, Z+
st X+, temp1
cpi XL, lo8(eeprom_end)
brne eeprom_default
eeprom_good:
; Check reset cause
sbrs i_sreg, PORF ; Power-on reset
rjmp init_no_porf
rcall beep_f1 ; Usual startup beeps
rcall beep_f2
rcall beep_f3
rjmp control_start
init_no_porf:
sbrs i_sreg, BORF ; Brown-out reset
rjmp init_no_borf
rcall beep_f3 ; "dead cellphone"
rcall beep_f1
rjmp control_start
init_no_borf:
sbrs i_sreg, EXTRF ; External reset
rjmp init_no_extrf
rcall beep_f4 ; Single beep
rjmp control_start
init_no_extrf:
sbrs i_sreg, WDRF ; Watchdog reset
rjmp init_no_wdrf
init_wdrf1: rcall beep_f1 ; "siren"
rcall beep_f1
rcall beep_f3
rcall beep_f3
rjmp init_wdrf1 ; Loop forever
init_no_wdrf:
; Unknown reset cause: Beep out all 8 bits
; Sometimes I can cause this by touching the oscillator.
init_bitbeep1: rcall wait240ms
mov nfet_on, i_sreg
ldi nfet_off, 8
init_bitbeep2: sbrs nfet_on, 0
rcall beep_f2
sbrc nfet_on, 0
rcall beep_f4
rcall wait120ms
lsr nfet_on
dec nfet_off
brne init_bitbeep2
rjmp init_bitbeep1 ; Loop forever
;-----bko-----------------------------------------------------------------
; time2 overflow compare 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, etc. do not modify any flags, while dec does.
;
; The comparator (ACSR) is saved at the very end of the ON cycle, but
; since the nFET takes at least half a microsecond to turn off and the
; AVR buffers ACO for a few cycles, we do it after turning off the drive
; pin. For low duty cycles (with a longer off period), testing shows that
; waiting an extra 0.5us - 0.75us (8-12 cycles at 16MHz) actually helps
; to improve zero-crossing detection accuracy significantly, perhaps
; because the driven-low phase has had a chance to finish swinging down.
; However, some tiny boards such as 10A or less may have very low gate
; charge/capacitance, and so can turn off faster. We used to wait 8/9
; cycles, but now we wait 5 cycles (5/16ths of a microsecond), which
; still helps on ~30A boards without breaking 10A boards.
;
; 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 fet switch off as close to this as possible to avoid a
; significant bump at FULL_POWER.
;
; The pwm_*_high entry points are only called when the particular on/off
; cycle is longer than 8 bits. This is tracked in tcnt2h.
pwm_on_high:
in i_sreg, SREG
dec tcnt2h
brne pwm_on_again
ldi ZL, pm_lo8(pwm_on)
pwm_on_again: out SREG, i_sreg
reti
pwm_off_high:
in i_sreg, SREG
dec tcnt2h
brne pwm_off_again
ldi ZL, pm_lo8(pwm_off)
pwm_off_again: out SREG, i_sreg
reti
pwm_on:
st X, nfet_on
ldi ZL, pm_lo8(pwm_off)
cpse duty_h, ZH
ldi ZL, pm_lo8(pwm_off_high)
mov tcnt2h, duty_h
out TCNT2, duty_l
reti
.global pwm_off
pwm_off:
ldi ZL, pm_lo8(pwm_on) ; 1 cycle
cpse off_duty_h, ZH ; 1 cycle if not zero, 2 if zero
ldi ZL, pm_lo8(pwm_on_high) ; 1 cycle
mov tcnt2h, off_duty_h ; 1 cycle
wdr ; 1 cycle
st X, nfet_off ; 2 cycles (off at 7 cycles from entry)
out TCNT2, off_duty_l ; 1 cycle
reti ; 4 cycles
; this is now done in the Makefile; search for "pwm_off"
;.if pm_hi8(pwm_off)
;.error "pm_hi8(pwm_off) is non-zero; please move code closer to start or use 16-bit (ZH) jump registers"
;.endif
;-----bko-----------------------------------------------------------------
; timer output compare interrupt
t1oca_int: in i_sreg, SREG
lds i_temp1, ocr1ax
subi i_temp1, 1
brcc t1oca_int1
cbr flags0, (1<<OCT1_PENDING) ; signal OCT1A passed
t1oca_int1: sts ocr1ax, i_temp1
out SREG, i_sreg
reti
;-----bko-----------------------------------------------------------------
; time1 overflow interrupt (happens every 4096µs)
t1ovfl_int: in i_sreg, SREG
lds i_temp1, tcnt1x
inc i_temp1
sts tcnt1x, i_temp1
andi i_temp1, 15 ; Every 16 overflows
brne t1ovfl_int1
cpse _rc_timeout, ZH
dec _rc_timeout
tst _rc_timeout
brne t1ovfl_int1
ldi i_temp1, (1<<SPE)|(1<<MSTR)
out SPCR, i_temp1
in i_temp1, TWCR
out SPDR, i_temp1
wait_twcr: sbis SPSR, SPIF
rjmp wait_twcr
in i_temp1, TWAR
out SPDR, i_temp1
wait_twar: sbis SPSR, SPIF
rjmp wait_twar
in i_temp1, TWSR
out SPDR, i_temp1
wait_twsr: sbis SPSR, SPIF
rjmp wait_twsr
;in i_temp1, TWCR ; reset TWI bus
out TWCR, ZH
ldi i_temp1, (1<<TWEN) | (1<<TWIE) | (1<<TWEA)
out TWCR, i_temp1
;T_pulse T4
t1ovfl_int1: out SREG, i_sreg
reti
;-----bko-----------------------------------------------------------------
; NOTE: This interrupt uses the 16-bit atomic timer read/write register
; by reading TCNT1L and TCNT1H, so this interrupt must be disabled before
; any other 16-bit timer options happen that might use the same register
; (see "Accessing 16-bit registers" in the Atmel documentation)
; icp1 = rc pulse input, if enabled
rcp_int:
.if USE_ICP || USE_INT0
.if USE_ICP
in i_temp1, ICR1L ; get captured timer values
in i_temp2, ICR1H
in i_sreg, TCCR1B ; abuse i_sreg to hold value
sbrs i_sreg, ICES1 ; evaluate edge of this interrupt
.else
;-----bko-----------------------------------------------------------------
in i_temp1, TCNT1L ; get timer1 values
in i_temp2, TCNT1H
sbis PIND, rcp_in ; evaluate edge of this interrupt
.endif
rjmp falling_edge ; bit is clear = falling edge
rising_edge: ; Flags not saved here!
sts start_rcpuls_l, i_temp1 ; Save pulse start time
rcp_int_falling_edge i_temp1 ; Set next int to falling edge
sts start_rcpuls_h, i_temp2
lds i_temp1, tcnt1x
sts start_rcpuls_x, i_temp1
sbrc i_temp2, 7
reti
in i_temp2, TIFR
sbrs i_temp2, TOV1
reti
in i_sreg, SREG
inc i_temp1 ; Compensate for postponed tcnt1x update
sts start_rcpuls_x, i_temp1
out SREG, i_sreg
reti
rcpint_fail: cpse _rc_timeout, ZH
dec _rc_timeout
rjmp rcpint_exit
falling_edge:
in i_sreg, SREG
rcp_int_rising_edge XH ; Set next int to rising edge
lds XH, tcnt1x ; We borrow XH and ZH (normally 0)
sbrc i_temp2, 7 ; as additional registers and
rjmp falling_edge1 ; clear them before returning.
in ZH, TIFR
sbrc ZH, TOV1
inc XH ; Compensate for postponed tcnt1x update
falling_edge1: lds ZH, start_rcpuls_l
sub i_temp1, ZH
lds ZH, start_rcpuls_h
sbc i_temp2, ZH
lds ZH, start_rcpuls_x
sbc XH, ZH
.if _hlo8(MAX_RC_PULS*CPU_MHZ)
.error "MAX_RC_PULS too high: adjust it or the pulse length checking code"
.endif
cpi i_temp1, lo8(MAX_RC_PULS*CPU_MHZ)
ldi ZH, hi8(MAX_RC_PULS*CPU_MHZ)
cpc i_temp2, ZH
ldi ZH, 0 ; Return ZH to 0; clr clobbers flags
cpc XH, ZH
ldi XH, 0 ; Return XH to 0
brsh rcpint_fail ; throw away (too long pulse)
.if 0
cpi i_temp1, lo8(180*CPU_MHZ)
ldi XH, hi8(180*CPU_MHZ)
cpc i_temp2, XH
ldi XH, 0 ; Return XH to 0
brlo rcpint_fail ; throw away (too short pulse)
.endif
movw _rx_l, i_temp1
sbr flags1, (1<<EVAL_RC)
rcpint_exit: out SREG, i_sreg
reti
.endif
;-----bko-----------------------------------------------------------------
urxc_int:
; This is Bernhard's serial protocol implementation in the UART
; version here: http://home.versanet.de/~b-konze/blc_6a/blc_6a.htm
; This seems to be implemented for a project described here:
; http://www.control.aau.dk/uav/reports/10gr833/10gr833_student_report.pdf
; The UART runs at 38400 baud, N81. Input is ignored until >= 0xf5
; is received, where we start counting to MOTOR_ID, at which
; the received byte is used as throttle input. 0 is POWER_OFF,
; >= 200 is FULL_POWER.
.if USE_UART
in i_sreg, SREG
in i_temp1, UDR
cpi i_temp1, 0xf5 ; Start throttle byte sequence
breq urxc_x3d_sync
sbrs flags0, UART_SYNC
rjmp urxc_exit ; Throw away if not UART_SYNC
brcc urxc_unknown
lds i_temp2, motor_count
dec i_temp2
brne urxc_set_exit ; Skip when motor_count != 0
mov _rx_h, i_temp1 ; Save 8-bit input
sbr flags1, (1<<EVAL_RC)+(1<<UART_MODE)
urxc_unknown: cbr flags0, (1<<UART_SYNC)
rjmp urxc_exit
urxc_x3d_sync: sbr flags0, (1<<UART_SYNC)
ldi i_temp2, MOTOR_ID ; Start counting down from MOTOR_ID
urxc_set_exit: sts motor_count, i_temp2
urxc_exit: out SREG, i_sreg
reti
.endif
;-----bko-----------------------------------------------------------------
; beeper: timer0 is set to 1µs/count
beep_f1: ldi temp4, 200
ldi temp2, 80
BpFET_on
AnFET_on
rjmp beep
beep_f2: ldi temp4, 180
ldi temp2, 100
CpFET_on
BnFET_on
rjmp beep
beep_f3: ldi temp4, 160
ldi temp2, 120
ApFET_on
CnFET_on
rjmp beep
beep_f4: ldi temp4, 140
ldi temp2, 140
CpFET_on
AnFET_on
; Fall through
;-----bko-----------------------------------------------------------------
; Interrupts no longer need to be disabled to beep, but the PWM interrupt
; must be muted first
beep: in temp5, PORTB ; Save ON state
in temp6, PORTC
in temp7, PORTD
beep_on: out PORTB, temp5 ; Restore ON state
out PORTC, temp6
out PORTD, temp7
out TCNT0, ZH
beep_BpCn10: in temp1, TCNT0
cpi temp1, 2*CPU_MHZ ; 32µs on
brlo beep_BpCn10
all_nFETs_off temp3
all_pFETs_off temp3
ldi temp3, CPU_MHZ ; 2040µs off
beep_BpCn12: out TCNT0, ZH
wdr
beep_BpCn13: in temp1, TCNT0
cp temp1, temp4
brlo beep_BpCn13
dec temp3
brne beep_BpCn12
dec temp2
brne beep_on
ret
wait240ms: rcall wait120ms
wait120ms: rcall wait60ms
wait60ms: rcall wait30ms
wait30ms: ldi temp2, 15
beep_BpCn20: ldi temp3, CPU_MHZ
beep_BpCn21: out TCNT0, ZH
ldi temp1, (1<<TOV0) ; Clear TOV0 by setting it
out TIFR, temp1
wdr
beep_BpCn22: in temp1, TIFR
sbrs temp1, TOV0
rjmp beep_BpCn22
dec temp3
brne beep_BpCn21
dec temp2
brne beep_BpCn20
ret
;-----bko-----------------------------------------------------------------
; Read from or write to the EEPROM block. To avoid duplication, we use the
; global interrupts flag (I) to enable writing versus reading mde. Only
; changed bytes are written. We restore OSCCAL to the boot-time value as
; the EEPROM timing is affected by it. We always return by falling through
; to osccal_set.
eeprom_read_block: ; When interrupts disabled
eeprom_write_block: ; When interrupts enabled
lds temp1, orig_osccal
out OSCCAL, temp1
ldi YL, lo8(eeprom_sig_l)
ldi YH, hi8(eeprom_sig_l)
ldi temp1, lo8(EEPROM_OFFSET)
ldi temp2, hi8(EEPROM_OFFSET)
eeprom_rw1: wdr
sbic EECR, EEWE
rjmp eeprom_rw1 ; Loop while writing EEPROM
in temp3, SPMCR
sbrc temp3, SPMEN
rjmp eeprom_rw1 ; Loop while flashing
cpi YL, lo8(eeprom_end)
breq eeprom_rw4
out EEARH, temp2
out EEARL, temp1
adiw temp1, 1
sbi EECR, EERE ; Read existing EEPROM byte
in temp3, EEDR
brts eeprom_rw2
st Y+, temp3 ; Store the byte to RAM
rjmp eeprom_rw1
eeprom_rw2: ld temp4, Y+ ; Compare with the byte in RAM
out EEDR, temp4
cli
sbi EECR, EEMWE
cpse temp3, temp4
sbi EECR, EEWE
sei
rjmp eeprom_rw1
eeprom_rw4: rcall wait30ms
; Fall through to set the oscillator calibration
;-----bko-----------------------------------------------------------------
; Set the oscillator calibration for 8MHz operation, or set it to 0xff for
; approximately 16MHz operation even without an external oscillator. This
; should be safe as long as we restore it during EEPROM accesses. This
; will have no effect on boards with external oscillators, except that
; the EEPROM still uses the internal oscillator (at 1MHz).
osccal_set:
.if CPU_MHZ == 16
ldi temp1, 0xff ; Almost 16MHz
.else
ldi temp1, 0x9f ; Almost 8MHz
.endif
out OSCCAL, temp1
ret
;-----bko-----------------------------------------------------------------
; Shift left temp7:temp6:temp5 temp1 times.
lsl_temp567:
lsl temp5
rol temp6
rol temp7
dec temp1
brne lsl_temp567
ret
;-----bko-----------------------------------------------------------------
; Multiply temp1:temp2 by temp3:temp4 and adds high 16 bits of result to Y.
; Clobbers temp5, temp6, temp7.
mul_y_12x34:
mul temp1, temp3 ; Scale raw pulse length to POWER_RANGE: 16x16->32 (bottom 16 discarded)
mov temp7, temp6 ; Save byte 2 of result, discard byte 1 already
mul temp2, temp3
add temp7, temp5
adc YL, temp6
adc YH, ZH
mul temp1, temp4
add temp7, temp5
adc YL, temp6
adc YH, ZH
mul temp2, temp4
add YL, temp5
adc YH, temp6 ; Product is now in Y, flags set
ret
;-----bko-----------------------------------------------------------------
; Unlike the normal evaluate_rc, we look here for programming mode (pulses
; above PROGRAM_RC_PULS), unless we have received I2C or UART input.
;
; With pulse position modulation (PPM) input, we have to be careful about
; oscillator drift. If we are running on a board without an external
; crystal/resonator/oscillator, the internal RC oscillator must be used,
; which can drift significantly with temperature and voltage. So, we must
; use some margins while calibrating. The internal RC speeds up when cold,
; causing arming problems if the learned pulse is too low. Likewise, the
; internal RC slows down when hot, making it impossible to reach full
; throttle.
evaluate_rc_init:
.if USE_UART
sbrc flags1, UART_MODE
rjmp evaluate_rc_uart
.endif
.if USE_I2C
sbrc flags1, I2C_MODE
rjmp evaluate_rc_i2c
.endif
.if USE_ICP || USE_INT0
cbr flags1, (1<<EVAL_RC)
; If input is above PROGRAM_RC_PULS, we try calibrating throttle
ldi YL, lo8(puls_high_l) ; Start with high pulse calibration
ldi YH, hi8(puls_high_l)
rjmp rc_prog1
rc_prog0: rcall wait240ms ; Wait for stick movement to settle
; Collect average of throttle input pulse length
rc_prog1: movw temp3, _rx_l ; Save the starting pulse length
wdr
rc_prog2: mul ZH, ZH ; Clear 24-bit result registers (0 * 0 -> temp5:temp6)
clr temp7
cpi YL, lo8(puls_high_l) ; Are we learning the high pulse?
brne rc_prog3 ; No, maybe the low pulse
cpi temp3, lo8(PROGRAM_RC_PULS * CPU_MHZ)
ldi temp1, hi8(PROGRAM_RC_PULS * CPU_MHZ)
cpc temp4, temp1
brcs evaluate_rc_puls ; Lower than PROGRAM_RC_PULS - exit programming
ldi temp1, 32 * 31/32 ; Full speed pulse averaging count (slightly below exact)
rjmp rc_prog5
rc_prog3: lds temp1, puls_high_l ; If not learning the high pulse, we should stay below it
cp temp3, temp1
lds temp1, puls_high_h
cpc temp4, temp1
brcc rc_prog1 ; Restart while pulse not lower than learned high pulse
cpi YL, lo8(puls_low_l) ; Are we learning the low pulse?
brne rc_prog4 ; No, must be the neutral pulse
ldi temp1, 32 * 17/16 ; Stop/reverse pulse (slightly above exact)
rjmp rc_prog5
rc_prog4: lds temp1, puls_low_l
cp temp3, temp1
lds temp1, puls_low_h
cpc temp4, temp1
brcs rc_prog1 ; Restart while pulse lower than learned low pulse
ldi temp1, 32 ; Neutral pulse measurement (exact)
rc_prog5: mov tcnt2h, temp1 ; Abuse tcnt2h as pulse counter
rc_prog6: wdr
sbrs flags1, EVAL_RC ; Wait for next pulse
rjmp rc_prog6
cbr flags1, (1<<EVAL_RC)
movw temp1, _rx_l ; Atomic copy of new rc pulse length
add temp5, temp1 ; Accumulate 24-bit average
adc temp6, temp2
adc temp7, ZH
sub temp1, temp3 ; Subtract the starting pulse from this one
sbc temp2, temp4 ; to find the drift since the starting pulse
; Check for excessive drift with an emulated signed comparison -
; add the drift amount to offset the negative side to 0
subi temp1, lo8(-MAX_DRIFT_PULS * CPU_MHZ)
sbci temp2, -1 - _hi8(MAX_DRIFT_PULS * CPU_MHZ)
; ..then subtract the 2*drift + 1 -- carry will be clear if
; we drifted outside of the range
subi temp1, lo8(2 * MAX_DRIFT_PULS * CPU_MHZ + 1)
sbci temp2, hi8(2 * MAX_DRIFT_PULS * CPU_MHZ + 1)
brcc rc_prog0 ; Wait and start over if input moved
dec tcnt2h
brne rc_prog6 ; Loop until average accumulated
ldi temp1, 3
rcall lsl_temp567 ; Multiply by 8 (so that 32 loops makes average*256)
st Y+, temp6 ; Save the top 16 bits as the result
st Y+, temp7
; One beep: high (full speed) pulse received
rcall beep_f3
cpi YL, lo8(puls_high_l+2)
breq rc_prog1 ; Go back to get low pulse
; Two beeps: low (stop/reverse) pulse received
rcall wait30ms
rcall beep_f3
cpi YL, lo8(puls_low_l+2)