0,0 → 1,1376 |
1 .file "timer0.c" |
2 .arch atmega1284p |
3 __SREG__ = 0x3f |
4 __SP_H__ = 0x3e |
5 __SP_L__ = 0x3d |
6 __tmp_reg__ = 0 |
7 __zero_reg__ = 1 |
8 .global __do_copy_data |
9 .global __do_clear_bss |
10 .global ServoRollValue |
11 .global ServoRollValue |
12 .section .bss |
15 ServoRollValue: |
16 0000 0000 .skip 2,0 |
17 .global ServoNickValue |
18 .global ServoNickValue |
21 ServoNickValue: |
22 0002 0000 .skip 2,0 |
23 .global NickServoValue |
24 .data |
27 NickServoValue: |
28 0000 0020 .word 8192 |
29 .global BeepMuster |
32 BeepMuster: |
33 0002 FFFF .word -1 |
34 .global ServoRollOffset |
37 ServoRollOffset: |
38 0004 C01F .word 8128 |
39 .global ServoNickOffset |
42 ServoNickOffset: |
43 0006 C01F .word 8128 |
44 .global RemainingPulse |
45 .global RemainingPulse |
46 .section .bss |
49 RemainingPulse: |
50 0004 0000 .skip 2,0 |
51 .global CalculateServoSignals |
52 .data |
55 CalculateServoSignals: |
56 0008 01 .byte 1 |
57 .global ServoActive |
58 .global ServoActive |
59 .section .bss |
62 ServoActive: |
63 0006 00 .skip 1,0 |
64 .global SendSPI |
65 .global SendSPI |
68 SendSPI: |
69 0007 00 .skip 1,0 |
70 .global beeptime |
71 .global beeptime |
74 beeptime: |
75 0008 0000 .skip 2,0 |
76 .global cntKompass |
77 .global cntKompass |
80 cntKompass: |
81 000a 0000 .skip 2,0 |
82 .global UpdateMotor |
83 .global UpdateMotor |
86 UpdateMotor: |
87 000c 00 .skip 1,0 |
88 .global CountMilliseconds |
89 .global CountMilliseconds |
92 CountMilliseconds: |
93 000d 0000 .skip 2,0 |
94 .data |
97 cnt_1ms.3: |
98 0009 01 .byte 1 |
99 .lcomm cnt.4,1 |
100 .lcomm compass_active.5,1 |
101 .text |
102 .global __vector_18 |
104 __vector_18: |
105 /* prologue: frame size=0 */ |
106 0000 1F92 push __zero_reg__ |
107 0002 0F92 push __tmp_reg__ |
108 0004 0FB6 in __tmp_reg__,__SREG__ |
109 0006 0F92 push __tmp_reg__ |
110 0008 1124 clr __zero_reg__ |
111 000a 2F93 push r18 |
112 000c 3F93 push r19 |
113 000e 5F93 push r21 |
114 0010 6F93 push r22 |
115 0012 7F93 push r23 |
116 0014 8F93 push r24 |
117 0016 9F93 push r25 |
118 0018 AF93 push r26 |
119 001a BF93 push r27 |
120 001c EF93 push r30 |
121 001e FF93 push r31 |
122 /* prologue end (size=16) */ |
123 0020 20E0 ldi r18,lo8(0) |
124 0022 8091 0000 lds r24,SendSPI |
125 0026 8823 tst r24 |
126 0028 01F0 breq .L2 |
127 002a 8091 0000 lds r24,SendSPI |
128 002e 8150 subi r24,lo8(-(-1)) |
129 0030 8093 0000 sts SendSPI,r24 |
130 .L2: |
131 0034 8091 0000 lds r24,SpektrumTimer |
132 0038 8823 tst r24 |
133 003a 01F0 breq .L3 |
134 003c 8150 subi r24,lo8(-(-1)) |
135 003e 8093 0000 sts SpektrumTimer,r24 |
136 .L3: |
137 0042 8091 0000 lds r24,cnt.4 |
138 0046 8150 subi r24,lo8(-(-1)) |
139 0048 8093 0000 sts cnt.4,r24 |
140 004c 8F3F cpi r24,lo8(-1) |
141 004e 01F0 breq .+2 |
142 0050 00C0 rjmp .L4 |
143 0052 89E0 ldi r24,lo8(9) |
144 0054 8093 0000 sts cnt.4,r24 |
145 0058 8091 0000 lds r24,CountMilliseconds |
146 005c 9091 0000 lds r25,(CountMilliseconds)+1 |
147 0060 0196 adiw r24,1 |
148 0062 9093 0000 sts (CountMilliseconds)+1,r25 |
149 0066 8093 0000 sts CountMilliseconds,r24 |
150 006a 8091 0000 lds r24,cnt_1ms.3 |
151 006e 8F5F subi r24,lo8(-(1)) |
152 0070 8170 andi r24,lo8(1) |
153 0072 8093 0000 sts cnt_1ms.3,r24 |
154 0076 8823 tst r24 |
155 0078 01F4 brne .L5 |
156 007a 81E0 ldi r24,lo8(1) |
157 007c 8093 0000 sts UpdateMotor,r24 |
158 .L5: |
159 0080 3499 sbic 38-0x20,4 |
160 0082 00C0 rjmp .L6 |
161 0084 81E0 ldi r24,lo8(1) |
162 0086 8093 0000 sts compass_active.5,r24 |
163 .L6: |
164 008a 8091 0000 lds r24,beeptime |
165 008e 9091 0000 lds r25,(beeptime)+1 |
166 0092 892B or r24,r25 |
167 0094 01F0 breq .L7 |
168 0096 8091 0000 lds r24,beeptime |
169 009a 9091 0000 lds r25,(beeptime)+1 |
170 009e 0B97 sbiw r24,11 |
171 00a0 00F0 brlo .L8 |
172 00a2 8091 0000 lds r24,beeptime |
173 00a6 9091 0000 lds r25,(beeptime)+1 |
174 00aa 0A97 sbiw r24,10 |
175 00ac 9093 0000 sts (beeptime)+1,r25 |
176 00b0 8093 0000 sts beeptime,r24 |
177 00b4 00C0 rjmp .L9 |
178 .L8: |
179 00b6 1092 0000 sts (beeptime)+1,__zero_reg__ |
180 00ba 1092 0000 sts beeptime,__zero_reg__ |
181 .L9: |
182 00be 8091 0000 lds r24,beeptime |
183 00c2 9091 0000 lds r25,(beeptime)+1 |
184 00c6 2091 0000 lds r18,BeepMuster |
185 00ca 3091 0000 lds r19,(BeepMuster)+1 |
186 00ce 8223 and r24,r18 |
187 00d0 9323 and r25,r19 |
188 00d2 892B or r24,r25 |
189 00d4 01F0 breq .L10 |
190 00d6 21E0 ldi r18,lo8(1) |
191 00d8 00C0 rjmp .L12 |
192 .L10: |
193 00da 20E0 ldi r18,lo8(0) |
194 00dc 00C0 rjmp .L12 |
195 .L7: |
196 00de 8FEF ldi r24,lo8(-1) |
197 00e0 9FEF ldi r25,hi8(-1) |
198 00e2 9093 0000 sts (BeepMuster)+1,r25 |
199 00e6 8093 0000 sts BeepMuster,r24 |
200 .L12: |
201 00ea 8091 0000 lds r24,PlatinenVersion |
202 00ee 2223 tst r18 |
203 00f0 01F0 breq .L13 |
204 00f2 8A30 cpi r24,lo8(10) |
205 00f4 01F4 brne .L14 |
206 00f6 5A9A sbi 43-0x20,2 |
207 00f8 00C0 rjmp .L4 |
208 .L14: |
209 00fa 479A sbi 40-0x20,7 |
210 00fc 00C0 rjmp .L4 |
211 .L13: |
212 00fe 8A30 cpi r24,lo8(10) |
213 0100 01F4 brne .L17 |
214 0102 5A98 cbi 43-0x20,2 |
215 0104 00C0 rjmp .L4 |
216 .L17: |
217 0106 4798 cbi 40-0x20,7 |
218 .L4: |
219 0108 8091 0000 lds r24,compass_active.5 |
220 010c 8823 tst r24 |
221 010e 01F4 brne .+2 |
222 0110 00C0 rjmp .L1 |
223 0112 2091 0000 lds r18,NaviDataOkay |
224 0116 2223 tst r18 |
225 0118 01F0 breq .+2 |
226 011a 00C0 rjmp .L1 |
227 011c 8091 0000 lds r24,Parameter_GlobalConfig |
228 0120 83FF sbrs r24,3 |
229 0122 00C0 rjmp .L1 |
230 0124 86B1 in r24,38-0x20 |
231 0126 9927 clr r25 |
232 0128 FC01 movw r30,r24 |
233 012a E071 andi r30,lo8(16) |
234 012c F070 andi r31,hi8(16) |
235 012e 84FF sbrs r24,4 |
236 0130 00C0 rjmp .L20 |
237 0132 8091 0000 lds r24,cntKompass |
238 0136 9091 0000 lds r25,(cntKompass)+1 |
239 013a 0196 adiw r24,1 |
240 013c 9093 0000 sts (cntKompass)+1,r25 |
241 0140 8093 0000 sts cntKompass,r24 |
242 0144 8091 0000 lds r24,cntKompass |
243 0148 9091 0000 lds r25,(cntKompass)+1 |
244 014c 895E subi r24,lo8(1001) |
245 014e 9340 sbci r25,hi8(1001) |
246 0150 00F0 brlo .L1 |
247 0152 2093 0000 sts compass_active.5,r18 |
248 0156 00C0 rjmp .L1 |
249 .L20: |
250 0158 8091 0000 lds r24,cntKompass |
251 015c 9091 0000 lds r25,(cntKompass)+1 |
252 0160 892B or r24,r25 |
253 0162 01F0 breq .L23 |
254 0164 8091 0000 lds r24,cntKompass |
255 0168 9091 0000 lds r25,(cntKompass)+1 |
256 016c 8A56 subi r24,lo8(362) |
257 016e 9140 sbci r25,hi8(362) |
258 0170 00F4 brsh .L23 |
259 0172 8091 0000 lds r24,cntKompass |
260 0176 9091 0000 lds r25,(cntKompass)+1 |
261 017a 69E2 ldi r22,lo8(41) |
262 017c 70E0 ldi r23,hi8(41) |
263 017e 0E94 0000 call __udivmodhi4 |
264 0182 8091 0000 lds r24,cntKompass |
265 0186 9091 0000 lds r25,(cntKompass)+1 |
266 018a 860F add r24,r22 |
267 018c 971F adc r25,r23 |
268 018e 9093 0000 sts (cntKompass)+1,r25 |
269 0192 8093 0000 sts cntKompass,r24 |
270 0196 8091 0000 lds r24,cntKompass |
271 019a 9091 0000 lds r25,(cntKompass)+1 |
272 019e 0B97 sbiw r24,11 |
273 01a0 00F0 brlo .L24 |
274 01a2 8091 0000 lds r24,cntKompass |
275 01a6 9091 0000 lds r25,(cntKompass)+1 |
276 01aa 0A97 sbiw r24,10 |
277 01ac 9093 0000 sts (KompassValue)+1,r25 |
278 01b0 8093 0000 sts KompassValue,r24 |
279 01b4 00C0 rjmp .L23 |
280 .L24: |
281 01b6 F093 0000 sts (KompassValue)+1,r31 |
282 01ba E093 0000 sts KompassValue,r30 |
283 .L23: |
284 01be 1092 0000 sts (cntKompass)+1,__zero_reg__ |
285 01c2 1092 0000 sts cntKompass,__zero_reg__ |
286 .L1: |
287 /* epilogue: frame size=0 */ |
288 01c6 FF91 pop r31 |
289 01c8 EF91 pop r30 |
290 01ca BF91 pop r27 |
291 01cc AF91 pop r26 |
292 01ce 9F91 pop r25 |
293 01d0 8F91 pop r24 |
294 01d2 7F91 pop r23 |
295 01d4 6F91 pop r22 |
296 01d6 5F91 pop r21 |
297 01d8 3F91 pop r19 |
298 01da 2F91 pop r18 |
299 01dc 0F90 pop __tmp_reg__ |
300 01de 0FBE out __SREG__,__tmp_reg__ |
301 01e0 0F90 pop __tmp_reg__ |
302 01e2 1F90 pop __zero_reg__ |
303 01e4 1895 reti |
304 /* epilogue end (size=16) */ |
305 /* function __vector_18 size 247 (215) */ |
307 .global SetDelay |
309 SetDelay: |
310 /* prologue: frame size=0 */ |
311 /* prologue end (size=0) */ |
312 01e6 2091 0000 lds r18,CountMilliseconds |
313 01ea 3091 0000 lds r19,(CountMilliseconds)+1 |
314 01ee 280F add r18,r24 |
315 01f0 391F adc r19,r25 |
316 01f2 C901 movw r24,r18 |
317 01f4 0196 adiw r24,1 |
318 /* epilogue: frame size=0 */ |
319 01f6 0895 ret |
320 /* epilogue end (size=1) */ |
321 /* function SetDelay size 9 (8) */ |
323 .global CheckDelay |
325 CheckDelay: |
326 /* prologue: frame size=0 */ |
327 /* prologue end (size=0) */ |
328 01f8 2091 0000 lds r18,CountMilliseconds |
329 01fc 3091 0000 lds r19,(CountMilliseconds)+1 |
330 0200 821B sub r24,r18 |
331 0202 930B sbc r25,r19 |
332 0204 892F mov r24,r25 |
333 0206 9927 clr r25 |
334 0208 8695 lsr r24 |
335 020a 8074 andi r24,lo8(64) |
336 020c 9070 andi r25,hi8(64) |
337 /* epilogue: frame size=0 */ |
338 020e 0895 ret |
339 /* epilogue end (size=1) */ |
340 /* function CheckDelay size 12 (11) */ |
342 .global Delay_ms |
344 Delay_ms: |
345 /* prologue: frame size=0 */ |
346 0210 CF93 push r28 |
347 0212 DF93 push r29 |
348 /* prologue end (size=2) */ |
349 0214 0E94 0000 call SetDelay |
350 0218 EC01 movw r28,r24 |
351 .L29: |
352 021a CE01 movw r24,r28 |
353 021c 0E94 0000 call CheckDelay |
354 0220 8823 tst r24 |
355 0222 01F0 breq .L29 |
356 /* epilogue: frame size=0 */ |
357 0224 DF91 pop r29 |
358 0226 CF91 pop r28 |
359 0228 0895 ret |
360 /* epilogue end (size=3) */ |
361 /* function Delay_ms size 13 (8) */ |
363 .global Delay_ms_Mess |
365 Delay_ms_Mess: |
366 /* prologue: frame size=0 */ |
367 022a CF93 push r28 |
368 022c DF93 push r29 |
369 /* prologue end (size=2) */ |
370 022e 0E94 0000 call SetDelay |
371 0232 EC01 movw r28,r24 |
372 0234 00C0 rjmp .L40 |
373 .L36: |
374 0236 8091 0000 lds r24,AdReady |
375 023a 8823 tst r24 |
376 023c 01F0 breq .L40 |
377 023e 9093 0000 sts AdReady,r25 |
378 0242 8FEC ldi r24,lo8(-49) |
379 0244 8093 7A00 sts 122,r24 |
380 .L40: |
381 0248 CE01 movw r24,r28 |
382 024a 0E94 0000 call CheckDelay |
383 024e 982F mov r25,r24 |
384 0250 8823 tst r24 |
385 0252 01F0 breq .L36 |
386 /* epilogue: frame size=0 */ |
387 0254 DF91 pop r29 |
388 0256 CF91 pop r28 |
389 0258 0895 ret |
390 /* epilogue end (size=3) */ |
391 /* function Delay_ms_Mess size 24 (19) */ |
393 .global TIMER2_Init |
395 TIMER2_Init: |
396 /* prologue: frame size=0 */ |
397 /* prologue end (size=0) */ |
398 025a 9FB7 in r25,95-0x20 |
399 /* #APP */ |
400 025c F894 cli |
401 /* #NOAPP */ |
402 025e 5F98 cbi 43-0x20,7 |
403 0260 3E9A sbi 39-0x20,6 |
404 0262 469A sbi 40-0x20,6 |
405 0264 8091 B000 lds r24,176 |
406 0268 8F70 andi r24,lo8(15) |
407 026a 8093 B000 sts 176,r24 |
408 026e 8091 B000 lds r24,176 |
409 0272 8360 ori r24,lo8(3) |
410 0274 8093 B000 sts 176,r24 |
411 0278 8091 B100 lds r24,177 |
412 027c 8B73 andi r24,lo8(59) |
413 027e 8093 B100 sts 177,r24 |
414 0282 8091 B100 lds r24,177 |
415 0286 8B60 ori r24,lo8(11) |
416 0288 8093 B100 sts 177,r24 |
417 028c 1092 B200 sts 178,__zero_reg__ |
418 0290 8FEF ldi r24,lo8(-1) |
419 0292 8093 B300 sts 179,r24 |
420 0296 8091 B000 lds r24,176 |
421 029a 8068 ori r24,lo8(-128) |
422 029c 8093 B000 sts 176,r24 |
423 02a0 8091 7000 lds r24,112 |
424 02a4 8A7F andi r24,lo8(-6) |
425 02a6 8093 7000 sts 112,r24 |
426 02aa 8091 7000 lds r24,112 |
427 02ae 8260 ori r24,lo8(2) |
428 02b0 8093 7000 sts 112,r24 |
429 02b4 9FBF out 95-0x20,r25 |
430 /* epilogue: frame size=0 */ |
431 02b6 0895 ret |
432 /* epilogue end (size=1) */ |
433 /* function TIMER2_Init size 48 (47) */ |
435 .global Timer_Init |
437 Timer_Init: |
438 /* prologue: frame size=0 */ |
439 /* prologue end (size=0) */ |
440 02b8 8AE0 ldi r24,lo8(10) |
441 02ba 90E0 ldi r25,hi8(10) |
442 02bc 0E94 0000 call SetDelay |
443 02c0 9093 0000 sts (tim_main)+1,r25 |
444 02c4 8093 0000 sts tim_main,r24 |
445 02c8 82E0 ldi r24,lo8(2) |
446 02ca 85BD out 69-0x20,r24 |
447 02cc 83EA ldi r24,lo8(-93) |
448 02ce 84BD out 68-0x20,r24 |
449 02d0 17BC out 71-0x20,__zero_reg__ |
450 02d2 84EB ldi r24,lo8(-76) |
451 02d4 88BD out 72-0x20,r24 |
452 02d6 86E0 ldi r24,lo8(6) |
453 02d8 86BD out 70-0x20,r24 |
454 02da 8091 6E00 lds r24,110 |
455 02de 8160 ori r24,lo8(1) |
456 02e0 8093 6E00 sts 110,r24 |
457 /* epilogue: frame size=0 */ |
458 02e4 0895 ret |
459 /* epilogue end (size=1) */ |
460 /* function Timer_Init size 23 (22) */ |
462 .global CalcNickServoValue |
464 CalcNickServoValue: |
465 /* prologue: frame size=0 */ |
466 /* prologue end (size=0) */ |
467 02e6 8091 0000 lds r24,EE_Parameter+109 |
468 02ea 2091 0000 lds r18,Parameter_ServoNickControl |
469 02ee 82FF sbrs r24,2 |
470 02f0 00C0 rjmp .L44 |
471 02f2 9091 0000 lds r25,EE_Parameter+48 |
472 02f6 8CE3 ldi r24,lo8(60) |
473 02f8 989F mul r25,r24 |
474 02fa B001 movw r22,r0 |
475 02fc 1124 clr r1 |
476 02fe 9091 0000 lds r25,EE_Parameter+47 |
477 0302 80E5 ldi r24,lo8(80) |
478 0304 989F mul r25,r24 |
479 0306 A001 movw r20,r0 |
480 0308 1124 clr r1 |
481 030a 922F mov r25,r18 |
482 030c 9058 subi r25,lo8(-(-128)) |
483 030e 97FD sbrc r25,7 |
484 0310 9D5F subi r25,lo8(-(3)) |
485 .L45: |
486 0312 9595 asr r25 |
487 0314 9595 asr r25 |
488 0316 86E0 ldi r24,lo8(6) |
489 0318 9802 muls r25,r24 |
490 031a C001 movw r24,r0 |
491 031c 1124 clr r1 |
492 031e 2091 0000 lds r18,NickServoValue |
493 0322 3091 0000 lds r19,(NickServoValue)+1 |
494 0326 281B sub r18,r24 |
495 0328 390B sbc r19,r25 |
496 032a 3093 0000 sts (NickServoValue)+1,r19 |
497 032e 2093 0000 sts NickServoValue,r18 |
498 0332 4217 cp r20,r18 |
499 0334 5307 cpc r21,r19 |
500 0336 04F0 brlt .L46 |
501 0338 5093 0000 sts (NickServoValue)+1,r21 |
502 033c 4093 0000 sts NickServoValue,r20 |
503 0340 0895 ret |
504 .L46: |
505 0342 2617 cp r18,r22 |
506 0344 3707 cpc r19,r23 |
507 0346 04F0 brlt .L43 |
508 0348 7093 0000 sts (NickServoValue)+1,r23 |
509 034c 6093 0000 sts NickServoValue,r22 |
510 0350 0895 ret |
511 .L44: |
512 0352 822F mov r24,r18 |
513 0354 9927 clr r25 |
514 0356 36E0 ldi r19,6 |
515 0358 880F 1: lsl r24 |
516 035a 991F rol r25 |
517 035c 3A95 dec r19 |
518 035e 01F4 brne 1b |
519 0360 9093 0000 sts (NickServoValue)+1,r25 |
520 0364 8093 0000 sts NickServoValue,r24 |
521 .L43: |
522 0368 0895 ret |
523 /* epilogue: frame size=0 */ |
524 /* epilogue: noreturn */ |
525 /* epilogue end (size=0) */ |
526 /* function CalcNickServoValue size 67 (67) */ |
528 .global CalculateServo |
530 CalculateServo: |
531 /* prologue: frame size=0 */ |
532 036a A0E0 ldi r26,lo8(0) |
533 036c B0E0 ldi r27,hi8(0) |
534 036e E0E0 ldi r30,pm_lo8(1f) |
535 0370 F0E0 ldi r31,pm_hi8(1f) |
536 0372 0C94 0000 jmp __prologue_saves__+22 |
537 1: |
538 /* prologue end (size=6) */ |
539 0376 8091 0000 lds r24,EE_Parameter+55 |
540 037a E82F mov r30,r24 |
541 037c FF27 clr r31 |
542 037e E050 subi r30,lo8(-(sintab)) |
543 0380 F040 sbci r31,hi8(-(sintab)) |
544 0382 9681 ldd r25,Z+6 |
545 0384 D080 ld r13,Z |
546 0386 8091 0000 lds r24,CalculateServoSignals |
547 038a A92F mov r26,r25 |
548 038c BB27 clr r27 |
549 038e A7FD sbrc r26,7 |
550 0390 B095 com r27 |
551 0392 CB2F mov r28,r27 |
552 0394 DB2F mov r29,r27 |
553 0396 8130 cpi r24,lo8(1) |
554 0398 01F0 breq .+2 |
555 039a 00C0 rjmp .L51 |
556 039c 2091 0000 lds r18,IntegralNick |
557 03a0 3091 0000 lds r19,(IntegralNick)+1 |
558 03a4 4091 0000 lds r20,(IntegralNick)+2 |
559 03a8 5091 0000 lds r21,(IntegralNick)+3 |
560 03ac BD01 movw r22,r26 |
561 03ae CE01 movw r24,r28 |
562 03b0 0E94 0000 call __mulsi3 |
563 03b4 97FF sbrs r25,7 |
564 03b6 00C0 rjmp .L52 |
565 03b8 6158 subi r22,lo8(-(127)) |
566 03ba 7F4F sbci r23,hi8(-(127)) |
567 03bc 8F4F sbci r24,hlo8(-(127)) |
568 03be 9F4F sbci r25,hhi8(-(127)) |
569 .L52: |
570 03c0 7B01 movw r14,r22 |
571 03c2 8C01 movw r16,r24 |
572 03c4 C7E0 ldi r28,7 |
573 03c6 1595 1: asr r17 |
574 03c8 0795 ror r16 |
575 03ca F794 ror r15 |
576 03cc E794 ror r14 |
577 03ce CA95 dec r28 |
578 03d0 01F4 brne 1b |
579 03d2 8D2D mov r24,r13 |
580 03d4 9927 clr r25 |
581 03d6 87FD sbrc r24,7 |
582 03d8 9095 com r25 |
583 03da A92F mov r26,r25 |
584 03dc B92F mov r27,r25 |
585 03de 2091 0000 lds r18,IntegralRoll |
586 03e2 3091 0000 lds r19,(IntegralRoll)+1 |
587 03e6 4091 0000 lds r20,(IntegralRoll)+2 |
588 03ea 5091 0000 lds r21,(IntegralRoll)+3 |
589 03ee BC01 movw r22,r24 |
590 03f0 CD01 movw r24,r26 |
591 03f2 0E94 0000 call __mulsi3 |
592 03f6 97FF sbrs r25,7 |
593 03f8 00C0 rjmp .L53 |
594 03fa 6158 subi r22,lo8(-(127)) |
595 03fc 7F4F sbci r23,hi8(-(127)) |
596 03fe 8F4F sbci r24,hlo8(-(127)) |
597 0400 9F4F sbci r25,hhi8(-(127)) |
598 .L53: |
599 0402 B7E0 ldi r27,7 |
600 0404 9595 1: asr r25 |
601 0406 8795 ror r24 |
602 0408 7795 ror r23 |
603 040a 6795 ror r22 |
604 040c BA95 dec r27 |
605 040e 01F4 brne 1b |
606 0410 F801 movw r30,r16 |
607 0412 E701 movw r28,r14 |
608 0414 C61B sub r28,r22 |
609 0416 D70B sbc r29,r23 |
610 0418 E80B sbc r30,r24 |
611 041a F90B sbc r31,r25 |
612 041c 2091 0000 lds r18,POI_KameraNick |
613 0420 3091 0000 lds r19,(POI_KameraNick)+1 |
614 0424 C901 movw r24,r18 |
615 0426 A3E0 ldi r26,3 |
616 0428 880F 1: lsl r24 |
617 042a 991F rol r25 |
618 042c AA95 dec r26 |
619 042e 01F4 brne 1b |
620 0430 821B sub r24,r18 |
621 0432 930B sbc r25,r19 |
622 0434 AA27 clr r26 |
623 0436 97FD sbrc r25,7 |
624 0438 A095 com r26 |
625 043a BA2F mov r27,r26 |
626 043c C81B sub r28,r24 |
627 043e D90B sbc r29,r25 |
628 0440 EA0B sbc r30,r26 |
629 0442 FB0B sbc r31,r27 |
630 0444 8091 0000 lds r24,EE_Parameter+46 |
631 0448 9927 clr r25 |
632 044a AA27 clr r26 |
633 044c BB27 clr r27 |
634 044e BC01 movw r22,r24 |
635 0450 CD01 movw r24,r26 |
636 0452 9E01 movw r18,r28 |
637 0454 AF01 movw r20,r30 |
638 0456 0E94 0000 call __mulsi3 |
639 045a 97FF sbrs r25,7 |
640 045c 00C0 rjmp .L54 |
641 045e 6150 subi r22,lo8(-(511)) |
642 0460 7E4F sbci r23,hi8(-(511)) |
643 0462 8F4F sbci r24,hlo8(-(511)) |
644 0464 9F4F sbci r25,hhi8(-(511)) |
645 .L54: |
646 0466 FC01 movw r30,r24 |
647 0468 EB01 movw r28,r22 |
648 046a 79E0 ldi r23,9 |
649 046c F595 1: asr r31 |
650 046e E795 ror r30 |
651 0470 D795 ror r29 |
652 0472 C795 ror r28 |
653 0474 7A95 dec r23 |
654 0476 01F4 brne 1b |
655 0478 1091 0000 lds r17,EE_Parameter+109 |
656 047c 6091 0000 lds r22,NickServoValue |
657 0480 7091 0000 lds r23,(NickServoValue)+1 |
658 0484 12FF sbrs r17,2 |
659 0486 00C0 rjmp .L55 |
660 0488 7093 0000 sts (ServoNickOffset)+1,r23 |
661 048c 6093 0000 sts ServoNickOffset,r22 |
662 0490 00C0 rjmp .L56 |
663 .L55: |
664 0492 4091 0000 lds r20,ServoNickOffset |
665 0496 5091 0000 lds r21,(ServoNickOffset)+1 |
666 049a 8091 0000 lds r24,EE_Parameter+54 |
667 049e 282F mov r18,r24 |
668 04a0 3327 clr r19 |
669 04a2 CB01 movw r24,r22 |
670 04a4 841B sub r24,r20 |
671 04a6 950B sbc r25,r21 |
672 04a8 B901 movw r22,r18 |
673 04aa 0E94 0000 call __divmodhi4 |
674 04ae 8091 0000 lds r24,ServoNickOffset |
675 04b2 9091 0000 lds r25,(ServoNickOffset)+1 |
676 04b6 860F add r24,r22 |
677 04b8 971F adc r25,r23 |
678 04ba 9093 0000 sts (ServoNickOffset)+1,r25 |
679 04be 8093 0000 sts ServoNickOffset,r24 |
680 .L56: |
681 04c2 10FF sbrs r17,0 |
682 04c4 00C0 rjmp .L57 |
683 04c6 8091 0000 lds r24,ServoNickOffset |
684 04ca 9091 0000 lds r25,(ServoNickOffset)+1 |
685 04ce 97FD sbrc r25,7 |
686 04d0 0F96 adiw r24,15 |
687 .L58: |
688 04d2 64E0 ldi r22,4 |
689 04d4 9595 1: asr r25 |
690 04d6 8795 ror r24 |
691 04d8 6A95 dec r22 |
692 04da 01F4 brne 1b |
693 04dc AA27 clr r26 |
694 04de 97FD sbrc r25,7 |
695 04e0 A095 com r26 |
696 04e2 BA2F mov r27,r26 |
697 04e4 C80F add r28,r24 |
698 04e6 D91F adc r29,r25 |
699 04e8 EA1F adc r30,r26 |
700 04ea FB1F adc r31,r27 |
701 04ec 00C0 rjmp .L59 |
702 .L57: |
703 04ee 8091 0000 lds r24,ServoNickOffset |
704 04f2 9091 0000 lds r25,(ServoNickOffset)+1 |
705 04f6 97FD sbrc r25,7 |
706 04f8 0F96 adiw r24,15 |
707 .L60: |
708 04fa 54E0 ldi r21,4 |
709 04fc 9595 1: asr r25 |
710 04fe 8795 ror r24 |
711 0500 5A95 dec r21 |
712 0502 01F4 brne 1b |
713 0504 AA27 clr r26 |
714 0506 97FD sbrc r25,7 |
715 0508 A095 com r26 |
716 050a BA2F mov r27,r26 |
717 050c 8C1B sub r24,r28 |
718 050e 9D0B sbc r25,r29 |
719 0510 AE0B sbc r26,r30 |
720 0512 BF0B sbc r27,r31 |
721 0514 FD01 movw r30,r26 |
722 0516 EC01 movw r28,r24 |
723 .L59: |
724 0518 2091 0000 lds r18,EE_Parameter+106 |
725 051c 2223 tst r18 |
726 051e 01F0 breq .L61 |
727 0520 3327 clr r19 |
728 0522 8091 0000 lds r24,ServoNickValue |
729 0526 9091 0000 lds r25,(ServoNickValue)+1 |
730 052a 829F mul r24,r18 |
731 052c A001 movw r20,r0 |
732 052e 839F mul r24,r19 |
733 0530 500D add r21,r0 |
734 0532 929F mul r25,r18 |
735 0534 500D add r21,r0 |
736 0536 1124 clr r1 |
737 0538 CA01 movw r24,r20 |
738 053a AA27 clr r26 |
739 053c 97FD sbrc r25,7 |
740 053e A095 com r26 |
741 0540 BA2F mov r27,r26 |
742 0542 8C0F add r24,r28 |
743 0544 9D1F adc r25,r29 |
744 0546 AE1F adc r26,r30 |
745 0548 BF1F adc r27,r31 |
746 054a 2F5F subi r18,lo8(-(1)) |
747 054c 3F4F sbci r19,hi8(-(1)) |
748 054e 4427 clr r20 |
749 0550 37FD sbrc r19,7 |
750 0552 4095 com r20 |
751 0554 542F mov r21,r20 |
752 0556 BC01 movw r22,r24 |
753 0558 CD01 movw r24,r26 |
754 055a 0E94 0000 call __divmodsi4 |
755 055e 3093 0000 sts (ServoNickValue)+1,r19 |
756 0562 2093 0000 sts ServoNickValue,r18 |
757 0566 00C0 rjmp .L62 |
758 .L61: |
759 0568 D093 0000 sts (ServoNickValue)+1,r29 |
760 056c C093 0000 sts ServoNickValue,r28 |
761 .L62: |
762 0570 8091 0000 lds r24,EE_Parameter+47 |
763 0574 282F mov r18,r24 |
764 0576 3327 clr r19 |
765 0578 220F lsl r18 |
766 057a 331F rol r19 |
767 057c 220F lsl r18 |
768 057e 331F rol r19 |
769 0580 8091 0000 lds r24,ServoNickValue |
770 0584 9091 0000 lds r25,(ServoNickValue)+1 |
771 0588 8217 cp r24,r18 |
772 058a 9307 cpc r25,r19 |
773 058c 04F0 brlt .L81 |
774 058e 8091 0000 lds r24,EE_Parameter+48 |
775 0592 282F mov r18,r24 |
776 0594 3327 clr r19 |
777 0596 220F lsl r18 |
778 0598 331F rol r19 |
779 059a 220F lsl r18 |
780 059c 331F rol r19 |
781 059e 8091 0000 lds r24,ServoNickValue |
782 05a2 9091 0000 lds r25,(ServoNickValue)+1 |
783 05a6 2817 cp r18,r24 |
784 05a8 3907 cpc r19,r25 |
785 05aa 04F4 brge .L64 |
786 .L81: |
787 05ac 3093 0000 sts (ServoNickValue)+1,r19 |
788 05b0 2093 0000 sts ServoNickValue,r18 |
789 .L64: |
790 05b4 8091 0000 lds r24,PlatinenVersion |
791 05b8 8431 cpi r24,lo8(20) |
792 05ba 00F4 brsh .+2 |
793 05bc 00C0 rjmp .L79 |
794 05be 8091 0000 lds r24,CalculateServoSignals |
795 05c2 8F5F subi r24,lo8(-(1)) |
796 05c4 8093 0000 sts CalculateServoSignals,r24 |
797 05c8 00C0 rjmp .L50 |
798 .L51: |
799 05ca 2091 0000 lds r18,IntegralRoll |
800 05ce 3091 0000 lds r19,(IntegralRoll)+1 |
801 05d2 4091 0000 lds r20,(IntegralRoll)+2 |
802 05d6 5091 0000 lds r21,(IntegralRoll)+3 |
803 05da BD01 movw r22,r26 |
804 05dc CE01 movw r24,r28 |
805 05de 0E94 0000 call __mulsi3 |
806 05e2 97FF sbrs r25,7 |
807 05e4 00C0 rjmp .L69 |
808 05e6 6158 subi r22,lo8(-(127)) |
809 05e8 7F4F sbci r23,hi8(-(127)) |
810 05ea 8F4F sbci r24,hlo8(-(127)) |
811 05ec 9F4F sbci r25,hhi8(-(127)) |
812 .L69: |
813 05ee 7B01 movw r14,r22 |
814 05f0 8C01 movw r16,r24 |
815 05f2 87E0 ldi r24,7 |
816 05f4 1595 1: asr r17 |
817 05f6 0795 ror r16 |
818 05f8 F794 ror r15 |
819 05fa E794 ror r14 |
820 05fc 8A95 dec r24 |
821 05fe 01F4 brne 1b |
822 0600 8D2D mov r24,r13 |
823 0602 9927 clr r25 |
824 0604 87FD sbrc r24,7 |
825 0606 9095 com r25 |
826 0608 A92F mov r26,r25 |
827 060a B92F mov r27,r25 |
828 060c 2091 0000 lds r18,IntegralNick |
829 0610 3091 0000 lds r19,(IntegralNick)+1 |
830 0614 4091 0000 lds r20,(IntegralNick)+2 |
831 0618 5091 0000 lds r21,(IntegralNick)+3 |
832 061c BC01 movw r22,r24 |
833 061e CD01 movw r24,r26 |
834 0620 0E94 0000 call __mulsi3 |
835 0624 9B01 movw r18,r22 |
836 0626 AC01 movw r20,r24 |
837 0628 97FF sbrs r25,7 |
838 062a 00C0 rjmp .L70 |
839 062c 2158 subi r18,lo8(-(127)) |
840 062e 3F4F sbci r19,hi8(-(127)) |
841 0630 4F4F sbci r20,hlo8(-(127)) |
842 0632 5F4F sbci r21,hhi8(-(127)) |
843 .L70: |
844 0634 C7E0 ldi r28,7 |
845 0636 5595 1: asr r21 |
846 0638 4795 ror r20 |
847 063a 3795 ror r19 |
848 063c 2795 ror r18 |
849 063e CA95 dec r28 |
850 0640 01F4 brne 1b |
851 0642 2E0D add r18,r14 |
852 0644 3F1D adc r19,r15 |
853 0646 401F adc r20,r16 |
854 0648 511F adc r21,r17 |
855 064a 8091 0000 lds r24,EE_Parameter+50 |
856 064e 9927 clr r25 |
857 0650 AA27 clr r26 |
858 0652 BB27 clr r27 |
859 0654 BC01 movw r22,r24 |
860 0656 CD01 movw r24,r26 |
861 0658 0E94 0000 call __mulsi3 |
862 065c 97FF sbrs r25,7 |
863 065e 00C0 rjmp .L71 |
864 0660 6150 subi r22,lo8(-(511)) |
865 0662 7E4F sbci r23,hi8(-(511)) |
866 0664 8F4F sbci r24,hlo8(-(511)) |
867 0666 9F4F sbci r25,hhi8(-(511)) |
868 .L71: |
869 0668 FC01 movw r30,r24 |
870 066a EB01 movw r28,r22 |
871 066c B9E0 ldi r27,9 |
872 066e F595 1: asr r31 |
873 0670 E795 ror r30 |
874 0672 D795 ror r29 |
875 0674 C795 ror r28 |
876 0676 BA95 dec r27 |
877 0678 01F4 brne 1b |
878 067a 8091 0000 lds r24,Parameter_ServoRollControl |
879 067e 9927 clr r25 |
880 0680 A6E0 ldi r26,6 |
881 0682 880F 1: lsl r24 |
882 0684 991F rol r25 |
883 0686 AA95 dec r26 |
884 0688 01F4 brne 1b |
885 068a 4091 0000 lds r20,ServoRollOffset |
886 068e 5091 0000 lds r21,(ServoRollOffset)+1 |
887 0692 2091 0000 lds r18,EE_Parameter+54 |
888 0696 3327 clr r19 |
889 0698 841B sub r24,r20 |
890 069a 950B sbc r25,r21 |
891 069c B901 movw r22,r18 |
892 069e 0E94 0000 call __divmodhi4 |
893 06a2 8091 0000 lds r24,ServoRollOffset |
894 06a6 9091 0000 lds r25,(ServoRollOffset)+1 |
895 06aa 860F add r24,r22 |
896 06ac 971F adc r25,r23 |
897 06ae 9093 0000 sts (ServoRollOffset)+1,r25 |
898 06b2 8093 0000 sts ServoRollOffset,r24 |
899 06b6 8091 0000 lds r24,EE_Parameter+109 |
900 06ba 81FF sbrs r24,1 |
901 06bc 00C0 rjmp .L72 |
902 06be 8091 0000 lds r24,ServoRollOffset |
903 06c2 9091 0000 lds r25,(ServoRollOffset)+1 |
904 06c6 97FD sbrc r25,7 |
905 06c8 0F96 adiw r24,15 |
906 .L73: |
907 06ca 74E0 ldi r23,4 |
908 06cc 9595 1: asr r25 |
909 06ce 8795 ror r24 |
910 06d0 7A95 dec r23 |
911 06d2 01F4 brne 1b |
912 06d4 AA27 clr r26 |
913 06d6 97FD sbrc r25,7 |
914 06d8 A095 com r26 |
915 06da BA2F mov r27,r26 |
916 06dc C80F add r28,r24 |
917 06de D91F adc r29,r25 |
918 06e0 EA1F adc r30,r26 |
919 06e2 FB1F adc r31,r27 |
920 06e4 00C0 rjmp .L74 |
921 .L72: |
922 06e6 8091 0000 lds r24,ServoRollOffset |
923 06ea 9091 0000 lds r25,(ServoRollOffset)+1 |
924 06ee 97FD sbrc r25,7 |
925 06f0 0F96 adiw r24,15 |
926 .L75: |
927 06f2 64E0 ldi r22,4 |
928 06f4 9595 1: asr r25 |
929 06f6 8795 ror r24 |
930 06f8 6A95 dec r22 |
931 06fa 01F4 brne 1b |
932 06fc AA27 clr r26 |
933 06fe 97FD sbrc r25,7 |
934 0700 A095 com r26 |
935 0702 BA2F mov r27,r26 |
936 0704 8C1B sub r24,r28 |
937 0706 9D0B sbc r25,r29 |
938 0708 AE0B sbc r26,r30 |
939 070a BF0B sbc r27,r31 |
940 070c FD01 movw r30,r26 |
941 070e EC01 movw r28,r24 |
942 .L74: |
943 0710 2091 0000 lds r18,EE_Parameter+107 |
944 0714 2223 tst r18 |
945 0716 01F0 breq .L76 |
946 0718 3327 clr r19 |
947 071a 8091 0000 lds r24,ServoRollValue |
948 071e 9091 0000 lds r25,(ServoRollValue)+1 |
949 0722 829F mul r24,r18 |
950 0724 A001 movw r20,r0 |
951 0726 839F mul r24,r19 |
952 0728 500D add r21,r0 |
953 072a 929F mul r25,r18 |
954 072c 500D add r21,r0 |
955 072e 1124 clr r1 |
956 0730 CA01 movw r24,r20 |
957 0732 AA27 clr r26 |
958 0734 97FD sbrc r25,7 |
959 0736 A095 com r26 |
960 0738 BA2F mov r27,r26 |
961 073a 8C0F add r24,r28 |
962 073c 9D1F adc r25,r29 |
963 073e AE1F adc r26,r30 |
964 0740 BF1F adc r27,r31 |
965 0742 2F5F subi r18,lo8(-(1)) |
966 0744 3F4F sbci r19,hi8(-(1)) |
967 0746 4427 clr r20 |
968 0748 37FD sbrc r19,7 |
969 074a 4095 com r20 |
970 074c 542F mov r21,r20 |
971 074e BC01 movw r22,r24 |
972 0750 CD01 movw r24,r26 |
973 0752 0E94 0000 call __divmodsi4 |
974 0756 3093 0000 sts (ServoRollValue)+1,r19 |
975 075a 2093 0000 sts ServoRollValue,r18 |
976 075e 00C0 rjmp .L77 |
977 .L76: |
978 0760 D093 0000 sts (ServoRollValue)+1,r29 |
979 0764 C093 0000 sts ServoRollValue,r28 |
980 .L77: |
981 0768 8091 0000 lds r24,EE_Parameter+51 |
982 076c 282F mov r18,r24 |
983 076e 3327 clr r19 |
984 0770 220F lsl r18 |
985 0772 331F rol r19 |
986 0774 220F lsl r18 |
987 0776 331F rol r19 |
988 0778 8091 0000 lds r24,ServoRollValue |
989 077c 9091 0000 lds r25,(ServoRollValue)+1 |
990 0780 8217 cp r24,r18 |
991 0782 9307 cpc r25,r19 |
992 0784 04F0 brlt .L82 |
993 0786 8091 0000 lds r24,EE_Parameter+52 |
994 078a 282F mov r18,r24 |
995 078c 3327 clr r19 |
996 078e 220F lsl r18 |
997 0790 331F rol r19 |
998 0792 220F lsl r18 |
999 0794 331F rol r19 |
1000 0796 8091 0000 lds r24,ServoRollValue |
1001 079a 9091 0000 lds r25,(ServoRollValue)+1 |
1002 079e 2817 cp r18,r24 |
1003 07a0 3907 cpc r19,r25 |
1004 07a2 04F4 brge .L79 |
1005 .L82: |
1006 07a4 3093 0000 sts (ServoRollValue)+1,r19 |
1007 07a8 2093 0000 sts ServoRollValue,r18 |
1008 .L79: |
1009 07ac 1092 0000 sts CalculateServoSignals,__zero_reg__ |
1010 .L50: |
1011 /* epilogue: frame size=0 */ |
1012 07b0 E7E0 ldi r30,7 |
1013 07b2 CDB7 in r28,__SP_L__ |
1014 07b4 DEB7 in r29,__SP_H__ |
1015 07b6 0C94 0000 jmp __epilogue_restores__+22 |
1016 /* epilogue end (size=5) */ |
1017 /* function CalculateServo size 556 (545) */ |
1019 .lcomm PulseOutput.0,1 |
1020 .lcomm ServoFrameTime.1,2 |
1021 .lcomm ServoIndex.2,1 |
1022 .global __vector_9 |
1024 __vector_9: |
1025 /* prologue: frame size=0 */ |
1026 07ba 1F92 push __zero_reg__ |
1027 07bc 0F92 push __tmp_reg__ |
1028 07be 0FB6 in __tmp_reg__,__SREG__ |
1029 07c0 0F92 push __tmp_reg__ |
1030 07c2 1124 clr __zero_reg__ |
1031 07c4 2F93 push r18 |
1032 07c6 3F93 push r19 |
1033 07c8 4F93 push r20 |
1034 07ca 5F93 push r21 |
1035 07cc 8F93 push r24 |
1036 07ce 9F93 push r25 |
1037 07d0 EF93 push r30 |
1038 07d2 FF93 push r31 |
1039 /* prologue end (size=13) */ |
1040 07d4 8091 0000 lds r24,PlatinenVersion |
1041 07d8 9091 0000 lds r25,PulseOutput.0 |
1042 07dc 8431 cpi r24,lo8(20) |
1043 07de 00F0 brlo .+2 |
1044 07e0 00C0 rjmp .L84 |
1045 07e2 9923 tst r25 |
1046 07e4 01F0 breq .+2 |
1047 07e6 00C0 rjmp .L91 |
1048 07e8 8091 B000 lds r24,176 |
1049 07ec 86FF sbrs r24,6 |
1050 07ee 00C0 rjmp .L86 |
1051 07f0 8091 B000 lds r24,176 |
1052 07f4 8F7B andi r24,lo8(-65) |
1053 07f6 8093 B000 sts 176,r24 |
1054 07fa 8091 0000 lds r24,ServoNickValue |
1055 07fe 9091 0000 lds r25,(ServoNickValue)+1 |
1056 0802 8755 subi r24,lo8(-(425)) |
1057 0804 9E4F sbci r25,hi8(-(425)) |
1058 0806 9093 0000 sts (RemainingPulse)+1,r25 |
1059 080a 8093 0000 sts RemainingPulse,r24 |
1060 080e 25E0 ldi r18,hi8(1501) |
1061 0810 8D3D cpi r24,lo8(1501) |
1062 0812 9207 cpc r25,r18 |
1063 0814 00F0 brlo .L87 |
1064 0816 8CED ldi r24,lo8(1500) |
1065 0818 95E0 ldi r25,hi8(1500) |
1066 081a 00C0 rjmp .L121 |
1067 .L87: |
1068 081c 8757 subi r24,lo8(375) |
1069 081e 9140 sbci r25,hi8(375) |
1070 0820 00F4 brsh .L88 |
1071 0822 87E7 ldi r24,lo8(375) |
1072 0824 91E0 ldi r25,hi8(375) |
1073 .L121: |
1074 0826 9093 0000 sts (RemainingPulse)+1,r25 |
1075 082a 8093 0000 sts RemainingPulse,r24 |
1076 .L88: |
1077 082e 8091 0000 lds r24,RemainingPulse |
1078 0832 9091 0000 lds r25,(RemainingPulse)+1 |
1079 0836 00C0 rjmp .L126 |
1080 .L86: |
1081 0838 8091 B000 lds r24,176 |
1082 083c 8064 ori r24,lo8(64) |
1083 083e 8093 B000 sts 176,r24 |
1084 0842 8091 0000 lds r24,EE_Parameter+53 |
1085 0846 282F mov r18,r24 |
1086 0848 3327 clr r19 |
1087 084a 8DED ldi r24,lo8(1757) |
1088 084c 96E0 ldi r25,hi8(1757) |
1089 084e 289F mul r18,r24 |
1090 0850 A001 movw r20,r0 |
1091 0852 299F mul r18,r25 |
1092 0854 500D add r21,r0 |
1093 0856 389F mul r19,r24 |
1094 0858 500D add r21,r0 |
1095 085a 1124 clr r1 |
1096 085c CA01 movw r24,r20 |
1097 085e 2091 0000 lds r18,ServoFrameTime.1 |
1098 0862 3091 0000 lds r19,(ServoFrameTime.1)+1 |
1099 0866 821B sub r24,r18 |
1100 0868 930B sbc r25,r19 |
1101 086a 9093 0000 sts (RemainingPulse)+1,r25 |
1102 086e 8093 0000 sts RemainingPulse,r24 |
1103 0872 81E0 ldi r24,lo8(1) |
1104 0874 8093 0000 sts CalculateServoSignals,r24 |
1105 0878 00C0 rjmp .L109 |
1106 .L84: |
1107 087a 9923 tst r25 |
1108 087c 01F0 breq .+2 |
1109 087e 00C0 rjmp .L91 |
1110 0880 8091 B000 lds r24,176 |
1111 0884 86FF sbrs r24,6 |
1112 0886 00C0 rjmp .L93 |
1113 0888 8091 B000 lds r24,176 |
1114 088c 8F7B andi r24,lo8(-65) |
1115 088e 8093 B000 sts 176,r24 |
1116 0892 E091 0000 lds r30,ServoIndex.2 |
1117 0896 2091 0000 lds r18,EE_Parameter+53 |
1118 089a EE23 tst r30 |
1119 089c 01F4 brne .L94 |
1120 089e 3327 clr r19 |
1121 08a0 8DED ldi r24,lo8(1757) |
1122 08a2 96E0 ldi r25,hi8(1757) |
1123 08a4 289F mul r18,r24 |
1124 08a6 A001 movw r20,r0 |
1125 08a8 299F mul r18,r25 |
1126 08aa 500D add r21,r0 |
1127 08ac 389F mul r19,r24 |
1128 08ae 500D add r21,r0 |
1129 08b0 1124 clr r1 |
1130 08b2 CA01 movw r24,r20 |
1131 08b4 2091 0000 lds r18,ServoFrameTime.1 |
1132 08b8 3091 0000 lds r19,(ServoFrameTime.1)+1 |
1133 08bc 821B sub r24,r18 |
1134 08be 930B sbc r25,r19 |
1135 08c0 9093 0000 sts (RemainingPulse)+1,r25 |
1136 08c4 8093 0000 sts RemainingPulse,r24 |
1137 08c8 1092 0000 sts (ServoFrameTime.1)+1,__zero_reg__ |
1138 08cc 1092 0000 sts ServoFrameTime.1,__zero_reg__ |
1139 08d0 469A sbi 40-0x20,6 |
1140 08d2 00C0 rjmp .L109 |
1141 .L94: |
1142 08d4 2E17 cp r18,r30 |
1143 08d6 00F4 brsh .L96 |
1144 08d8 8AE0 ldi r24,lo8(10) |
1145 08da 90E0 ldi r25,hi8(10) |
1146 08dc 9093 0000 sts (RemainingPulse)+1,r25 |
1147 08e0 8093 0000 sts RemainingPulse,r24 |
1148 08e4 00C0 rjmp .L109 |
1149 .L96: |
1150 08e6 89EA ldi r24,lo8(937) |
1151 08e8 93E0 ldi r25,hi8(937) |
1152 08ea 9093 0000 sts (RemainingPulse)+1,r25 |
1153 08ee 8093 0000 sts RemainingPulse,r24 |
1154 08f2 8E2F mov r24,r30 |
1155 08f4 9927 clr r25 |
1156 08f6 8330 cpi r24,3 |
1157 08f8 9105 cpc r25,__zero_reg__ |
1158 08fa 01F0 breq .L101 |
1159 08fc 8430 cpi r24,4 |
1160 08fe 9105 cpc r25,__zero_reg__ |
1161 0900 04F4 brge .L105 |
1162 0902 8130 cpi r24,1 |
1163 0904 9105 cpc r25,__zero_reg__ |
1164 0906 01F0 breq .L99 |
1165 0908 0297 sbiw r24,2 |
1166 090a 01F0 breq .L100 |
1167 090c 00C0 rjmp .L104 |
1168 .L105: |
1169 090e 8430 cpi r24,4 |
1170 0910 9105 cpc r25,__zero_reg__ |
1171 0912 01F0 breq .L102 |
1172 0914 0597 sbiw r24,5 |
1173 0916 01F0 breq .L103 |
1174 0918 00C0 rjmp .L104 |
1175 .L99: |
1176 091a 8091 0000 lds r24,ServoNickValue |
1177 091e 9091 0000 lds r25,(ServoNickValue)+1 |
1178 0922 00C0 rjmp .L125 |
1179 .L100: |
1180 0924 8091 0000 lds r24,ServoRollValue |
1181 0928 9091 0000 lds r25,(ServoRollValue)+1 |
1182 .L125: |
1183 092c 8755 subi r24,lo8(-(425)) |
1184 092e 9E4F sbci r25,hi8(-(425)) |
1185 0930 00C0 rjmp .L122 |
1186 .L101: |
1187 0932 8091 0000 lds r24,Parameter_Servo3 |
1188 0936 00C0 rjmp .L127 |
1189 .L102: |
1190 0938 8091 0000 lds r24,Parameter_Servo4 |
1191 093c 00C0 rjmp .L127 |
1192 .L103: |
1193 093e 8091 0000 lds r24,Parameter_Servo5 |
1194 .L127: |
1195 0942 9927 clr r25 |
1196 0944 880F lsl r24 |
1197 0946 991F rol r25 |
1198 0948 880F lsl r24 |
1199 094a 991F rol r25 |
1200 094c 00C0 rjmp .L125 |
1201 .L104: |
1202 094e FF27 clr r31 |
1203 0950 EE0F add r30,r30 |
1204 0952 FF1F adc r31,r31 |
1205 0954 E050 subi r30,lo8(-(PPM_in)) |
1206 0956 F040 sbci r31,hi8(-(PPM_in)) |
1207 0958 2081 ld r18,Z |
1208 095a 3181 ldd r19,Z+1 |
1209 095c 220F add r18,r18 |
1210 095e 331F adc r19,r19 |
1211 0960 8091 0000 lds r24,RemainingPulse |
1212 0964 9091 0000 lds r25,(RemainingPulse)+1 |
1213 0968 820F add r24,r18 |
1214 096a 931F adc r25,r19 |
1215 .L122: |
1216 096c 9093 0000 sts (RemainingPulse)+1,r25 |
1217 0970 8093 0000 sts RemainingPulse,r24 |
1218 0974 8091 0000 lds r24,RemainingPulse |
1219 0978 9091 0000 lds r25,(RemainingPulse)+1 |
1220 097c 55E0 ldi r21,hi8(1501) |
1221 097e 8D3D cpi r24,lo8(1501) |
1222 0980 9507 cpc r25,r21 |
1223 0982 00F0 brlo .L106 |
1224 0984 8CED ldi r24,lo8(1500) |
1225 0986 95E0 ldi r25,hi8(1500) |
1226 0988 00C0 rjmp .L123 |
1227 .L106: |
1228 098a 8757 subi r24,lo8(375) |
1229 098c 9140 sbci r25,hi8(375) |
1230 098e 00F4 brsh .L107 |
1231 0990 87E7 ldi r24,lo8(375) |
1232 0992 91E0 ldi r25,hi8(375) |
1233 .L123: |
1234 0994 9093 0000 sts (RemainingPulse)+1,r25 |
1235 0998 8093 0000 sts RemainingPulse,r24 |
1236 .L107: |
1237 099c 2091 0000 lds r18,RemainingPulse |
1238 09a0 3091 0000 lds r19,(RemainingPulse)+1 |
1239 09a4 2C5B subi r18,lo8(-(-188)) |
1240 09a6 3040 sbci r19,hi8(-(-188)) |
1241 09a8 3093 0000 sts (RemainingPulse)+1,r19 |
1242 09ac 2093 0000 sts RemainingPulse,r18 |
1243 09b0 8091 0000 lds r24,ServoFrameTime.1 |
1244 09b4 9091 0000 lds r25,(ServoFrameTime.1)+1 |
1245 09b8 820F add r24,r18 |
1246 09ba 931F adc r25,r19 |
1247 .L126: |
1248 09bc 9093 0000 sts (ServoFrameTime.1)+1,r25 |
1249 09c0 8093 0000 sts ServoFrameTime.1,r24 |
1250 09c4 00C0 rjmp .L109 |
1251 .L93: |
1252 09c6 8091 B000 lds r24,176 |
1253 09ca 8064 ori r24,lo8(64) |
1254 09cc 8093 B000 sts 176,r24 |
1255 09d0 8CEB ldi r24,lo8(188) |
1256 09d2 90E0 ldi r25,hi8(188) |
1257 09d4 9093 0000 sts (RemainingPulse)+1,r25 |
1258 09d8 8093 0000 sts RemainingPulse,r24 |
1259 09dc 8091 0000 lds r24,ServoFrameTime.1 |
1260 09e0 9091 0000 lds r25,(ServoFrameTime.1)+1 |
1261 09e4 8454 subi r24,lo8(-(188)) |
1262 09e6 9F4F sbci r25,hi8(-(188)) |
1263 09e8 9093 0000 sts (ServoFrameTime.1)+1,r25 |
1264 09ec 8093 0000 sts ServoFrameTime.1,r24 |
1265 09f0 8091 0000 lds r24,ServoActive |
1266 09f4 8823 tst r24 |
1267 09f6 01F0 breq .L112 |
1268 09f8 8091 0000 lds r24,SenderOkay |
1269 09fc 8823 tst r24 |
1270 09fe 01F4 brne .L111 |
1271 .L112: |
1272 0a00 8091 0000 lds r24,ServoActive |
1273 0a04 8230 cpi r24,lo8(2) |
1274 0a06 01F4 brne .L110 |
1275 .L111: |
1276 0a08 4698 cbi 40-0x20,6 |
1277 0a0a 00C0 rjmp .L113 |
1278 .L110: |
1279 0a0c 469A sbi 40-0x20,6 |
1280 .L113: |
1281 0a0e 8091 0000 lds r24,ServoIndex.2 |
1282 0a12 8F5F subi r24,lo8(-(1)) |
1283 0a14 8093 0000 sts ServoIndex.2,r24 |
1284 0a18 282F mov r18,r24 |
1285 0a1a 3327 clr r19 |
1286 0a1c 8091 0000 lds r24,EE_Parameter+53 |
1287 0a20 9927 clr r25 |
1288 0a22 0196 adiw r24,1 |
1289 0a24 8217 cp r24,r18 |
1290 0a26 9307 cpc r25,r19 |
1291 0a28 04F4 brge .L109 |
1292 0a2a 81E0 ldi r24,lo8(1) |
1293 0a2c 8093 0000 sts CalculateServoSignals,r24 |
1294 0a30 1092 0000 sts ServoIndex.2,__zero_reg__ |
1295 .L109: |
1296 0a34 81E0 ldi r24,lo8(1) |
1297 0a36 8093 0000 sts PulseOutput.0,r24 |
1298 .L91: |
1299 0a3a 2091 0000 lds r18,RemainingPulse |
1300 0a3e 3091 0000 lds r19,(RemainingPulse)+1 |
1301 0a42 41E0 ldi r20,hi8(383) |
1302 0a44 2F37 cpi r18,lo8(383) |
1303 0a46 3407 cpc r19,r20 |
1304 0a48 00F0 brlo .L115 |
1305 0a4a 8FEF ldi r24,lo8(-1) |
1306 0a4c 8093 B300 sts 179,r24 |
1307 0a50 2F5F subi r18,lo8(-(-255)) |
1308 0a52 3040 sbci r19,hi8(-(-255)) |
1309 0a54 00C0 rjmp .L124 |
1310 .L115: |
1311 0a56 2F3F cpi r18,255 |
1312 0a58 3105 cpc r19,__zero_reg__ |
1313 0a5a 01F0 breq .L117 |
1314 0a5c 00F0 brlo .L117 |
1315 0a5e A901 movw r20,r18 |
1316 0a60 4F5F subi r20,lo8(-(-255)) |
1317 0a62 5040 sbci r21,hi8(-(-255)) |
1318 0a64 4F37 cpi r20,127 |
1319 0a66 5105 cpc r21,__zero_reg__ |
1320 0a68 00F4 brsh .L118 |
1321 0a6a 80E8 ldi r24,lo8(-128) |
1322 0a6c 8093 B300 sts 179,r24 |
1323 0a70 2058 subi r18,lo8(-(-128)) |
1324 0a72 3040 sbci r19,hi8(-(-128)) |
1325 .L124: |
1326 0a74 3093 0000 sts (RemainingPulse)+1,r19 |
1327 0a78 2093 0000 sts RemainingPulse,r18 |
1328 0a7c 00C0 rjmp .L83 |
1329 .L118: |
1330 0a7e 8FEF ldi r24,lo8(-1) |
1331 0a80 8093 B300 sts 179,r24 |
1332 0a84 5093 0000 sts (RemainingPulse)+1,r21 |
1333 0a88 4093 0000 sts RemainingPulse,r20 |
1334 0a8c 00C0 rjmp .L83 |
1335 .L117: |
1336 0a8e 8091 0000 lds r24,RemainingPulse |
1337 0a92 8093 B300 sts 179,r24 |
1338 0a96 1092 0000 sts (RemainingPulse)+1,__zero_reg__ |
1339 0a9a 1092 0000 sts RemainingPulse,__zero_reg__ |
1340 0a9e 1092 0000 sts PulseOutput.0,__zero_reg__ |
1341 .L83: |
1342 /* epilogue: frame size=0 */ |
1343 0aa2 FF91 pop r31 |
1344 0aa4 EF91 pop r30 |
1345 0aa6 9F91 pop r25 |
1346 0aa8 8F91 pop r24 |
1347 0aaa 5F91 pop r21 |
1348 0aac 4F91 pop r20 |
1349 0aae 3F91 pop r19 |
1350 0ab0 2F91 pop r18 |
1351 0ab2 0F90 pop __tmp_reg__ |
1352 0ab4 0FBE out __SREG__,__tmp_reg__ |
1353 0ab6 0F90 pop __tmp_reg__ |
1354 0ab8 1F90 pop __zero_reg__ |
1355 0aba 1895 reti |
1356 /* epilogue end (size=13) */ |
1357 /* function __vector_9 size 389 (363) */ |
1359 .comm tim_main,2,1 |
1360 /* File "timer0.c": code 1388 = 0x056c (1305), prologues 39, epilogues 44 */ |
DEFINED SYMBOLS |
*ABS*:00000000 timer0.c |
C:\Temp/ccwra2gD.s:3 *ABS*:0000003f __SREG__ |
C:\Temp/ccwra2gD.s:4 *ABS*:0000003e __SP_H__ |
C:\Temp/ccwra2gD.s:5 *ABS*:0000003d __SP_L__ |
C:\Temp/ccwra2gD.s:6 *ABS*:00000000 __tmp_reg__ |
C:\Temp/ccwra2gD.s:7 *ABS*:00000001 __zero_reg__ |
C:\Temp/ccwra2gD.s:15 .bss:00000000 ServoRollValue |
C:\Temp/ccwra2gD.s:21 .bss:00000002 ServoNickValue |
C:\Temp/ccwra2gD.s:27 .data:00000000 NickServoValue |
C:\Temp/ccwra2gD.s:32 .data:00000002 BeepMuster |
C:\Temp/ccwra2gD.s:37 .data:00000004 ServoRollOffset |
C:\Temp/ccwra2gD.s:42 .data:00000006 ServoNickOffset |
C:\Temp/ccwra2gD.s:49 .bss:00000004 RemainingPulse |
C:\Temp/ccwra2gD.s:55 .data:00000008 CalculateServoSignals |
C:\Temp/ccwra2gD.s:62 .bss:00000006 ServoActive |
C:\Temp/ccwra2gD.s:68 .bss:00000007 SendSPI |
C:\Temp/ccwra2gD.s:74 .bss:00000008 beeptime |
C:\Temp/ccwra2gD.s:80 .bss:0000000a cntKompass |
C:\Temp/ccwra2gD.s:86 .bss:0000000c UpdateMotor |
C:\Temp/ccwra2gD.s:92 .bss:0000000d CountMilliseconds |
C:\Temp/ccwra2gD.s:97 .data:00000009 cnt_1ms.3 |
.bss:0000000f cnt.4 |
C:\Temp/ccwra2gD.s:99 .bss:00000010 compass_active.5 |
C:\Temp/ccwra2gD.s:104 .text:00000000 __vector_18 |
C:\Temp/ccwra2gD.s:309 .text:000001e6 SetDelay |
C:\Temp/ccwra2gD.s:325 .text:000001f8 CheckDelay |
C:\Temp/ccwra2gD.s:344 .text:00000210 Delay_ms |
C:\Temp/ccwra2gD.s:365 .text:0000022a Delay_ms_Mess |
C:\Temp/ccwra2gD.s:395 .text:0000025a TIMER2_Init |
C:\Temp/ccwra2gD.s:437 .text:000002b8 Timer_Init |
*COM*:00000002 tim_main |
C:\Temp/ccwra2gD.s:464 .text:000002e6 CalcNickServoValue |
C:\Temp/ccwra2gD.s:530 .text:0000036a CalculateServo |
C:\Temp/ccwra2gD.s:100 .bss:00000011 PulseOutput.0 |
C:\Temp/ccwra2gD.s:1019 .bss:00000012 ServoFrameTime.1 |
C:\Temp/ccwra2gD.s:1020 .bss:00000014 ServoIndex.2 |
C:\Temp/ccwra2gD.s:1024 .text:000007ba __vector_9 |
|
UNDEFINED SYMBOLS |
__do_copy_data |
__do_clear_bss |
SpektrumTimer |
PlatinenVersion |
NaviDataOkay |
Parameter_GlobalConfig |
__udivmodhi4 |
KompassValue |
AdReady |
EE_Parameter |
Parameter_ServoNickControl |
__prologue_saves__ |
sintab |
IntegralNick |
__mulsi3 |
IntegralRoll |
POI_KameraNick |
__divmodhi4 |
__divmodsi4 |
Parameter_ServoRollControl |
__epilogue_restores__ |
Parameter_Servo3 |
Parameter_Servo4 |
Parameter_Servo5 |
PPM_in |
SenderOkay |