forked from bitdump/BLHeli
-
Notifications
You must be signed in to change notification settings - Fork 0
/
BLHeliTxPgm.inc
660 lines (577 loc) · 13.4 KB
/
BLHeliTxPgm.inc
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
;**** **** **** **** ****
;
; BLHeli program for controlling brushless motors in helicopters and 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/>.
;
;**** **** **** **** ****
;
; BLHeliTxPgm Atmel
;
;**** **** **** **** ****
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Read all eeprom parameters routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
read_all_eeprom_parameters:
.IF MODE == 0 ; Main
; Check initialized signature
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
xcall read_eeprom_byte
cpi Temp1, 0xA5
breq PC+2
rjmp read_eeprom_store_defaults
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
xcall read_eeprom_byte
cpi Temp1, 0x5A
breq PC+2
rjmp read_eeprom_store_defaults
.ENDIF
.IF MODE == 1 ; Tail
; Check initialized signature
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
xcall read_eeprom_byte
cpi Temp1, 0x5A
breq PC+2
rjmp read_eeprom_store_defaults
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
xcall read_eeprom_byte
cpi Temp1, 0xA5
breq PC+2
rjmp read_eeprom_store_defaults
.ENDIF
.IF MODE == 2 ; Multi
; Check initialized signature
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
xcall read_eeprom_byte
cpi Temp1, 0x55
breq PC+2
rjmp read_eeprom_store_defaults
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
xcall read_eeprom_byte
cpi Temp1, 0xAA
breq PC+2
rjmp read_eeprom_store_defaults
.ENDIF
; Read eeprom
ldi Temp4, 10 ; 10 parameters
ldi XL, low(Pgm_Gov_P_Gain)
ldi XH, high(Pgm_Gov_P_Gain)
.IF MODE == 0 || MODE ==2
ldi ZL, low(Eep_Pgm_Gov_P_Gain)
ldi ZH, high(Eep_Pgm_Gov_P_Gain)
.ENDIF
.IF MODE == 1
ldi ZL, low(_Eep_Pgm_Gov_P_Gain)
ldi ZH, high(_Eep_Pgm_Gov_P_Gain)
.ENDIF
read_eeprom_block1:
xcall read_eeprom_byte
st X+, Temp1
adiw Z, 1
dec Temp4
brne read_eeprom_block1
ldi Temp4, 21 ; 21 parameters
ldi XL, low(Pgm_Enable_TX_Program)
ldi XH, high(Pgm_Enable_TX_Program)
ldi ZL, low(Eep_Enable_TX_Program)
ldi ZH, high(Eep_Enable_TX_Program)
read_eeprom_block2:
xcall read_eeprom_byte
st X+, Temp1
adiw Z, 1
dec Temp4
brne read_eeprom_block2
rjmp read_eeprom_exit
read_eeprom_store_defaults:
xcall set_default_parameters
xcall store_all_in_eeprom
read_eeprom_exit:
ldi ZL, low(Eep_Dummy) ; Set pointer to uncritical area
ldi ZH, high(Eep_Dummy)
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Read eeprom byte routine
;
; Assumes data in Temp1 and address in Z
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
read_eeprom_byte:
; Wait for completion of previous write
Check_Eeprom_Ready
rjmp read_eeprom_byte
; Set up address in address register
Set_Eeprom_Address ZL, ZH
; Start eeprom read
sbi EECR, EERE
in Temp1, EEDR
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Store all parameter values in EEPROM routine
;
; Assumes interrupts to be off
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
store_all_in_eeprom:
; Clear initialized signature
xcall clear_eeprom_signature
; Write eeprom
ldi Temp4, 10 ; 10 parameters
ldi XL, low(Pgm_Gov_P_Gain)
ldi XH, high(Pgm_Gov_P_Gain)
.IF MODE == 0 || MODE ==2
ldi ZL, low(Eep_Pgm_Gov_P_Gain)
ldi ZH, high(Eep_Pgm_Gov_P_Gain)
.ENDIF
.IF MODE == 1
ldi ZL, low(_Eep_Pgm_Gov_P_Gain)
ldi ZH, high(_Eep_Pgm_Gov_P_Gain)
.ENDIF
write_eeprom_block1:
ld Temp1, X+
xcall write_eeprom_byte
adiw Z, 1
dec Temp4
brne write_eeprom_block1
ldi Temp4, 21 ; 21 parameters
ldi XL, low(Pgm_Enable_TX_Program)
ldi XH, high(Pgm_Enable_TX_Program)
ldi ZL, low(Eep_Enable_TX_Program)
ldi ZH, high(Eep_Enable_TX_Program)
write_eeprom_block2:
ld Temp1, X+
xcall write_eeprom_byte
adiw Z, 1
dec Temp4
brne write_eeprom_block2
; Write initialized signature
xcall write_eeprom_signature
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Write eeprom byte routine
;
; Assumes data in Temp1 and address in Z
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
write_eeprom_byte:
; Wait for completion of previous write
Check_Eeprom_Ready
rjmp write_eeprom_byte
; Set up address in address register
out EEDR, Temp1
Set_Eeprom_Address ZL, ZH
; Set write enables and start writing
Start_Eeprom_Write
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Clear eeprom signature routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
clear_eeprom_signature:
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
ldi Temp1, 0xFF
xcall write_eeprom_byte
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
ldi Temp1, 0xFF
xcall write_eeprom_byte
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Write eeprom signature routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
write_eeprom_signature:
.IF MODE == 0 ; Main
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
ldi Temp1, 0xA5
xcall write_eeprom_byte
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
ldi Temp1, 0x5A
xcall write_eeprom_byte
.ENDIF
.IF MODE == 1 ; Tail
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
ldi Temp1, 0x5A
xcall write_eeprom_byte
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
ldi Temp1, 0xA5
xcall write_eeprom_byte
.ENDIF
.IF MODE == 2 ; Multi
ldi ZL, low(Eep_Initialized_L)
ldi ZH, high(Eep_Initialized_L)
ldi Temp1, 0x55
xcall write_eeprom_byte
ldi ZL, low(Eep_Initialized_H)
ldi ZH, high(Eep_Initialized_H)
ldi Temp1, 0xAA
xcall write_eeprom_byte
.ENDIF
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Store new parameter value in ram routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
store_new_value_in_ram:
lds Temp4, Tx_Pgm_Func_No ; Function no
lds Temp1, Tx_Pgm_Paraval_No ; Parameter value no
.IF MODE == 0
cpi Temp4, 1
brne PC+3
sts Pgm_Gov_P_Gain, Temp1
cpi Temp4, 2
brne PC+3
sts Pgm_Gov_I_Gain, Temp1
cpi Temp4, 3
brne PC+3
sts Pgm_Gov_Mode, Temp1
cpi Temp4, 4
brne PC+3
sts Pgm_Gov_Range, Temp1
cpi Temp4, 5
brne PC+3
sts Pgm_Low_Voltage_Lim, Temp1
cpi Temp4, 6
brne PC+3
sts Pgm_Startup_Pwr, Temp1
cpi Temp4, 7
brne PC+3
sts Pgm_Comm_Timing, Temp1
cpi Temp4, 8
brne PC+3
sts Pgm_Throttle_Rate, Temp1
cpi Temp4, 9
brne PC+3
sts Pgm_Damping_Force, Temp1
cpi Temp4, 10
brne PC+3
sts Pgm_Pwm_Freq, Temp1
cpi Temp4, 11
brne PC+3
sts Pgm_Demag_Comp, Temp1
cpi Temp4, 12
brne PC+3
sts Pgm_Direction, Temp1
cpi Temp4, 13
brne PC+3
sts Pgm_Input_Pol, Temp1
.ENDIF
.IF MODE == 1
cpi Temp4, 1
brne PC+3
sts Pgm_Motor_Gain, Temp1
cpi Temp4, 2
brne PC+3
sts Pgm_Motor_Idle, Temp1
cpi Temp4, 3
brne PC+3
sts Pgm_Startup_Pwr, Temp1
cpi Temp4, 4
brne PC+3
sts Pgm_Comm_Timing, Temp1
cpi Temp4, 5
brne PC+3
sts Pgm_Throttle_Rate, Temp1
cpi Temp4, 6
brne PC+3
sts Pgm_Damping_Force, Temp1
cpi Temp4, 7
brne PC+3
sts Pgm_Pwm_Freq, Temp1
cpi Temp4, 8
brne PC+3
sts Pgm_Demag_Comp, Temp1
cpi Temp4, 9
brne PC+3
sts Pgm_Direction, Temp1
cpi Temp4, 10
brne PC+3
sts Pgm_Input_Pol, Temp1
.ENDIF
.IF MODE == 2
cpi Temp4, 1
brne PC+3
sts Pgm_Gov_P_Gain, Temp1
cpi Temp4, 2
brne PC+3
sts Pgm_Gov_I_Gain, Temp1
cpi Temp4, 3
brne PC+3
sts Pgm_Gov_Mode, Temp1
cpi Temp4, 4
brne PC+3
sts Pgm_Motor_Gain, Temp1
cpi Temp4, 5
brne PC+3
sts Pgm_Low_Voltage_Lim, Temp1
cpi Temp4, 6
brne PC+3
sts Pgm_Startup_Pwr, Temp1
cpi Temp4, 7
brne PC+3
sts Pgm_Comm_Timing, Temp1
cpi Temp4, 8
brne PC+3
sts Pgm_Throttle_Rate, Temp1
cpi Temp4, 9
brne PC+3
sts Pgm_Damping_Force, Temp1
cpi Temp4, 10
brne PC+3
sts Pgm_Pwm_Freq, Temp1
cpi Temp4, 11
brne PC+3
sts Pgm_Demag_Comp, Temp1
cpi Temp4, 12
brne PC+3
sts Pgm_Direction, Temp1
cpi Temp4, 13
brne PC+3
sts Pgm_Input_Pol, Temp1
.ENDIF
ret
;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Wait 1 second routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
wait1s:
ldi Temp3, 5
wait1s_loop:
xcall wait200ms
dec Temp3
brne wait1s_loop
ret
;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Success beep routine
;
; Assumes interrupts to be off
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
success_beep:
xcall beep_f1
xcall beep_f2
xcall beep_f3
xcall beep_f4
xcall wait10ms
xcall beep_f1
xcall beep_f2
xcall beep_f3
xcall beep_f4
xcall wait10ms
xcall beep_f1
xcall beep_f2
xcall beep_f3
xcall beep_f4
ret
;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Success beep inverted routine
;
; Assumes interrupts to be off
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
success_beep_inverted:
xcall beep_f4
xcall beep_f3
xcall beep_f2
xcall beep_f1
xcall wait10ms
xcall beep_f4
xcall beep_f3
xcall beep_f2
xcall beep_f1
xcall wait10ms
xcall beep_f4
xcall beep_f3
xcall beep_f2
xcall beep_f1
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Function and parameter value beep routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
function_paraval_beep:
lds Temp7, Tx_Pgm_Func_No ; Function no
lds Temp8, Tx_Pgm_Paraval_No ; Parameter value no
function_beep:
xcall beep_f1
xcall beep_f1
xcall beep_f1
xcall wait10ms
dec Temp7
brne function_beep
paraval_beep:
xcall beep_f4
xcall wait10ms
dec Temp8
brne paraval_beep
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; Program by TX routine
;
; No assumptions
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
program_by_tx:
; Programming mode entry beeps
cli
xcall success_beep
sei
xcall wait1s
xcall wait1s
; Start at function 1, parameter value 1
ldi XH, 1
sts Tx_Pgm_Func_No, XH
paraval_no_entry:
ldi XH, 1
sts Tx_Pgm_Paraval_No, XH
beep_no_entry:
sts Tx_Pgm_Beep_No, Zero
func_paraval:
cli
xcall function_paraval_beep
sei
ldi XH, 5 ; Wait is 5x 200ms
mov Temp5, XH
func_paraval_wait:
lds Temp6, New_Rcp ; Load RC pulse
xcall wait200ms
lds XH, New_Rcp
cp Temp6, XH ; Is RC pulse stable? (Avoid issues from 3in1 interference)
brne func_paraval_wait ; No - branch
cpi XH, RCP_STOP ; Below stop?
brcs func_paraval_store ; Yes - branch
cpi XH, RCP_MAX ; Below max?
brcs function_next ; Yes - branch
rjmp func_paraval_cont_wait ; No - branch
func_paraval_store:
xcall store_new_value_in_ram ; Yes - store new value in RAM
cli ; Disable all interrupts
xcall store_all_in_eeprom ; Store all values in EEPROM
xcall success_beep ; Beep success
Enable_Watchdog XH ; Generate hardware reset from watchdog
xcall wait1s
func_paraval_cont_wait:
dec Temp5
brne func_paraval_wait
lds XH, Tx_Pgm_Beep_No ; Check number of beeps
inc XH
sts Tx_Pgm_Beep_No, XH
cpi XH, 3 ; Three beeps done?
brcc paraval_next ; Yes - Next parameter value
rjmp func_paraval ; No - go back
paraval_next:
xcall wait1s
lds XH, Tx_Pgm_Paraval_No ; Parameter value no
inc XH
sts Tx_Pgm_Paraval_No, XH
cli
.IF MODE == 0
lds XH, Tx_Pgm_Func_No ; Decode number of parameters
dec XH
ldi ZL, low(TX_PGM_PARAMS_MAIN<<1)
ldi ZH, high(TX_PGM_PARAMS_MAIN<<1)
add ZL, XH
adc ZH, Zero
lpm Temp1, Z
.ENDIF
.IF MODE == 1
lds XH, Tx_Pgm_Func_No ; Decode number of parameters
dec XH
ldi ZL, low(TX_PGM_PARAMS_TAIL<<1)
ldi ZH, high(TX_PGM_PARAMS_TAIL<<1)
add ZL, XH
adc ZH, Zero
lpm Temp1, Z
.ENDIF
.IF MODE == 2
lds XH, Tx_Pgm_Func_No ; Decode number of parameters
dec XH
ldi ZL, low(TX_PGM_PARAMS_MULTI<<1)
ldi ZH, high(TX_PGM_PARAMS_MULTI<<1)
add ZL, XH
adc ZH, Zero
lpm Temp1, Z
.ENDIF
sei
inc Temp1
lds XH, Tx_Pgm_Paraval_No
cp XH, Temp1
brcc function_next ; Last parameter value?
rjmp beep_no_entry ; No - go back
function_next: ; Yes - Next function value
xcall wait1s
xcall wait1s
lds XH, Tx_Pgm_Func_No ; Function value no
inc XH
sts Tx_Pgm_Func_No, XH
.IF MODE == 0
lds XH, Tx_Pgm_Func_No
cpi XH, 14 ; Main has 13 functions
.ENDIF
.IF MODE == 1
lds XH, Tx_Pgm_Func_No
cpi XH, 11 ; Tail has 10 functions
.ENDIF
.IF MODE == 2
lds XH, Tx_Pgm_Func_No
cpi XH, 14 ; Multi has 13 functions
.ENDIF
brcc program_by_tx_exit ; Last function value?
rjmp paraval_no_entry ; No - go back
program_by_tx_exit:
xcall set_default_parameters ; Load all defaults
cli ; Disable all interrupts
xcall store_all_in_eeprom ; Erase flash and program
Enable_Watchdog XH ; Generate hardware reset from watchdog
xcall wait1s