0,0 → 1,10174 |
1 .file "fc.c" |
2 __SREG__ = 0x3f |
3 __SP_H__ = 0x3e |
4 __SP_L__ = 0x3d |
5 __CCP__ = 0x34 |
6 __tmp_reg__ = 0 |
7 __zero_reg__ = 1 |
8 .text |
9 .global MotorSmoothing |
11 MotorSmoothing: |
12 /* prologue: function */ |
13 /* frame size = 0 */ |
14 0000 9B01 movw r18,r22 |
15 0002 6817 cp r22,r24 |
16 0004 7907 cpc r23,r25 |
17 0006 04F0 brlt .L7 |
18 0008 BC01 movw r22,r24 |
19 000a 660F lsl r22 |
20 000c 771F rol r23 |
21 000e 621B sub r22,r18 |
22 0010 730B sbc r23,r19 |
23 0012 CB01 movw r24,r22 |
24 /* epilogue start */ |
25 0014 0895 ret |
26 .L7: |
27 0016 680F add r22,r24 |
28 0018 791F adc r23,r25 |
29 001a 77FD sbrc r23,7 |
30 001c 00C0 rjmp .L8 |
31 .L3: |
32 001e 7595 asr r23 |
33 0020 6795 ror r22 |
34 0022 CB01 movw r24,r22 |
35 0024 0895 ret |
36 .L8: |
37 0026 6F5F subi r22,lo8(-(1)) |
38 0028 7F4F sbci r23,hi8(-(1)) |
39 002a 00C0 rjmp .L3 |
41 .global Mean |
43 Mean: |
44 002c 2F92 push r2 |
45 002e 3F92 push r3 |
46 0030 4F92 push r4 |
47 0032 5F92 push r5 |
48 0034 6F92 push r6 |
49 0036 7F92 push r7 |
50 0038 8F92 push r8 |
51 003a 9F92 push r9 |
52 003c AF92 push r10 |
53 003e BF92 push r11 |
54 0040 CF92 push r12 |
55 0042 DF92 push r13 |
56 0044 EF92 push r14 |
57 0046 FF92 push r15 |
58 0048 0F93 push r16 |
59 004a 1F93 push r17 |
60 004c DF93 push r29 |
61 004e CF93 push r28 |
62 0050 CDB7 in r28,__SP_L__ |
63 0052 DEB7 in r29,__SP_H__ |
64 0054 AD97 sbiw r28,45 |
65 0056 0FB6 in __tmp_reg__,__SREG__ |
66 0058 F894 cli |
67 005a DEBF out __SP_H__,r29 |
68 005c 0FBE out __SREG__,__tmp_reg__ |
69 005e CDBF out __SP_L__,r28 |
70 /* prologue: function */ |
71 /* frame size = 45 */ |
72 0060 8091 0000 lds r24,HiResGyroNick |
73 0064 9091 0000 lds r25,(HiResGyroNick)+1 |
74 0068 97FD sbrc r25,7 |
75 006a 00C0 rjmp .L67 |
76 .L10: |
77 006c 9595 asr r25 |
78 006e 8795 ror r24 |
79 0070 9595 asr r25 |
80 0072 8795 ror r24 |
81 0074 9595 asr r25 |
82 0076 8795 ror r24 |
83 0078 9093 0000 sts (GyroNick)+1,r25 |
84 007c 8093 0000 sts GyroNick,r24 |
85 0080 2091 0000 lds r18,FilterHiResGyroNick |
86 0084 3091 0000 lds r19,(FilterHiResGyroNick)+1 |
87 0088 37FD sbrc r19,7 |
88 008a 00C0 rjmp .L68 |
89 .L11: |
90 008c C901 movw r24,r18 |
91 008e 9595 asr r25 |
92 0090 8795 ror r24 |
93 0092 9595 asr r25 |
94 0094 8795 ror r24 |
95 0096 9595 asr r25 |
96 0098 8795 ror r24 |
97 009a 9D8F std Y+29,r25 |
98 009c 8C8F std Y+28,r24 |
99 009e 8091 0000 lds r24,AdValueGyroNick |
100 00a2 9091 0000 lds r25,(AdValueGyroNick)+1 |
101 00a6 0F97 sbiw r24,15 |
102 00a8 04F4 brge .L12 |
103 00aa 88E1 ldi r24,lo8(-1000) |
104 00ac 9CEF ldi r25,hi8(-1000) |
105 00ae 9093 0000 sts (GyroNick)+1,r25 |
106 00b2 8093 0000 sts GyroNick,r24 |
107 .L12: |
108 00b6 8091 0000 lds r24,AdValueGyroNick |
109 00ba 9091 0000 lds r25,(AdValueGyroNick)+1 |
110 00be 0797 sbiw r24,7 |
111 00c0 04F4 brge .L13 |
112 00c2 80E3 ldi r24,lo8(-2000) |
113 00c4 98EF ldi r25,hi8(-2000) |
114 00c6 9093 0000 sts (GyroNick)+1,r25 |
115 00ca 8093 0000 sts GyroNick,r24 |
116 .L13: |
117 00ce 4091 0000 lds r20,BoardRelease |
118 00d2 4A30 cpi r20,lo8(10) |
119 00d4 01F4 brne .+2 |
120 00d6 00C0 rjmp .L69 |
121 00d8 8091 0000 lds r24,AdValueGyroNick |
122 00dc 9091 0000 lds r25,(AdValueGyroNick)+1 |
123 00e0 815D subi r24,lo8(2001) |
124 00e2 9740 sbci r25,hi8(2001) |
125 00e4 04F0 brlt .L17 |
126 00e6 88EE ldi r24,lo8(1000) |
127 00e8 93E0 ldi r25,hi8(1000) |
128 00ea 9093 0000 sts (GyroNick)+1,r25 |
129 00ee 8093 0000 sts GyroNick,r24 |
130 .L17: |
131 00f2 8091 0000 lds r24,AdValueGyroNick |
132 00f6 9091 0000 lds r25,(AdValueGyroNick)+1 |
133 00fa 805E subi r24,lo8(2016) |
134 00fc 9740 sbci r25,hi8(2016) |
135 00fe 04F0 brlt .+2 |
136 0100 00C0 rjmp .L65 |
137 .L16: |
138 0102 8091 0000 lds r24,HiResGyroRoll |
139 0106 9091 0000 lds r25,(HiResGyroRoll)+1 |
140 010a 97FD sbrc r25,7 |
141 010c 00C0 rjmp .L70 |
142 .L18: |
143 010e 9595 asr r25 |
144 0110 8795 ror r24 |
145 0112 9595 asr r25 |
146 0114 8795 ror r24 |
147 0116 9595 asr r25 |
148 0118 8795 ror r24 |
149 011a 9093 0000 sts (GyroRoll)+1,r25 |
150 011e 8093 0000 sts GyroRoll,r24 |
151 0122 2091 0000 lds r18,FilterHiResGyroRoll |
152 0126 3091 0000 lds r19,(FilterHiResGyroRoll)+1 |
153 012a 37FD sbrc r19,7 |
154 012c 00C0 rjmp .L71 |
155 .L19: |
156 012e C901 movw r24,r18 |
157 0130 9595 asr r25 |
158 0132 8795 ror r24 |
159 0134 9595 asr r25 |
160 0136 8795 ror r24 |
161 0138 9595 asr r25 |
162 013a 8795 ror r24 |
163 013c 9B8F std Y+27,r25 |
164 013e 8A8F std Y+26,r24 |
165 0140 8091 0000 lds r24,AdValueGyroRoll |
166 0144 9091 0000 lds r25,(AdValueGyroRoll)+1 |
167 0148 0F97 sbiw r24,15 |
168 014a 04F4 brge .L20 |
169 014c 88E1 ldi r24,lo8(-1000) |
170 014e 9CEF ldi r25,hi8(-1000) |
171 0150 9093 0000 sts (GyroRoll)+1,r25 |
172 0154 8093 0000 sts GyroRoll,r24 |
173 .L20: |
174 0158 8091 0000 lds r24,AdValueGyroRoll |
175 015c 9091 0000 lds r25,(AdValueGyroRoll)+1 |
176 0160 0797 sbiw r24,7 |
177 0162 04F4 brge .L21 |
178 0164 80E3 ldi r24,lo8(-2000) |
179 0166 98EF ldi r25,hi8(-2000) |
180 0168 9093 0000 sts (GyroRoll)+1,r25 |
181 016c 8093 0000 sts GyroRoll,r24 |
182 .L21: |
183 0170 4A30 cpi r20,lo8(10) |
184 0172 01F4 brne .+2 |
185 0174 00C0 rjmp .L72 |
186 0176 8091 0000 lds r24,AdValueGyroRoll |
187 017a 9091 0000 lds r25,(AdValueGyroRoll)+1 |
188 017e 815D subi r24,lo8(2001) |
189 0180 9740 sbci r25,hi8(2001) |
190 0182 04F0 brlt .L25 |
191 0184 88EE ldi r24,lo8(1000) |
192 0186 93E0 ldi r25,hi8(1000) |
193 0188 9093 0000 sts (GyroRoll)+1,r25 |
194 018c 8093 0000 sts GyroRoll,r24 |
195 .L25: |
196 0190 8091 0000 lds r24,AdValueGyroRoll |
197 0194 9091 0000 lds r25,(AdValueGyroRoll)+1 |
198 0198 805E subi r24,lo8(2016) |
199 019a 9740 sbci r25,hi8(2016) |
200 019c 04F0 brlt .+2 |
201 019e 00C0 rjmp .L66 |
202 .L24: |
203 01a0 8091 0000 lds r24,AdValueGyroYaw |
204 01a4 9091 0000 lds r25,(AdValueGyroYaw)+1 |
205 01a8 2091 0000 lds r18,AdBiasGyroYaw |
206 01ac 3091 0000 lds r19,(AdBiasGyroYaw)+1 |
207 01b0 281B sub r18,r24 |
208 01b2 390B sbc r19,r25 |
209 01b4 3DA7 std Y+45,r19 |
210 01b6 2CA7 std Y+44,r18 |
211 01b8 3093 0000 sts (GyroYaw)+1,r19 |
212 01bc 2093 0000 sts GyroYaw,r18 |
213 01c0 6091 0000 lds r22,AdValueAccNick |
214 01c4 7091 0000 lds r23,(AdValueAccNick)+1 |
215 01c8 8827 clr r24 |
216 01ca 77FD sbrc r23,7 |
217 01cc 8095 com r24 |
218 01ce 982F mov r25,r24 |
219 01d0 26E0 ldi r18,lo8(6) |
220 01d2 30E0 ldi r19,hi8(6) |
221 01d4 40E0 ldi r20,hlo8(6) |
222 01d6 50E0 ldi r21,hhi8(6) |
223 01d8 0E94 0000 call __mulsi3 |
224 01dc 7B01 movw r14,r22 |
225 01de 8C01 movw r16,r24 |
226 01e0 2091 0000 lds r18,AccNick |
227 01e4 3091 0000 lds r19,(AccNick)+1 |
228 01e8 4427 clr r20 |
229 01ea 37FD sbrc r19,7 |
230 01ec 4095 com r20 |
231 01ee 542F mov r21,r20 |
232 01f0 CA01 movw r24,r20 |
233 01f2 B901 movw r22,r18 |
234 01f4 660F lsl r22 |
235 01f6 771F rol r23 |
236 01f8 881F rol r24 |
237 01fa 991F rol r25 |
238 01fc 620F add r22,r18 |
239 01fe 731F adc r23,r19 |
240 0200 841F adc r24,r20 |
241 0202 951F adc r25,r21 |
242 0204 6E0D add r22,r14 |
243 0206 7F1D adc r23,r15 |
244 0208 801F adc r24,r16 |
245 020a 911F adc r25,r17 |
246 020c 24E0 ldi r18,lo8(4) |
247 020e 30E0 ldi r19,hi8(4) |
248 0210 40E0 ldi r20,hlo8(4) |
249 0212 50E0 ldi r21,hhi8(4) |
250 0214 0E94 0000 call __divmodsi4 |
251 0218 3093 0000 sts (AccNick)+1,r19 |
252 021c 2093 0000 sts AccNick,r18 |
253 0220 6091 0000 lds r22,AdValueAccRoll |
254 0224 7091 0000 lds r23,(AdValueAccRoll)+1 |
255 0228 8827 clr r24 |
256 022a 77FD sbrc r23,7 |
257 022c 8095 com r24 |
258 022e 982F mov r25,r24 |
259 0230 26E0 ldi r18,lo8(6) |
260 0232 30E0 ldi r19,hi8(6) |
261 0234 40E0 ldi r20,hlo8(6) |
262 0236 50E0 ldi r21,hhi8(6) |
263 0238 0E94 0000 call __mulsi3 |
264 023c 7B01 movw r14,r22 |
265 023e 8C01 movw r16,r24 |
266 0240 2091 0000 lds r18,AccRoll |
267 0244 3091 0000 lds r19,(AccRoll)+1 |
268 0248 4427 clr r20 |
269 024a 37FD sbrc r19,7 |
270 024c 4095 com r20 |
271 024e 542F mov r21,r20 |
272 0250 CA01 movw r24,r20 |
273 0252 B901 movw r22,r18 |
274 0254 660F lsl r22 |
275 0256 771F rol r23 |
276 0258 881F rol r24 |
277 025a 991F rol r25 |
278 025c 620F add r22,r18 |
279 025e 731F adc r23,r19 |
280 0260 841F adc r24,r20 |
281 0262 951F adc r25,r21 |
282 0264 6E0D add r22,r14 |
283 0266 7F1D adc r23,r15 |
284 0268 801F adc r24,r16 |
285 026a 911F adc r25,r17 |
286 026c 24E0 ldi r18,lo8(4) |
287 026e 30E0 ldi r19,hi8(4) |
288 0270 40E0 ldi r20,hlo8(4) |
289 0272 50E0 ldi r21,hhi8(4) |
290 0274 0E94 0000 call __divmodsi4 |
291 0278 3093 0000 sts (AccRoll)+1,r19 |
292 027c 2093 0000 sts AccRoll,r18 |
293 0280 E090 0000 lds r14,AdValueAccTop |
294 0284 F090 0000 lds r15,(AdValueAccTop)+1 |
295 0288 2091 0000 lds r18,AccTop |
296 028c 3091 0000 lds r19,(AccTop)+1 |
297 0290 4427 clr r20 |
298 0292 37FD sbrc r19,7 |
299 0294 4095 com r20 |
300 0296 542F mov r21,r20 |
301 0298 CA01 movw r24,r20 |
302 029a B901 movw r22,r18 |
303 029c 660F lsl r22 |
304 029e 771F rol r23 |
305 02a0 881F rol r24 |
306 02a2 991F rol r25 |
307 02a4 620F add r22,r18 |
308 02a6 731F adc r23,r19 |
309 02a8 841F adc r24,r20 |
310 02aa 951F adc r25,r21 |
311 02ac 0027 clr r16 |
312 02ae F7FC sbrc r15,7 |
313 02b0 0095 com r16 |
314 02b2 102F mov r17,r16 |
315 02b4 6E0D add r22,r14 |
316 02b6 7F1D adc r23,r15 |
317 02b8 801F adc r24,r16 |
318 02ba 911F adc r25,r17 |
319 02bc 24E0 ldi r18,lo8(4) |
320 02be 30E0 ldi r19,hi8(4) |
321 02c0 40E0 ldi r20,hlo8(4) |
322 02c2 50E0 ldi r21,hhi8(4) |
323 02c4 0E94 0000 call __divmodsi4 |
324 02c8 3093 0000 sts (AccTop)+1,r19 |
325 02cc 2093 0000 sts AccTop,r18 |
326 02d0 2091 0000 lds r18,AdValueAccNick |
327 02d4 3091 0000 lds r19,(AdValueAccNick)+1 |
328 02d8 C901 movw r24,r18 |
329 02da 880F lsl r24 |
330 02dc 991F rol r25 |
331 02de 820F add r24,r18 |
332 02e0 931F adc r25,r19 |
333 02e2 880F lsl r24 |
334 02e4 991F rol r25 |
335 02e6 AA27 clr r26 |
336 02e8 97FD sbrc r25,7 |
337 02ea A095 com r26 |
338 02ec BA2F mov r27,r26 |
339 02ee 2091 0000 lds r18,MeanAccNick |
340 02f2 3091 0000 lds r19,(MeanAccNick)+1 |
341 02f6 4091 0000 lds r20,(MeanAccNick)+2 |
342 02fa 5091 0000 lds r21,(MeanAccNick)+3 |
343 02fe 280F add r18,r24 |
344 0300 391F adc r19,r25 |
345 0302 4A1F adc r20,r26 |
346 0304 5B1F adc r21,r27 |
347 0306 2093 0000 sts MeanAccNick,r18 |
348 030a 3093 0000 sts (MeanAccNick)+1,r19 |
349 030e 4093 0000 sts (MeanAccNick)+2,r20 |
350 0312 5093 0000 sts (MeanAccNick)+3,r21 |
351 0316 2091 0000 lds r18,AdValueAccRoll |
352 031a 3091 0000 lds r19,(AdValueAccRoll)+1 |
353 031e C901 movw r24,r18 |
354 0320 880F lsl r24 |
355 0322 991F rol r25 |
356 0324 820F add r24,r18 |
357 0326 931F adc r25,r19 |
358 0328 880F lsl r24 |
359 032a 991F rol r25 |
360 032c AA27 clr r26 |
361 032e 97FD sbrc r25,7 |
362 0330 A095 com r26 |
363 0332 BA2F mov r27,r26 |
364 0334 2091 0000 lds r18,MeanAccRoll |
365 0338 3091 0000 lds r19,(MeanAccRoll)+1 |
366 033c 4091 0000 lds r20,(MeanAccRoll)+2 |
367 0340 5091 0000 lds r21,(MeanAccRoll)+3 |
368 0344 280F add r18,r24 |
369 0346 391F adc r19,r25 |
370 0348 4A1F adc r20,r26 |
371 034a 5B1F adc r21,r27 |
372 034c 2093 0000 sts MeanAccRoll,r18 |
373 0350 3093 0000 sts (MeanAccRoll)+1,r19 |
374 0354 4093 0000 sts (MeanAccRoll)+2,r20 |
375 0358 5093 0000 sts (MeanAccRoll)+3,r21 |
376 035c 2091 0000 lds r18,AdValueAccNick |
377 0360 3091 0000 lds r19,(AdValueAccNick)+1 |
378 0364 8091 0000 lds r24,NaviAccNick |
379 0368 9091 0000 lds r25,(NaviAccNick)+1 |
380 036c 820F add r24,r18 |
381 036e 931F adc r25,r19 |
382 0370 9093 0000 sts (NaviAccNick)+1,r25 |
383 0374 8093 0000 sts NaviAccNick,r24 |
384 0378 2091 0000 lds r18,AdValueAccRoll |
385 037c 3091 0000 lds r19,(AdValueAccRoll)+1 |
386 0380 8091 0000 lds r24,NaviAccRoll |
387 0384 9091 0000 lds r25,(NaviAccRoll)+1 |
388 0388 820F add r24,r18 |
389 038a 931F adc r25,r19 |
390 038c 9093 0000 sts (NaviAccRoll)+1,r25 |
391 0390 8093 0000 sts NaviAccRoll,r24 |
392 0394 8091 0000 lds r24,NaviCntAcc |
393 0398 9091 0000 lds r25,(NaviCntAcc)+1 |
394 039c 0196 adiw r24,1 |
395 039e 9093 0000 sts (NaviCntAcc)+1,r25 |
396 03a2 8093 0000 sts NaviCntAcc,r24 |
397 03a6 8091 7A00 lds r24,122 |
398 03aa 886C ori r24,lo8(-56) |
399 03ac 8093 7A00 sts 122,r24 |
400 03b0 1092 0000 sts ADReady,__zero_reg__ |
401 03b4 8091 0000 lds r24,ReadingIntegralGyroNick |
402 03b8 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 |
403 03bc A091 0000 lds r26,(ReadingIntegralGyroNick)+2 |
404 03c0 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 |
405 03c4 8E8B std Y+22,r24 |
406 03c6 9F8B std Y+23,r25 |
407 03c8 A88F std Y+24,r26 |
408 03ca B98F std Y+25,r27 |
409 03cc 8091 0000 lds r24,ReadingIntegralGyroRoll |
410 03d0 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 |
411 03d4 A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 |
412 03d8 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 |
413 03dc 8A8B std Y+18,r24 |
414 03de 9B8B std Y+19,r25 |
415 03e0 AC8B std Y+20,r26 |
416 03e2 BD8B std Y+21,r27 |
417 03e4 ACA5 ldd r26,Y+44 |
418 03e6 BDA5 ldd r27,Y+45 |
419 03e8 3D01 movw r6,r26 |
420 03ea 8824 clr r8 |
421 03ec 77FC sbrc r7,7 |
422 03ee 8094 com r8 |
423 03f0 982C mov r9,r8 |
424 03f2 E090 0000 lds r14,YawGyroHeading |
425 03f6 F090 0000 lds r15,(YawGyroHeading)+1 |
426 03fa 0091 0000 lds r16,(YawGyroHeading)+2 |
427 03fe 1091 0000 lds r17,(YawGyroHeading)+3 |
428 0402 E60C add r14,r6 |
429 0404 F71C adc r15,r7 |
430 0406 081D adc r16,r8 |
431 0408 191D adc r17,r9 |
432 040a ECA2 std Y+36,r14 |
433 040c FDA2 std Y+37,r15 |
434 040e 0EA3 std Y+38,r16 |
435 0410 1FA3 std Y+39,r17 |
436 0412 E092 0000 sts YawGyroHeading,r14 |
437 0416 F092 0000 sts (YawGyroHeading)+1,r15 |
438 041a 0093 0000 sts (YawGyroHeading)+2,r16 |
439 041e 1093 0000 sts (YawGyroHeading)+3,r17 |
440 0422 2090 0000 lds r2,ReadingIntegralGyroYaw |
441 0426 3090 0000 lds r3,(ReadingIntegralGyroYaw)+1 |
442 042a 4090 0000 lds r4,(ReadingIntegralGyroYaw)+2 |
443 042e 5090 0000 lds r5,(ReadingIntegralGyroYaw)+3 |
444 0432 260C add r2,r6 |
445 0434 371C adc r3,r7 |
446 0436 481C adc r4,r8 |
447 0438 591C adc r5,r9 |
448 043a 28A2 std Y+32,r2 |
449 043c 39A2 std Y+33,r3 |
450 043e 4AA2 std Y+34,r4 |
451 0440 5BA2 std Y+35,r5 |
452 0442 2092 0000 sts ReadingIntegralGyroYaw,r2 |
453 0446 3092 0000 sts (ReadingIntegralGyroYaw)+1,r3 |
454 044a 4092 0000 sts (ReadingIntegralGyroYaw)+2,r4 |
455 044e 5092 0000 sts (ReadingIntegralGyroYaw)+3,r5 |
456 0452 B091 0000 lds r27,LoopingNick |
457 0456 BF8F std Y+31,r27 |
458 0458 BB23 tst r27 |
459 045a 01F4 brne .L26 |
460 045c 8091 0000 lds r24,LoopingRoll |
461 0460 8823 tst r24 |
462 0462 01F4 brne .+2 |
463 0464 00C0 rjmp .L27 |
464 .L26: |
465 0466 E091 0000 lds r30,ParamSet+8 |
466 046a E98B std Y+17,r30 |
467 .L28: |
468 046c 1092 0000 sts (TrimNick)+1,__zero_reg__ |
469 0470 1092 0000 sts TrimNick,__zero_reg__ |
470 0474 1092 0000 sts (TrimRoll)+1,__zero_reg__ |
471 0478 1092 0000 sts TrimRoll,__zero_reg__ |
472 047c 1D86 std Y+13,__zero_reg__ |
473 047e 1E86 std Y+14,__zero_reg__ |
474 0480 1F86 std Y+15,__zero_reg__ |
475 0482 188A std Y+16,__zero_reg__ |
476 0484 1986 std Y+9,__zero_reg__ |
477 0486 1A86 std Y+10,__zero_reg__ |
478 0488 1B86 std Y+11,__zero_reg__ |
479 048a 1C86 std Y+12,__zero_reg__ |
480 .L40: |
481 048c 8091 0000 lds r24,ParamSet+23 |
482 0490 A82E mov r10,r24 |
483 0492 BB24 clr r11 |
484 0494 CC24 clr r12 |
485 0496 DD24 clr r13 |
486 0498 C601 movw r24,r12 |
487 049a B501 movw r22,r10 |
488 049c 20E1 ldi r18,lo8(15120) |
489 049e 3BE3 ldi r19,hi8(15120) |
490 04a0 40E0 ldi r20,hlo8(15120) |
491 04a2 50E0 ldi r21,hhi8(15120) |
492 04a4 0E94 0000 call __mulsi3 |
493 04a8 3B01 movw r6,r22 |
494 04aa 4C01 movw r8,r24 |
495 04ac E616 cp r14,r22 |
496 04ae F706 cpc r15,r23 |
497 04b0 0807 cpc r16,r24 |
498 04b2 1907 cpc r17,r25 |
499 04b4 04F0 brlt .L41 |
500 04b6 C601 movw r24,r12 |
501 04b8 B501 movw r22,r10 |
502 04ba 20EF ldi r18,lo8(-15120) |
503 04bc 34EC ldi r19,hi8(-15120) |
504 04be 4FEF ldi r20,hlo8(-15120) |
505 04c0 5FEF ldi r21,hhi8(-15120) |
506 04c2 0E94 0000 call __mulsi3 |
507 04c6 6E0D add r22,r14 |
508 04c8 7F1D adc r23,r15 |
509 04ca 801F adc r24,r16 |
510 04cc 911F adc r25,r17 |
511 04ce 6093 0000 sts YawGyroHeading,r22 |
512 04d2 7093 0000 sts (YawGyroHeading)+1,r23 |
513 04d6 8093 0000 sts (YawGyroHeading)+2,r24 |
514 04da 9093 0000 sts (YawGyroHeading)+3,r25 |
515 04de 7B01 movw r14,r22 |
516 04e0 8C01 movw r16,r24 |
517 .L41: |
518 04e2 17FD sbrc r17,7 |
519 04e4 00C0 rjmp .L73 |
520 .L42: |
521 04e6 6090 0000 lds r6,TrimRoll |
522 04ea 7090 0000 lds r7,(TrimRoll)+1 |
523 04ee 2A8D ldd r18,Y+26 |
524 04f0 3B8D ldd r19,Y+27 |
525 04f2 260D add r18,r6 |
526 04f4 371D adc r19,r7 |
527 04f6 7901 movw r14,r18 |
528 04f8 0027 clr r16 |
529 04fa F7FC sbrc r15,7 |
530 04fc 0095 com r16 |
531 04fe 102F mov r17,r16 |
532 0500 8091 0000 lds r24,ReadingIntegralGyroRoll2 |
533 0504 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 |
534 0508 A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 |
535 050c B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 |
536 0510 8E0D add r24,r14 |
537 0512 9F1D adc r25,r15 |
538 0514 A01F adc r26,r16 |
539 0516 B11F adc r27,r17 |
540 0518 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
541 051c 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
542 0520 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
543 0524 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
544 0528 8091 0000 lds r24,AttitudeCorrectionRoll |
545 052c 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 |
546 0530 281B sub r18,r24 |
547 0532 390B sbc r19,r25 |
548 0534 4427 clr r20 |
549 0536 37FD sbrc r19,7 |
550 0538 4095 com r20 |
551 053a 542F mov r21,r20 |
552 053c 8A89 ldd r24,Y+18 |
553 053e 9B89 ldd r25,Y+19 |
554 0540 AC89 ldd r26,Y+20 |
555 0542 BD89 ldd r27,Y+21 |
556 0544 280F add r18,r24 |
557 0546 391F adc r19,r25 |
558 0548 4A1F adc r20,r26 |
559 054a 5B1F adc r21,r27 |
560 054c 2093 0000 sts ReadingIntegralGyroRoll,r18 |
561 0550 3093 0000 sts (ReadingIntegralGyroRoll)+1,r19 |
562 0554 4093 0000 sts (ReadingIntegralGyroRoll)+2,r20 |
563 0558 5093 0000 sts (ReadingIntegralGyroRoll)+3,r21 |
564 055c E090 0000 lds r14,TurnOver180Roll |
565 0560 F090 0000 lds r15,(TurnOver180Roll)+1 |
566 0564 0091 0000 lds r16,(TurnOver180Roll)+2 |
567 0568 1091 0000 lds r17,(TurnOver180Roll)+3 |
568 056c E216 cp r14,r18 |
569 056e F306 cpc r15,r19 |
570 0570 0407 cpc r16,r20 |
571 0572 1507 cpc r17,r21 |
572 0574 04F4 brge .L43 |
573 0576 80E1 ldi r24,lo8(10000) |
574 0578 97E2 ldi r25,hi8(10000) |
575 057a A0E0 ldi r26,hlo8(10000) |
576 057c B0E0 ldi r27,hhi8(10000) |
577 057e 8E19 sub r24,r14 |
578 0580 9F09 sbc r25,r15 |
579 0582 A00B sbc r26,r16 |
580 0584 B10B sbc r27,r17 |
581 0586 8093 0000 sts ReadingIntegralGyroRoll,r24 |
582 058a 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 |
583 058e A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 |
584 0592 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 |
585 0596 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
586 059a 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
587 059e A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
588 05a2 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
589 .L43: |
590 05a6 8827 clr r24 |
591 05a8 9927 clr r25 |
592 05aa DC01 movw r26,r24 |
593 05ac 8E19 sub r24,r14 |
594 05ae 9F09 sbc r25,r15 |
595 05b0 A00B sbc r26,r16 |
596 05b2 B10B sbc r27,r17 |
597 05b4 A090 0000 lds r10,ReadingIntegralGyroRoll |
598 05b8 B090 0000 lds r11,(ReadingIntegralGyroRoll)+1 |
599 05bc C090 0000 lds r12,(ReadingIntegralGyroRoll)+2 |
600 05c0 D090 0000 lds r13,(ReadingIntegralGyroRoll)+3 |
601 05c4 A816 cp r10,r24 |
602 05c6 B906 cpc r11,r25 |
603 05c8 CA06 cpc r12,r26 |
604 05ca DB06 cpc r13,r27 |
605 05cc 04F4 brge .L44 |
606 05ce D801 movw r26,r16 |
607 05d0 C701 movw r24,r14 |
608 05d2 8051 subi r24,lo8(-(-10000)) |
609 05d4 9742 sbci r25,hi8(-(-10000)) |
610 05d6 A040 sbci r26,hlo8(-(-10000)) |
611 05d8 B040 sbci r27,hhi8(-(-10000)) |
612 05da 8093 0000 sts ReadingIntegralGyroRoll,r24 |
613 05de 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 |
614 05e2 A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 |
615 05e6 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 |
616 05ea 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
617 05ee 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
618 05f2 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
619 05f6 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
620 05fa 5C01 movw r10,r24 |
621 05fc 6D01 movw r12,r26 |
622 .L44: |
623 05fe 8090 0000 lds r8,TrimNick |
624 0602 9090 0000 lds r9,(TrimNick)+1 |
625 0606 2C8D ldd r18,Y+28 |
626 0608 3D8D ldd r19,Y+29 |
627 060a 280D add r18,r8 |
628 060c 391D adc r19,r9 |
629 060e 7901 movw r14,r18 |
630 0610 0027 clr r16 |
631 0612 F7FC sbrc r15,7 |
632 0614 0095 com r16 |
633 0616 102F mov r17,r16 |
634 0618 8091 0000 lds r24,ReadingIntegralGyroNick2 |
635 061c 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 |
636 0620 A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 |
637 0624 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 |
638 0628 8E0D add r24,r14 |
639 062a 9F1D adc r25,r15 |
640 062c A01F adc r26,r16 |
641 062e B11F adc r27,r17 |
642 0630 8093 0000 sts ReadingIntegralGyroNick2,r24 |
643 0634 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
644 0638 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
645 063c B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
646 0640 8091 0000 lds r24,AttitudeCorrectionNick |
647 0644 9091 0000 lds r25,(AttitudeCorrectionNick)+1 |
648 0648 281B sub r18,r24 |
649 064a 390B sbc r19,r25 |
650 064c 4427 clr r20 |
651 064e 37FD sbrc r19,7 |
652 0650 4095 com r20 |
653 0652 542F mov r21,r20 |
654 0654 8E89 ldd r24,Y+22 |
655 0656 9F89 ldd r25,Y+23 |
656 0658 A88D ldd r26,Y+24 |
657 065a B98D ldd r27,Y+25 |
658 065c 280F add r18,r24 |
659 065e 391F adc r19,r25 |
660 0660 4A1F adc r20,r26 |
661 0662 5B1F adc r21,r27 |
662 0664 2093 0000 sts ReadingIntegralGyroNick,r18 |
663 0668 3093 0000 sts (ReadingIntegralGyroNick)+1,r19 |
664 066c 4093 0000 sts (ReadingIntegralGyroNick)+2,r20 |
665 0670 5093 0000 sts (ReadingIntegralGyroNick)+3,r21 |
666 0674 E090 0000 lds r14,TurnOver180Nick |
667 0678 F090 0000 lds r15,(TurnOver180Nick)+1 |
668 067c 0091 0000 lds r16,(TurnOver180Nick)+2 |
669 0680 1091 0000 lds r17,(TurnOver180Nick)+3 |
670 0684 E216 cp r14,r18 |
671 0686 F306 cpc r15,r19 |
672 0688 0407 cpc r16,r20 |
673 068a 1507 cpc r17,r21 |
674 068c 04F4 brge .L45 |
675 068e 88EA ldi r24,lo8(25000) |
676 0690 91E6 ldi r25,hi8(25000) |
677 0692 A0E0 ldi r26,hlo8(25000) |
678 0694 B0E0 ldi r27,hhi8(25000) |
679 0696 8E19 sub r24,r14 |
680 0698 9F09 sbc r25,r15 |
681 069a A00B sbc r26,r16 |
682 069c B10B sbc r27,r17 |
683 069e 8093 0000 sts ReadingIntegralGyroNick,r24 |
684 06a2 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 |
685 06a6 A093 0000 sts (ReadingIntegralGyroNick)+2,r26 |
686 06aa B093 0000 sts (ReadingIntegralGyroNick)+3,r27 |
687 06ae 8093 0000 sts ReadingIntegralGyroNick2,r24 |
688 06b2 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
689 06b6 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
690 06ba B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
691 .L45: |
692 06be 8827 clr r24 |
693 06c0 9927 clr r25 |
694 06c2 DC01 movw r26,r24 |
695 06c4 8E19 sub r24,r14 |
696 06c6 9F09 sbc r25,r15 |
697 06c8 A00B sbc r26,r16 |
698 06ca B10B sbc r27,r17 |
699 06cc 2091 0000 lds r18,ReadingIntegralGyroNick |
700 06d0 3091 0000 lds r19,(ReadingIntegralGyroNick)+1 |
701 06d4 4091 0000 lds r20,(ReadingIntegralGyroNick)+2 |
702 06d8 5091 0000 lds r21,(ReadingIntegralGyroNick)+3 |
703 06dc 2817 cp r18,r24 |
704 06de 3907 cpc r19,r25 |
705 06e0 4A07 cpc r20,r26 |
706 06e2 5B07 cpc r21,r27 |
707 06e4 04F4 brge .L46 |
708 06e6 D801 movw r26,r16 |
709 06e8 C701 movw r24,r14 |
710 06ea 885A subi r24,lo8(-(-25000)) |
711 06ec 9146 sbci r25,hi8(-(-25000)) |
712 06ee A040 sbci r26,hlo8(-(-25000)) |
713 06f0 B040 sbci r27,hhi8(-(-25000)) |
714 06f2 8093 0000 sts ReadingIntegralGyroNick,r24 |
715 06f6 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 |
716 06fa A093 0000 sts (ReadingIntegralGyroNick)+2,r26 |
717 06fe B093 0000 sts (ReadingIntegralGyroNick)+3,r27 |
718 0702 8093 0000 sts ReadingIntegralGyroNick2,r24 |
719 0706 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
720 070a A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
721 070e B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
722 0712 9C01 movw r18,r24 |
723 0714 AD01 movw r20,r26 |
724 .L46: |
725 0716 2092 0000 sts IntegralGyroYaw,r2 |
726 071a 3092 0000 sts (IntegralGyroYaw)+1,r3 |
727 071e 4092 0000 sts (IntegralGyroYaw)+2,r4 |
728 0722 5092 0000 sts (IntegralGyroYaw)+3,r5 |
729 0726 2093 0000 sts IntegralGyroNick,r18 |
730 072a 3093 0000 sts (IntegralGyroNick)+1,r19 |
731 072e 4093 0000 sts (IntegralGyroNick)+2,r20 |
732 0732 5093 0000 sts (IntegralGyroNick)+3,r21 |
733 0736 A092 0000 sts IntegralGyroRoll,r10 |
734 073a B092 0000 sts (IntegralGyroRoll)+1,r11 |
735 073e C092 0000 sts (IntegralGyroRoll)+2,r12 |
736 0742 D092 0000 sts (IntegralGyroRoll)+3,r13 |
737 0746 8091 0000 lds r24,ReadingIntegralGyroNick2 |
738 074a 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 |
739 074e A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 |
740 0752 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 |
741 0756 8093 0000 sts IntegralGyroNick2,r24 |
742 075a 9093 0000 sts (IntegralGyroNick2)+1,r25 |
743 075e A093 0000 sts (IntegralGyroNick2)+2,r26 |
744 0762 B093 0000 sts (IntegralGyroNick2)+3,r27 |
745 0766 8091 0000 lds r24,ReadingIntegralGyroRoll2 |
746 076a 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 |
747 076e A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 |
748 0772 B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 |
749 0776 8093 0000 sts IntegralGyroRoll2,r24 |
750 077a 9093 0000 sts (IntegralGyroRoll2)+1,r25 |
751 077e A093 0000 sts (IntegralGyroRoll2)+2,r26 |
752 0782 B093 0000 sts (IntegralGyroRoll2)+3,r27 |
753 0786 6091 0000 lds r22,FCParam+6 |
754 078a 6623 tst r22 |
755 078c 01F4 brne .+2 |
756 078e 00C0 rjmp .L47 |
757 0790 4091 0000 lds r20,HiResGyroNick |
758 0794 5091 0000 lds r21,(HiResGyroNick)+1 |
759 0798 8091 0000 lds r24,Last_GyroNick.2093 |
760 079c 9091 0000 lds r25,(Last_GyroNick.2093)+1 |
761 07a0 481B sub r20,r24 |
762 07a2 590B sbc r21,r25 |
763 07a4 2091 0000 lds r18,HiResGyroNick |
764 07a8 3091 0000 lds r19,(HiResGyroNick)+1 |
765 07ac 280F add r18,r24 |
766 07ae 391F adc r19,r25 |
767 07b0 37FD sbrc r19,7 |
768 07b2 00C0 rjmp .L74 |
769 .L48: |
770 07b4 3595 asr r19 |
771 07b6 2795 ror r18 |
772 07b8 3093 0000 sts (Last_GyroNick.2093)+1,r19 |
773 07bc 2093 0000 sts Last_GyroNick.2093,r18 |
774 07c0 CA01 movw r24,r20 |
775 07c2 4138 cpi r20,129 |
776 07c4 5105 cpc r21,__zero_reg__ |
777 07c6 04F0 brlt .+2 |
778 07c8 00C0 rjmp .L75 |
779 07ca E62F mov r30,r22 |
780 07cc F0E0 ldi r31,lo8(0) |
781 07ce AFEF ldi r26,hi8(-128) |
782 07d0 8038 cpi r24,lo8(-128) |
783 07d2 9A07 cpc r25,r26 |
784 07d4 04F4 brge .+2 |
785 07d6 00C0 rjmp .L76 |
786 .L50: |
787 07d8 8E9F mul r24,r30 |
788 07da B001 movw r22,r0 |
789 07dc 8F9F mul r24,r31 |
790 07de 700D add r23,r0 |
791 07e0 9E9F mul r25,r30 |
792 07e2 700D add r23,r0 |
793 07e4 1124 clr r1 |
794 07e6 9B01 movw r18,r22 |
795 07e8 77FD sbrc r23,7 |
796 07ea 00C0 rjmp .L77 |
797 .L51: |
798 07ec 3595 asr r19 |
799 07ee 2795 ror r18 |
800 07f0 3595 asr r19 |
801 07f2 2795 ror r18 |
802 07f4 3595 asr r19 |
803 07f6 2795 ror r18 |
804 07f8 3595 asr r19 |
805 07fa 2795 ror r18 |
806 07fc 8091 0000 lds r24,GyroNick |
807 0800 9091 0000 lds r25,(GyroNick)+1 |
808 0804 820F add r24,r18 |
809 0806 931F adc r25,r19 |
810 0808 9093 0000 sts (GyroNick)+1,r25 |
811 080c 8093 0000 sts GyroNick,r24 |
812 0810 4091 0000 lds r20,HiResGyroRoll |
813 0814 5091 0000 lds r21,(HiResGyroRoll)+1 |
814 0818 8091 0000 lds r24,Last_GyroRoll.2092 |
815 081c 9091 0000 lds r25,(Last_GyroRoll.2092)+1 |
816 0820 481B sub r20,r24 |
817 0822 590B sbc r21,r25 |
818 0824 2091 0000 lds r18,HiResGyroRoll |
819 0828 3091 0000 lds r19,(HiResGyroRoll)+1 |
820 082c 280F add r18,r24 |
821 082e 391F adc r19,r25 |
822 0830 37FD sbrc r19,7 |
823 0832 00C0 rjmp .L78 |
824 .L52: |
825 0834 3595 asr r19 |
826 0836 2795 ror r18 |
827 0838 3093 0000 sts (Last_GyroRoll.2092)+1,r19 |
828 083c 2093 0000 sts Last_GyroRoll.2092,r18 |
829 0840 CA01 movw r24,r20 |
830 0842 4138 cpi r20,129 |
831 0844 5105 cpc r21,__zero_reg__ |
832 0846 04F0 brlt .+2 |
833 0848 00C0 rjmp .L79 |
834 084a BFEF ldi r27,hi8(-128) |
835 084c 8038 cpi r24,lo8(-128) |
836 084e 9B07 cpc r25,r27 |
837 0850 04F4 brge .+2 |
838 0852 00C0 rjmp .L80 |
839 .L54: |
840 0854 8E9F mul r24,r30 |
841 0856 A001 movw r20,r0 |
842 0858 8F9F mul r24,r31 |
843 085a 500D add r21,r0 |
844 085c 9E9F mul r25,r30 |
845 085e 500D add r21,r0 |
846 0860 1124 clr r1 |
847 0862 9A01 movw r18,r20 |
848 0864 57FD sbrc r21,7 |
849 0866 00C0 rjmp .L81 |
850 .L55: |
851 0868 3595 asr r19 |
852 086a 2795 ror r18 |
853 086c 3595 asr r19 |
854 086e 2795 ror r18 |
855 0870 3595 asr r19 |
856 0872 2795 ror r18 |
857 0874 3595 asr r19 |
858 0876 2795 ror r18 |
859 0878 8091 0000 lds r24,GyroRoll |
860 087c 9091 0000 lds r25,(GyroRoll)+1 |
861 0880 820F add r24,r18 |
862 0882 931F adc r25,r19 |
863 0884 9093 0000 sts (GyroRoll)+1,r25 |
864 0888 8093 0000 sts GyroRoll,r24 |
865 088c 8091 0000 lds r24,HiResGyroNick |
866 0890 9091 0000 lds r25,(HiResGyroNick)+1 |
867 0894 680F add r22,r24 |
868 0896 791F adc r23,r25 |
869 0898 7093 0000 sts (HiResGyroNick)+1,r23 |
870 089c 6093 0000 sts HiResGyroNick,r22 |
871 08a0 8091 0000 lds r24,HiResGyroRoll |
872 08a4 9091 0000 lds r25,(HiResGyroRoll)+1 |
873 08a8 480F add r20,r24 |
874 08aa 591F adc r21,r25 |
875 08ac 5093 0000 sts (HiResGyroRoll)+1,r21 |
876 08b0 4093 0000 sts HiResGyroRoll,r20 |
877 .L47: |
878 08b4 EC8D ldd r30,Y+28 |
879 08b6 FD8D ldd r31,Y+29 |
880 08b8 1E16 cp __zero_reg__,r30 |
881 08ba 1F06 cpc __zero_reg__,r31 |
882 08bc 04F0 brlt .+2 |
883 08be 00C0 rjmp .L56 |
884 08c0 6D85 ldd r22,Y+13 |
885 08c2 7E85 ldd r23,Y+14 |
886 08c4 8F85 ldd r24,Y+15 |
887 08c6 9889 ldd r25,Y+16 |
888 08c8 20E4 ldi r18,lo8(64) |
889 08ca 30E0 ldi r19,hi8(64) |
890 08cc 40E0 ldi r20,hlo8(64) |
891 08ce 50E0 ldi r21,hhi8(64) |
892 08d0 0E94 0000 call __divmodsi4 |
893 08d4 820E add r8,r18 |
894 08d6 931E adc r9,r19 |
895 08d8 9092 0000 sts (TrimNick)+1,r9 |
896 08dc 8092 0000 sts TrimNick,r8 |
897 .L57: |
898 08e0 2A8D ldd r18,Y+26 |
899 08e2 3B8D ldd r19,Y+27 |
900 08e4 1216 cp __zero_reg__,r18 |
901 08e6 1306 cpc __zero_reg__,r19 |
902 08e8 04F0 brlt .+2 |
903 08ea 00C0 rjmp .L58 |
904 08ec 6985 ldd r22,Y+9 |
905 08ee 7A85 ldd r23,Y+10 |
906 08f0 8B85 ldd r24,Y+11 |
907 08f2 9C85 ldd r25,Y+12 |
908 08f4 20E4 ldi r18,lo8(64) |
909 08f6 30E0 ldi r19,hi8(64) |
910 08f8 40E0 ldi r20,hlo8(64) |
911 08fa 50E0 ldi r21,hhi8(64) |
912 08fc 0E94 0000 call __divmodsi4 |
913 0900 620E add r6,r18 |
914 0902 731E adc r7,r19 |
915 0904 7092 0000 sts (TrimRoll)+1,r7 |
916 0908 6092 0000 sts TrimRoll,r6 |
917 .L59: |
918 090c 3989 ldd r19,Y+17 |
919 090e 37FD sbrc r19,7 |
920 0910 00C0 rjmp .L82 |
921 .L64: |
922 /* epilogue start */ |
923 0912 AD96 adiw r28,45 |
924 0914 0FB6 in __tmp_reg__,__SREG__ |
925 0916 F894 cli |
926 0918 DEBF out __SP_H__,r29 |
927 091a 0FBE out __SREG__,__tmp_reg__ |
928 091c CDBF out __SP_L__,r28 |
929 091e CF91 pop r28 |
930 0920 DF91 pop r29 |
931 0922 1F91 pop r17 |
932 0924 0F91 pop r16 |
933 0926 FF90 pop r15 |
934 0928 EF90 pop r14 |
935 092a DF90 pop r13 |
936 092c CF90 pop r12 |
937 092e BF90 pop r11 |
938 0930 AF90 pop r10 |
939 0932 9F90 pop r9 |
940 0934 8F90 pop r8 |
941 0936 7F90 pop r7 |
942 0938 6F90 pop r6 |
943 093a 5F90 pop r5 |
944 093c 4F90 pop r4 |
945 093e 3F90 pop r3 |
946 0940 2F90 pop r2 |
947 0942 0895 ret |
948 .L72: |
949 0944 8091 0000 lds r24,AdValueGyroRoll |
950 0948 9091 0000 lds r25,(AdValueGyroRoll)+1 |
951 094c 835F subi r24,lo8(1011) |
952 094e 9340 sbci r25,hi8(1011) |
953 0950 04F0 brlt .L23 |
954 0952 88EE ldi r24,lo8(1000) |
955 0954 93E0 ldi r25,hi8(1000) |
956 0956 9093 0000 sts (GyroRoll)+1,r25 |
957 095a 8093 0000 sts GyroRoll,r24 |
958 .L23: |
959 095e 8091 0000 lds r24,AdValueGyroRoll |
960 0962 9091 0000 lds r25,(AdValueGyroRoll)+1 |
961 0966 8A5F subi r24,lo8(1018) |
962 0968 9340 sbci r25,hi8(1018) |
963 096a 04F4 brge .+2 |
964 096c 00C0 rjmp .L24 |
965 .L66: |
966 096e 80ED ldi r24,lo8(2000) |
967 0970 97E0 ldi r25,hi8(2000) |
968 0972 9093 0000 sts (GyroRoll)+1,r25 |
969 0976 8093 0000 sts GyroRoll,r24 |
970 097a 00C0 rjmp .L24 |
971 .L69: |
972 097c 8091 0000 lds r24,AdValueGyroNick |
973 0980 9091 0000 lds r25,(AdValueGyroNick)+1 |
974 0984 835F subi r24,lo8(1011) |
975 0986 9340 sbci r25,hi8(1011) |
976 0988 04F0 brlt .L15 |
977 098a 88EE ldi r24,lo8(1000) |
978 098c 93E0 ldi r25,hi8(1000) |
979 098e 9093 0000 sts (GyroNick)+1,r25 |
980 0992 8093 0000 sts GyroNick,r24 |
981 .L15: |
982 0996 8091 0000 lds r24,AdValueGyroNick |
983 099a 9091 0000 lds r25,(AdValueGyroNick)+1 |
984 099e 8A5F subi r24,lo8(1018) |
985 09a0 9340 sbci r25,hi8(1018) |
986 09a2 04F4 brge .+2 |
987 09a4 00C0 rjmp .L16 |
988 .L65: |
989 09a6 80ED ldi r24,lo8(2000) |
990 09a8 97E0 ldi r25,hi8(2000) |
991 09aa 9093 0000 sts (GyroNick)+1,r25 |
992 09ae 8093 0000 sts GyroNick,r24 |
993 09b2 8091 0000 lds r24,HiResGyroRoll |
994 09b6 9091 0000 lds r25,(HiResGyroRoll)+1 |
995 09ba 97FF sbrs r25,7 |
996 09bc 00C0 rjmp .L18 |
997 .L70: |
998 09be 0796 adiw r24,7 |
999 09c0 00C0 rjmp .L18 |
1000 .L80: |
1001 09c2 80E8 ldi r24,lo8(-128) |
1002 09c4 9FEF ldi r25,hi8(-128) |
1003 09c6 00C0 rjmp .L54 |
1004 .L79: |
1005 09c8 80E8 ldi r24,lo8(128) |
1006 09ca 90E0 ldi r25,hi8(128) |
1007 09cc BFEF ldi r27,hi8(-128) |
1008 09ce 8038 cpi r24,lo8(-128) |
1009 09d0 9B07 cpc r25,r27 |
1010 09d2 04F0 brlt .+2 |
1011 09d4 00C0 rjmp .L54 |
1012 09d6 00C0 rjmp .L80 |
1013 .L76: |
1014 09d8 80E8 ldi r24,lo8(-128) |
1015 09da 9FEF ldi r25,hi8(-128) |
1016 09dc 00C0 rjmp .L50 |
1017 .L75: |
1018 09de 80E8 ldi r24,lo8(128) |
1019 09e0 90E0 ldi r25,hi8(128) |
1020 09e2 E62F mov r30,r22 |
1021 09e4 F0E0 ldi r31,lo8(0) |
1022 09e6 AFEF ldi r26,hi8(-128) |
1023 09e8 8038 cpi r24,lo8(-128) |
1024 09ea 9A07 cpc r25,r26 |
1025 09ec 04F0 brlt .+2 |
1026 09ee 00C0 rjmp .L50 |
1027 09f0 00C0 rjmp .L76 |
1028 .L27: |
1029 09f2 F091 0000 lds r31,ParamSet+8 |
1030 09f6 F98B std Y+17,r31 |
1031 09f8 F6FF sbrs r31,6 |
1032 09fa 00C0 rjmp .L28 |
1033 09fc 2E89 ldd r18,Y+22 |
1034 09fe 3F89 ldd r19,Y+23 |
1035 0a00 488D ldd r20,Y+24 |
1036 0a02 598D ldd r21,Y+25 |
1037 0a04 2934 cpi r18,lo8(93001) |
1038 0a06 6BE6 ldi r22,hi8(93001) |
1039 0a08 3607 cpc r19,r22 |
1040 0a0a 61E0 ldi r22,hlo8(93001) |
1041 0a0c 4607 cpc r20,r22 |
1042 0a0e 60E0 ldi r22,hhi8(93001) |
1043 0a10 5607 cpc r21,r22 |
1044 0a12 04F0 brlt .L29 |
1045 0a14 28E4 ldi r18,lo8(93000) |
1046 0a16 3BE6 ldi r19,hi8(93000) |
1047 0a18 41E0 ldi r20,hlo8(93000) |
1048 0a1a 50E0 ldi r21,hhi8(93000) |
1049 .L29: |
1050 0a1c 2D83 std Y+5,r18 |
1051 0a1e 3E83 std Y+6,r19 |
1052 0a20 4F83 std Y+7,r20 |
1053 0a22 5887 std Y+8,r21 |
1054 0a24 285B subi r18,lo8(-93000) |
1055 0a26 3449 sbci r19,hi8(-93000) |
1056 0a28 4E4F sbci r20,hlo8(-93000) |
1057 0a2a 5F4F sbci r21,hhi8(-93000) |
1058 0a2c 04F4 brge .L30 |
1059 0a2e 88EB ldi r24,lo8(-93000) |
1060 0a30 94E9 ldi r25,hi8(-93000) |
1061 0a32 AEEF ldi r26,hlo8(-93000) |
1062 0a34 BFEF ldi r27,hhi8(-93000) |
1063 0a36 8D83 std Y+5,r24 |
1064 0a38 9E83 std Y+6,r25 |
1065 0a3a AF83 std Y+7,r26 |
1066 0a3c B887 std Y+8,r27 |
1067 .L30: |
1068 0a3e 2A89 ldd r18,Y+18 |
1069 0a40 3B89 ldd r19,Y+19 |
1070 0a42 4C89 ldd r20,Y+20 |
1071 0a44 5D89 ldd r21,Y+21 |
1072 0a46 2934 cpi r18,lo8(93001) |
1073 0a48 9BE6 ldi r25,hi8(93001) |
1074 0a4a 3907 cpc r19,r25 |
1075 0a4c 91E0 ldi r25,hlo8(93001) |
1076 0a4e 4907 cpc r20,r25 |
1077 0a50 90E0 ldi r25,hhi8(93001) |
1078 0a52 5907 cpc r21,r25 |
1079 0a54 04F0 brlt .L31 |
1080 0a56 28E4 ldi r18,lo8(93000) |
1081 0a58 3BE6 ldi r19,hi8(93000) |
1082 0a5a 41E0 ldi r20,hlo8(93000) |
1083 0a5c 50E0 ldi r21,hhi8(93000) |
1084 .L31: |
1085 0a5e 2983 std Y+1,r18 |
1086 0a60 3A83 std Y+2,r19 |
1087 0a62 4B83 std Y+3,r20 |
1088 0a64 5C83 std Y+4,r21 |
1089 0a66 285B subi r18,lo8(-93000) |
1090 0a68 3449 sbci r19,hi8(-93000) |
1091 0a6a 4E4F sbci r20,hlo8(-93000) |
1092 0a6c 5F4F sbci r21,hhi8(-93000) |
1093 0a6e 04F4 brge .L32 |
1094 0a70 88EB ldi r24,lo8(-93000) |
1095 0a72 94E9 ldi r25,hi8(-93000) |
1096 0a74 AEEF ldi r26,hlo8(-93000) |
1097 0a76 BFEF ldi r27,hhi8(-93000) |
1098 0a78 8983 std Y+1,r24 |
1099 0a7a 9A83 std Y+2,r25 |
1100 0a7c AB83 std Y+3,r26 |
1101 0a7e BC83 std Y+4,r27 |
1102 .L32: |
1103 0a80 AA8D ldd r26,Y+26 |
1104 0a82 BB8D ldd r27,Y+27 |
1105 0a84 9D01 movw r18,r26 |
1106 0a86 4427 clr r20 |
1107 0a88 37FD sbrc r19,7 |
1108 0a8a 4095 com r20 |
1109 0a8c 542F mov r21,r20 |
1110 0a8e 6D81 ldd r22,Y+5 |
1111 0a90 7E81 ldd r23,Y+6 |
1112 0a92 8F81 ldd r24,Y+7 |
1113 0a94 9885 ldd r25,Y+8 |
1114 0a96 0E94 0000 call __mulsi3 |
1115 0a9a 20E0 ldi r18,lo8(2048) |
1116 0a9c 38E0 ldi r19,hi8(2048) |
1117 0a9e 40E0 ldi r20,hlo8(2048) |
1118 0aa0 50E0 ldi r21,hhi8(2048) |
1119 0aa2 0E94 0000 call __divmodsi4 |
1120 0aa6 CA01 movw r24,r20 |
1121 0aa8 A090 0000 lds r10,FCParam+25 |
1122 0aac BB24 clr r11 |
1123 0aae CC24 clr r12 |
1124 0ab0 DD24 clr r13 |
1125 0ab2 B901 movw r22,r18 |
1126 0ab4 A601 movw r20,r12 |
1127 0ab6 9501 movw r18,r10 |
1128 0ab8 0E94 0000 call __mulsi3 |
1129 0abc 20E0 ldi r18,lo8(4096) |
1130 0abe 30E1 ldi r19,hi8(4096) |
1131 0ac0 40E0 ldi r20,hlo8(4096) |
1132 0ac2 50E0 ldi r21,hhi8(4096) |
1133 0ac4 0E94 0000 call __divmodsi4 |
1134 0ac8 7901 movw r14,r18 |
1135 0aca 8A01 movw r16,r20 |
1136 0acc 2AA7 std Y+42,r18 |
1137 0ace FBA6 std Y+43,r15 |
1138 0ad0 EC8D ldd r30,Y+28 |
1139 0ad2 FD8D ldd r31,Y+29 |
1140 0ad4 9F01 movw r18,r30 |
1141 0ad6 4427 clr r20 |
1142 0ad8 37FD sbrc r19,7 |
1143 0ada 4095 com r20 |
1144 0adc 542F mov r21,r20 |
1145 0ade 6981 ldd r22,Y+1 |
1146 0ae0 7A81 ldd r23,Y+2 |
1147 0ae2 8B81 ldd r24,Y+3 |
1148 0ae4 9C81 ldd r25,Y+4 |
1149 0ae6 0E94 0000 call __mulsi3 |
1150 0aea 20E0 ldi r18,lo8(2048) |
1151 0aec 38E0 ldi r19,hi8(2048) |
1152 0aee 40E0 ldi r20,hlo8(2048) |
1153 0af0 50E0 ldi r21,hhi8(2048) |
1154 0af2 0E94 0000 call __divmodsi4 |
1155 0af6 CA01 movw r24,r20 |
1156 0af8 B901 movw r22,r18 |
1157 0afa A601 movw r20,r12 |
1158 0afc 9501 movw r18,r10 |
1159 0afe 0E94 0000 call __mulsi3 |
1160 0b02 20E0 ldi r18,lo8(4096) |
1161 0b04 30E1 ldi r19,hi8(4096) |
1162 0b06 40E0 ldi r20,hlo8(4096) |
1163 0b08 50E0 ldi r21,hhi8(4096) |
1164 0b0a 0E94 0000 call __divmodsi4 |
1165 0b0e C901 movw r24,r18 |
1166 0b10 DA01 movw r26,r20 |
1167 0b12 28A7 std Y+40,r18 |
1168 0b14 99A7 std Y+41,r25 |
1169 0b16 5C01 movw r10,r24 |
1170 0b18 6D01 movw r12,r26 |
1171 0b1a AE18 sub r10,r14 |
1172 0b1c BF08 sbc r11,r15 |
1173 0b1e C00A sbc r12,r16 |
1174 0b20 D10A sbc r13,r17 |
1175 0b22 8CA1 ldd r24,Y+36 |
1176 0b24 9DA1 ldd r25,Y+37 |
1177 0b26 AEA1 ldd r26,Y+38 |
1178 0b28 BFA1 ldd r27,Y+39 |
1179 0b2a 8A0D add r24,r10 |
1180 0b2c 9B1D adc r25,r11 |
1181 0b2e AC1D adc r26,r12 |
1182 0b30 BD1D adc r27,r13 |
1183 0b32 8093 0000 sts YawGyroHeading,r24 |
1184 0b36 9093 0000 sts (YawGyroHeading)+1,r25 |
1185 0b3a A093 0000 sts (YawGyroHeading)+2,r26 |
1186 0b3e B093 0000 sts (YawGyroHeading)+3,r27 |
1187 0b42 9091 0000 lds r25,FCParam+26 |
1188 0b46 9E8F std Y+30,r25 |
1189 0b48 9923 tst r25 |
1190 0b4a 01F4 brne .L33 |
1191 0b4c C601 movw r24,r12 |
1192 0b4e B501 movw r22,r10 |
1193 0b50 22E0 ldi r18,lo8(2) |
1194 0b52 30E0 ldi r19,hi8(2) |
1195 0b54 40E0 ldi r20,hlo8(2) |
1196 0b56 50E0 ldi r21,hhi8(2) |
1197 0b58 0E94 0000 call __divmodsi4 |
1198 0b5c 88A1 ldd r24,Y+32 |
1199 0b5e 99A1 ldd r25,Y+33 |
1200 0b60 AAA1 ldd r26,Y+34 |
1201 0b62 BBA1 ldd r27,Y+35 |
1202 0b64 821B sub r24,r18 |
1203 0b66 930B sbc r25,r19 |
1204 0b68 A40B sbc r26,r20 |
1205 0b6a B50B sbc r27,r21 |
1206 0b6c 8093 0000 sts ReadingIntegralGyroYaw,r24 |
1207 0b70 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 |
1208 0b74 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 |
1209 0b78 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 |
1210 0b7c 1C01 movw r2,r24 |
1211 0b7e 2D01 movw r4,r26 |
1212 .L33: |
1213 0b80 A60C add r10,r6 |
1214 0b82 B71C adc r11,r7 |
1215 0b84 C81C adc r12,r8 |
1216 0b86 D91C adc r13,r9 |
1217 0b88 C601 movw r24,r12 |
1218 0b8a B501 movw r22,r10 |
1219 0b8c 2D81 ldd r18,Y+5 |
1220 0b8e 3E81 ldd r19,Y+6 |
1221 0b90 4F81 ldd r20,Y+7 |
1222 0b92 5885 ldd r21,Y+8 |
1223 0b94 0E94 0000 call __mulsi3 |
1224 0b98 20E0 ldi r18,lo8(2048) |
1225 0b9a 38E0 ldi r19,hi8(2048) |
1226 0b9c 40E0 ldi r20,hlo8(2048) |
1227 0b9e 50E0 ldi r21,hhi8(2048) |
1228 0ba0 0E94 0000 call __divmodsi4 |
1229 0ba4 CA01 movw r24,r20 |
1230 0ba6 E090 0000 lds r14,FCParam+24 |
1231 0baa FF24 clr r15 |
1232 0bac 00E0 ldi r16,lo8(0) |
1233 0bae 10E0 ldi r17,hi8(0) |
1234 0bb0 B901 movw r22,r18 |
1235 0bb2 A801 movw r20,r16 |
1236 0bb4 9701 movw r18,r14 |
1237 0bb6 0E94 0000 call __mulsi3 |
1238 0bba 20E0 ldi r18,lo8(4096) |
1239 0bbc 30E1 ldi r19,hi8(4096) |
1240 0bbe 40E0 ldi r20,hlo8(4096) |
1241 0bc0 50E0 ldi r21,hhi8(4096) |
1242 0bc2 0E94 0000 call __divmodsi4 |
1243 0bc6 3901 movw r6,r18 |
1244 0bc8 4A01 movw r8,r20 |
1245 0bca C601 movw r24,r12 |
1246 0bcc B501 movw r22,r10 |
1247 0bce 2981 ldd r18,Y+1 |
1248 0bd0 3A81 ldd r19,Y+2 |
1249 0bd2 4B81 ldd r20,Y+3 |
1250 0bd4 5C81 ldd r21,Y+4 |
1251 0bd6 0E94 0000 call __mulsi3 |
1252 0bda 20E0 ldi r18,lo8(2048) |
1253 0bdc 38E0 ldi r19,hi8(2048) |
1254 0bde 40E0 ldi r20,hlo8(2048) |
1255 0be0 50E0 ldi r21,hhi8(2048) |
1256 0be2 0E94 0000 call __divmodsi4 |
1257 0be6 CA01 movw r24,r20 |
1258 0be8 B901 movw r22,r18 |
1259 0bea A801 movw r20,r16 |
1260 0bec 9701 movw r18,r14 |
1261 0bee 0E94 0000 call __mulsi3 |
1262 0bf2 20E0 ldi r18,lo8(4096) |
1263 0bf4 30E1 ldi r19,hi8(4096) |
1264 0bf6 40E0 ldi r20,hlo8(4096) |
1265 0bf8 50E0 ldi r21,hhi8(4096) |
1266 0bfa 0E94 0000 call __divmodsi4 |
1267 0bfe 7901 movw r14,r18 |
1268 0c00 8A01 movw r16,r20 |
1269 0c02 ACA5 ldd r26,Y+44 |
1270 0c04 BDA5 ldd r27,Y+45 |
1271 0c06 A134 cpi r26,65 |
1272 0c08 B105 cpc r27,__zero_reg__ |
1273 0c0a 04F0 brlt .L34 |
1274 0c0c D401 movw r26,r8 |
1275 0c0e C301 movw r24,r6 |
1276 0c10 97FC sbrc r9,7 |
1277 0c12 00C0 rjmp .L83 |
1278 .L36: |
1279 0c14 8138 cpi r24,lo8(129) |
1280 0c16 9105 cpc r25,__zero_reg__ |
1281 0c18 A105 cpc r26,__zero_reg__ |
1282 0c1a B105 cpc r27,__zero_reg__ |
1283 0c1c 04F0 brlt .+2 |
1284 0c1e 00C0 rjmp .L35 |
1285 0c20 D801 movw r26,r16 |
1286 0c22 C701 movw r24,r14 |
1287 0c24 17FD sbrc r17,7 |
1288 0c26 00C0 rjmp .L84 |
1289 .L37: |
1290 0c28 8138 cpi r24,lo8(129) |
1291 0c2a 9105 cpc r25,__zero_reg__ |
1292 0c2c A105 cpc r26,__zero_reg__ |
1293 0c2e B105 cpc r27,__zero_reg__ |
1294 0c30 04F0 brlt .+2 |
1295 0c32 00C0 rjmp .L35 |
1296 .L34: |
1297 0c34 C401 movw r24,r8 |
1298 0c36 B301 movw r22,r6 |
1299 0c38 24E6 ldi r18,lo8(100) |
1300 0c3a 30E0 ldi r19,hi8(100) |
1301 0c3c 40E0 ldi r20,hlo8(100) |
1302 0c3e 50E0 ldi r21,hhi8(100) |
1303 0c40 0E94 0000 call __divmodsi4 |
1304 0c44 2E19 sub r18,r14 |
1305 0c46 3F09 sbc r19,r15 |
1306 0c48 3093 0000 sts (TrimNick)+1,r19 |
1307 0c4c 2093 0000 sts TrimNick,r18 |
1308 0c50 C801 movw r24,r16 |
1309 0c52 B701 movw r22,r14 |
1310 0c54 2CE9 ldi r18,lo8(-100) |
1311 0c56 3FEF ldi r19,hi8(-100) |
1312 0c58 4FEF ldi r20,hlo8(-100) |
1313 0c5a 5FEF ldi r21,hhi8(-100) |
1314 0c5c 0E94 0000 call __divmodsi4 |
1315 0c60 260D add r18,r6 |
1316 0c62 371D adc r19,r7 |
1317 0c64 3093 0000 sts (TrimRoll)+1,r19 |
1318 0c68 2093 0000 sts TrimRoll,r18 |
1319 0c6c BE8D ldd r27,Y+30 |
1320 0c6e EB2E mov r14,r27 |
1321 0c70 FF24 clr r15 |
1322 0c72 00E0 ldi r16,lo8(0) |
1323 0c74 10E0 ldi r17,hi8(0) |
1324 0c76 28A5 ldd r18,Y+40 |
1325 0c78 39A5 ldd r19,Y+41 |
1326 0c7a C901 movw r24,r18 |
1327 0c7c 9C01 movw r18,r24 |
1328 0c7e 97FD sbrc r25,7 |
1329 0c80 00C0 rjmp .L85 |
1330 .L38: |
1331 0c82 B901 movw r22,r18 |
1332 0c84 8827 clr r24 |
1333 0c86 77FD sbrc r23,7 |
1334 0c88 8095 com r24 |
1335 0c8a 982F mov r25,r24 |
1336 0c8c A801 movw r20,r16 |
1337 0c8e 9701 movw r18,r14 |
1338 0c90 0E94 0000 call __mulsi3 |
1339 0c94 6D87 std Y+13,r22 |
1340 0c96 7E87 std Y+14,r23 |
1341 0c98 8F87 std Y+15,r24 |
1342 0c9a 988B std Y+16,r25 |
1343 0c9c 2AA5 ldd r18,Y+42 |
1344 0c9e 3BA5 ldd r19,Y+43 |
1345 0ca0 C901 movw r24,r18 |
1346 0ca2 9C01 movw r18,r24 |
1347 0ca4 97FD sbrc r25,7 |
1348 0ca6 00C0 rjmp .L86 |
1349 .L39: |
1350 0ca8 B901 movw r22,r18 |
1351 0caa 8827 clr r24 |
1352 0cac 77FD sbrc r23,7 |
1353 0cae 8095 com r24 |
1354 0cb0 982F mov r25,r24 |
1355 0cb2 A801 movw r20,r16 |
1356 0cb4 9701 movw r18,r14 |
1357 0cb6 0E94 0000 call __mulsi3 |
1358 0cba 6987 std Y+9,r22 |
1359 0cbc 7A87 std Y+10,r23 |
1360 0cbe 8B87 std Y+11,r24 |
1361 0cc0 9C87 std Y+12,r25 |
1362 0cc2 E090 0000 lds r14,YawGyroHeading |
1363 0cc6 F090 0000 lds r15,(YawGyroHeading)+1 |
1364 0cca 0091 0000 lds r16,(YawGyroHeading)+2 |
1365 0cce 1091 0000 lds r17,(YawGyroHeading)+3 |
1366 0cd2 00C0 rjmp .L40 |
1367 .L82: |
1368 0cd4 6F8D ldd r22,Y+31 |
1369 0cd6 6623 tst r22 |
1370 0cd8 01F0 breq .+2 |
1371 0cda 00C0 rjmp .L64 |
1372 0cdc 8091 0000 lds r24,LoopingRoll |
1373 0ce0 8823 tst r24 |
1374 0ce2 01F0 breq .+2 |
1375 0ce4 00C0 rjmp .L64 |
1376 0ce6 8C8D ldd r24,Y+28 |
1377 0ce8 9D8D ldd r25,Y+29 |
1378 0cea 8150 subi r24,lo8(257) |
1379 0cec 9140 sbci r25,hi8(257) |
1380 0cee 04F0 brlt .+2 |
1381 0cf0 00C0 rjmp .L87 |
1382 0cf2 2C8D ldd r18,Y+28 |
1383 0cf4 3D8D ldd r19,Y+29 |
1384 0cf6 6FEF ldi r22,hi8(-256) |
1385 0cf8 2030 cpi r18,lo8(-256) |
1386 0cfa 3607 cpc r19,r22 |
1387 0cfc 04F4 brge .L62 |
1388 0cfe 8091 0000 lds r24,GyroNick |
1389 0d02 9091 0000 lds r25,(GyroNick)+1 |
1390 0d06 8050 subi r24,lo8(-(256)) |
1391 0d08 9F4F sbci r25,hi8(-(256)) |
1392 0d0a 820F add r24,r18 |
1393 0d0c 931F adc r25,r19 |
1394 0d0e 9093 0000 sts (GyroNick)+1,r25 |
1395 0d12 8093 0000 sts GyroNick,r24 |
1396 .L62: |
1397 0d16 8A8D ldd r24,Y+26 |
1398 0d18 9B8D ldd r25,Y+27 |
1399 0d1a 8150 subi r24,lo8(257) |
1400 0d1c 9140 sbci r25,hi8(257) |
1401 0d1e 04F0 brlt .+2 |
1402 0d20 00C0 rjmp .L88 |
1403 0d22 2A8D ldd r18,Y+26 |
1404 0d24 3B8D ldd r19,Y+27 |
1405 0d26 6FEF ldi r22,hi8(-256) |
1406 0d28 2030 cpi r18,lo8(-256) |
1407 0d2a 3607 cpc r19,r22 |
1408 0d2c 04F0 brlt .+2 |
1409 0d2e 00C0 rjmp .L64 |
1410 0d30 8091 0000 lds r24,GyroRoll |
1411 0d34 9091 0000 lds r25,(GyroRoll)+1 |
1412 0d38 8050 subi r24,lo8(-(256)) |
1413 0d3a 9F4F sbci r25,hi8(-(256)) |
1414 0d3c 820F add r24,r18 |
1415 0d3e 931F adc r25,r19 |
1416 0d40 9093 0000 sts (GyroRoll)+1,r25 |
1417 0d44 8093 0000 sts GyroRoll,r24 |
1418 0d48 00C0 rjmp .L64 |
1419 .L73: |
1420 0d4a E60C add r14,r6 |
1421 0d4c F71C adc r15,r7 |
1422 0d4e 081D adc r16,r8 |
1423 0d50 191D adc r17,r9 |
1424 0d52 E092 0000 sts YawGyroHeading,r14 |
1425 0d56 F092 0000 sts (YawGyroHeading)+1,r15 |
1426 0d5a 0093 0000 sts (YawGyroHeading)+2,r16 |
1427 0d5e 1093 0000 sts (YawGyroHeading)+3,r17 |
1428 0d62 00C0 rjmp .L42 |
1429 .L58: |
1430 0d64 6985 ldd r22,Y+9 |
1431 0d66 7A85 ldd r23,Y+10 |
1432 0d68 8B85 ldd r24,Y+11 |
1433 0d6a 9C85 ldd r25,Y+12 |
1434 0d6c 20E4 ldi r18,lo8(64) |
1435 0d6e 30E0 ldi r19,hi8(64) |
1436 0d70 40E0 ldi r20,hlo8(64) |
1437 0d72 50E0 ldi r21,hhi8(64) |
1438 0d74 0E94 0000 call __divmodsi4 |
1439 0d78 5095 com r21 |
1440 0d7a 4095 com r20 |
1441 0d7c 3095 com r19 |
1442 0d7e 2195 neg r18 |
1443 0d80 3F4F sbci r19,lo8(-1) |
1444 0d82 4F4F sbci r20,lo8(-1) |
1445 0d84 5F4F sbci r21,lo8(-1) |
1446 0d86 620E add r6,r18 |
1447 0d88 731E adc r7,r19 |
1448 0d8a 7092 0000 sts (TrimRoll)+1,r7 |
1449 0d8e 6092 0000 sts TrimRoll,r6 |
1450 0d92 00C0 rjmp .L59 |
1451 .L56: |
1452 0d94 6D85 ldd r22,Y+13 |
1453 0d96 7E85 ldd r23,Y+14 |
1454 0d98 8F85 ldd r24,Y+15 |
1455 0d9a 9889 ldd r25,Y+16 |
1456 0d9c 20E4 ldi r18,lo8(64) |
1457 0d9e 30E0 ldi r19,hi8(64) |
1458 0da0 40E0 ldi r20,hlo8(64) |
1459 0da2 50E0 ldi r21,hhi8(64) |
1460 0da4 0E94 0000 call __divmodsi4 |
1461 0da8 5095 com r21 |
1462 0daa 4095 com r20 |
1463 0dac 3095 com r19 |
1464 0dae 2195 neg r18 |
1465 0db0 3F4F sbci r19,lo8(-1) |
1466 0db2 4F4F sbci r20,lo8(-1) |
1467 0db4 5F4F sbci r21,lo8(-1) |
1468 0db6 820E add r8,r18 |
1469 0db8 931E adc r9,r19 |
1470 0dba 9092 0000 sts (TrimNick)+1,r9 |
1471 0dbe 8092 0000 sts TrimNick,r8 |
1472 0dc2 00C0 rjmp .L57 |
1473 .L67: |
1474 0dc4 0796 adiw r24,7 |
1475 0dc6 00C0 rjmp .L10 |
1476 .L71: |
1477 0dc8 295F subi r18,lo8(-(7)) |
1478 0dca 3F4F sbci r19,hi8(-(7)) |
1479 0dcc 00C0 rjmp .L19 |
1480 .L68: |
1481 0dce 295F subi r18,lo8(-(7)) |
1482 0dd0 3F4F sbci r19,hi8(-(7)) |
1483 0dd2 00C0 rjmp .L11 |
1484 .L81: |
1485 0dd4 215F subi r18,lo8(-(15)) |
1486 0dd6 3F4F sbci r19,hi8(-(15)) |
1487 0dd8 00C0 rjmp .L55 |
1488 .L74: |
1489 0dda 2F5F subi r18,lo8(-(1)) |
1490 0ddc 3F4F sbci r19,hi8(-(1)) |
1491 0dde 00C0 rjmp .L48 |
1492 .L77: |
1493 0de0 215F subi r18,lo8(-(15)) |
1494 0de2 3F4F sbci r19,hi8(-(15)) |
1495 0de4 00C0 rjmp .L51 |
1496 .L78: |
1497 0de6 2F5F subi r18,lo8(-(1)) |
1498 0de8 3F4F sbci r19,hi8(-(1)) |
1499 0dea 00C0 rjmp .L52 |
1500 .L87: |
1501 0dec 8091 0000 lds r24,GyroNick |
1502 0df0 9091 0000 lds r25,(GyroNick)+1 |
1503 0df4 8050 subi r24,lo8(-(-256)) |
1504 0df6 9140 sbci r25,hi8(-(-256)) |
1505 0df8 EC8D ldd r30,Y+28 |
1506 0dfa FD8D ldd r31,Y+29 |
1507 0dfc 8E0F add r24,r30 |
1508 0dfe 9F1F adc r25,r31 |
1509 0e00 9093 0000 sts (GyroNick)+1,r25 |
1510 0e04 8093 0000 sts GyroNick,r24 |
1511 0e08 00C0 rjmp .L62 |
1512 .L35: |
1513 0e0a 81E0 ldi r24,lo8(1) |
1514 0e0c 8093 0000 sts FunnelCourse,r24 |
1515 0e10 00C0 rjmp .L34 |
1516 .L83: |
1517 0e12 8827 clr r24 |
1518 0e14 9927 clr r25 |
1519 0e16 DC01 movw r26,r24 |
1520 0e18 8619 sub r24,r6 |
1521 0e1a 9709 sbc r25,r7 |
1522 0e1c A809 sbc r26,r8 |
1523 0e1e B909 sbc r27,r9 |
1524 0e20 00C0 rjmp .L36 |
1525 .L88: |
1526 0e22 8091 0000 lds r24,GyroRoll |
1527 0e26 9091 0000 lds r25,(GyroRoll)+1 |
1528 0e2a 8050 subi r24,lo8(-(-256)) |
1529 0e2c 9140 sbci r25,hi8(-(-256)) |
1530 0e2e EA8D ldd r30,Y+26 |
1531 0e30 FB8D ldd r31,Y+27 |
1532 0e32 8E0F add r24,r30 |
1533 0e34 9F1F adc r25,r31 |
1534 0e36 9093 0000 sts (GyroRoll)+1,r25 |
1535 0e3a 8093 0000 sts GyroRoll,r24 |
1536 0e3e 00C0 rjmp .L64 |
1537 .L86: |
1538 0e40 3095 com r19 |
1539 0e42 2195 neg r18 |
1540 0e44 3F4F sbci r19,lo8(-1) |
1541 0e46 00C0 rjmp .L39 |
1542 .L85: |
1543 0e48 3095 com r19 |
1544 0e4a 2195 neg r18 |
1545 0e4c 3F4F sbci r19,lo8(-1) |
1546 0e4e 00C0 rjmp .L38 |
1547 .L84: |
1548 0e50 8827 clr r24 |
1549 0e52 9927 clr r25 |
1550 0e54 DC01 movw r26,r24 |
1551 0e56 8E19 sub r24,r14 |
1552 0e58 9F09 sbc r25,r15 |
1553 0e5a A00B sbc r26,r16 |
1554 0e5c B10B sbc r27,r17 |
1555 0e5e 00C0 rjmp .L37 |
1557 .global ParameterMapping |
1559 ParameterMapping: |
1560 /* prologue: function */ |
1561 /* frame size = 0 */ |
1562 0e60 8091 0000 lds r24,RC_Quality |
1563 0e64 813A cpi r24,lo8(-95) |
1564 0e66 00F0 brlo .L253 |
1565 0e68 8091 0000 lds r24,ParamSet+11 |
1566 0e6c 8B3F cpi r24,lo8(-5) |
1567 0e6e 00F4 brsh .L261 |
1568 0e70 8093 0000 sts FCParam+1,r24 |
1569 .L93: |
1570 0e74 8091 0000 lds r24,ParamSet+10 |
1571 0e78 8B3F cpi r24,lo8(-5) |
1572 0e7a 00F0 brlo .L96 |
1573 0e7c 8B3F cpi r24,lo8(-5) |
1574 0e7e 01F4 brne .+2 |
1575 0e80 00C0 rjmp .L262 |
1576 0e82 8C3F cpi r24,lo8(-4) |
1577 0e84 01F4 brne .+2 |
1578 0e86 00C0 rjmp .L263 |
1579 0e88 8D3F cpi r24,lo8(-3) |
1580 0e8a 01F4 brne .+2 |
1581 0e8c 00C0 rjmp .L264 |
1582 0e8e 8E3F cpi r24,lo8(-2) |
1583 0e90 01F4 brne .L98 |
1584 0e92 8091 0000 lds r24,Poti4 |
1585 0e96 8093 0000 sts FCParam,r24 |
1586 0e9a 00C0 rjmp .L98 |
1587 .L248: |
1588 0e9c 8093 0000 sts FCParam+28,r24 |
1589 .L250: |
1590 0ea0 6091 0000 lds r22,FCParam+12 |
1591 0ea4 70E0 ldi r23,lo8(0) |
1592 0ea6 6F5F subi r22,lo8(-(1)) |
1593 0ea8 7F4F sbci r23,hi8(-(1)) |
1594 0eaa 8CE3 ldi r24,lo8(10300) |
1595 0eac 98E2 ldi r25,hi8(10300) |
1596 0eae 0E94 0000 call __divmodhi4 |
1597 0eb2 7093 0000 sts (Ki)+1,r23 |
1598 0eb6 6093 0000 sts Ki,r22 |
1599 .L253: |
1600 0eba 0895 ret |
1601 .L261: |
1602 0ebc 8B3F cpi r24,lo8(-5) |
1603 0ebe 01F4 brne .+2 |
1604 0ec0 00C0 rjmp .L265 |
1605 0ec2 8C3F cpi r24,lo8(-4) |
1606 0ec4 01F4 brne .+2 |
1607 0ec6 00C0 rjmp .L266 |
1608 0ec8 8D3F cpi r24,lo8(-3) |
1609 0eca 01F4 brne .+2 |
1610 0ecc 00C0 rjmp .L267 |
1611 0ece 8E3F cpi r24,lo8(-2) |
1612 0ed0 01F4 brne .L93 |
1613 0ed2 8091 0000 lds r24,Poti4 |
1614 0ed6 8093 0000 sts FCParam+1,r24 |
1615 0eda 00C0 rjmp .L93 |
1616 .L96: |
1617 0edc 8093 0000 sts FCParam,r24 |
1618 .L98: |
1619 0ee0 8091 0000 lds r24,FCParam |
1620 0ee4 8823 tst r24 |
1621 0ee6 01F4 brne .L101 |
1622 0ee8 1092 0000 sts FCParam,__zero_reg__ |
1623 .L102: |
1624 0eec 8091 0000 lds r24,ParamSet+12 |
1625 0ef0 8B3F cpi r24,lo8(-5) |
1626 0ef2 00F0 brlo .L103 |
1627 .L271: |
1628 0ef4 8B3F cpi r24,lo8(-5) |
1629 0ef6 01F4 brne .+2 |
1630 0ef8 00C0 rjmp .L268 |
1631 0efa 8C3F cpi r24,lo8(-4) |
1632 0efc 01F4 brne .+2 |
1633 0efe 00C0 rjmp .L269 |
1634 0f00 8D3F cpi r24,lo8(-3) |
1635 0f02 01F4 brne .+2 |
1636 0f04 00C0 rjmp .L270 |
1637 0f06 8E3F cpi r24,lo8(-2) |
1638 0f08 01F4 brne .L105 |
1639 0f0a 8091 0000 lds r24,Poti4 |
1640 0f0e 8093 0000 sts FCParam+2,r24 |
1641 0f12 00C0 rjmp .L105 |
1642 .L101: |
1643 0f14 8436 cpi r24,lo8(100) |
1644 0f16 00F0 brlo .L102 |
1645 0f18 84E6 ldi r24,lo8(100) |
1646 0f1a 8093 0000 sts FCParam,r24 |
1647 0f1e 8091 0000 lds r24,ParamSet+12 |
1648 0f22 8B3F cpi r24,lo8(-5) |
1649 0f24 00F4 brsh .L271 |
1650 .L103: |
1651 0f26 8093 0000 sts FCParam+2,r24 |
1652 .L105: |
1653 0f2a 8091 0000 lds r24,FCParam+2 |
1654 0f2e 8823 tst r24 |
1655 0f30 01F4 brne .L108 |
1656 0f32 1092 0000 sts FCParam+2,__zero_reg__ |
1657 .L109: |
1658 0f36 8091 0000 lds r24,ParamSet+14 |
1659 0f3a 8B3F cpi r24,lo8(-5) |
1660 0f3c 00F0 brlo .L110 |
1661 .L275: |
1662 0f3e 8B3F cpi r24,lo8(-5) |
1663 0f40 01F4 brne .+2 |
1664 0f42 00C0 rjmp .L272 |
1665 0f44 8C3F cpi r24,lo8(-4) |
1666 0f46 01F4 brne .+2 |
1667 0f48 00C0 rjmp .L273 |
1668 0f4a 8D3F cpi r24,lo8(-3) |
1669 0f4c 01F4 brne .+2 |
1670 0f4e 00C0 rjmp .L274 |
1671 0f50 8E3F cpi r24,lo8(-2) |
1672 0f52 01F4 brne .L112 |
1673 0f54 8091 0000 lds r24,Poti4 |
1674 0f58 8093 0000 sts FCParam+3,r24 |
1675 0f5c 00C0 rjmp .L112 |
1676 .L108: |
1677 0f5e 8436 cpi r24,lo8(100) |
1678 0f60 00F0 brlo .L109 |
1679 0f62 84E6 ldi r24,lo8(100) |
1680 0f64 8093 0000 sts FCParam+2,r24 |
1681 0f68 8091 0000 lds r24,ParamSet+14 |
1682 0f6c 8B3F cpi r24,lo8(-5) |
1683 0f6e 00F4 brsh .L275 |
1684 .L110: |
1685 0f70 8093 0000 sts FCParam+3,r24 |
1686 .L112: |
1687 0f74 8091 0000 lds r24,ParamSet+16 |
1688 0f78 8B3F cpi r24,lo8(-5) |
1689 0f7a 00F0 brlo .L115 |
1690 0f7c 8B3F cpi r24,lo8(-5) |
1691 0f7e 01F4 brne .+2 |
1692 0f80 00C0 rjmp .L276 |
1693 0f82 8C3F cpi r24,lo8(-4) |
1694 0f84 01F4 brne .+2 |
1695 0f86 00C0 rjmp .L277 |
1696 0f88 8D3F cpi r24,lo8(-3) |
1697 0f8a 01F4 brne .+2 |
1698 0f8c 00C0 rjmp .L278 |
1699 0f8e 8E3F cpi r24,lo8(-2) |
1700 0f90 01F4 brne .L117 |
1701 0f92 8091 0000 lds r24,Poti4 |
1702 0f96 8093 0000 sts FCParam+4,r24 |
1703 0f9a 00C0 rjmp .L117 |
1704 .L115: |
1705 0f9c 8093 0000 sts FCParam+4,r24 |
1706 .L117: |
1707 0fa0 8091 0000 lds r24,ParamSet+24 |
1708 0fa4 8B3F cpi r24,lo8(-5) |
1709 0fa6 00F0 brlo .L120 |
1710 0fa8 8B3F cpi r24,lo8(-5) |
1711 0faa 01F4 brne .+2 |
1712 0fac 00C0 rjmp .L279 |
1713 0fae 8C3F cpi r24,lo8(-4) |
1714 0fb0 01F4 brne .+2 |
1715 0fb2 00C0 rjmp .L280 |
1716 0fb4 8D3F cpi r24,lo8(-3) |
1717 0fb6 01F4 brne .+2 |
1718 0fb8 00C0 rjmp .L281 |
1719 0fba 8E3F cpi r24,lo8(-2) |
1720 0fbc 01F4 brne .L122 |
1721 0fbe 8091 0000 lds r24,Poti4 |
1722 0fc2 8093 0000 sts FCParam+5,r24 |
1723 0fc6 00C0 rjmp .L122 |
1724 .L120: |
1725 0fc8 8093 0000 sts FCParam+5,r24 |
1726 .L122: |
1727 0fcc 8091 0000 lds r24,ParamSet+25 |
1728 0fd0 8B3F cpi r24,lo8(-5) |
1729 0fd2 00F0 brlo .L125 |
1730 0fd4 8B3F cpi r24,lo8(-5) |
1731 0fd6 01F4 brne .+2 |
1732 0fd8 00C0 rjmp .L282 |
1733 0fda 8C3F cpi r24,lo8(-4) |
1734 0fdc 01F4 brne .+2 |
1735 0fde 00C0 rjmp .L283 |
1736 0fe0 8D3F cpi r24,lo8(-3) |
1737 0fe2 01F4 brne .+2 |
1738 0fe4 00C0 rjmp .L284 |
1739 0fe6 8E3F cpi r24,lo8(-2) |
1740 0fe8 01F4 brne .L127 |
1741 0fea 8091 0000 lds r24,Poti4 |
1742 0fee 8093 0000 sts FCParam+7,r24 |
1743 0ff2 00C0 rjmp .L127 |
1744 .L125: |
1745 0ff4 8093 0000 sts FCParam+7,r24 |
1746 .L127: |
1747 0ff8 8091 0000 lds r24,FCParam+7 |
1748 0ffc 8B30 cpi r24,lo8(11) |
1749 0ffe 00F4 brsh .L130 |
1750 1000 8AE0 ldi r24,lo8(10) |
1751 1002 8093 0000 sts FCParam+7,r24 |
1752 .L131: |
1753 1006 8091 0000 lds r24,ParamSet+26 |
1754 100a 8B3F cpi r24,lo8(-5) |
1755 100c 00F0 brlo .L132 |
1756 100e 8B3F cpi r24,lo8(-5) |
1757 1010 01F4 brne .+2 |
1758 1012 00C0 rjmp .L285 |
1759 1014 8C3F cpi r24,lo8(-4) |
1760 1016 01F4 brne .+2 |
1761 1018 00C0 rjmp .L286 |
1762 101a 8D3F cpi r24,lo8(-3) |
1763 101c 01F4 brne .+2 |
1764 101e 00C0 rjmp .L287 |
1765 1020 8E3F cpi r24,lo8(-2) |
1766 1022 01F4 brne .L134 |
1767 1024 8091 0000 lds r24,Poti4 |
1768 1028 8093 0000 sts FCParam+8,r24 |
1769 102c 00C0 rjmp .L134 |
1770 .L130: |
1771 102e 8F3F cpi r24,lo8(-1) |
1772 1030 01F4 brne .L131 |
1773 1032 8093 0000 sts FCParam+7,r24 |
1774 1036 00C0 rjmp .L131 |
1775 .L132: |
1776 1038 8093 0000 sts FCParam+8,r24 |
1777 .L134: |
1778 103c 8091 0000 lds r24,ParamSet+27 |
1779 1040 8B3F cpi r24,lo8(-5) |
1780 1042 00F0 brlo .L137 |
1781 1044 8B3F cpi r24,lo8(-5) |
1782 1046 01F4 brne .+2 |
1783 1048 00C0 rjmp .L288 |
1784 104a 8C3F cpi r24,lo8(-4) |
1785 104c 01F4 brne .+2 |
1786 104e 00C0 rjmp .L289 |
1787 1050 8D3F cpi r24,lo8(-3) |
1788 1052 01F4 brne .+2 |
1789 1054 00C0 rjmp .L290 |
1790 1056 8E3F cpi r24,lo8(-2) |
1791 1058 01F4 brne .L139 |
1792 105a 8091 0000 lds r24,Poti4 |
1793 105e 8093 0000 sts FCParam+6,r24 |
1794 1062 00C0 rjmp .L139 |
1795 .L137: |
1796 1064 8093 0000 sts FCParam+6,r24 |
1797 .L139: |
1798 1068 8091 0000 lds r24,ParamSet+28 |
1799 106c 8B3F cpi r24,lo8(-5) |
1800 106e 00F0 brlo .L142 |
1801 1070 8B3F cpi r24,lo8(-5) |
1802 1072 01F4 brne .+2 |
1803 1074 00C0 rjmp .L291 |
1804 1076 8C3F cpi r24,lo8(-4) |
1805 1078 01F4 brne .+2 |
1806 107a 00C0 rjmp .L292 |
1807 107c 8D3F cpi r24,lo8(-3) |
1808 107e 01F4 brne .+2 |
1809 1080 00C0 rjmp .L293 |
1810 1082 8E3F cpi r24,lo8(-2) |
1811 1084 01F4 brne .L144 |
1812 1086 8091 0000 lds r24,Poti4 |
1813 108a 8093 0000 sts FCParam+9,r24 |
1814 108e 00C0 rjmp .L144 |
1815 .L142: |
1816 1090 8093 0000 sts FCParam+9,r24 |
1817 .L144: |
1818 1094 8091 0000 lds r24,FCParam+9 |
1819 1098 8B30 cpi r24,lo8(11) |
1820 109a 00F4 brsh .L147 |
1821 109c 8AE0 ldi r24,lo8(10) |
1822 109e 8093 0000 sts FCParam+9,r24 |
1823 .L148: |
1824 10a2 8091 0000 lds r24,ParamSet+29 |
1825 10a6 8B3F cpi r24,lo8(-5) |
1826 10a8 00F0 brlo .L149 |
1827 10aa 8B3F cpi r24,lo8(-5) |
1828 10ac 01F4 brne .+2 |
1829 10ae 00C0 rjmp .L294 |
1830 10b0 8C3F cpi r24,lo8(-4) |
1831 10b2 01F4 brne .+2 |
1832 10b4 00C0 rjmp .L295 |
1833 10b6 8D3F cpi r24,lo8(-3) |
1834 10b8 01F4 brne .+2 |
1835 10ba 00C0 rjmp .L296 |
1836 10bc 8E3F cpi r24,lo8(-2) |
1837 10be 01F4 brne .L151 |
1838 10c0 8091 0000 lds r24,Poti4 |
1839 10c4 8093 0000 sts FCParam+10,r24 |
1840 10c8 00C0 rjmp .L151 |
1841 .L147: |
1842 10ca 8F3F cpi r24,lo8(-1) |
1843 10cc 01F4 brne .L148 |
1844 10ce 8093 0000 sts FCParam+9,r24 |
1845 10d2 00C0 rjmp .L148 |
1846 .L149: |
1847 10d4 8093 0000 sts FCParam+10,r24 |
1848 .L151: |
1849 10d8 8091 0000 lds r24,ParamSet+34 |
1850 10dc 8B3F cpi r24,lo8(-5) |
1851 10de 00F0 brlo .L154 |
1852 10e0 8B3F cpi r24,lo8(-5) |
1853 10e2 01F4 brne .+2 |
1854 10e4 00C0 rjmp .L297 |
1855 10e6 8C3F cpi r24,lo8(-4) |
1856 10e8 01F4 brne .+2 |
1857 10ea 00C0 rjmp .L298 |
1858 10ec 8D3F cpi r24,lo8(-3) |
1859 10ee 01F4 brne .+2 |
1860 10f0 00C0 rjmp .L299 |
1861 10f2 8E3F cpi r24,lo8(-2) |
1862 10f4 01F4 brne .L156 |
1863 10f6 8091 0000 lds r24,Poti4 |
1864 10fa 8093 0000 sts FCParam+12,r24 |
1865 10fe 00C0 rjmp .L156 |
1866 .L154: |
1867 1100 8093 0000 sts FCParam+12,r24 |
1868 .L156: |
1869 1104 8091 0000 lds r24,ParamSet+35 |
1870 1108 8B3F cpi r24,lo8(-5) |
1871 110a 00F0 brlo .L159 |
1872 110c 8B3F cpi r24,lo8(-5) |
1873 110e 01F4 brne .+2 |
1874 1110 00C0 rjmp .L300 |
1875 1112 8C3F cpi r24,lo8(-4) |
1876 1114 01F4 brne .+2 |
1877 1116 00C0 rjmp .L301 |
1878 1118 8D3F cpi r24,lo8(-3) |
1879 111a 01F4 brne .+2 |
1880 111c 00C0 rjmp .L302 |
1881 111e 8E3F cpi r24,lo8(-2) |
1882 1120 01F4 brne .L161 |
1883 1122 8091 0000 lds r24,Poti4 |
1884 1126 8093 0000 sts FCParam+13,r24 |
1885 112a 00C0 rjmp .L161 |
1886 .L159: |
1887 112c 8093 0000 sts FCParam+13,r24 |
1888 .L161: |
1889 1130 8091 0000 lds r24,ParamSet+36 |
1890 1134 8B3F cpi r24,lo8(-5) |
1891 1136 00F0 brlo .L164 |
1892 1138 8B3F cpi r24,lo8(-5) |
1893 113a 01F4 brne .+2 |
1894 113c 00C0 rjmp .L303 |
1895 113e 8C3F cpi r24,lo8(-4) |
1896 1140 01F4 brne .+2 |
1897 1142 00C0 rjmp .L304 |
1898 1144 8D3F cpi r24,lo8(-3) |
1899 1146 01F4 brne .+2 |
1900 1148 00C0 rjmp .L305 |
1901 114a 8E3F cpi r24,lo8(-2) |
1902 114c 01F4 brne .L166 |
1903 114e 8091 0000 lds r24,Poti4 |
1904 1152 8093 0000 sts FCParam+14,r24 |
1905 1156 00C0 rjmp .L166 |
1906 .L164: |
1907 1158 8093 0000 sts FCParam+14,r24 |
1908 .L166: |
1909 115c 8091 0000 lds r24,ParamSet+37 |
1910 1160 8B3F cpi r24,lo8(-5) |
1911 1162 00F0 brlo .L169 |
1912 1164 8B3F cpi r24,lo8(-5) |
1913 1166 01F4 brne .+2 |
1914 1168 00C0 rjmp .L306 |
1915 116a 8C3F cpi r24,lo8(-4) |
1916 116c 01F4 brne .+2 |
1917 116e 00C0 rjmp .L307 |
1918 1170 8D3F cpi r24,lo8(-3) |
1919 1172 01F4 brne .+2 |
1920 1174 00C0 rjmp .L308 |
1921 1176 8E3F cpi r24,lo8(-2) |
1922 1178 01F4 brne .L171 |
1923 117a 8091 0000 lds r24,Poti4 |
1924 117e 8093 0000 sts FCParam+15,r24 |
1925 1182 00C0 rjmp .L171 |
1926 .L169: |
1927 1184 8093 0000 sts FCParam+15,r24 |
1928 .L171: |
1929 1188 8091 0000 lds r24,ParamSet+38 |
1930 118c 8B3F cpi r24,lo8(-5) |
1931 118e 00F0 brlo .L174 |
1932 1190 8B3F cpi r24,lo8(-5) |
1933 1192 01F4 brne .+2 |
1934 1194 00C0 rjmp .L309 |
1935 1196 8C3F cpi r24,lo8(-4) |
1936 1198 01F4 brne .+2 |
1937 119a 00C0 rjmp .L310 |
1938 119c 8D3F cpi r24,lo8(-3) |
1939 119e 01F4 brne .+2 |
1940 11a0 00C0 rjmp .L311 |
1941 11a2 8E3F cpi r24,lo8(-2) |
1942 11a4 01F4 brne .L176 |
1943 11a6 8091 0000 lds r24,Poti4 |
1944 11aa 8093 0000 sts FCParam+16,r24 |
1945 11ae 00C0 rjmp .L176 |
1946 .L174: |
1947 11b0 8093 0000 sts FCParam+16,r24 |
1948 .L176: |
1949 11b4 8091 0000 lds r24,ParamSet+59 |
1950 11b8 8B3F cpi r24,lo8(-5) |
1951 11ba 00F0 brlo .L179 |
1952 11bc 8B3F cpi r24,lo8(-5) |
1953 11be 01F4 brne .+2 |
1954 11c0 00C0 rjmp .L312 |
1955 11c2 8C3F cpi r24,lo8(-4) |
1956 11c4 01F4 brne .+2 |
1957 11c6 00C0 rjmp .L313 |
1958 11c8 8D3F cpi r24,lo8(-3) |
1959 11ca 01F4 brne .+2 |
1960 11cc 00C0 rjmp .L314 |
1961 11ce 8E3F cpi r24,lo8(-2) |
1962 11d0 01F4 brne .L181 |
1963 11d2 8091 0000 lds r24,Poti4 |
1964 11d6 8093 0000 sts FCParam+17,r24 |
1965 11da 00C0 rjmp .L181 |
1966 .L179: |
1967 11dc 8093 0000 sts FCParam+17,r24 |
1968 .L181: |
1969 11e0 8091 0000 lds r24,ParamSet+60 |
1970 11e4 8B3F cpi r24,lo8(-5) |
1971 11e6 00F0 brlo .L184 |
1972 11e8 8B3F cpi r24,lo8(-5) |
1973 11ea 01F4 brne .+2 |
1974 11ec 00C0 rjmp .L315 |
1975 11ee 8C3F cpi r24,lo8(-4) |
1976 11f0 01F4 brne .+2 |
1977 11f2 00C0 rjmp .L316 |
1978 11f4 8D3F cpi r24,lo8(-3) |
1979 11f6 01F4 brne .+2 |
1980 11f8 00C0 rjmp .L317 |
1981 11fa 8E3F cpi r24,lo8(-2) |
1982 11fc 01F4 brne .L186 |
1983 11fe 8091 0000 lds r24,Poti4 |
1984 1202 8093 0000 sts FCParam+18,r24 |
1985 1206 00C0 rjmp .L186 |
1986 .L184: |
1987 1208 8093 0000 sts FCParam+18,r24 |
1988 .L186: |
1989 120c 8091 0000 lds r24,ParamSet+61 |
1990 1210 8B3F cpi r24,lo8(-5) |
1991 1212 00F0 brlo .L189 |
1992 1214 8B3F cpi r24,lo8(-5) |
1993 1216 01F4 brne .+2 |
1994 1218 00C0 rjmp .L318 |
1995 121a 8C3F cpi r24,lo8(-4) |
1996 121c 01F4 brne .+2 |
1997 121e 00C0 rjmp .L319 |
1998 1220 8D3F cpi r24,lo8(-3) |
1999 1222 01F4 brne .+2 |
2000 1224 00C0 rjmp .L320 |
2001 1226 8E3F cpi r24,lo8(-2) |
2002 1228 01F4 brne .L191 |
2003 122a 8091 0000 lds r24,Poti4 |
2004 122e 8093 0000 sts FCParam+19,r24 |
2005 1232 00C0 rjmp .L191 |
2006 .L189: |
2007 1234 8093 0000 sts FCParam+19,r24 |
2008 .L191: |
2009 1238 8091 0000 lds r24,ParamSet+62 |
2010 123c 8B3F cpi r24,lo8(-5) |
2011 123e 00F0 brlo .L194 |
2012 1240 8B3F cpi r24,lo8(-5) |
2013 1242 01F4 brne .+2 |
2014 1244 00C0 rjmp .L321 |
2015 1246 8C3F cpi r24,lo8(-4) |
2016 1248 01F4 brne .+2 |
2017 124a 00C0 rjmp .L322 |
2018 124c 8D3F cpi r24,lo8(-3) |
2019 124e 01F4 brne .+2 |
2020 1250 00C0 rjmp .L323 |
2021 1252 8E3F cpi r24,lo8(-2) |
2022 1254 01F4 brne .L196 |
2023 1256 8091 0000 lds r24,Poti4 |
2024 125a 8093 0000 sts FCParam+20,r24 |
2025 125e 00C0 rjmp .L196 |
2026 .L194: |
2027 1260 8093 0000 sts FCParam+20,r24 |
2028 .L196: |
2029 1264 8091 0000 lds r24,ParamSet+39 |
2030 1268 8B3F cpi r24,lo8(-5) |
2031 126a 00F0 brlo .L199 |
2032 126c 8B3F cpi r24,lo8(-5) |
2033 126e 01F4 brne .+2 |
2034 1270 00C0 rjmp .L324 |
2035 1272 8C3F cpi r24,lo8(-4) |
2036 1274 01F4 brne .+2 |
2037 1276 00C0 rjmp .L325 |
2038 1278 8D3F cpi r24,lo8(-3) |
2039 127a 01F4 brne .+2 |
2040 127c 00C0 rjmp .L326 |
2041 127e 8E3F cpi r24,lo8(-2) |
2042 1280 01F4 brne .L201 |
2043 1282 8091 0000 lds r24,Poti4 |
2044 1286 8093 0000 sts FCParam+21,r24 |
2045 128a 00C0 rjmp .L201 |
2046 .L199: |
2047 128c 8093 0000 sts FCParam+21,r24 |
2048 .L201: |
2049 1290 8091 0000 lds r24,ParamSet+43 |
2050 1294 8B3F cpi r24,lo8(-5) |
2051 1296 00F0 brlo .L204 |
2052 1298 8B3F cpi r24,lo8(-5) |
2053 129a 01F4 brne .+2 |
2054 129c 00C0 rjmp .L327 |
2055 129e 8C3F cpi r24,lo8(-4) |
2056 12a0 01F4 brne .+2 |
2057 12a2 00C0 rjmp .L328 |
2058 12a4 8D3F cpi r24,lo8(-3) |
2059 12a6 01F4 brne .+2 |
2060 12a8 00C0 rjmp .L329 |
2061 12aa 8E3F cpi r24,lo8(-2) |
2062 12ac 01F4 brne .L206 |
2063 12ae 8091 0000 lds r24,Poti4 |
2064 12b2 8093 0000 sts FCParam+22,r24 |
2065 12b6 00C0 rjmp .L206 |
2066 .L204: |
2067 12b8 8093 0000 sts FCParam+22,r24 |
2068 .L206: |
2069 12bc 8091 0000 lds r24,ParamSet+48 |
2070 12c0 8B3F cpi r24,lo8(-5) |
2071 12c2 00F0 brlo .L209 |
2072 12c4 8B3F cpi r24,lo8(-5) |
2073 12c6 01F4 brne .+2 |
2074 12c8 00C0 rjmp .L330 |
2075 12ca 8C3F cpi r24,lo8(-4) |
2076 12cc 01F4 brne .+2 |
2077 12ce 00C0 rjmp .L331 |
2078 12d0 8D3F cpi r24,lo8(-3) |
2079 12d2 01F4 brne .+2 |
2080 12d4 00C0 rjmp .L332 |
2081 12d6 8E3F cpi r24,lo8(-2) |
2082 12d8 01F4 brne .L211 |
2083 12da 8091 0000 lds r24,Poti4 |
2084 12de 8093 0000 sts FCParam+23,r24 |
2085 12e2 00C0 rjmp .L211 |
2086 .L209: |
2087 12e4 8093 0000 sts FCParam+23,r24 |
2088 .L211: |
2089 12e8 8091 0000 lds r24,ParamSet+51 |
2090 12ec 8B3F cpi r24,lo8(-5) |
2091 12ee 00F0 brlo .L214 |
2092 12f0 8B3F cpi r24,lo8(-5) |
2093 12f2 01F4 brne .+2 |
2094 12f4 00C0 rjmp .L333 |
2095 12f6 8C3F cpi r24,lo8(-4) |
2096 12f8 01F4 brne .+2 |
2097 12fa 00C0 rjmp .L334 |
2098 12fc 8D3F cpi r24,lo8(-3) |
2099 12fe 01F4 brne .+2 |
2100 1300 00C0 rjmp .L335 |
2101 1302 8E3F cpi r24,lo8(-2) |
2102 1304 01F4 brne .L216 |
2103 1306 8091 0000 lds r24,Poti4 |
2104 130a 8093 0000 sts FCParam+24,r24 |
2105 130e 00C0 rjmp .L216 |
2106 .L214: |
2107 1310 8093 0000 sts FCParam+24,r24 |
2108 .L216: |
2109 1314 8091 0000 lds r24,ParamSet+52 |
2110 1318 8B3F cpi r24,lo8(-5) |
2111 131a 00F0 brlo .L219 |
2112 131c 8B3F cpi r24,lo8(-5) |
2113 131e 01F4 brne .+2 |
2114 1320 00C0 rjmp .L336 |
2115 1322 8C3F cpi r24,lo8(-4) |
2116 1324 01F4 brne .+2 |
2117 1326 00C0 rjmp .L337 |
2118 1328 8D3F cpi r24,lo8(-3) |
2119 132a 01F4 brne .+2 |
2120 132c 00C0 rjmp .L338 |
2121 132e 8E3F cpi r24,lo8(-2) |
2122 1330 01F4 brne .L221 |
2123 1332 8091 0000 lds r24,Poti4 |
2124 1336 8093 0000 sts FCParam+25,r24 |
2125 133a 00C0 rjmp .L221 |
2126 .L219: |
2127 133c 8093 0000 sts FCParam+25,r24 |
2128 .L221: |
2129 1340 8091 0000 lds r24,ParamSet+53 |
2130 1344 8B3F cpi r24,lo8(-5) |
2131 1346 00F0 brlo .L224 |
2132 1348 8B3F cpi r24,lo8(-5) |
2133 134a 01F4 brne .+2 |
2134 134c 00C0 rjmp .L339 |
2135 134e 8C3F cpi r24,lo8(-4) |
2136 1350 01F4 brne .+2 |
2137 1352 00C0 rjmp .L340 |
2138 1354 8D3F cpi r24,lo8(-3) |
2139 1356 01F4 brne .+2 |
2140 1358 00C0 rjmp .L341 |
2141 135a 8E3F cpi r24,lo8(-2) |
2142 135c 01F4 brne .L226 |
2143 135e 8091 0000 lds r24,Poti4 |
2144 1362 8093 0000 sts FCParam+26,r24 |
2145 1366 00C0 rjmp .L226 |
2146 .L224: |
2147 1368 8093 0000 sts FCParam+26,r24 |
2148 .L226: |
2149 136c 8091 0000 lds r24,ParamSet+58 |
2150 1370 8B3F cpi r24,lo8(-5) |
2151 1372 00F0 brlo .L229 |
2152 1374 8B3F cpi r24,lo8(-5) |
2153 1376 01F4 brne .+2 |
2154 1378 00C0 rjmp .L342 |
2155 137a 8C3F cpi r24,lo8(-4) |
2156 137c 01F4 brne .+2 |
2157 137e 00C0 rjmp .L343 |
2158 1380 8D3F cpi r24,lo8(-3) |
2159 1382 01F4 brne .+2 |
2160 1384 00C0 rjmp .L344 |
2161 1386 8E3F cpi r24,lo8(-2) |
2162 1388 01F4 brne .L231 |
2163 138a 8091 0000 lds r24,Poti4 |
2164 138e 8093 0000 sts FCParam+27,r24 |
2165 1392 00C0 rjmp .L231 |
2166 .L229: |
2167 1394 8093 0000 sts FCParam+27,r24 |
2168 .L231: |
2169 1398 8091 0000 lds r24,ParamSet+64 |
2170 139c 8B3F cpi r24,lo8(-5) |
2171 139e 00F0 brlo .L234 |
2172 13a0 8B3F cpi r24,lo8(-5) |
2173 13a2 01F4 brne .+2 |
2174 13a4 00C0 rjmp .L345 |
2175 13a6 8C3F cpi r24,lo8(-4) |
2176 13a8 01F4 brne .+2 |
2177 13aa 00C0 rjmp .L346 |
2178 13ac 8D3F cpi r24,lo8(-3) |
2179 13ae 01F4 brne .+2 |
2180 13b0 00C0 rjmp .L347 |
2181 13b2 8E3F cpi r24,lo8(-2) |
2182 13b4 01F4 brne .L236 |
2183 13b6 8091 0000 lds r24,Poti4 |
2184 13ba 8093 0000 sts FCParam+29,r24 |
2185 13be 00C0 rjmp .L236 |
2186 .L234: |
2187 13c0 8093 0000 sts FCParam+29,r24 |
2188 .L236: |
2189 13c4 8091 0000 lds r24,FCParam+29 |
2190 13c8 8230 cpi r24,lo8(2) |
2191 13ca 00F4 brsh .+2 |
2192 13cc 00C0 rjmp .L348 |
2193 13ce 8F3F cpi r24,lo8(-1) |
2194 13d0 01F4 brne .+2 |
2195 13d2 00C0 rjmp .L349 |
2196 .L240: |
2197 13d4 8091 0000 lds r24,ParamSet+66 |
2198 13d8 8B3F cpi r24,lo8(-5) |
2199 13da 00F0 brlo .L241 |
2200 13dc 8B3F cpi r24,lo8(-5) |
2201 13de 01F4 brne .+2 |
2202 13e0 00C0 rjmp .L350 |
2203 13e2 8C3F cpi r24,lo8(-4) |
2204 13e4 01F4 brne .+2 |
2205 13e6 00C0 rjmp .L351 |
2206 13e8 8D3F cpi r24,lo8(-3) |
2207 13ea 01F4 brne .+2 |
2208 13ec 00C0 rjmp .L352 |
2209 13ee 8E3F cpi r24,lo8(-2) |
2210 13f0 01F4 brne .L243 |
2211 13f2 8091 0000 lds r24,Poti4 |
2212 13f6 8093 0000 sts FCParam+30,r24 |
2213 13fa 00C0 rjmp .L243 |
2214 .L241: |
2215 13fc 8093 0000 sts FCParam+30,r24 |
2216 .L243: |
2217 1400 8091 0000 lds r24,FCParam+30 |
2218 1404 8230 cpi r24,lo8(2) |
2219 1406 00F4 brsh .+2 |
2220 1408 00C0 rjmp .L353 |
2221 140a 8F3F cpi r24,lo8(-1) |
2222 140c 01F4 brne .+2 |
2223 140e 00C0 rjmp .L354 |
2224 .L247: |
2225 1410 8091 0000 lds r24,ParamSet+85 |
2226 1414 8B3F cpi r24,lo8(-5) |
2227 1416 00F4 brsh .+2 |
2228 1418 00C0 rjmp .L248 |
2229 141a 8B3F cpi r24,lo8(-5) |
2230 141c 01F0 breq .L355 |
2231 141e 8C3F cpi r24,lo8(-4) |
2232 1420 01F4 brne .+2 |
2233 1422 00C0 rjmp .L356 |
2234 1424 8D3F cpi r24,lo8(-3) |
2235 1426 01F4 brne .+2 |
2236 1428 00C0 rjmp .L357 |
2237 142a 8E3F cpi r24,lo8(-2) |
2238 142c 01F0 breq .+2 |
2239 142e 00C0 rjmp .L250 |
2240 1430 8091 0000 lds r24,Poti4 |
2241 1434 8093 0000 sts FCParam+28,r24 |
2242 1438 00C0 rjmp .L250 |
2243 .L355: |
2244 143a 8091 0000 lds r24,Poti1 |
2245 143e 8093 0000 sts FCParam+28,r24 |
2246 1442 00C0 rjmp .L250 |
2247 .L276: |
2248 1444 8091 0000 lds r24,Poti1 |
2249 1448 8093 0000 sts FCParam+4,r24 |
2250 144c 00C0 rjmp .L117 |
2251 .L282: |
2252 144e 8091 0000 lds r24,Poti1 |
2253 1452 8093 0000 sts FCParam+7,r24 |
2254 1456 00C0 rjmp .L127 |
2255 .L279: |
2256 1458 8091 0000 lds r24,Poti1 |
2257 145c 8093 0000 sts FCParam+5,r24 |
2258 1460 00C0 rjmp .L122 |
2259 .L265: |
2260 1462 8091 0000 lds r24,Poti1 |
2261 1466 8093 0000 sts FCParam+1,r24 |
2262 146a 00C0 rjmp .L93 |
2263 .L262: |
2264 146c 8091 0000 lds r24,Poti1 |
2265 1470 8093 0000 sts FCParam,r24 |
2266 1474 00C0 rjmp .L98 |
2267 .L268: |
2268 1476 8091 0000 lds r24,Poti1 |
2269 147a 8093 0000 sts FCParam+2,r24 |
2270 147e 00C0 rjmp .L105 |
2271 .L272: |
2272 1480 8091 0000 lds r24,Poti1 |
2273 1484 8093 0000 sts FCParam+3,r24 |
2274 1488 00C0 rjmp .L112 |
2275 .L288: |
2276 148a 8091 0000 lds r24,Poti1 |
2277 148e 8093 0000 sts FCParam+6,r24 |
2278 1492 00C0 rjmp .L139 |
2279 .L285: |
2280 1494 8091 0000 lds r24,Poti1 |
2281 1498 8093 0000 sts FCParam+8,r24 |
2282 149c 00C0 rjmp .L134 |
2283 .L294: |
2284 149e 8091 0000 lds r24,Poti1 |
2285 14a2 8093 0000 sts FCParam+10,r24 |
2286 14a6 00C0 rjmp .L151 |
2287 .L291: |
2288 14a8 8091 0000 lds r24,Poti1 |
2289 14ac 8093 0000 sts FCParam+9,r24 |
2290 14b0 00C0 rjmp .L144 |
2291 .L303: |
2292 14b2 8091 0000 lds r24,Poti1 |
2293 14b6 8093 0000 sts FCParam+14,r24 |
2294 14ba 00C0 rjmp .L166 |
2295 .L300: |
2296 14bc 8091 0000 lds r24,Poti1 |
2297 14c0 8093 0000 sts FCParam+13,r24 |
2298 14c4 00C0 rjmp .L161 |
2299 .L297: |
2300 14c6 8091 0000 lds r24,Poti1 |
2301 14ca 8093 0000 sts FCParam+12,r24 |
2302 14ce 00C0 rjmp .L156 |
2303 .L350: |
2304 14d0 8091 0000 lds r24,Poti1 |
2305 14d4 8093 0000 sts FCParam+30,r24 |
2306 14d8 00C0 rjmp .L243 |
2307 .L345: |
2308 14da 8091 0000 lds r24,Poti1 |
2309 14de 8093 0000 sts FCParam+29,r24 |
2310 14e2 00C0 rjmp .L236 |
2311 .L342: |
2312 14e4 8091 0000 lds r24,Poti1 |
2313 14e8 8093 0000 sts FCParam+27,r24 |
2314 14ec 00C0 rjmp .L231 |
2315 .L339: |
2316 14ee 8091 0000 lds r24,Poti1 |
2317 14f2 8093 0000 sts FCParam+26,r24 |
2318 14f6 00C0 rjmp .L226 |
2319 .L336: |
2320 14f8 8091 0000 lds r24,Poti1 |
2321 14fc 8093 0000 sts FCParam+25,r24 |
2322 1500 00C0 rjmp .L221 |
2323 .L333: |
2324 1502 8091 0000 lds r24,Poti1 |
2325 1506 8093 0000 sts FCParam+24,r24 |
2326 150a 00C0 rjmp .L216 |
2327 .L330: |
2328 150c 8091 0000 lds r24,Poti1 |
2329 1510 8093 0000 sts FCParam+23,r24 |
2330 1514 00C0 rjmp .L211 |
2331 .L327: |
2332 1516 8091 0000 lds r24,Poti1 |
2333 151a 8093 0000 sts FCParam+22,r24 |
2334 151e 00C0 rjmp .L206 |
2335 .L324: |
2336 1520 8091 0000 lds r24,Poti1 |
2337 1524 8093 0000 sts FCParam+21,r24 |
2338 1528 00C0 rjmp .L201 |
2339 .L321: |
2340 152a 8091 0000 lds r24,Poti1 |
2341 152e 8093 0000 sts FCParam+20,r24 |
2342 1532 00C0 rjmp .L196 |
2343 .L318: |
2344 1534 8091 0000 lds r24,Poti1 |
2345 1538 8093 0000 sts FCParam+19,r24 |
2346 153c 00C0 rjmp .L191 |
2347 .L315: |
2348 153e 8091 0000 lds r24,Poti1 |
2349 1542 8093 0000 sts FCParam+18,r24 |
2350 1546 00C0 rjmp .L186 |
2351 .L312: |
2352 1548 8091 0000 lds r24,Poti1 |
2353 154c 8093 0000 sts FCParam+17,r24 |
2354 1550 00C0 rjmp .L181 |
2355 .L309: |
2356 1552 8091 0000 lds r24,Poti1 |
2357 1556 8093 0000 sts FCParam+16,r24 |
2358 155a 00C0 rjmp .L176 |
2359 .L306: |
2360 155c 8091 0000 lds r24,Poti1 |
2361 1560 8093 0000 sts FCParam+15,r24 |
2362 1564 00C0 rjmp .L171 |
2363 .L348: |
2364 1566 81E0 ldi r24,lo8(1) |
2365 1568 8093 0000 sts FCParam+29,r24 |
2366 156c 00C0 rjmp .L240 |
2367 .L353: |
2368 156e 81E0 ldi r24,lo8(1) |
2369 1570 8093 0000 sts FCParam+30,r24 |
2370 1574 00C0 rjmp .L247 |
2371 .L349: |
2372 1576 8093 0000 sts FCParam+29,r24 |
2373 157a 00C0 rjmp .L240 |
2374 .L354: |
2375 157c 8093 0000 sts FCParam+30,r24 |
2376 1580 00C0 rjmp .L247 |
2377 .L319: |
2378 1582 8091 0000 lds r24,Poti2 |
2379 1586 8093 0000 sts FCParam+19,r24 |
2380 158a 00C0 rjmp .L191 |
2381 .L322: |
2382 158c 8091 0000 lds r24,Poti2 |
2383 1590 8093 0000 sts FCParam+20,r24 |
2384 1594 00C0 rjmp .L196 |
2385 .L310: |
2386 1596 8091 0000 lds r24,Poti2 |
2387 159a 8093 0000 sts FCParam+16,r24 |
2388 159e 00C0 rjmp .L176 |
2389 .L313: |
2390 15a0 8091 0000 lds r24,Poti2 |
2391 15a4 8093 0000 sts FCParam+17,r24 |
2392 15a8 00C0 rjmp .L181 |
2393 .L328: |
2394 15aa 8091 0000 lds r24,Poti2 |
2395 15ae 8093 0000 sts FCParam+22,r24 |
2396 15b2 00C0 rjmp .L206 |
2397 .L325: |
2398 15b4 8091 0000 lds r24,Poti2 |
2399 15b8 8093 0000 sts FCParam+21,r24 |
2400 15bc 00C0 rjmp .L201 |
2401 .L337: |
2402 15be 8091 0000 lds r24,Poti2 |
2403 15c2 8093 0000 sts FCParam+25,r24 |
2404 15c6 00C0 rjmp .L221 |
2405 .L351: |
2406 15c8 8091 0000 lds r24,Poti2 |
2407 15cc 8093 0000 sts FCParam+30,r24 |
2408 15d0 00C0 rjmp .L243 |
2409 .L343: |
2410 15d2 8091 0000 lds r24,Poti2 |
2411 15d6 8093 0000 sts FCParam+27,r24 |
2412 15da 00C0 rjmp .L231 |
2413 .L346: |
2414 15dc 8091 0000 lds r24,Poti2 |
2415 15e0 8093 0000 sts FCParam+29,r24 |
2416 15e4 00C0 rjmp .L236 |
2417 .L316: |
2418 15e6 8091 0000 lds r24,Poti2 |
2419 15ea 8093 0000 sts FCParam+18,r24 |
2420 15ee 00C0 rjmp .L186 |
2421 .L307: |
2422 15f0 8091 0000 lds r24,Poti2 |
2423 15f4 8093 0000 sts FCParam+15,r24 |
2424 15f8 00C0 rjmp .L171 |
2425 .L334: |
2426 15fa 8091 0000 lds r24,Poti2 |
2427 15fe 8093 0000 sts FCParam+24,r24 |
2428 1602 00C0 rjmp .L216 |
2429 .L340: |
2430 1604 8091 0000 lds r24,Poti2 |
2431 1608 8093 0000 sts FCParam+26,r24 |
2432 160c 00C0 rjmp .L226 |
2433 .L286: |
2434 160e 8091 0000 lds r24,Poti2 |
2435 1612 8093 0000 sts FCParam+8,r24 |
2436 1616 00C0 rjmp .L134 |
2437 .L273: |
2438 1618 8091 0000 lds r24,Poti2 |
2439 161c 8093 0000 sts FCParam+3,r24 |
2440 1620 00C0 rjmp .L112 |
2441 .L331: |
2442 1622 8091 0000 lds r24,Poti2 |
2443 1626 8093 0000 sts FCParam+23,r24 |
2444 162a 00C0 rjmp .L211 |
2445 .L356: |
2446 162c 8091 0000 lds r24,Poti2 |
2447 1630 8093 0000 sts FCParam+28,r24 |
2448 1634 00C0 rjmp .L250 |
2449 .L283: |
2450 1636 8091 0000 lds r24,Poti2 |
2451 163a 8093 0000 sts FCParam+7,r24 |
2452 163e 00C0 rjmp .L127 |
2453 .L280: |
2454 1640 8091 0000 lds r24,Poti2 |
2455 1644 8093 0000 sts FCParam+5,r24 |
2456 1648 00C0 rjmp .L122 |
2457 .L269: |
2458 164a 8091 0000 lds r24,Poti2 |
2459 164e 8093 0000 sts FCParam+2,r24 |
2460 1652 00C0 rjmp .L105 |
2461 .L266: |
2462 1654 8091 0000 lds r24,Poti2 |
2463 1658 8093 0000 sts FCParam+1,r24 |
2464 165c 00C0 rjmp .L93 |
2465 .L304: |
2466 165e 8091 0000 lds r24,Poti2 |
2467 1662 8093 0000 sts FCParam+14,r24 |
2468 1666 00C0 rjmp .L166 |
2469 .L301: |
2470 1668 8091 0000 lds r24,Poti2 |
2471 166c 8093 0000 sts FCParam+13,r24 |
2472 1670 00C0 rjmp .L161 |
2473 .L277: |
2474 1672 8091 0000 lds r24,Poti2 |
2475 1676 8093 0000 sts FCParam+4,r24 |
2476 167a 00C0 rjmp .L117 |
2477 .L263: |
2478 167c 8091 0000 lds r24,Poti2 |
2479 1680 8093 0000 sts FCParam,r24 |
2480 1684 00C0 rjmp .L98 |
2481 .L292: |
2482 1686 8091 0000 lds r24,Poti2 |
2483 168a 8093 0000 sts FCParam+9,r24 |
2484 168e 00C0 rjmp .L144 |
2485 .L289: |
2486 1690 8091 0000 lds r24,Poti2 |
2487 1694 8093 0000 sts FCParam+6,r24 |
2488 1698 00C0 rjmp .L139 |
2489 .L298: |
2490 169a 8091 0000 lds r24,Poti2 |
2491 169e 8093 0000 sts FCParam+12,r24 |
2492 16a2 00C0 rjmp .L156 |
2493 .L295: |
2494 16a4 8091 0000 lds r24,Poti2 |
2495 16a8 8093 0000 sts FCParam+10,r24 |
2496 16ac 00C0 rjmp .L151 |
2497 .L352: |
2498 16ae 8091 0000 lds r24,Poti3 |
2499 16b2 8093 0000 sts FCParam+30,r24 |
2500 16b6 00C0 rjmp .L243 |
2501 .L357: |
2502 16b8 8091 0000 lds r24,Poti3 |
2503 16bc 8093 0000 sts FCParam+28,r24 |
2504 16c0 00C0 rjmp .L250 |
2505 .L329: |
2506 16c2 8091 0000 lds r24,Poti3 |
2507 16c6 8093 0000 sts FCParam+22,r24 |
2508 16ca 00C0 rjmp .L206 |
2509 .L305: |
2510 16cc 8091 0000 lds r24,Poti3 |
2511 16d0 8093 0000 sts FCParam+14,r24 |
2512 16d4 00C0 rjmp .L166 |
2513 .L341: |
2514 16d6 8091 0000 lds r24,Poti3 |
2515 16da 8093 0000 sts FCParam+26,r24 |
2516 16de 00C0 rjmp .L226 |
2517 .L284: |
2518 16e0 8091 0000 lds r24,Poti3 |
2519 16e4 8093 0000 sts FCParam+7,r24 |
2520 16e8 00C0 rjmp .L127 |
2521 .L323: |
2522 16ea 8091 0000 lds r24,Poti3 |
2523 16ee 8093 0000 sts FCParam+20,r24 |
2524 16f2 00C0 rjmp .L196 |
2525 .L299: |
2526 16f4 8091 0000 lds r24,Poti3 |
2527 16f8 8093 0000 sts FCParam+12,r24 |
2528 16fc 00C0 rjmp .L156 |
2529 .L335: |
2530 16fe 8091 0000 lds r24,Poti3 |
2531 1702 8093 0000 sts FCParam+24,r24 |
2532 1706 00C0 rjmp .L216 |
2533 .L278: |
2534 1708 8091 0000 lds r24,Poti3 |
2535 170c 8093 0000 sts FCParam+4,r24 |
2536 1710 00C0 rjmp .L117 |
2537 .L317: |
2538 1712 8091 0000 lds r24,Poti3 |
2539 1716 8093 0000 sts FCParam+18,r24 |
2540 171a 00C0 rjmp .L186 |
2541 .L293: |
2542 171c 8091 0000 lds r24,Poti3 |
2543 1720 8093 0000 sts FCParam+9,r24 |
2544 1724 00C0 rjmp .L144 |
2545 .L347: |
2546 1726 8091 0000 lds r24,Poti3 |
2547 172a 8093 0000 sts FCParam+29,r24 |
2548 172e 00C0 rjmp .L236 |
2549 .L270: |
2550 1730 8091 0000 lds r24,Poti3 |
2551 1734 8093 0000 sts FCParam+2,r24 |
2552 1738 00C0 rjmp .L105 |
2553 .L320: |
2554 173a 8091 0000 lds r24,Poti3 |
2555 173e 8093 0000 sts FCParam+19,r24 |
2556 1742 00C0 rjmp .L191 |
2557 .L296: |
2558 1744 8091 0000 lds r24,Poti3 |
2559 1748 8093 0000 sts FCParam+10,r24 |
2560 174c 00C0 rjmp .L151 |
2561 .L308: |
2562 174e 8091 0000 lds r24,Poti3 |
2563 1752 8093 0000 sts FCParam+15,r24 |
2564 1756 00C0 rjmp .L171 |
2565 .L274: |
2566 1758 8091 0000 lds r24,Poti3 |
2567 175c 8093 0000 sts FCParam+3,r24 |
2568 1760 00C0 rjmp .L112 |
2569 .L314: |
2570 1762 8091 0000 lds r24,Poti3 |
2571 1766 8093 0000 sts FCParam+17,r24 |
2572 176a 00C0 rjmp .L181 |
2573 .L290: |
2574 176c 8091 0000 lds r24,Poti3 |
2575 1770 8093 0000 sts FCParam+6,r24 |
2576 1774 00C0 rjmp .L139 |
2577 .L332: |
2578 1776 8091 0000 lds r24,Poti3 |
2579 177a 8093 0000 sts FCParam+23,r24 |
2580 177e 00C0 rjmp .L211 |
2581 .L264: |
2582 1780 8091 0000 lds r24,Poti3 |
2583 1784 8093 0000 sts FCParam,r24 |
2584 1788 00C0 rjmp .L98 |
2585 .L326: |
2586 178a 8091 0000 lds r24,Poti3 |
2587 178e 8093 0000 sts FCParam+21,r24 |
2588 1792 00C0 rjmp .L201 |
2589 .L302: |
2590 1794 8091 0000 lds r24,Poti3 |
2591 1798 8093 0000 sts FCParam+13,r24 |
2592 179c 00C0 rjmp .L161 |
2593 .L338: |
2594 179e 8091 0000 lds r24,Poti3 |
2595 17a2 8093 0000 sts FCParam+25,r24 |
2596 17a6 00C0 rjmp .L221 |
2597 .L281: |
2598 17a8 8091 0000 lds r24,Poti3 |
2599 17ac 8093 0000 sts FCParam+5,r24 |
2600 17b0 00C0 rjmp .L122 |
2601 .L311: |
2602 17b2 8091 0000 lds r24,Poti3 |
2603 17b6 8093 0000 sts FCParam+16,r24 |
2604 17ba 00C0 rjmp .L176 |
2605 .L287: |
2606 17bc 8091 0000 lds r24,Poti3 |
2607 17c0 8093 0000 sts FCParam+8,r24 |
2608 17c4 00C0 rjmp .L134 |
2609 .L344: |
2610 17c6 8091 0000 lds r24,Poti3 |
2611 17ca 8093 0000 sts FCParam+27,r24 |
2612 17ce 00C0 rjmp .L231 |
2613 .L267: |
2614 17d0 8091 0000 lds r24,Poti3 |
2615 17d4 8093 0000 sts FCParam+1,r24 |
2616 17d8 00C0 rjmp .L93 |
2618 .global SendMotorData |
2620 SendMotorData: |
2621 /* prologue: function */ |
2622 /* frame size = 0 */ |
2623 17da 8091 0000 lds r24,MKFlags |
2624 17de 80FD sbrc r24,0 |
2625 17e0 00C0 rjmp .L359 |
2626 17e2 8091 0000 lds r24,MKFlags |
2627 17e6 857F andi r24,lo8(-11) |
2628 17e8 8093 0000 sts MKFlags,r24 |
2629 17ec 9091 0000 lds r25,MotorTest_Active |
2630 17f0 E0E0 ldi r30,lo8(Motor) |
2631 17f2 F0E0 ldi r31,hi8(Motor) |
2632 17f4 A0E0 ldi r26,lo8(MotorTest) |
2633 17f6 B0E0 ldi r27,hi8(MotorTest) |
2634 17f8 00C0 rjmp .L362 |
2635 .L367: |
2636 17fa 1082 st Z,__zero_reg__ |
2637 17fc 3596 adiw r30,5 |
2638 17fe 1196 adiw r26,1 |
2639 1800 80E0 ldi r24,hi8(Motor+60) |
2640 1802 E030 cpi r30,lo8(Motor+60) |
2641 1804 F807 cpc r31,r24 |
2642 1806 01F0 breq .L366 |
2643 .L362: |
2644 1808 9923 tst r25 |
2645 180a 01F0 breq .L367 |
2646 180c 8C91 ld r24,X |
2647 180e 8083 st Z,r24 |
2648 1810 3596 adiw r30,5 |
2649 1812 1196 adiw r26,1 |
2650 1814 80E0 ldi r24,hi8(Motor+60) |
2651 1816 E030 cpi r30,lo8(Motor+60) |
2652 1818 F807 cpc r31,r24 |
2653 181a 01F4 brne .L362 |
2654 .L366: |
2655 181c 9923 tst r25 |
2656 181e 01F0 breq .L359 |
2657 1820 9150 subi r25,lo8(-(-1)) |
2658 1822 9093 0000 sts MotorTest_Active,r25 |
2659 .L359: |
2660 1826 E0E0 ldi r30,lo8(DebugOut+26) |
2661 1828 F0E0 ldi r31,hi8(DebugOut+26) |
2662 182a 8091 0000 lds r24,Motor |
2663 182e 8093 0000 sts DebugOut+26,r24 |
2664 1832 1092 0000 sts DebugOut+27,__zero_reg__ |
2665 1836 8091 0000 lds r24,Motor+5 |
2666 183a 8093 0000 sts DebugOut+28,r24 |
2667 183e 1382 std Z+3,__zero_reg__ |
2668 1840 8091 0000 lds r24,Motor+15 |
2669 1844 8093 0000 sts DebugOut+30,r24 |
2670 1848 1582 std Z+5,__zero_reg__ |
2671 184a 8091 0000 lds r24,Motor+10 |
2672 184e 8093 0000 sts DebugOut+32,r24 |
2673 1852 1782 std Z+7,__zero_reg__ |
2674 1854 80E0 ldi r24,lo8(0) |
2675 1856 0E94 0000 call I2C_Start |
2676 /* epilogue start */ |
2677 185a 0895 ret |
2679 .global SetNeutral |
2681 SetNeutral: |
2682 185c 2F92 push r2 |
2683 185e 3F92 push r3 |
2684 1860 4F92 push r4 |
2685 1862 5F92 push r5 |
2686 1864 6F92 push r6 |
2687 1866 7F92 push r7 |
2688 1868 8F92 push r8 |
2689 186a 9F92 push r9 |
2690 186c AF92 push r10 |
2691 186e BF92 push r11 |
2692 1870 CF92 push r12 |
2693 1872 DF92 push r13 |
2694 1874 EF92 push r14 |
2695 1876 FF92 push r15 |
2696 1878 0F93 push r16 |
2697 187a 1F93 push r17 |
2698 /* prologue: function */ |
2699 /* frame size = 0 */ |
2700 187c 582E mov r5,r24 |
2701 187e 1092 0000 sts (AdBiasAccNick)+1,__zero_reg__ |
2702 1882 1092 0000 sts AdBiasAccNick,__zero_reg__ |
2703 1886 1092 0000 sts (AdBiasAccRoll)+1,__zero_reg__ |
2704 188a 1092 0000 sts AdBiasAccRoll,__zero_reg__ |
2705 188e 80E0 ldi r24,lo8(0x0) |
2706 1890 90E0 ldi r25,hi8(0x0) |
2707 1892 A0E0 ldi r26,hlo8(0x0) |
2708 1894 B0E0 ldi r27,hhi8(0x0) |
2709 1896 8093 0000 sts AdBiasAccTop,r24 |
2710 189a 9093 0000 sts (AdBiasAccTop)+1,r25 |
2711 189e A093 0000 sts (AdBiasAccTop)+2,r26 |
2712 18a2 B093 0000 sts (AdBiasAccTop)+3,r27 |
2713 18a6 1092 0000 sts (BiasHiResGyroNick)+1,__zero_reg__ |
2714 18aa 1092 0000 sts BiasHiResGyroNick,__zero_reg__ |
2715 18ae 1092 0000 sts (BiasHiResGyroRoll)+1,__zero_reg__ |
2716 18b2 1092 0000 sts BiasHiResGyroRoll,__zero_reg__ |
2717 18b6 1092 0000 sts (AdBiasGyroYaw)+1,__zero_reg__ |
2718 18ba 1092 0000 sts AdBiasGyroYaw,__zero_reg__ |
2719 18be 1092 0000 sts FCParam+24,__zero_reg__ |
2720 18c2 1092 0000 sts FCParam+25,__zero_reg__ |
2721 18c6 1092 0000 sts ExpandBaro,__zero_reg__ |
2722 18ca 84E6 ldi r24,lo8(100) |
2723 18cc 90E0 ldi r25,hi8(100) |
2724 18ce 0E94 0000 call Delay_ms_Mess |
2725 18d2 8091 0000 lds r24,BoardRelease |
2726 18d6 8D30 cpi r24,lo8(13) |
2727 18d8 01F4 brne .+2 |
2728 18da 00C0 rjmp .L384 |
2729 .L369: |
2730 18dc 8091 0000 lds r24,ParamSet+8 |
2731 18e0 80FF sbrs r24,0 |
2732 18e2 00C0 rjmp .L370 |
2733 18e4 8091 0000 lds r24,AdAirPressure |
2734 18e8 9091 0000 lds r25,(AdAirPressure)+1 |
2735 18ec 875B subi r24,lo8(951) |
2736 18ee 9340 sbci r25,hi8(951) |
2737 18f0 04F0 brlt .+2 |
2738 18f2 00C0 rjmp .L371 |
2739 18f4 8091 0000 lds r24,AdAirPressure |
2740 18f8 9091 0000 lds r25,(AdAirPressure)+1 |
2741 18fc 8E5E subi r24,lo8(750) |
2742 18fe 9240 sbci r25,hi8(750) |
2743 1900 04F4 brge .+2 |
2744 1902 00C0 rjmp .L371 |
2745 .L370: |
2746 1904 2224 clr r2 |
2747 1906 EE24 clr r14 |
2748 1908 FF24 clr r15 |
2749 190a 8701 movw r16,r14 |
2750 190c AA24 clr r10 |
2751 190e BB24 clr r11 |
2752 1910 6501 movw r12,r10 |
2753 1912 6624 clr r6 |
2754 1914 7724 clr r7 |
2755 1916 4301 movw r8,r6 |
2756 .L372: |
2757 1918 8AE0 ldi r24,lo8(10) |
2758 191a 90E0 ldi r25,hi8(10) |
2759 191c 0E94 0000 call Delay_ms_Mess |
2760 1920 8091 0000 lds r24,AdValueGyroNick |
2761 1924 9091 0000 lds r25,(AdValueGyroNick)+1 |
2762 1928 880F lsl r24 |
2763 192a 991F rol r25 |
2764 192c 880F lsl r24 |
2765 192e 991F rol r25 |
2766 1930 880F lsl r24 |
2767 1932 991F rol r25 |
2768 1934 AA27 clr r26 |
2769 1936 97FD sbrc r25,7 |
2770 1938 A095 com r26 |
2771 193a BA2F mov r27,r26 |
2772 193c E80E add r14,r24 |
2773 193e F91E adc r15,r25 |
2774 1940 0A1F adc r16,r26 |
2775 1942 1B1F adc r17,r27 |
2776 1944 8091 0000 lds r24,AdValueGyroRoll |
2777 1948 9091 0000 lds r25,(AdValueGyroRoll)+1 |
2778 194c 880F lsl r24 |
2779 194e 991F rol r25 |
2780 1950 880F lsl r24 |
2781 1952 991F rol r25 |
2782 1954 880F lsl r24 |
2783 1956 991F rol r25 |
2784 1958 AA27 clr r26 |
2785 195a 97FD sbrc r25,7 |
2786 195c A095 com r26 |
2787 195e BA2F mov r27,r26 |
2788 1960 A80E add r10,r24 |
2789 1962 B91E adc r11,r25 |
2790 1964 CA1E adc r12,r26 |
2791 1966 DB1E adc r13,r27 |
2792 1968 8091 0000 lds r24,AdValueGyroYaw |
2793 196c 9091 0000 lds r25,(AdValueGyroYaw)+1 |
2794 1970 AA27 clr r26 |
2795 1972 97FD sbrc r25,7 |
2796 1974 A095 com r26 |
2797 1976 BA2F mov r27,r26 |
2798 1978 680E add r6,r24 |
2799 197a 791E adc r7,r25 |
2800 197c 8A1E adc r8,r26 |
2801 197e 9B1E adc r9,r27 |
2802 1980 2394 inc r2 |
2803 1982 B0E2 ldi r27,lo8(32) |
2804 1984 2B16 cp r2,r27 |
2805 1986 01F4 brne .L372 |
2806 1988 80E1 ldi r24,lo8(16) |
2807 198a 90E0 ldi r25,hi8(16) |
2808 198c A0E0 ldi r26,hlo8(16) |
2809 198e B0E0 ldi r27,hhi8(16) |
2810 1990 E80E add r14,r24 |
2811 1992 F91E adc r15,r25 |
2812 1994 0A1F adc r16,r26 |
2813 1996 1B1F adc r17,r27 |
2814 1998 C801 movw r24,r16 |
2815 199a B701 movw r22,r14 |
2816 199c 20E2 ldi r18,lo8(32) |
2817 199e 30E0 ldi r19,hi8(32) |
2818 19a0 40E0 ldi r20,hlo8(32) |
2819 19a2 50E0 ldi r21,hhi8(32) |
2820 19a4 0E94 0000 call __divmodsi4 |
2821 19a8 3093 0000 sts (BiasHiResGyroNick)+1,r19 |
2822 19ac 2093 0000 sts BiasHiResGyroNick,r18 |
2823 19b0 80E1 ldi r24,lo8(16) |
2824 19b2 90E0 ldi r25,hi8(16) |
2825 19b4 A0E0 ldi r26,hlo8(16) |
2826 19b6 B0E0 ldi r27,hhi8(16) |
2827 19b8 A80E add r10,r24 |
2828 19ba B91E adc r11,r25 |
2829 19bc CA1E adc r12,r26 |
2830 19be DB1E adc r13,r27 |
2831 19c0 C601 movw r24,r12 |
2832 19c2 B501 movw r22,r10 |
2833 19c4 20E2 ldi r18,lo8(32) |
2834 19c6 30E0 ldi r19,hi8(32) |
2835 19c8 40E0 ldi r20,hlo8(32) |
2836 19ca 50E0 ldi r21,hhi8(32) |
2837 19cc 0E94 0000 call __divmodsi4 |
2838 19d0 3093 0000 sts (BiasHiResGyroRoll)+1,r19 |
2839 19d4 2093 0000 sts BiasHiResGyroRoll,r18 |
2840 19d8 80E1 ldi r24,lo8(16) |
2841 19da 90E0 ldi r25,hi8(16) |
2842 19dc A0E0 ldi r26,hlo8(16) |
2843 19de B0E0 ldi r27,hhi8(16) |
2844 19e0 680E add r6,r24 |
2845 19e2 791E adc r7,r25 |
2846 19e4 8A1E adc r8,r26 |
2847 19e6 9B1E adc r9,r27 |
2848 19e8 C401 movw r24,r8 |
2849 19ea B301 movw r22,r6 |
2850 19ec 20E2 ldi r18,lo8(32) |
2851 19ee 30E0 ldi r19,hi8(32) |
2852 19f0 40E0 ldi r20,hlo8(32) |
2853 19f2 50E0 ldi r21,hhi8(32) |
2854 19f4 0E94 0000 call __divmodsi4 |
2855 19f8 3093 0000 sts (AdBiasGyroYaw)+1,r19 |
2856 19fc 2093 0000 sts AdBiasGyroYaw,r18 |
2857 1a00 5520 tst r5 |
2858 1a02 01F4 brne .+2 |
2859 1a04 00C0 rjmp .L373 |
2860 1a06 2224 clr r2 |
2861 1a08 EE24 clr r14 |
2862 1a0a FF24 clr r15 |
2863 1a0c 8701 movw r16,r14 |
2864 1a0e AA24 clr r10 |
2865 1a10 BB24 clr r11 |
2866 1a12 6501 movw r12,r10 |
2867 1a14 6624 clr r6 |
2868 1a16 7724 clr r7 |
2869 1a18 4301 movw r8,r6 |
2870 .L374: |
2871 1a1a 8AE0 ldi r24,lo8(10) |
2872 1a1c 90E0 ldi r25,hi8(10) |
2873 1a1e 0E94 0000 call Delay_ms_Mess |
2874 1a22 8091 0000 lds r24,AdValueAccNick |
2875 1a26 9091 0000 lds r25,(AdValueAccNick)+1 |
2876 1a2a AA27 clr r26 |
2877 1a2c 97FD sbrc r25,7 |
2878 1a2e A095 com r26 |
2879 1a30 BA2F mov r27,r26 |
2880 1a32 E80E add r14,r24 |
2881 1a34 F91E adc r15,r25 |
2882 1a36 0A1F adc r16,r26 |
2883 1a38 1B1F adc r17,r27 |
2884 1a3a 8091 0000 lds r24,AdValueAccRoll |
2885 1a3e 9091 0000 lds r25,(AdValueAccRoll)+1 |
2886 1a42 AA27 clr r26 |
2887 1a44 97FD sbrc r25,7 |
2888 1a46 A095 com r26 |
2889 1a48 BA2F mov r27,r26 |
2890 1a4a A80E add r10,r24 |
2891 1a4c B91E adc r11,r25 |
2892 1a4e CA1E adc r12,r26 |
2893 1a50 DB1E adc r13,r27 |
2894 1a52 8091 0000 lds r24,AdValueAccZ |
2895 1a56 9091 0000 lds r25,(AdValueAccZ)+1 |
2896 1a5a AA27 clr r26 |
2897 1a5c 97FD sbrc r25,7 |
2898 1a5e A095 com r26 |
2899 1a60 BA2F mov r27,r26 |
2900 1a62 680E add r6,r24 |
2901 1a64 791E adc r7,r25 |
2902 1a66 8A1E adc r8,r26 |
2903 1a68 9B1E adc r9,r27 |
2904 1a6a 2394 inc r2 |
2905 1a6c 9AE0 ldi r25,lo8(10) |
2906 1a6e 2916 cp r2,r25 |
2907 1a70 01F4 brne .L374 |
2908 1a72 C701 movw r24,r14 |
2909 1a74 F7FC sbrc r15,7 |
2910 1a76 00C0 rjmp .L385 |
2911 .L375: |
2912 1a78 0596 adiw r24,5 |
2913 1a7a 6AE0 ldi r22,lo8(10) |
2914 1a7c 70E0 ldi r23,hi8(10) |
2915 1a7e 0E94 0000 call __divmodhi4 |
2916 1a82 162F mov r17,r22 |
2917 1a84 072F mov r16,r23 |
2918 1a86 6093 0000 sts AdBiasAccNick,r22 |
2919 1a8a 7093 0000 sts AdBiasAccNick+1,r23 |
2920 1a8e C501 movw r24,r10 |
2921 1a90 B7FC sbrc r11,7 |
2922 1a92 00C0 rjmp .L386 |
2923 .L376: |
2924 1a94 0596 adiw r24,5 |
2925 1a96 6AE0 ldi r22,lo8(10) |
2926 1a98 70E0 ldi r23,hi8(10) |
2927 1a9a 0E94 0000 call __divmodhi4 |
2928 1a9e 7093 0000 sts (AdBiasAccRoll)+1,r23 |
2929 1aa2 6093 0000 sts AdBiasAccRoll,r22 |
2930 1aa6 C301 movw r24,r6 |
2931 1aa8 77FC sbrc r7,7 |
2932 1aaa 00C0 rjmp .L387 |
2933 .L377: |
2934 1aac 0596 adiw r24,5 |
2935 1aae 6AE0 ldi r22,lo8(10) |
2936 1ab0 70E0 ldi r23,hi8(10) |
2937 1ab2 0E94 0000 call __divmodhi4 |
2938 1ab6 8827 clr r24 |
2939 1ab8 77FD sbrc r23,7 |
2940 1aba 8095 com r24 |
2941 1abc 982F mov r25,r24 |
2942 1abe 0E94 0000 call __floatsisf |
2943 1ac2 6093 0000 sts AdBiasAccTop,r22 |
2944 1ac6 7093 0000 sts (AdBiasAccTop)+1,r23 |
2945 1aca 8093 0000 sts (AdBiasAccTop)+2,r24 |
2946 1ace 9093 0000 sts (AdBiasAccTop)+3,r25 |
2947 1ad2 84E0 ldi r24,lo8(4) |
2948 1ad4 90E0 ldi r25,hi8(4) |
2949 1ad6 612F mov r22,r17 |
2950 1ad8 702F mov r23,r16 |
2951 1ada 0E94 0000 call SetParamWord |
2952 1ade 6091 0000 lds r22,AdBiasAccRoll |
2953 1ae2 7091 0000 lds r23,(AdBiasAccRoll)+1 |
2954 1ae6 86E0 ldi r24,lo8(6) |
2955 1ae8 90E0 ldi r25,hi8(6) |
2956 1aea 0E94 0000 call SetParamWord |
2957 1aee 6091 0000 lds r22,AdBiasAccTop |
2958 1af2 7091 0000 lds r23,(AdBiasAccTop)+1 |
2959 1af6 8091 0000 lds r24,(AdBiasAccTop)+2 |
2960 1afa 9091 0000 lds r25,(AdBiasAccTop)+3 |
2961 1afe 0E94 0000 call __fixunssfsi |
2962 1b02 9B01 movw r18,r22 |
2963 1b04 AC01 movw r20,r24 |
2964 1b06 88E0 ldi r24,lo8(8) |
2965 1b08 90E0 ldi r25,hi8(8) |
2966 1b0a B901 movw r22,r18 |
2967 1b0c 0E94 0000 call SetParamWord |
2968 .L378: |
2969 1b10 8091 0000 lds r24,AirPressure |
2970 1b14 9091 0000 lds r25,(AirPressure)+1 |
2971 1b18 A091 0000 lds r26,(AirPressure)+2 |
2972 1b1c B091 0000 lds r27,(AirPressure)+3 |
2973 1b20 8093 0000 sts StartAirPressure,r24 |
2974 1b24 9093 0000 sts (StartAirPressure)+1,r25 |
2975 1b28 A093 0000 sts (StartAirPressure)+2,r26 |
2976 1b2c B093 0000 sts (StartAirPressure)+3,r27 |
2977 1b30 84E6 ldi r24,lo8(100) |
2978 1b32 90E0 ldi r25,hi8(100) |
2979 1b34 0E94 0000 call Delay_ms_Mess |
2980 1b38 1092 0000 sts (ReadingVario)+1,__zero_reg__ |
2981 1b3c 1092 0000 sts ReadingVario,__zero_reg__ |
2982 1b40 6091 0000 lds r22,AdValueAccNick |
2983 1b44 7091 0000 lds r23,(AdValueAccNick)+1 |
2984 1b48 8827 clr r24 |
2985 1b4a 77FD sbrc r23,7 |
2986 1b4c 8095 com r24 |
2987 1b4e 982F mov r25,r24 |
2988 1b50 26E0 ldi r18,lo8(6) |
2989 1b52 30E0 ldi r19,hi8(6) |
2990 1b54 40E0 ldi r20,hlo8(6) |
2991 1b56 50E0 ldi r21,hhi8(6) |
2992 1b58 0E94 0000 call __mulsi3 |
2993 1b5c 3B01 movw r6,r22 |
2994 1b5e 4C01 movw r8,r24 |
2995 1b60 7093 0000 sts (AccNick)+1,r23 |
2996 1b64 6093 0000 sts AccNick,r22 |
2997 1b68 6091 0000 lds r22,AdValueAccRoll |
2998 1b6c 7091 0000 lds r23,(AdValueAccRoll)+1 |
2999 1b70 8827 clr r24 |
3000 1b72 77FD sbrc r23,7 |
3001 1b74 8095 com r24 |
3002 1b76 982F mov r25,r24 |
3003 1b78 0E94 0000 call __mulsi3 |
3004 1b7c 5B01 movw r10,r22 |
3005 1b7e 6C01 movw r12,r24 |
3006 1b80 7093 0000 sts (AccRoll)+1,r23 |
3007 1b84 6093 0000 sts AccRoll,r22 |
3008 1b88 8091 0000 lds r24,AdValueAccTop |
3009 1b8c 9091 0000 lds r25,(AdValueAccTop)+1 |
3010 1b90 9093 0000 sts (AccTop)+1,r25 |
3011 1b94 8093 0000 sts AccTop,r24 |
3012 1b98 8091 0000 lds r24,AdValueAccTop |
3013 1b9c 9091 0000 lds r25,(AdValueAccTop)+1 |
3014 1ba0 982F mov r25,r24 |
3015 1ba2 8827 clr r24 |
3016 1ba4 990F lsl r25 |
3017 1ba6 990F lsl r25 |
3018 1ba8 AA27 clr r26 |
3019 1baa 97FD sbrc r25,7 |
3020 1bac A095 com r26 |
3021 1bae BA2F mov r27,r26 |
3022 1bb0 8093 0000 sts ReadingIntegralTop,r24 |
3023 1bb4 9093 0000 sts (ReadingIntegralTop)+1,r25 |
3024 1bb8 A093 0000 sts (ReadingIntegralTop)+2,r26 |
3025 1bbc B093 0000 sts (ReadingIntegralTop)+3,r27 |
3026 1bc0 1092 0000 sts (GyroNick)+1,__zero_reg__ |
3027 1bc4 1092 0000 sts GyroNick,__zero_reg__ |
3028 1bc8 1092 0000 sts (GyroRoll)+1,__zero_reg__ |
3029 1bcc 1092 0000 sts GyroRoll,__zero_reg__ |
3030 1bd0 1092 0000 sts (GyroYaw)+1,__zero_reg__ |
3031 1bd4 1092 0000 sts GyroYaw,__zero_reg__ |
3032 1bd8 E090 0000 lds r14,ParamSet+23 |
3033 1bdc 2E2C mov r2,r14 |
3034 1bde 3324 clr r3 |
3035 1be0 4424 clr r4 |
3036 1be2 5524 clr r5 |
3037 1be4 8824 clr r8 |
3038 1be6 77FC sbrc r7,7 |
3039 1be8 8094 com r8 |
3040 1bea 982C mov r9,r8 |
3041 1bec C401 movw r24,r8 |
3042 1bee B301 movw r22,r6 |
3043 1bf0 A201 movw r20,r4 |
3044 1bf2 9101 movw r18,r2 |
3045 1bf4 0E94 0000 call __mulsi3 |
3046 1bf8 3B01 movw r6,r22 |
3047 1bfa 4C01 movw r8,r24 |
3048 1bfc 6093 0000 sts IntegralGyroNick,r22 |
3049 1c00 7093 0000 sts (IntegralGyroNick)+1,r23 |
3050 1c04 8093 0000 sts (IntegralGyroNick)+2,r24 |
3051 1c08 9093 0000 sts (IntegralGyroNick)+3,r25 |
3052 1c0c CC24 clr r12 |
3053 1c0e B7FC sbrc r11,7 |
3054 1c10 C094 com r12 |
3055 1c12 DC2C mov r13,r12 |
3056 1c14 C601 movw r24,r12 |
3057 1c16 B501 movw r22,r10 |
3058 1c18 0E94 0000 call __mulsi3 |
3059 1c1c 6093 0000 sts IntegralGyroRoll,r22 |
3060 1c20 7093 0000 sts (IntegralGyroRoll)+1,r23 |
3061 1c24 8093 0000 sts (IntegralGyroRoll)+2,r24 |
3062 1c28 9093 0000 sts (IntegralGyroRoll)+3,r25 |
3063 1c2c 6092 0000 sts ReadingIntegralGyroNick2,r6 |
3064 1c30 7092 0000 sts (ReadingIntegralGyroNick2)+1,r7 |
3065 1c34 8092 0000 sts (ReadingIntegralGyroNick2)+2,r8 |
3066 1c38 9092 0000 sts (ReadingIntegralGyroNick2)+3,r9 |
3067 1c3c 6093 0000 sts ReadingIntegralGyroRoll2,r22 |
3068 1c40 7093 0000 sts (ReadingIntegralGyroRoll2)+1,r23 |
3069 1c44 8093 0000 sts (ReadingIntegralGyroRoll2)+2,r24 |
3070 1c48 9093 0000 sts (ReadingIntegralGyroRoll2)+3,r25 |
3071 1c4c 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ |
3072 1c50 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ |
3073 1c54 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ |
3074 1c58 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ |
3075 1c5c 2091 0000 lds r18,CompassHeading |
3076 1c60 3091 0000 lds r19,(CompassHeading)+1 |
3077 1c64 3093 0000 sts (CompassCourse)+1,r19 |
3078 1c68 2093 0000 sts CompassCourse,r18 |
3079 1c6c 8AE2 ldi r24,lo8(42) |
3080 1c6e E89E mul r14,r24 |
3081 1c70 7001 movw r14,r0 |
3082 1c72 1124 clr r1 |
3083 1c74 0027 clr r16 |
3084 1c76 F7FC sbrc r15,7 |
3085 1c78 0095 com r16 |
3086 1c7a 102F mov r17,r16 |
3087 1c7c 4427 clr r20 |
3088 1c7e 37FD sbrc r19,7 |
3089 1c80 4095 com r20 |
3090 1c82 542F mov r21,r20 |
3091 1c84 C801 movw r24,r16 |
3092 1c86 B701 movw r22,r14 |
3093 1c88 0E94 0000 call __mulsi3 |
3094 1c8c 6093 0000 sts YawGyroHeading,r22 |
3095 1c90 7093 0000 sts (YawGyroHeading)+1,r23 |
3096 1c94 8093 0000 sts (YawGyroHeading)+2,r24 |
3097 1c98 9093 0000 sts (YawGyroHeading)+3,r25 |
3098 1c9c 1092 0000 sts (YawGyroDrift)+1,__zero_reg__ |
3099 1ca0 1092 0000 sts YawGyroDrift,__zero_reg__ |
3100 1ca4 82E3 ldi r24,lo8(50) |
3101 1ca6 90E0 ldi r25,hi8(50) |
3102 1ca8 9093 0000 sts (BeepTime)+1,r25 |
3103 1cac 8093 0000 sts BeepTime,r24 |
3104 1cb0 6091 0000 lds r22,ParamSet+54 |
3105 1cb4 70E0 ldi r23,lo8(0) |
3106 1cb6 80E0 ldi r24,lo8(0) |
3107 1cb8 90E0 ldi r25,hi8(0) |
3108 1cba 24EC ldi r18,lo8(2500) |
3109 1cbc 39E0 ldi r19,hi8(2500) |
3110 1cbe 40E0 ldi r20,hlo8(2500) |
3111 1cc0 50E0 ldi r21,hhi8(2500) |
3112 1cc2 0E94 0000 call __mulsi3 |
3113 1cc6 6856 subi r22,lo8(-(15000)) |
3114 1cc8 754C sbci r23,hi8(-(15000)) |
3115 1cca 8F4F sbci r24,hlo8(-(15000)) |
3116 1ccc 9F4F sbci r25,hhi8(-(15000)) |
3117 1cce 6093 0000 sts TurnOver180Nick,r22 |
3118 1cd2 7093 0000 sts (TurnOver180Nick)+1,r23 |
3119 1cd6 8093 0000 sts (TurnOver180Nick)+2,r24 |
3120 1cda 9093 0000 sts (TurnOver180Nick)+3,r25 |
3121 1cde 6091 0000 lds r22,ParamSet+55 |
3122 1ce2 70E0 ldi r23,lo8(0) |
3123 1ce4 80E0 ldi r24,lo8(0) |
3124 1ce6 90E0 ldi r25,hi8(0) |
3125 1ce8 0E94 0000 call __mulsi3 |
3126 1cec 6856 subi r22,lo8(-(15000)) |
3127 1cee 754C sbci r23,hi8(-(15000)) |
3128 1cf0 8F4F sbci r24,hlo8(-(15000)) |
3129 1cf2 9F4F sbci r25,hhi8(-(15000)) |
3130 1cf4 6093 0000 sts TurnOver180Roll,r22 |
3131 1cf8 7093 0000 sts (TurnOver180Roll)+1,r23 |
3132 1cfc 8093 0000 sts (TurnOver180Roll)+2,r24 |
3133 1d00 9093 0000 sts (TurnOver180Roll)+3,r25 |
3134 1d04 1092 0000 sts (ExternHeightValue)+1,__zero_reg__ |
3135 1d08 1092 0000 sts ExternHeightValue,__zero_reg__ |
3136 1d0c 1092 0000 sts (GPSStickNick)+1,__zero_reg__ |
3137 1d10 1092 0000 sts GPSStickNick,__zero_reg__ |
3138 1d14 1092 0000 sts (GPSStickRoll)+1,__zero_reg__ |
3139 1d18 1092 0000 sts GPSStickRoll,__zero_reg__ |
3140 1d1c 8091 0000 lds r24,MKFlags |
3141 1d20 8460 ori r24,lo8(4) |
3142 1d22 8093 0000 sts MKFlags,r24 |
3143 1d26 8FEF ldi r24,lo8(-1) |
3144 1d28 8093 0000 sts FCParam+31,r24 |
3145 1d2c 1092 0000 sts FCParam+32,__zero_reg__ |
3146 1d30 80E2 ldi r24,lo8(32) |
3147 1d32 8093 0000 sts FCParam+33,r24 |
3148 1d36 E091 0000 lds r30,ParamSet+4 |
3149 1d3a F0E0 ldi r31,lo8(0) |
3150 1d3c EE0F lsl r30 |
3151 1d3e FF1F rol r31 |
3152 1d40 E050 subi r30,lo8(-(PPM_in)) |
3153 1d42 F040 sbci r31,hi8(-(PPM_in)) |
3154 1d44 8081 ld r24,Z |
3155 1d46 9181 ldd r25,Z+1 |
3156 1d48 8259 subi r24,lo8(-(110)) |
3157 1d4a 9F4F sbci r25,hi8(-(110)) |
3158 1d4c 9093 0000 sts (Poti1)+1,r25 |
3159 1d50 8093 0000 sts Poti1,r24 |
3160 1d54 E091 0000 lds r30,ParamSet+5 |
3161 1d58 F0E0 ldi r31,lo8(0) |
3162 1d5a EE0F lsl r30 |
3163 1d5c FF1F rol r31 |
3164 1d5e E050 subi r30,lo8(-(PPM_in)) |
3165 1d60 F040 sbci r31,hi8(-(PPM_in)) |
3166 1d62 8081 ld r24,Z |
3167 1d64 9181 ldd r25,Z+1 |
3168 1d66 8259 subi r24,lo8(-(110)) |
3169 1d68 9F4F sbci r25,hi8(-(110)) |
3170 1d6a 9093 0000 sts (Poti2)+1,r25 |
3171 1d6e 8093 0000 sts Poti2,r24 |
3172 1d72 E091 0000 lds r30,ParamSet+6 |
3173 1d76 F0E0 ldi r31,lo8(0) |
3174 1d78 EE0F lsl r30 |
3175 1d7a FF1F rol r31 |
3176 1d7c E050 subi r30,lo8(-(PPM_in)) |
3177 1d7e F040 sbci r31,hi8(-(PPM_in)) |
3178 1d80 8081 ld r24,Z |
3179 1d82 9181 ldd r25,Z+1 |
3180 1d84 8259 subi r24,lo8(-(110)) |
3181 1d86 9F4F sbci r25,hi8(-(110)) |
3182 1d88 9093 0000 sts (Poti3)+1,r25 |
3183 1d8c 8093 0000 sts Poti3,r24 |
3184 1d90 E091 0000 lds r30,ParamSet+7 |
3185 1d94 F0E0 ldi r31,lo8(0) |
3186 1d96 EE0F lsl r30 |
3187 1d98 FF1F rol r31 |
3188 1d9a E050 subi r30,lo8(-(PPM_in)) |
3189 1d9c F040 sbci r31,hi8(-(PPM_in)) |
3190 1d9e 8081 ld r24,Z |
3191 1da0 9181 ldd r25,Z+1 |
3192 1da2 8259 subi r24,lo8(-(110)) |
3193 1da4 9F4F sbci r25,hi8(-(110)) |
3194 1da6 9093 0000 sts (Poti4)+1,r25 |
3195 1daa 8093 0000 sts Poti4,r24 |
3196 1dae 84E6 ldi r24,lo8(100) |
3197 1db0 8093 0000 sts RC_Quality,r24 |
3198 /* epilogue start */ |
3199 1db4 1F91 pop r17 |
3200 1db6 0F91 pop r16 |
3201 1db8 FF90 pop r15 |
3202 1dba EF90 pop r14 |
3203 1dbc DF90 pop r13 |
3204 1dbe CF90 pop r12 |
3205 1dc0 BF90 pop r11 |
3206 1dc2 AF90 pop r10 |
3207 1dc4 9F90 pop r9 |
3208 1dc6 8F90 pop r8 |
3209 1dc8 7F90 pop r7 |
3210 1dca 6F90 pop r6 |
3211 1dcc 5F90 pop r5 |
3212 1dce 4F90 pop r4 |
3213 1dd0 3F90 pop r3 |
3214 1dd2 2F90 pop r2 |
3215 1dd4 0895 ret |
3216 .L371: |
3217 1dd6 0E94 0000 call SearchAirPressureOffset |
3218 1dda 00C0 rjmp .L370 |
3219 .L373: |
3220 1ddc 84E0 ldi r24,lo8(4) |
3221 1dde 90E0 ldi r25,hi8(4) |
3222 1de0 0E94 0000 call GetParamWord |
3223 1de4 9093 0000 sts (AdBiasAccNick)+1,r25 |
3224 1de8 8093 0000 sts AdBiasAccNick,r24 |
3225 1dec 86E0 ldi r24,lo8(6) |
3226 1dee 90E0 ldi r25,hi8(6) |
3227 1df0 0E94 0000 call GetParamWord |
3228 1df4 9093 0000 sts (AdBiasAccRoll)+1,r25 |
3229 1df8 8093 0000 sts AdBiasAccRoll,r24 |
3230 1dfc 88E0 ldi r24,lo8(8) |
3231 1dfe 90E0 ldi r25,hi8(8) |
3232 1e00 0E94 0000 call GetParamWord |
3233 1e04 AA27 clr r26 |
3234 1e06 97FD sbrc r25,7 |
3235 1e08 A095 com r26 |
3236 1e0a BA2F mov r27,r26 |
3237 1e0c BC01 movw r22,r24 |
3238 1e0e CD01 movw r24,r26 |
3239 1e10 0E94 0000 call __floatsisf |
3240 1e14 6093 0000 sts AdBiasAccTop,r22 |
3241 1e18 7093 0000 sts (AdBiasAccTop)+1,r23 |
3242 1e1c 8093 0000 sts (AdBiasAccTop)+2,r24 |
3243 1e20 9093 0000 sts (AdBiasAccTop)+3,r25 |
3244 1e24 8091 0000 lds r24,AdBiasAccNick |
3245 1e28 9091 0000 lds r25,(AdBiasAccNick)+1 |
3246 1e2c 8150 subi r24,lo8(2049) |
3247 1e2e 9840 sbci r25,hi8(2049) |
3248 1e30 04F4 brge .L379 |
3249 1e32 8091 0000 lds r24,AdBiasAccRoll |
3250 1e36 9091 0000 lds r25,(AdBiasAccRoll)+1 |
3251 1e3a 8150 subi r24,lo8(2049) |
3252 1e3c 9840 sbci r25,hi8(2049) |
3253 1e3e 04F0 brlt .L388 |
3254 .L379: |
3255 1e40 00D0 rcall . |
3256 1e42 0F92 push __tmp_reg__ |
3257 1e44 EDB7 in r30,__SP_L__ |
3258 1e46 FEB7 in r31,__SP_H__ |
3259 1e48 1182 std Z+1,__zero_reg__ |
3260 1e4a 80E0 ldi r24,lo8(__c.1965) |
3261 1e4c 90E0 ldi r25,hi8(__c.1965) |
3262 1e4e 9383 std Z+3,r25 |
3263 1e50 8283 std Z+2,r24 |
3264 1e52 0E94 0000 call _printf_P |
3265 1e56 80E0 ldi r24,lo8(1024) |
3266 1e58 94E0 ldi r25,hi8(1024) |
3267 1e5a 9093 0000 sts (AdBiasAccNick)+1,r25 |
3268 1e5e 8093 0000 sts AdBiasAccNick,r24 |
3269 1e62 9093 0000 sts (AdBiasAccRoll)+1,r25 |
3270 1e66 8093 0000 sts AdBiasAccRoll,r24 |
3271 1e6a 80E0 ldi r24,lo8(0x44354000) |
3272 1e6c 90E4 ldi r25,hi8(0x44354000) |
3273 1e6e A5E3 ldi r26,hlo8(0x44354000) |
3274 1e70 B4E4 ldi r27,hhi8(0x44354000) |
3275 1e72 8093 0000 sts AdBiasAccTop,r24 |
3276 1e76 9093 0000 sts (AdBiasAccTop)+1,r25 |
3277 1e7a A093 0000 sts (AdBiasAccTop)+2,r26 |
3278 1e7e B093 0000 sts (AdBiasAccTop)+3,r27 |
3279 1e82 0F90 pop __tmp_reg__ |
3280 1e84 0F90 pop __tmp_reg__ |
3281 1e86 0F90 pop __tmp_reg__ |
3282 1e88 00C0 rjmp .L378 |
3283 .L388: |
3284 1e8a 6091 0000 lds r22,AdBiasAccTop |
3285 1e8e 7091 0000 lds r23,(AdBiasAccTop)+1 |
3286 1e92 8091 0000 lds r24,(AdBiasAccTop)+2 |
3287 1e96 9091 0000 lds r25,(AdBiasAccTop)+3 |
3288 1e9a 20E0 ldi r18,lo8(0x44800000) |
3289 1e9c 30E0 ldi r19,hi8(0x44800000) |
3290 1e9e 40E8 ldi r20,hlo8(0x44800000) |
3291 1ea0 54E4 ldi r21,hhi8(0x44800000) |
3292 1ea2 0E94 0000 call __gtsf2 |
3293 1ea6 1816 cp __zero_reg__,r24 |
3294 1ea8 04F0 brlt .L379 |
3295 1eaa 00C0 rjmp .L378 |
3296 .L384: |
3297 1eac 0E94 0000 call SearchDacGyroOffset |
3298 1eb0 00C0 rjmp .L369 |
3299 .L385: |
3300 1eb2 9095 com r25 |
3301 1eb4 8195 neg r24 |
3302 1eb6 9F4F sbci r25,lo8(-1) |
3303 1eb8 00C0 rjmp .L375 |
3304 .L386: |
3305 1eba 9095 com r25 |
3306 1ebc 8195 neg r24 |
3307 1ebe 9F4F sbci r25,lo8(-1) |
3308 1ec0 00C0 rjmp .L376 |
3309 .L387: |
3310 1ec2 9095 com r25 |
3311 1ec4 8195 neg r24 |
3312 1ec6 9F4F sbci r25,lo8(-1) |
3313 1ec8 00C0 rjmp .L377 |
3315 .global Beep |
3317 Beep: |
3318 1eca EF92 push r14 |
3319 1ecc FF92 push r15 |
3320 1ece 1F93 push r17 |
3321 1ed0 CF93 push r28 |
3322 1ed2 DF93 push r29 |
3323 /* prologue: function */ |
3324 /* frame size = 0 */ |
3325 1ed4 182F mov r17,r24 |
3326 1ed6 7B01 movw r14,r22 |
3327 1ed8 8091 0000 lds r24,MKFlags |
3328 1edc 80FD sbrc r24,0 |
3329 1ede 00C0 rjmp .L392 |
3330 1ee0 1123 tst r17 |
3331 1ee2 01F0 breq .L392 |
3332 1ee4 EB01 movw r28,r22 |
3333 1ee6 CC0F lsl r28 |
3334 1ee8 DD1F rol r29 |
3335 .L391: |
3336 1eea F092 0000 sts (BeepTime)+1,r15 |
3337 1eee E092 0000 sts BeepTime,r14 |
3338 1ef2 CE01 movw r24,r28 |
3339 1ef4 0E94 0000 call Delay_ms |
3340 1ef8 1150 subi r17,lo8(-(-1)) |
3341 1efa 01F4 brne .L391 |
3342 .L392: |
3343 /* epilogue start */ |
3344 1efc DF91 pop r29 |
3345 1efe CF91 pop r28 |
3346 1f00 1F91 pop r17 |
3347 1f02 FF90 pop r15 |
3348 1f04 EF90 pop r14 |
3349 1f06 0895 ret |
3351 .global SetCompassCalState |
3353 SetCompassCalState: |
3354 /* prologue: function */ |
3355 /* frame size = 0 */ |
3356 1f08 8091 0000 lds r24,ParamSet |
3357 1f0c A82F mov r26,r24 |
3358 1f0e B0E0 ldi r27,lo8(0) |
3359 1f10 AA0F lsl r26 |
3360 1f12 BB1F rol r27 |
3361 1f14 FD01 movw r30,r26 |
3362 1f16 E050 subi r30,lo8(-(PPM_in)) |
3363 1f18 F040 sbci r31,hi8(-(PPM_in)) |
3364 1f1a 8081 ld r24,Z |
3365 1f1c 9181 ldd r25,Z+1 |
3366 1f1e 8D5E subi r24,lo8(-19) |
3367 1f20 9F4F sbci r25,hi8(-19) |
3368 1f22 04F0 brlt .L395 |
3369 1f24 1092 0000 sts stick.2382,__zero_reg__ |
3370 .L395: |
3371 1f28 A050 subi r26,lo8(-(PPM_in)) |
3372 1f2a B040 sbci r27,hi8(-(PPM_in)) |
3373 1f2c 8D91 ld r24,X+ |
3374 1f2e 9C91 ld r25,X |
3375 1f30 8A5B subi r24,lo8(-70) |
3376 1f32 9F4F sbci r25,hi8(-70) |
3377 1f34 04F4 brge .L398 |
3378 1f36 8091 0000 lds r24,stick.2382 |
3379 1f3a 8823 tst r24 |
3380 1f3c 01F4 brne .L398 |
3381 1f3e 81E0 ldi r24,lo8(1) |
3382 1f40 8093 0000 sts stick.2382,r24 |
3383 1f44 8091 0000 lds r24,CompassCalState |
3384 1f48 8F5F subi r24,lo8(-(1)) |
3385 1f4a 8093 0000 sts CompassCalState,r24 |
3386 1f4e 8530 cpi r24,lo8(5) |
3387 1f50 00F0 brlo .L399 |
3388 1f52 88EE ldi r24,lo8(1000) |
3389 1f54 93E0 ldi r25,hi8(1000) |
3390 1f56 9093 0000 sts (BeepTime)+1,r25 |
3391 1f5a 8093 0000 sts BeepTime,r24 |
3392 .L398: |
3393 1f5e 0895 ret |
3394 .L399: |
3395 1f60 66E9 ldi r22,lo8(150) |
3396 1f62 70E0 ldi r23,hi8(150) |
3397 1f64 0E94 0000 call Beep |
3398 1f68 0895 ret |
3400 .global MotorControl |
3402 MotorControl: |
3403 1f6a 2F92 push r2 |
3404 1f6c 3F92 push r3 |
3405 1f6e 4F92 push r4 |
3406 1f70 5F92 push r5 |
3407 1f72 6F92 push r6 |
3408 1f74 7F92 push r7 |
3409 1f76 8F92 push r8 |
3410 1f78 9F92 push r9 |
3411 1f7a AF92 push r10 |
3412 1f7c BF92 push r11 |
3413 1f7e CF92 push r12 |
3414 1f80 DF92 push r13 |
3415 1f82 EF92 push r14 |
3416 1f84 FF92 push r15 |
3417 1f86 0F93 push r16 |
3418 1f88 1F93 push r17 |
3419 1f8a DF93 push r29 |
3420 1f8c CF93 push r28 |
3421 1f8e CDB7 in r28,__SP_L__ |
3422 1f90 DEB7 in r29,__SP_H__ |
3423 1f92 E997 sbiw r28,57 |
3424 1f94 0FB6 in __tmp_reg__,__SREG__ |
3425 1f96 F894 cli |
3426 1f98 DEBF out __SP_H__,r29 |
3427 1f9a 0FBE out __SREG__,__tmp_reg__ |
3428 1f9c CDBF out __SP_L__,r28 |
3429 /* prologue: function */ |
3430 /* frame size = 57 */ |
3431 1f9e 0E94 0000 call Mean |
3432 1fa2 D090 0000 lds r13,BoardRelease |
3433 1fa6 BBE0 ldi r27,lo8(11) |
3434 1fa8 AB2E mov r10,r27 |
3435 1faa AD14 cp r10,r13 |
3436 1fac 00F4 brsh .+2 |
3437 1fae 00C0 rjmp .L401 |
3438 1fb0 299A sbi 37-32,1 |
3439 1fb2 8091 0000 lds r24,RC_Quality |
3440 1fb6 8436 cpi r24,lo8(100) |
3441 1fb8 00F0 brlo .+2 |
3442 1fba 00C0 rjmp .L403 |
3443 .L767: |
3444 1fbc 8091 0000 lds r24,RcLostTimer.2415 |
3445 1fc0 9091 0000 lds r25,(RcLostTimer.2415)+1 |
3446 1fc4 0097 sbiw r24,0 |
3447 1fc6 01F0 breq .+4 |
3448 1fc8 0C94 0000 jmp .L732 |
3449 1fcc 8091 0000 lds r24,MKFlags |
3450 1fd0 8E7E andi r24,lo8(-18) |
3451 1fd2 8093 0000 sts MKFlags,r24 |
3452 1fd6 AAE0 ldi r26,lo8(10) |
3453 1fd8 BA2E mov r11,r26 |
3454 1fda DB14 cp r13,r11 |
3455 1fdc 01F4 brne .+4 |
3456 1fde 0C94 0000 jmp .L406 |
3457 .L791: |
3458 1fe2 F4E1 ldi r31,lo8(20) |
3459 1fe4 CF2E mov r12,r31 |
3460 1fe6 DC14 cp r13,r12 |
3461 1fe8 01F4 brne .+4 |
3462 1fea 0C94 0000 jmp .L406 |
3463 1fee 2898 cbi 37-32,0 |
3464 .L408: |
3465 1ff0 8091 0000 lds r24,ModelIsFlying |
3466 1ff4 9091 0000 lds r25,(ModelIsFlying)+1 |
3467 1ff8 895E subi r24,lo8(1001) |
3468 1ffa 9340 sbci r25,hi8(1001) |
3469 1ffc 00F4 brsh .+4 |
3470 1ffe 0C94 0000 jmp .L727 |
3471 2002 8091 0000 lds r24,MKFlags |
3472 2006 8061 ori r24,lo8(16) |
3473 2008 8093 0000 sts MKFlags,r24 |
3474 200c E091 0000 lds r30,ParamSet |
3475 2010 2E2F mov r18,r30 |
3476 2012 30E0 ldi r19,lo8(0) |
3477 2014 220F lsl r18 |
3478 2016 331F rol r19 |
3479 2018 F901 movw r30,r18 |
3480 201a E050 subi r30,lo8(-(PPM_diff)) |
3481 201c F040 sbci r31,hi8(-(PPM_diff)) |
3482 201e 1182 std Z+1,__zero_reg__ |
3483 2020 1082 st Z,__zero_reg__ |
3484 2022 A091 0000 lds r26,ParamSet+1 |
3485 2026 B0E0 ldi r27,lo8(0) |
3486 2028 AA0F lsl r26 |
3487 202a BB1F rol r27 |
3488 202c FD01 movw r30,r26 |
3489 202e E050 subi r30,lo8(-(PPM_diff)) |
3490 2030 F040 sbci r31,hi8(-(PPM_diff)) |
3491 2032 1182 std Z+1,__zero_reg__ |
3492 2034 1082 st Z,__zero_reg__ |
3493 2036 E091 0000 lds r30,ParamSet+3 |
3494 203a 4E2F mov r20,r30 |
3495 203c 50E0 ldi r21,lo8(0) |
3496 203e 440F lsl r20 |
3497 2040 551F rol r21 |
3498 2042 CA01 movw r24,r20 |
3499 2044 8050 subi r24,lo8(-(PPM_diff)) |
3500 2046 9040 sbci r25,hi8(-(PPM_diff)) |
3501 2048 FC01 movw r30,r24 |
3502 204a 1182 std Z+1,__zero_reg__ |
3503 204c 1082 st Z,__zero_reg__ |
3504 204e 2050 subi r18,lo8(-(PPM_in)) |
3505 2050 3040 sbci r19,hi8(-(PPM_in)) |
3506 2052 F901 movw r30,r18 |
3507 2054 1182 std Z+1,__zero_reg__ |
3508 2056 1082 st Z,__zero_reg__ |
3509 2058 A050 subi r26,lo8(-(PPM_in)) |
3510 205a B040 sbci r27,hi8(-(PPM_in)) |
3511 205c 1196 adiw r26,1 |
3512 205e 1C92 st X,__zero_reg__ |
3513 2060 1E92 st -X,__zero_reg__ |
3514 2062 4050 subi r20,lo8(-(PPM_in)) |
3515 2064 5040 sbci r21,hi8(-(PPM_in)) |
3516 2066 DA01 movw r26,r20 |
3517 2068 1196 adiw r26,1 |
3518 206a 1C92 st X,__zero_reg__ |
3519 206c 1E92 st -X,__zero_reg__ |
3520 .L410: |
3521 206e 8091 0000 lds r24,NewPpmData |
3522 2072 8150 subi r24,lo8(-(-1)) |
3523 2074 8093 0000 sts NewPpmData,r24 |
3524 2078 8F5F subi r24,lo8(-(1)) |
3525 207a 01F0 breq .L471 |
3526 207c 8091 0000 lds r24,MKFlags |
3527 2080 84FF sbrs r24,4 |
3528 2082 00C0 rjmp .L472 |
3529 .L471: |
3530 2084 0E94 0000 call ParameterMapping |
3531 2088 E091 0000 lds r30,ParamSet |
3532 208c F0E0 ldi r31,lo8(0) |
3533 208e EE0F lsl r30 |
3534 2090 FF1F rol r31 |
3535 2092 E050 subi r30,lo8(-(PPM_in)) |
3536 2094 F040 sbci r31,hi8(-(PPM_in)) |
3537 2096 2081 ld r18,Z |
3538 2098 3181 ldd r19,Z+1 |
3539 209a 8091 0000 lds r24,ParamSet+18 |
3540 209e 90E0 ldi r25,lo8(0) |
3541 20a0 289F mul r18,r24 |
3542 20a2 A001 movw r20,r0 |
3543 20a4 299F mul r18,r25 |
3544 20a6 500D add r21,r0 |
3545 20a8 389F mul r19,r24 |
3546 20aa 500D add r21,r0 |
3547 20ac 1124 clr r1 |
3548 20ae 2091 0000 lds r18,stick_nick.2424 |
3549 20b2 3091 0000 lds r19,(stick_nick.2424)+1 |
3550 20b6 C901 movw r24,r18 |
3551 20b8 880F lsl r24 |
3552 20ba 991F rol r25 |
3553 20bc 820F add r24,r18 |
3554 20be 931F adc r25,r19 |
3555 20c0 480F add r20,r24 |
3556 20c2 591F adc r21,r25 |
3557 20c4 57FF sbrs r21,7 |
3558 20c6 00C0 rjmp .+4 |
3559 20c8 0C94 0000 jmp .L733 |
3560 .L473: |
3561 20cc 5595 asr r21 |
3562 20ce 4795 ror r20 |
3563 20d0 5595 asr r21 |
3564 20d2 4795 ror r20 |
3565 20d4 5093 0000 sts (stick_nick.2424)+1,r21 |
3566 20d8 4093 0000 sts stick_nick.2424,r20 |
3567 20dc E091 0000 lds r30,ParamSet |
3568 20e0 F0E0 ldi r31,lo8(0) |
3569 20e2 EE0F lsl r30 |
3570 20e4 FF1F rol r31 |
3571 20e6 E050 subi r30,lo8(-(PPM_diff)) |
3572 20e8 F040 sbci r31,hi8(-(PPM_diff)) |
3573 20ea 2081 ld r18,Z |
3574 20ec 3181 ldd r19,Z+1 |
3575 20ee 8091 0000 lds r24,ParamSet+19 |
3576 20f2 90E0 ldi r25,lo8(0) |
3577 20f4 BC01 movw r22,r24 |
3578 20f6 269F mul r18,r22 |
3579 20f8 C001 movw r24,r0 |
3580 20fa 279F mul r18,r23 |
3581 20fc 900D add r25,r0 |
3582 20fe 369F mul r19,r22 |
3583 2100 900D add r25,r0 |
3584 2102 1124 clr r1 |
3585 2104 840F add r24,r20 |
3586 2106 951F adc r25,r21 |
3587 2108 9093 0000 sts (stick_nick.2424)+1,r25 |
3588 210c 8093 0000 sts stick_nick.2424,r24 |
3589 2110 2091 0000 lds r18,GPSStickNick |
3590 2114 3091 0000 lds r19,(GPSStickNick)+1 |
3591 2118 821B sub r24,r18 |
3592 211a 930B sbc r25,r19 |
3593 211c 9093 0000 sts (StickNick)+1,r25 |
3594 2120 8093 0000 sts StickNick,r24 |
3595 2124 E091 0000 lds r30,ParamSet+1 |
3596 2128 F0E0 ldi r31,lo8(0) |
3597 212a EE0F lsl r30 |
3598 212c FF1F rol r31 |
3599 212e E050 subi r30,lo8(-(PPM_in)) |
3600 2130 F040 sbci r31,hi8(-(PPM_in)) |
3601 2132 2081 ld r18,Z |
3602 2134 3181 ldd r19,Z+1 |
3603 2136 8091 0000 lds r24,ParamSet+18 |
3604 213a 90E0 ldi r25,lo8(0) |
3605 213c 289F mul r18,r24 |
3606 213e A001 movw r20,r0 |
3607 2140 299F mul r18,r25 |
3608 2142 500D add r21,r0 |
3609 2144 389F mul r19,r24 |
3610 2146 500D add r21,r0 |
3611 2148 1124 clr r1 |
3612 214a 2091 0000 lds r18,stick_roll.2425 |
3613 214e 3091 0000 lds r19,(stick_roll.2425)+1 |
3614 2152 C901 movw r24,r18 |
3615 2154 880F lsl r24 |
3616 2156 991F rol r25 |
3617 2158 820F add r24,r18 |
3618 215a 931F adc r25,r19 |
3619 215c 480F add r20,r24 |
3620 215e 591F adc r21,r25 |
3621 2160 57FF sbrs r21,7 |
3622 2162 00C0 rjmp .+4 |
3623 2164 0C94 0000 jmp .L734 |
3624 .L474: |
3625 2168 5595 asr r21 |
3626 216a 4795 ror r20 |
3627 216c 5595 asr r21 |
3628 216e 4795 ror r20 |
3629 2170 5093 0000 sts (stick_roll.2425)+1,r21 |
3630 2174 4093 0000 sts stick_roll.2425,r20 |
3631 2178 E091 0000 lds r30,ParamSet+1 |
3632 217c F0E0 ldi r31,lo8(0) |
3633 217e EE0F lsl r30 |
3634 2180 FF1F rol r31 |
3635 2182 E050 subi r30,lo8(-(PPM_diff)) |
3636 2184 F040 sbci r31,hi8(-(PPM_diff)) |
3637 2186 2081 ld r18,Z |
3638 2188 3181 ldd r19,Z+1 |
3639 218a 8091 0000 lds r24,ParamSet+19 |
3640 218e 90E0 ldi r25,lo8(0) |
3641 2190 DC01 movw r26,r24 |
3642 2192 2A9F mul r18,r26 |
3643 2194 C001 movw r24,r0 |
3644 2196 2B9F mul r18,r27 |
3645 2198 900D add r25,r0 |
3646 219a 3A9F mul r19,r26 |
3647 219c 900D add r25,r0 |
3648 219e 1124 clr r1 |
3649 21a0 840F add r24,r20 |
3650 21a2 951F adc r25,r21 |
3651 21a4 9093 0000 sts (stick_roll.2425)+1,r25 |
3652 21a8 8093 0000 sts stick_roll.2425,r24 |
3653 21ac 2091 0000 lds r18,GPSStickRoll |
3654 21b0 3091 0000 lds r19,(GPSStickRoll)+1 |
3655 21b4 821B sub r24,r18 |
3656 21b6 930B sbc r25,r19 |
3657 21b8 9093 0000 sts (StickRoll)+1,r25 |
3658 21bc 8093 0000 sts StickRoll,r24 |
3659 21c0 E091 0000 lds r30,ParamSet+3 |
3660 21c4 F0E0 ldi r31,lo8(0) |
3661 21c6 EE0F lsl r30 |
3662 21c8 FF1F rol r31 |
3663 21ca E050 subi r30,lo8(-(PPM_in)) |
3664 21cc F040 sbci r31,hi8(-(PPM_in)) |
3665 21ce 8081 ld r24,Z |
3666 21d0 9181 ldd r25,Z+1 |
3667 21d2 2227 clr r18 |
3668 21d4 3327 clr r19 |
3669 21d6 281B sub r18,r24 |
3670 21d8 390B sbc r19,r25 |
3671 21da 3093 0000 sts (StickYaw)+1,r19 |
3672 21de 2093 0000 sts StickYaw,r18 |
3673 21e2 8091 0000 lds r24,ParamSet+8 |
3674 21e6 8872 andi r24,lo8(40) |
3675 21e8 01F0 breq .L475 |
3676 21ea 2330 cpi r18,3 |
3677 21ec 3105 cpc r19,__zero_reg__ |
3678 21ee 04F0 brlt .+4 |
3679 21f0 0C94 0000 jmp .L735 |
3680 21f4 BFEF ldi r27,hi8(-2) |
3681 21f6 2E3F cpi r18,lo8(-2) |
3682 21f8 3B07 cpc r19,r27 |
3683 21fa 04F0 brlt .+4 |
3684 21fc 0C94 0000 jmp .L477 |
3685 2200 2E5F subi r18,lo8(-(2)) |
3686 2202 3F4F sbci r19,hi8(-(2)) |
3687 2204 3093 0000 sts (StickYaw)+1,r19 |
3688 2208 2093 0000 sts StickYaw,r18 |
3689 .L475: |
3690 220c E091 0000 lds r30,ParamSet+2 |
3691 2210 F0E0 ldi r31,lo8(0) |
3692 2212 EE0F lsl r30 |
3693 2214 FF1F rol r31 |
3694 2216 E050 subi r30,lo8(-(PPM_in)) |
3695 2218 F040 sbci r31,hi8(-(PPM_in)) |
3696 221a 0190 ld __tmp_reg__,Z+ |
3697 221c F081 ld r31,Z |
3698 221e E02D mov r30,__tmp_reg__ |
3699 2220 E858 subi r30,lo8(-(120)) |
3700 2222 FF4F sbci r31,hi8(-(120)) |
3701 2224 F093 0000 sts (StickGas)+1,r31 |
3702 2228 E093 0000 sts StickGas,r30 |
3703 222c 8091 0000 lds r24,FCParam+7 |
3704 2230 865F subi r24,lo8(-(10)) |
3705 2232 8093 0000 sts GyroPFactor,r24 |
3706 2236 8091 0000 lds r24,FCParam+8 |
3707 223a 8093 0000 sts GyroIFactor,r24 |
3708 223e 8091 0000 lds r24,FCParam+9 |
3709 2242 865F subi r24,lo8(-(10)) |
3710 2244 8093 0000 sts GyroYawPFactor,r24 |
3711 2248 8091 0000 lds r24,FCParam+10 |
3712 224c 8093 0000 sts GyroYawIFactor,r24 |
3713 2250 8091 0000 lds r24,ExternControl+10 |
3714 2254 80FF sbrs r24,0 |
3715 2256 00C0 rjmp .L478 |
3716 2258 8091 0000 lds r24,FCParam+28 |
3717 225c 8138 cpi r24,lo8(-127) |
3718 225e 00F4 brsh .+2 |
3719 2260 00C0 rjmp .L478 |
3720 2262 2091 0000 lds r18,ParamSet+18 |
3721 2266 30E0 ldi r19,lo8(0) |
3722 2268 8091 0000 lds r24,ExternControl+3 |
3723 226c 9927 clr r25 |
3724 226e 87FD sbrc r24,7 |
3725 2270 9095 com r25 |
3726 2272 289F mul r18,r24 |
3727 2274 A001 movw r20,r0 |
3728 2276 299F mul r18,r25 |
3729 2278 500D add r21,r0 |
3730 227a 389F mul r19,r24 |
3731 227c 500D add r21,r0 |
3732 227e 1124 clr r1 |
3733 2280 8091 0000 lds r24,StickNick |
3734 2284 9091 0000 lds r25,(StickNick)+1 |
3735 2288 840F add r24,r20 |
3736 228a 951F adc r25,r21 |
3737 228c 9093 0000 sts (StickNick)+1,r25 |
3738 2290 8093 0000 sts StickNick,r24 |
3739 2294 8091 0000 lds r24,ExternControl+4 |
3740 2298 9927 clr r25 |
3741 229a 87FD sbrc r24,7 |
3742 229c 9095 com r25 |
3743 229e 5901 movw r10,r18 |
3744 22a0 A89E mul r10,r24 |
3745 22a2 9001 movw r18,r0 |
3746 22a4 A99E mul r10,r25 |
3747 22a6 300D add r19,r0 |
3748 22a8 B89E mul r11,r24 |
3749 22aa 300D add r19,r0 |
3750 22ac 1124 clr r1 |
3751 22ae 8091 0000 lds r24,StickRoll |
3752 22b2 9091 0000 lds r25,(StickRoll)+1 |
3753 22b6 820F add r24,r18 |
3754 22b8 931F adc r25,r19 |
3755 22ba 9093 0000 sts (StickRoll)+1,r25 |
3756 22be 8093 0000 sts StickRoll,r24 |
3757 22c2 2091 0000 lds r18,ExternControl+5 |
3758 22c6 3327 clr r19 |
3759 22c8 27FD sbrc r18,7 |
3760 22ca 3095 com r19 |
3761 22cc 8091 0000 lds r24,StickYaw |
3762 22d0 9091 0000 lds r25,(StickYaw)+1 |
3763 22d4 820F add r24,r18 |
3764 22d6 931F adc r25,r19 |
3765 22d8 9093 0000 sts (StickYaw)+1,r25 |
3766 22dc 8093 0000 sts StickYaw,r24 |
3767 22e0 2091 0000 lds r18,ParamSet+13 |
3768 22e4 30E0 ldi r19,lo8(0) |
3769 22e6 8091 0000 lds r24,ExternControl+7 |
3770 22ea 9927 clr r25 |
3771 22ec 87FD sbrc r24,7 |
3772 22ee 9095 com r25 |
3773 22f0 6C01 movw r12,r24 |
3774 22f2 2C9D mul r18,r12 |
3775 22f4 C001 movw r24,r0 |
3776 22f6 2D9D mul r18,r13 |
3777 22f8 900D add r25,r0 |
3778 22fa 3C9D mul r19,r12 |
3779 22fc 900D add r25,r0 |
3780 22fe 1124 clr r1 |
3781 2300 9093 0000 sts (ExternHeightValue)+1,r25 |
3782 2304 8093 0000 sts ExternHeightValue,r24 |
3783 2308 8091 0000 lds r24,ExternControl+6 |
3784 230c 90E0 ldi r25,lo8(0) |
3785 230e 8E17 cp r24,r30 |
3786 2310 9F07 cpc r25,r31 |
3787 2312 04F4 brge .L478 |
3788 2314 9093 0000 sts (StickGas)+1,r25 |
3789 2318 8093 0000 sts StickGas,r24 |
3790 .L478: |
3791 231c 8091 0000 lds r24,StickGas |
3792 2320 9091 0000 lds r25,(StickGas)+1 |
3793 2324 97FF sbrs r25,7 |
3794 2326 00C0 rjmp .+4 |
3795 2328 0C94 0000 jmp .L736 |
3796 232c 8091 0000 lds r24,ParamSet+8 |
3797 2330 82FF sbrs r24,2 |
3798 2332 00C0 rjmp .+4 |
3799 2334 0C94 0000 jmp .L737 |
3800 .L480: |
3801 2338 6091 0000 lds r22,StickNick |
3802 233c 7091 0000 lds r23,StickNick+1 |
3803 2340 4091 0000 lds r20,MaxStickNick |
3804 2344 5091 0000 lds r21,(MaxStickNick)+1 |
3805 2348 CB01 movw r24,r22 |
3806 234a 9C01 movw r18,r24 |
3807 234c 97FF sbrs r25,7 |
3808 234e 00C0 rjmp .+4 |
3809 2350 0C94 0000 jmp .L738 |
3810 .L482: |
3811 2354 3595 asr r19 |
3812 2356 2795 ror r18 |
3813 2358 3595 asr r19 |
3814 235a 2795 ror r18 |
3815 235c 37FF sbrs r19,7 |
3816 235e 00C0 rjmp .+4 |
3817 2360 0C94 0000 jmp .L739 |
3818 .L483: |
3819 2364 4217 cp r20,r18 |
3820 2366 5307 cpc r21,r19 |
3821 2368 04F4 brge .+4 |
3822 236a 0C94 0000 jmp .L740 |
3823 236e 4150 subi r20,lo8(-(-1)) |
3824 2370 5040 sbci r21,hi8(-(-1)) |
3825 2372 5093 0000 sts (MaxStickNick)+1,r21 |
3826 2376 4093 0000 sts MaxStickNick,r20 |
3827 .L485: |
3828 237a 6091 0000 lds r22,StickRoll |
3829 237e 7091 0000 lds r23,StickRoll+1 |
3830 2382 4091 0000 lds r20,MaxStickRoll |
3831 2386 5091 0000 lds r21,(MaxStickRoll)+1 |
3832 238a CB01 movw r24,r22 |
3833 238c 9C01 movw r18,r24 |
3834 238e 97FF sbrs r25,7 |
3835 2390 00C0 rjmp .+4 |
3836 2392 0C94 0000 jmp .L741 |
3837 .L487: |
3838 2396 3595 asr r19 |
3839 2398 2795 ror r18 |
3840 239a 3595 asr r19 |
3841 239c 2795 ror r18 |
3842 239e 37FF sbrs r19,7 |
3843 23a0 00C0 rjmp .+4 |
3844 23a2 0C94 0000 jmp .L742 |
3845 .L488: |
3846 23a6 4217 cp r20,r18 |
3847 23a8 5307 cpc r21,r19 |
3848 23aa 04F0 brlt .+4 |
3849 23ac 0C94 0000 jmp .L486 |
3850 23b0 9B01 movw r18,r22 |
3851 23b2 77FF sbrs r23,7 |
3852 23b4 00C0 rjmp .+4 |
3853 23b6 0C94 0000 jmp .L743 |
3854 .L489: |
3855 23ba C901 movw r24,r18 |
3856 23bc 9595 asr r25 |
3857 23be 8795 ror r24 |
3858 23c0 9595 asr r25 |
3859 23c2 8795 ror r24 |
3860 23c4 9093 0000 sts (MaxStickRoll)+1,r25 |
3861 23c8 8093 0000 sts MaxStickRoll,r24 |
3862 23cc 8536 cpi r24,101 |
3863 23ce 9105 cpc r25,__zero_reg__ |
3864 23d0 04F0 brlt .+4 |
3865 23d2 0C94 0000 jmp .L744 |
3866 .L490: |
3867 23d6 8091 0000 lds r24,ParamSet+1 |
3868 23da A82F mov r26,r24 |
3869 23dc B0E0 ldi r27,lo8(0) |
3870 23de AA0F lsl r26 |
3871 23e0 BB1F rol r27 |
3872 23e2 FD01 movw r30,r26 |
3873 23e4 E050 subi r30,lo8(-(PPM_in)) |
3874 23e6 F040 sbci r31,hi8(-(PPM_in)) |
3875 23e8 2081 ld r18,Z |
3876 23ea 3181 ldd r19,Z+1 |
3877 23ec 8091 0000 lds r24,ParamSet+49 |
3878 23f0 482F mov r20,r24 |
3879 23f2 50E0 ldi r21,lo8(0) |
3880 23f4 4217 cp r20,r18 |
3881 23f6 5307 cpc r21,r19 |
3882 23f8 04F4 brge .L491 |
3883 23fa 8091 0000 lds r24,ParamSet+86 |
3884 23fe 82FF sbrs r24,2 |
3885 2400 00C0 rjmp .+4 |
3886 2402 0C94 0000 jmp .L745 |
3887 .L491: |
3888 2406 7091 0000 lds r23,LoopingLeft |
3889 240a 7723 tst r23 |
3890 240c 01F0 breq .L492 |
3891 240e FD01 movw r30,r26 |
3892 2410 E050 subi r30,lo8(-(PPM_in)) |
3893 2412 F040 sbci r31,hi8(-(PPM_in)) |
3894 2414 2081 ld r18,Z |
3895 2416 3181 ldd r19,Z+1 |
3896 2418 8091 0000 lds r24,ParamSet+50 |
3897 241c FA01 movw r30,r20 |
3898 241e E81B sub r30,r24 |
3899 2420 F109 sbc r31,__zero_reg__ |
3900 2422 2E17 cp r18,r30 |
3901 2424 3F07 cpc r19,r31 |
3902 2426 04F4 brge .+4 |
3903 2428 0C94 0000 jmp .L746 |
3904 .L492: |
3905 242c FD01 movw r30,r26 |
3906 242e E050 subi r30,lo8(-(PPM_in)) |
3907 2430 F040 sbci r31,hi8(-(PPM_in)) |
3908 2432 2081 ld r18,Z |
3909 2434 3181 ldd r19,Z+1 |
3910 2436 8827 clr r24 |
3911 2438 9927 clr r25 |
3912 243a 841B sub r24,r20 |
3913 243c 950B sbc r25,r21 |
3914 243e 2817 cp r18,r24 |
3915 2440 3907 cpc r19,r25 |
3916 2442 04F4 brge .L493 |
3917 2444 8091 0000 lds r24,ParamSet+86 |
3918 2448 83FF sbrs r24,3 |
3919 244a 00C0 rjmp .+4 |
3920 244c 0C94 0000 jmp .L747 |
3921 .L493: |
3922 2450 8091 0000 lds r24,LoopingRight |
3923 2454 8823 tst r24 |
3924 2456 01F0 breq .L494 |
3925 2458 A050 subi r26,lo8(-(PPM_in)) |
3926 245a B040 sbci r27,hi8(-(PPM_in)) |
3927 245c 2D91 ld r18,X+ |
3928 245e 3C91 ld r19,X |
3929 2460 8091 0000 lds r24,ParamSet+50 |
3930 2464 90E0 ldi r25,lo8(0) |
3931 2466 841B sub r24,r20 |
3932 2468 950B sbc r25,r21 |
3933 246a 8217 cp r24,r18 |
3934 246c 9307 cpc r25,r19 |
3935 246e 04F4 brge .+4 |
3936 2470 0C94 0000 jmp .L748 |
3937 .L494: |
3938 2474 8091 0000 lds r24,ParamSet |
3939 2478 A82F mov r26,r24 |
3940 247a B0E0 ldi r27,lo8(0) |
3941 247c AA0F lsl r26 |
3942 247e BB1F rol r27 |
3943 2480 FD01 movw r30,r26 |
3944 2482 E050 subi r30,lo8(-(PPM_in)) |
3945 2484 F040 sbci r31,hi8(-(PPM_in)) |
3946 2486 8081 ld r24,Z |
3947 2488 9181 ldd r25,Z+1 |
3948 248a 4817 cp r20,r24 |
3949 248c 5907 cpc r21,r25 |
3950 248e 04F4 brge .L495 |
3951 2490 8091 0000 lds r24,ParamSet+86 |
3952 2494 80FF sbrs r24,0 |
3953 2496 00C0 rjmp .+4 |
3954 2498 0C94 0000 jmp .L749 |
3955 .L495: |
3956 249c 6091 0000 lds r22,LoopingTop |
3957 24a0 6623 tst r22 |
3958 24a2 01F0 breq .L496 |
3959 24a4 FD01 movw r30,r26 |
3960 24a6 E050 subi r30,lo8(-(PPM_in)) |
3961 24a8 F040 sbci r31,hi8(-(PPM_in)) |
3962 24aa 2081 ld r18,Z |
3963 24ac 3181 ldd r19,Z+1 |
3964 24ae 8091 0000 lds r24,ParamSet+50 |
3965 24b2 5A01 movw r10,r20 |
3966 24b4 A81A sub r10,r24 |
3967 24b6 B108 sbc r11,__zero_reg__ |
3968 24b8 2A15 cp r18,r10 |
3969 24ba 3B05 cpc r19,r11 |
3970 24bc 04F4 brge .+4 |
3971 24be 0C94 0000 jmp .L750 |
3972 .L496: |
3973 24c2 FD01 movw r30,r26 |
3974 24c4 E050 subi r30,lo8(-(PPM_in)) |
3975 24c6 F040 sbci r31,hi8(-(PPM_in)) |
3976 24c8 2081 ld r18,Z |
3977 24ca 3181 ldd r19,Z+1 |
3978 24cc 8827 clr r24 |
3979 24ce 9927 clr r25 |
3980 24d0 841B sub r24,r20 |
3981 24d2 950B sbc r25,r21 |
3982 24d4 2817 cp r18,r24 |
3983 24d6 3907 cpc r19,r25 |
3984 24d8 04F4 brge .L497 |
3985 24da 8091 0000 lds r24,ParamSet+86 |
3986 24de 81FF sbrs r24,1 |
3987 24e0 00C0 rjmp .+4 |
3988 24e2 0C94 0000 jmp .L751 |
3989 .L497: |
3990 24e6 8091 0000 lds r24,LoopingDown |
3991 24ea 8823 tst r24 |
3992 24ec 01F0 breq .L498 |
3993 24ee A050 subi r26,lo8(-(PPM_in)) |
3994 24f0 B040 sbci r27,hi8(-(PPM_in)) |
3995 24f2 2D91 ld r18,X+ |
3996 24f4 3C91 ld r19,X |
3997 24f6 8091 0000 lds r24,ParamSet+50 |
3998 24fa 90E0 ldi r25,lo8(0) |
3999 24fc 841B sub r24,r20 |
4000 24fe 950B sbc r25,r21 |
4001 2500 8217 cp r24,r18 |
4002 2502 9307 cpc r25,r19 |
4003 2504 04F4 brge .+4 |
4004 2506 0C94 0000 jmp .L752 |
4005 .L498: |
4006 250a 7723 tst r23 |
4007 250c 01F4 brne .L499 |
4008 250e 8091 0000 lds r24,LoopingRight |
4009 2512 8823 tst r24 |
4010 2514 01F4 brne .+4 |
4011 2516 0C94 0000 jmp .L500 |
4012 .L499: |
4013 251a 81E0 ldi r24,lo8(1) |
4014 251c 8093 0000 sts LoopingRoll,r24 |
4015 .L501: |
4016 2520 6623 tst r22 |
4017 2522 01F4 brne .L502 |
4018 2524 8091 0000 lds r24,LoopingDown |
4019 2528 8823 tst r24 |
4020 252a 01F4 brne .+4 |
4021 252c 0C94 0000 jmp .L503 |
4022 .L502: |
4023 2530 81E0 ldi r24,lo8(1) |
4024 2532 8093 0000 sts LoopingNick,r24 |
4025 2536 1092 0000 sts LoopingRoll,__zero_reg__ |
4026 253a 1092 0000 sts LoopingLeft,__zero_reg__ |
4027 253e 1092 0000 sts LoopingRight,__zero_reg__ |
4028 .L472: |
4029 2542 8091 0000 lds r24,LoopingRoll |
4030 2546 8823 tst r24 |
4031 2548 01F0 breq .+2 |
4032 254a 00C0 rjmp .L504 |
4033 254c 8091 0000 lds r24,LoopingNick |
4034 2550 8823 tst r24 |
4035 2552 01F0 breq .+2 |
4036 2554 00C0 rjmp .L504 |
4037 .L505: |
4038 2556 8091 0000 lds r24,MKFlags |
4039 255a 84FF sbrs r24,4 |
4040 255c 00C0 rjmp .L507 |
4041 255e 1092 0000 sts (StickYaw)+1,__zero_reg__ |
4042 2562 1092 0000 sts StickYaw,__zero_reg__ |
4043 2566 1092 0000 sts (StickNick)+1,__zero_reg__ |
4044 256a 1092 0000 sts StickNick,__zero_reg__ |
4045 256e 1092 0000 sts (StickRoll)+1,__zero_reg__ |
4046 2572 1092 0000 sts StickRoll,__zero_reg__ |
4047 2576 8091 0000 lds r24,ParamSet+31 |
4048 257a 8093 0000 sts StickGas,r24 |
4049 257e 1092 0000 sts StickGas+1,__zero_reg__ |
4050 2582 9AE5 ldi r25,lo8(90) |
4051 2584 9093 0000 sts GyroPFactor,r25 |
4052 2588 88E7 ldi r24,lo8(120) |
4053 258a 8093 0000 sts GyroIFactor,r24 |
4054 258e 9093 0000 sts GyroYawPFactor,r25 |
4055 2592 8093 0000 sts GyroYawIFactor,r24 |
4056 2596 1092 0000 sts LoopingRoll,__zero_reg__ |
4057 259a 1092 0000 sts LoopingNick,__zero_reg__ |
4058 259e 1092 0000 sts (MaxStickNick)+1,__zero_reg__ |
4059 25a2 1092 0000 sts MaxStickNick,__zero_reg__ |
4060 25a6 1092 0000 sts (MaxStickRoll)+1,__zero_reg__ |
4061 25aa 1092 0000 sts MaxStickRoll,__zero_reg__ |
4062 .L507: |
4063 25ae 2091 0000 lds r18,MeanIntegralGyroNick |
4064 25b2 3091 0000 lds r19,(MeanIntegralGyroNick)+1 |
4065 25b6 4091 0000 lds r20,(MeanIntegralGyroNick)+2 |
4066 25ba 5091 0000 lds r21,(MeanIntegralGyroNick)+3 |
4067 25be 28A3 std Y+32,r18 |
4068 25c0 39A3 std Y+33,r19 |
4069 25c2 4AA3 std Y+34,r20 |
4070 25c4 5BA3 std Y+35,r21 |
4071 25c6 4091 0000 lds r20,IntegralGyroNick |
4072 25ca 5091 0000 lds r21,(IntegralGyroNick)+1 |
4073 25ce 6091 0000 lds r22,(IntegralGyroNick)+2 |
4074 25d2 7091 0000 lds r23,(IntegralGyroNick)+3 |
4075 25d6 48A7 std Y+40,r20 |
4076 25d8 59A7 std Y+41,r21 |
4077 25da 6AA7 std Y+42,r22 |
4078 25dc 7BA7 std Y+43,r23 |
4079 25de 88A1 ldd r24,Y+32 |
4080 25e0 99A1 ldd r25,Y+33 |
4081 25e2 AAA1 ldd r26,Y+34 |
4082 25e4 BBA1 ldd r27,Y+35 |
4083 25e6 840F add r24,r20 |
4084 25e8 951F adc r25,r21 |
4085 25ea A61F adc r26,r22 |
4086 25ec B71F adc r27,r23 |
4087 25ee 8093 0000 sts MeanIntegralGyroNick,r24 |
4088 25f2 9093 0000 sts (MeanIntegralGyroNick)+1,r25 |
4089 25f6 A093 0000 sts (MeanIntegralGyroNick)+2,r26 |
4090 25fa B093 0000 sts (MeanIntegralGyroNick)+3,r27 |
4091 25fe 2090 0000 lds r2,MeanIntegralGyroRoll |
4092 2602 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 |
4093 2606 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 |
4094 260a 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 |
4095 260e A090 0000 lds r10,IntegralGyroRoll |
4096 2612 B090 0000 lds r11,(IntegralGyroRoll)+1 |
4097 2616 C090 0000 lds r12,(IntegralGyroRoll)+2 |
4098 261a D090 0000 lds r13,(IntegralGyroRoll)+3 |
4099 261e ACA2 std Y+36,r10 |
4100 2620 BDA2 std Y+37,r11 |
4101 2622 CEA2 std Y+38,r12 |
4102 2624 DFA2 std Y+39,r13 |
4103 2626 2A0C add r2,r10 |
4104 2628 3B1C adc r3,r11 |
4105 262a 4C1C adc r4,r12 |
4106 262c 5D1C adc r5,r13 |
4107 262e 2092 0000 sts MeanIntegralGyroRoll,r2 |
4108 2632 3092 0000 sts (MeanIntegralGyroRoll)+1,r3 |
4109 2636 4092 0000 sts (MeanIntegralGyroRoll)+2,r4 |
4110 263a 5092 0000 sts (MeanIntegralGyroRoll)+3,r5 |
4111 263e B090 0000 lds r11,LoopingNick |
4112 2642 BD8E std Y+29,r11 |
4113 2644 BB20 tst r11 |
4114 2646 01F4 brne .L508 |
4115 2648 8091 0000 lds r24,LoopingRoll |
4116 264c 8823 tst r24 |
4117 264e 01F4 brne .+4 |
4118 2650 0C94 0000 jmp .L509 |
4119 .L508: |
4120 2654 1092 0000 sts (MeasurementCounter)+1,__zero_reg__ |
4121 2658 1092 0000 sts MeasurementCounter,__zero_reg__ |
4122 265c 1092 0000 sts MeanAccNick,__zero_reg__ |
4123 2660 1092 0000 sts (MeanAccNick)+1,__zero_reg__ |
4124 2664 1092 0000 sts (MeanAccNick)+2,__zero_reg__ |
4125 2668 1092 0000 sts (MeanAccNick)+3,__zero_reg__ |
4126 266c 1092 0000 sts MeanAccRoll,__zero_reg__ |
4127 2670 1092 0000 sts (MeanAccRoll)+1,__zero_reg__ |
4128 2674 1092 0000 sts (MeanAccRoll)+2,__zero_reg__ |
4129 2678 1092 0000 sts (MeanAccRoll)+3,__zero_reg__ |
4130 267c 1092 0000 sts MeanIntegralGyroNick,__zero_reg__ |
4131 2680 1092 0000 sts (MeanIntegralGyroNick)+1,__zero_reg__ |
4132 2684 1092 0000 sts (MeanIntegralGyroNick)+2,__zero_reg__ |
4133 2688 1092 0000 sts (MeanIntegralGyroNick)+3,__zero_reg__ |
4134 268c 1092 0000 sts MeanIntegralGyroRoll,__zero_reg__ |
4135 2690 1092 0000 sts (MeanIntegralGyroRoll)+1,__zero_reg__ |
4136 2694 1092 0000 sts (MeanIntegralGyroRoll)+2,__zero_reg__ |
4137 2698 1092 0000 sts (MeanIntegralGyroRoll)+3,__zero_reg__ |
4138 269c 8091 0000 lds r24,ReadingIntegralGyroNick |
4139 26a0 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 |
4140 26a4 A091 0000 lds r26,(ReadingIntegralGyroNick)+2 |
4141 26a8 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 |
4142 26ac 8093 0000 sts ReadingIntegralGyroNick2,r24 |
4143 26b0 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
4144 26b4 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
4145 26b8 B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
4146 26bc 8091 0000 lds r24,ReadingIntegralGyroRoll |
4147 26c0 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 |
4148 26c4 A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 |
4149 26c8 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 |
4150 26cc 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
4151 26d0 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
4152 26d4 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
4153 26d8 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
4154 26dc 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ |
4155 26e0 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ |
4156 26e4 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ |
4157 26e8 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ |
4158 26ec CD8C ldd r12,Y+29 |
4159 26ee CC20 tst r12 |
4160 26f0 01F4 brne .+4 |
4161 26f2 0C94 0000 jmp .L753 |
4162 .L510: |
4163 26f6 8091 0000 lds r24,MeasurementCounter |
4164 26fa 9091 0000 lds r25,(MeasurementCounter)+1 |
4165 26fe 8F3F cpi r24,255 |
4166 2700 9105 cpc r25,__zero_reg__ |
4167 2702 01F0 breq .+2 |
4168 2704 00F4 brsh .+2 |
4169 2706 00C0 rjmp .L529 |
4170 2708 6D8D ldd r22,Y+29 |
4171 270a 6623 tst r22 |
4172 270c 01F4 brne .+4 |
4173 270e 0C94 0000 jmp .L754 |
4174 .L530: |
4175 2712 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ |
4176 2716 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ |
4177 271a 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ |
4178 271e 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ |
4179 2722 1092 0000 sts FunnelCourse,__zero_reg__ |
4180 2726 4091 0000 lds r20,MeanIntegralGyroNick |
4181 272a 5091 0000 lds r21,(MeanIntegralGyroNick)+1 |
4182 272e 6091 0000 lds r22,(MeanIntegralGyroNick)+2 |
4183 2732 7091 0000 lds r23,(MeanIntegralGyroNick)+3 |
4184 2736 48A3 std Y+32,r20 |
4185 2738 59A3 std Y+33,r21 |
4186 273a 6AA3 std Y+34,r22 |
4187 273c 7BA3 std Y+35,r23 |
4188 273e 2090 0000 lds r2,MeanIntegralGyroRoll |
4189 2742 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 |
4190 2746 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 |
4191 274a 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 |
4192 .L567: |
4193 274e 8091 0000 lds r24,GyroIFactor |
4194 2752 8823 tst r24 |
4195 2754 01F4 brne .L568 |
4196 2756 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ |
4197 275a 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ |
4198 275e 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ |
4199 2762 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ |
4200 .L568: |
4201 2766 68A1 ldd r22,Y+32 |
4202 2768 79A1 ldd r23,Y+33 |
4203 276a 8AA1 ldd r24,Y+34 |
4204 276c 9BA1 ldd r25,Y+35 |
4205 276e 6093 0000 sts MeanIntegralGyroNick_old.2431,r22 |
4206 2772 7093 0000 sts (MeanIntegralGyroNick_old.2431)+1,r23 |
4207 2776 8093 0000 sts (MeanIntegralGyroNick_old.2431)+2,r24 |
4208 277a 9093 0000 sts (MeanIntegralGyroNick_old.2431)+3,r25 |
4209 277e 2092 0000 sts MeanIntegralGyroRoll_old.2432,r2 |
4210 2782 3092 0000 sts (MeanIntegralGyroRoll_old.2432)+1,r3 |
4211 2786 4092 0000 sts (MeanIntegralGyroRoll_old.2432)+2,r4 |
4212 278a 5092 0000 sts (MeanIntegralGyroRoll_old.2432)+3,r5 |
4213 278e 1092 0000 sts MeanAccNick,__zero_reg__ |
4214 2792 1092 0000 sts (MeanAccNick)+1,__zero_reg__ |
4215 2796 1092 0000 sts (MeanAccNick)+2,__zero_reg__ |
4216 279a 1092 0000 sts (MeanAccNick)+3,__zero_reg__ |
4217 279e 1092 0000 sts MeanAccRoll,__zero_reg__ |
4218 27a2 1092 0000 sts (MeanAccRoll)+1,__zero_reg__ |
4219 27a6 1092 0000 sts (MeanAccRoll)+2,__zero_reg__ |
4220 27aa 1092 0000 sts (MeanAccRoll)+3,__zero_reg__ |
4221 27ae 1092 0000 sts MeanIntegralGyroNick,__zero_reg__ |
4222 27b2 1092 0000 sts (MeanIntegralGyroNick)+1,__zero_reg__ |
4223 27b6 1092 0000 sts (MeanIntegralGyroNick)+2,__zero_reg__ |
4224 27ba 1092 0000 sts (MeanIntegralGyroNick)+3,__zero_reg__ |
4225 27be 1092 0000 sts MeanIntegralGyroRoll,__zero_reg__ |
4226 27c2 1092 0000 sts (MeanIntegralGyroRoll)+1,__zero_reg__ |
4227 27c6 1092 0000 sts (MeanIntegralGyroRoll)+2,__zero_reg__ |
4228 27ca 1092 0000 sts (MeanIntegralGyroRoll)+3,__zero_reg__ |
4229 27ce 1092 0000 sts (MeasurementCounter)+1,__zero_reg__ |
4230 27d2 1092 0000 sts MeasurementCounter,__zero_reg__ |
4231 .L529: |
4232 27d6 E090 0000 lds r14,StickYaw |
4233 27da F090 0000 lds r15,(StickYaw)+1 |
4234 27de C701 movw r24,r14 |
4235 27e0 F7FE sbrs r15,7 |
4236 27e2 00C0 rjmp .+4 |
4237 27e4 0C94 0000 jmp .L755 |
4238 .L570: |
4239 27e8 4097 sbiw r24,16 |
4240 27ea 04F0 brlt .L569 |
4241 27ec 88EE ldi r24,lo8(1000) |
4242 27ee 93E0 ldi r25,hi8(1000) |
4243 27f0 9093 0000 sts (BadCompassHeading)+1,r25 |
4244 27f4 8093 0000 sts BadCompassHeading,r24 |
4245 27f8 8091 0000 lds r24,ParamSet+8 |
4246 27fc 84FD sbrc r24,4 |
4247 27fe 00C0 rjmp .+4 |
4248 2800 0C94 0000 jmp .L756 |
4249 .L569: |
4250 2804 1091 0000 lds r17,ParamSet+20 |
4251 2808 B701 movw r22,r14 |
4252 280a 8827 clr r24 |
4253 280c 77FD sbrc r23,7 |
4254 280e 8095 com r24 |
4255 2810 982F mov r25,r24 |
4256 2812 212F mov r18,r17 |
4257 2814 30E0 ldi r19,lo8(0) |
4258 2816 40E0 ldi r20,lo8(0) |
4259 2818 50E0 ldi r21,hi8(0) |
4260 281a 0E94 0000 call __mulsi3 |
4261 281e F701 movw r30,r14 |
4262 2820 F7FE sbrs r15,7 |
4263 2822 00C0 rjmp .+4 |
4264 2824 0C94 0000 jmp .L757 |
4265 .L571: |
4266 2828 9F01 movw r18,r30 |
4267 282a 4427 clr r20 |
4268 282c 37FD sbrc r19,7 |
4269 282e 4095 com r20 |
4270 2830 542F mov r21,r20 |
4271 2832 0E94 0000 call __mulsi3 |
4272 2836 20E0 ldi r18,lo8(512) |
4273 2838 32E0 ldi r19,hi8(512) |
4274 283a 40E0 ldi r20,hlo8(512) |
4275 283c 50E0 ldi r21,hhi8(512) |
4276 283e 0E94 0000 call __divmodsi4 |
4277 2842 812F mov r24,r17 |
4278 2844 90E0 ldi r25,lo8(0) |
4279 2846 8E9D mul r24,r14 |
4280 2848 B001 movw r22,r0 |
4281 284a 8F9D mul r24,r15 |
4282 284c 700D add r23,r0 |
4283 284e 9E9D mul r25,r14 |
4284 2850 700D add r23,r0 |
4285 2852 1124 clr r1 |
4286 2854 77FF sbrs r23,7 |
4287 2856 00C0 rjmp .+4 |
4288 2858 0C94 0000 jmp .L758 |
4289 .L572: |
4290 285c 7595 asr r23 |
4291 285e 6795 ror r22 |
4292 2860 7595 asr r23 |
4293 2862 6795 ror r22 |
4294 2864 260F add r18,r22 |
4295 2866 371F adc r19,r23 |
4296 2868 C901 movw r24,r18 |
4297 286a AA27 clr r26 |
4298 286c 97FD sbrc r25,7 |
4299 286e A095 com r26 |
4300 2870 BA2F mov r27,r26 |
4301 2872 8093 0000 sts SetPointYaw.2410,r24 |
4302 2876 9093 0000 sts (SetPointYaw.2410)+1,r25 |
4303 287a A093 0000 sts (SetPointYaw.2410)+2,r26 |
4304 287e B093 0000 sts (SetPointYaw.2410)+3,r27 |
4305 2882 2091 0000 lds r18,ReadingIntegralGyroYaw |
4306 2886 3091 0000 lds r19,(ReadingIntegralGyroYaw)+1 |
4307 288a 4091 0000 lds r20,(ReadingIntegralGyroYaw)+2 |
4308 288e 5091 0000 lds r21,(ReadingIntegralGyroYaw)+3 |
4309 2892 281B sub r18,r24 |
4310 2894 390B sbc r19,r25 |
4311 2896 4A0B sbc r20,r26 |
4312 2898 5B0B sbc r21,r27 |
4313 289a 2093 0000 sts ReadingIntegralGyroYaw,r18 |
4314 289e 3093 0000 sts (ReadingIntegralGyroYaw)+1,r19 |
4315 28a2 4093 0000 sts (ReadingIntegralGyroYaw)+2,r20 |
4316 28a6 5093 0000 sts (ReadingIntegralGyroYaw)+3,r21 |
4317 28aa 203B cpi r18,lo8(-50000) |
4318 28ac 7CE3 ldi r23,hi8(-50000) |
4319 28ae 3707 cpc r19,r23 |
4320 28b0 7FEF ldi r23,hlo8(-50000) |
4321 28b2 4707 cpc r20,r23 |
4322 28b4 7FEF ldi r23,hhi8(-50000) |
4323 28b6 5707 cpc r21,r23 |
4324 28b8 04F0 brlt .+4 |
4325 28ba 0C94 0000 jmp .L573 |
4326 28be 80EB ldi r24,lo8(-50000) |
4327 28c0 9CE3 ldi r25,hi8(-50000) |
4328 28c2 AFEF ldi r26,hlo8(-50000) |
4329 28c4 BFEF ldi r27,hhi8(-50000) |
4330 28c6 8093 0000 sts ReadingIntegralGyroYaw,r24 |
4331 28ca 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 |
4332 28ce A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 |
4333 28d2 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 |
4334 .L574: |
4335 28d6 8091 0000 lds r24,ParamSet+8 |
4336 28da 8872 andi r24,lo8(40) |
4337 28dc 01F4 brne .+2 |
4338 28de 00C0 rjmp .L728 |
4339 28e0 8091 0000 lds r24,CompassCalState |
4340 28e4 8823 tst r24 |
4341 28e6 01F0 breq .+2 |
4342 28e8 00C0 rjmp .L759 |
4343 .L576: |
4344 28ea 68A5 ldd r22,Y+40 |
4345 28ec 79A5 ldd r23,Y+41 |
4346 28ee 8AA5 ldd r24,Y+42 |
4347 28f0 9BA5 ldd r25,Y+43 |
4348 28f2 20E0 ldi r18,lo8(512) |
4349 28f4 32E0 ldi r19,hi8(512) |
4350 28f6 40E0 ldi r20,hlo8(512) |
4351 28f8 50E0 ldi r21,hhi8(512) |
4352 28fa 0E94 0000 call __divmodsi4 |
4353 28fe 8901 movw r16,r18 |
4354 2900 37FF sbrs r19,7 |
4355 2902 00C0 rjmp .+4 |
4356 2904 0C94 0000 jmp .L760 |
4357 .L577: |
4358 2908 6CA1 ldd r22,Y+36 |
4359 290a 7DA1 ldd r23,Y+37 |
4360 290c 8EA1 ldd r24,Y+38 |
4361 290e 9FA1 ldd r25,Y+39 |
4362 2910 20E0 ldi r18,lo8(512) |
4363 2912 32E0 ldi r19,hi8(512) |
4364 2914 40E0 ldi r20,hlo8(512) |
4365 2916 50E0 ldi r21,hhi8(512) |
4366 2918 0E94 0000 call __divmodsi4 |
4367 291c 4901 movw r8,r18 |
4368 291e 37FF sbrs r19,7 |
4369 2920 00C0 rjmp .+4 |
4370 2922 0C94 0000 jmp .L761 |
4371 .L578: |
4372 2926 8016 cp r8,r16 |
4373 2928 9106 cpc r9,r17 |
4374 292a 04F4 brge .L579 |
4375 292c 4801 movw r8,r16 |
4376 .L579: |
4377 292e A090 0000 lds r10,CompassHeading |
4378 2932 B090 0000 lds r11,(CompassHeading)+1 |
4379 2936 B7FE sbrs r11,7 |
4380 2938 00C0 rjmp .+4 |
4381 293a 0C94 0000 jmp .L762 |
4382 293e ACE1 ldi r26,lo8(540) |
4383 2940 B2E0 ldi r27,hi8(540) |
4384 2942 AA0E add r10,r26 |
4385 2944 BB1E adc r11,r27 |
4386 2946 7501 movw r14,r10 |
4387 2948 0027 clr r16 |
4388 294a F7FC sbrc r15,7 |
4389 294c 0095 com r16 |
4390 294e 102F mov r17,r16 |
4391 2950 E4EE ldi r30,lo8(-540) |
4392 2952 FDEF ldi r31,hi8(-540) |
4393 2954 AE0E add r10,r30 |
4394 2956 BF1E adc r11,r31 |
4395 2958 2091 0000 lds r18,ParamSet+23 |
4396 295c 8AE2 ldi r24,lo8(42) |
4397 295e 289F mul r18,r24 |
4398 2960 9001 movw r18,r0 |
4399 2962 1124 clr r1 |
4400 2964 4427 clr r20 |
4401 2966 37FD sbrc r19,7 |
4402 2968 4095 com r20 |
4403 296a 542F mov r21,r20 |
4404 296c 6091 0000 lds r22,YawGyroHeading |
4405 2970 7091 0000 lds r23,(YawGyroHeading)+1 |
4406 2974 8091 0000 lds r24,(YawGyroHeading)+2 |
4407 2978 9091 0000 lds r25,(YawGyroHeading)+3 |
4408 297c 0E94 0000 call __divmodsi4 |
4409 2980 E21A sub r14,r18 |
4410 2982 F30A sbc r15,r19 |
4411 2984 040B sbc r16,r20 |
4412 2986 150B sbc r17,r21 |
4413 2988 C801 movw r24,r16 |
4414 298a B701 movw r22,r14 |
4415 298c 28E6 ldi r18,lo8(360) |
4416 298e 31E0 ldi r19,hi8(360) |
4417 2990 40E0 ldi r20,hlo8(360) |
4418 2992 50E0 ldi r21,hhi8(360) |
4419 2994 0E94 0000 call __divmodsi4 |
4420 2998 645B subi r22,lo8(-(-180)) |
4421 299a 7040 sbci r23,hi8(-(-180)) |
4422 .L581: |
4423 299c 2090 0000 lds r2,GyroYaw |
4424 29a0 3090 0000 lds r3,(GyroYaw)+1 |
4425 29a4 C101 movw r24,r2 |
4426 29a6 37FE sbrs r3,7 |
4427 29a8 00C0 rjmp .+4 |
4428 29aa 0C94 0000 jmp .L763 |
4429 .L583: |
4430 29ae 8138 cpi r24,129 |
4431 29b0 9105 cpc r25,__zero_reg__ |
4432 29b2 04F4 brge .+4 |
4433 29b4 0C94 0000 jmp .L582 |
4434 29b8 60E0 ldi r22,lo8(0) |
4435 29ba 70E0 ldi r23,hi8(0) |
4436 29bc CC24 clr r12 |
4437 29be DD24 clr r13 |
4438 .L584: |
4439 29c0 4090 0000 lds r4,BadCompassHeading |
4440 29c4 5090 0000 lds r5,(BadCompassHeading)+1 |
4441 29c8 4114 cp r4,__zero_reg__ |
4442 29ca 5104 cpc r5,__zero_reg__ |
4443 29cc 01F4 brne .L585 |
4444 29ce F9E1 ldi r31,lo8(25) |
4445 29d0 8F16 cp r8,r31 |
4446 29d2 9104 cpc r9,__zero_reg__ |
4447 29d4 04F4 brge .+4 |
4448 29d6 0C94 0000 jmp .L764 |
4449 .L585: |
4450 29da B401 movw r22,r8 |
4451 29dc 7595 asr r23 |
4452 29de 6795 ror r22 |
4453 29e0 7595 asr r23 |
4454 29e2 6795 ror r22 |
4455 29e4 7595 asr r23 |
4456 29e6 6795 ror r22 |
4457 29e8 6F5F subi r22,lo8(-(1)) |
4458 29ea 7F4F sbci r23,hi8(-(1)) |
4459 29ec C601 movw r24,r12 |
4460 29ee 0E94 0000 call __divmodhi4 |
4461 29f2 5B01 movw r10,r22 |
4462 29f4 CC24 clr r12 |
4463 29f6 B7FC sbrc r11,7 |
4464 29f8 C094 com r12 |
4465 29fa DC2C mov r13,r12 |
4466 29fc 8091 0000 lds r24,YawGyroHeading |
4467 2a00 9091 0000 lds r25,(YawGyroHeading)+1 |
4468 2a04 A091 0000 lds r26,(YawGyroHeading)+2 |
4469 2a08 B091 0000 lds r27,(YawGyroHeading)+3 |
4470 2a0c A80E add r10,r24 |
4471 2a0e B91E adc r11,r25 |
4472 2a10 CA1E adc r12,r26 |
4473 2a12 DB1E adc r13,r27 |
4474 2a14 A092 0000 sts YawGyroHeading,r10 |
4475 2a18 B092 0000 sts (YawGyroHeading)+1,r11 |
4476 2a1c C092 0000 sts (YawGyroHeading)+2,r12 |
4477 2a20 D092 0000 sts (YawGyroHeading)+3,r13 |
4478 2a24 8091 0000 lds r24,FCParam+5 |
4479 2a28 682E mov r6,r24 |
4480 2a2a 7724 clr r7 |
4481 2a2c 869C mul r8,r6 |
4482 2a2e C001 movw r24,r0 |
4483 2a30 879C mul r8,r7 |
4484 2a32 900D add r25,r0 |
4485 2a34 969C mul r9,r6 |
4486 2a36 900D add r25,r0 |
4487 2a38 1124 clr r1 |
4488 2a3a 97FF sbrs r25,7 |
4489 2a3c 00C0 rjmp .+4 |
4490 2a3e 0C94 0000 jmp .L765 |
4491 .L586: |
4492 2a42 9595 asr r25 |
4493 2a44 8795 ror r24 |
4494 2a46 9595 asr r25 |
4495 2a48 8795 ror r24 |
4496 2a4a 9595 asr r25 |
4497 2a4c 8795 ror r24 |
4498 2a4e 9595 asr r25 |
4499 2a50 8795 ror r24 |
4500 2a52 9595 asr r25 |
4501 2a54 8795 ror r24 |
4502 2a56 4301 movw r8,r6 |
4503 2a58 881A sub r8,r24 |
4504 2a5a 990A sbc r9,r25 |
4505 2a5c 97FE sbrs r9,7 |
4506 2a5e 00C0 rjmp .+4 |
4507 2a60 0C94 0000 jmp .L587 |
4508 2a64 4114 cp r4,__zero_reg__ |
4509 2a66 5104 cpc r5,__zero_reg__ |
4510 2a68 01F0 breq .+4 |
4511 2a6a 0C94 0000 jmp .L588 |
4512 2a6e 4090 0000 lds r4,MaxStickRoll |
4513 2a72 5090 0000 lds r5,(MaxStickRoll)+1 |
4514 2a76 8091 0000 lds r24,MaxStickNick |
4515 2a7a 9091 0000 lds r25,(MaxStickNick)+1 |
4516 2a7e 480E add r4,r24 |
4517 2a80 591E adc r5,r25 |
4518 2a82 57FE sbrs r5,7 |
4519 2a84 00C0 rjmp .+4 |
4520 2a86 0C94 0000 jmp .L766 |
4521 .L589: |
4522 2a8a 5594 asr r5 |
4523 2a8c 4794 ror r4 |
4524 2a8e 5594 asr r5 |
4525 2a90 4794 ror r4 |
4526 2a92 5594 asr r5 |
4527 2a94 4794 ror r4 |
4528 2a96 40E4 ldi r20,lo8(64) |
4529 2a98 50E0 ldi r21,hi8(64) |
4530 2a9a 440E add r4,r20 |
4531 2a9c 551E adc r5,r21 |
4532 2a9e 8091 0000 lds r24,CompassCourse |
4533 2aa2 9091 0000 lds r25,(CompassCourse)+1 |
4534 2aa6 AA27 clr r26 |
4535 2aa8 97FD sbrc r25,7 |
4536 2aaa A095 com r26 |
4537 2aac BA2F mov r27,r26 |
4538 2aae 7CE1 ldi r23,lo8(540) |
4539 2ab0 E72E mov r14,r23 |
4540 2ab2 72E0 ldi r23,hi8(540) |
4541 2ab4 F72E mov r15,r23 |
4542 2ab6 012D mov r16,__zero_reg__ |
4543 2ab8 112D mov r17,__zero_reg__ |
4544 2aba E81A sub r14,r24 |
4545 2abc F90A sbc r15,r25 |
4546 2abe 0A0B sbc r16,r26 |
4547 2ac0 1B0B sbc r17,r27 |
4548 2ac2 2091 0000 lds r18,ParamSet+23 |
4549 2ac6 8AE2 ldi r24,lo8(42) |
4550 2ac8 289F mul r18,r24 |
4551 2aca 9001 movw r18,r0 |
4552 2acc 1124 clr r1 |
4553 2ace 4427 clr r20 |
4554 2ad0 37FD sbrc r19,7 |
4555 2ad2 4095 com r20 |
4556 2ad4 542F mov r21,r20 |
4557 2ad6 C601 movw r24,r12 |
4558 2ad8 B501 movw r22,r10 |
4559 2ada 0E94 0000 call __divmodsi4 |
4560 2ade E20E add r14,r18 |
4561 2ae0 F31E adc r15,r19 |
4562 2ae2 041F adc r16,r20 |
4563 2ae4 151F adc r17,r21 |
4564 2ae6 C801 movw r24,r16 |
4565 2ae8 B701 movw r22,r14 |
4566 2aea 28E6 ldi r18,lo8(360) |
4567 2aec 31E0 ldi r19,hi8(360) |
4568 2aee 40E0 ldi r20,hlo8(360) |
4569 2af0 50E0 ldi r21,hhi8(360) |
4570 2af2 0E94 0000 call __divmodsi4 |
4571 2af6 645B subi r22,lo8(-(-180)) |
4572 2af8 7040 sbci r23,hi8(-(-180)) |
4573 2afa 689D mul r22,r8 |
4574 2afc C001 movw r24,r0 |
4575 2afe 699D mul r22,r9 |
4576 2b00 900D add r25,r0 |
4577 2b02 789D mul r23,r8 |
4578 2b04 900D add r25,r0 |
4579 2b06 1124 clr r1 |
4580 2b08 B201 movw r22,r4 |
4581 2b0a 0E94 0000 call __divmodhi4 |
4582 2b0e 9B01 movw r18,r22 |
4583 2b10 C301 movw r24,r6 |
4584 2b12 880F lsl r24 |
4585 2b14 991F rol r25 |
4586 2b16 860D add r24,r6 |
4587 2b18 971D adc r25,r7 |
4588 2b1a 8217 cp r24,r18 |
4589 2b1c 9307 cpc r25,r19 |
4590 2b1e 04F0 brlt .+4 |
4591 2b20 0C94 0000 jmp .L590 |
4592 2b24 BC01 movw r22,r24 |
4593 .L591: |
4594 2b26 9B01 movw r18,r22 |
4595 2b28 4427 clr r20 |
4596 2b2a 37FD sbrc r19,7 |
4597 2b2c 4095 com r20 |
4598 2b2e 542F mov r21,r20 |
4599 2b30 8091 0000 lds r24,ReadingIntegralGyroYaw |
4600 2b34 9091 0000 lds r25,(ReadingIntegralGyroYaw)+1 |
4601 2b38 A091 0000 lds r26,(ReadingIntegralGyroYaw)+2 |
4602 2b3c B091 0000 lds r27,(ReadingIntegralGyroYaw)+3 |
4603 2b40 820F add r24,r18 |
4604 2b42 931F adc r25,r19 |
4605 2b44 A41F adc r26,r20 |
4606 2b46 B51F adc r27,r21 |
4607 2b48 8093 0000 sts ReadingIntegralGyroYaw,r24 |
4608 2b4c 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 |
4609 2b50 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 |
4610 2b54 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 |
4611 2b58 00C0 rjmp .L575 |
4612 .L401: |
4613 2b5a 2998 cbi 37-32,1 |
4614 2b5c 8091 0000 lds r24,RC_Quality |
4615 2b60 8436 cpi r24,lo8(100) |
4616 2b62 00F4 brsh .+2 |
4617 2b64 00C0 rjmp .L767 |
4618 .L403: |
4619 2b66 8091 0000 lds r24,RC_Quality |
4620 2b6a 8D38 cpi r24,lo8(-115) |
4621 2b6c 00F4 brsh .+2 |
4622 2b6e 00C0 rjmp .L410 |
4623 2b70 8091 0000 lds r24,MKFlags |
4624 2b74 8F7E andi r24,lo8(-17) |
4625 2b76 8093 0000 sts MKFlags,r24 |
4626 2b7a 8091 0000 lds r24,ParamSet+32 |
4627 2b7e 22E3 ldi r18,lo8(50) |
4628 2b80 829F mul r24,r18 |
4629 2b82 C001 movw r24,r0 |
4630 2b84 1124 clr r1 |
4631 2b86 9093 0000 sts (RcLostTimer.2415)+1,r25 |
4632 2b8a 8093 0000 sts RcLostTimer.2415,r24 |
4633 2b8e 8091 0000 lds r24,StickGas |
4634 2b92 9091 0000 lds r25,(StickGas)+1 |
4635 2b96 8997 sbiw r24,41 |
4636 2b98 04F0 brlt .L411 |
4637 2b9a 8091 0000 lds r24,MKFlags |
4638 2b9e 80FF sbrs r24,0 |
4639 2ba0 00C0 rjmp .L411 |
4640 2ba2 8091 0000 lds r24,ModelIsFlying |
4641 2ba6 9091 0000 lds r25,(ModelIsFlying)+1 |
4642 2baa BFEF ldi r27,hi8(-1) |
4643 2bac 8F3F cpi r24,lo8(-1) |
4644 2bae 9B07 cpc r25,r27 |
4645 2bb0 01F4 brne .+4 |
4646 2bb2 0C94 0000 jmp .L412 |
4647 2bb6 0196 adiw r24,1 |
4648 2bb8 9093 0000 sts (ModelIsFlying)+1,r25 |
4649 2bbc 8093 0000 sts ModelIsFlying,r24 |
4650 .L411: |
4651 2bc0 8091 0000 lds r24,ModelIsFlying |
4652 2bc4 9091 0000 lds r25,(ModelIsFlying)+1 |
4653 2bc8 8F3F cpi r24,255 |
4654 2bca 9105 cpc r25,__zero_reg__ |
4655 2bcc 01F0 breq .+6 |
4656 2bce 00F0 brlo .+4 |
4657 2bd0 0C94 0000 jmp .L412 |
4658 2bd4 1092 0000 sts IPartNick.2408,__zero_reg__ |
4659 2bd8 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ |
4660 2bdc 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ |
4661 2be0 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ |
4662 2be4 1092 0000 sts IPartRoll.2409,__zero_reg__ |
4663 2be8 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ |
4664 2bec 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ |
4665 2bf0 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ |
4666 2bf4 1092 0000 sts (StickYaw)+1,__zero_reg__ |
4667 2bf8 1092 0000 sts StickYaw,__zero_reg__ |
4668 2bfc 8A3F cpi r24,250 |
4669 2bfe 9105 cpc r25,__zero_reg__ |
4670 2c00 01F4 brne .+4 |
4671 2c02 0C94 0000 jmp .L768 |
4672 .L413: |
4673 2c06 8091 0000 lds r24,ParamSet+4 |
4674 2c0a E82F mov r30,r24 |
4675 2c0c F0E0 ldi r31,lo8(0) |
4676 2c0e EE0F lsl r30 |
4677 2c10 FF1F rol r31 |
4678 2c12 E050 subi r30,lo8(-(PPM_in)) |
4679 2c14 F040 sbci r31,hi8(-(PPM_in)) |
4680 2c16 8081 ld r24,Z |
4681 2c18 9181 ldd r25,Z+1 |
4682 2c1a E090 0000 lds r14,Poti1 |
4683 2c1e F090 0000 lds r15,(Poti1)+1 |
4684 2c22 8359 subi r24,lo8(-(109)) |
4685 2c24 9F4F sbci r25,hi8(-(109)) |
4686 2c26 8E15 cp r24,r14 |
4687 2c28 9F05 cpc r25,r15 |
4688 2c2a 04F4 brge .+4 |
4689 2c2c 0C94 0000 jmp .L414 |
4690 2c30 C701 movw r24,r14 |
4691 2c32 0196 adiw r24,1 |
4692 2c34 9093 0000 sts (Poti1)+1,r25 |
4693 2c38 8093 0000 sts Poti1,r24 |
4694 2c3c 7C01 movw r14,r24 |
4695 .L415: |
4696 2c3e 8091 0000 lds r24,ParamSet+5 |
4697 2c42 E82F mov r30,r24 |
4698 2c44 F0E0 ldi r31,lo8(0) |
4699 2c46 EE0F lsl r30 |
4700 2c48 FF1F rol r31 |
4701 2c4a E050 subi r30,lo8(-(PPM_in)) |
4702 2c4c F040 sbci r31,hi8(-(PPM_in)) |
4703 2c4e 8081 ld r24,Z |
4704 2c50 9181 ldd r25,Z+1 |
4705 2c52 0091 0000 lds r16,Poti2 |
4706 2c56 1091 0000 lds r17,(Poti2)+1 |
4707 2c5a 8359 subi r24,lo8(-(109)) |
4708 2c5c 9F4F sbci r25,hi8(-(109)) |
4709 2c5e 8017 cp r24,r16 |
4710 2c60 9107 cpc r25,r17 |
4711 2c62 04F4 brge .+4 |
4712 2c64 0C94 0000 jmp .L416 |
4713 2c68 C801 movw r24,r16 |
4714 2c6a 0196 adiw r24,1 |
4715 2c6c 9093 0000 sts (Poti2)+1,r25 |
4716 2c70 8093 0000 sts Poti2,r24 |
4717 2c74 8C01 movw r16,r24 |
4718 .L417: |
4719 2c76 8091 0000 lds r24,ParamSet+6 |
4720 2c7a E82F mov r30,r24 |
4721 2c7c F0E0 ldi r31,lo8(0) |
4722 2c7e EE0F lsl r30 |
4723 2c80 FF1F rol r31 |
4724 2c82 E050 subi r30,lo8(-(PPM_in)) |
4725 2c84 F040 sbci r31,hi8(-(PPM_in)) |
4726 2c86 8081 ld r24,Z |
4727 2c88 9181 ldd r25,Z+1 |
4728 2c8a A090 0000 lds r10,Poti3 |
4729 2c8e B090 0000 lds r11,(Poti3)+1 |
4730 2c92 8359 subi r24,lo8(-(109)) |
4731 2c94 9F4F sbci r25,hi8(-(109)) |
4732 2c96 8A15 cp r24,r10 |
4733 2c98 9B05 cpc r25,r11 |
4734 2c9a 04F4 brge .+4 |
4735 2c9c 0C94 0000 jmp .L418 |
4736 2ca0 C501 movw r24,r10 |
4737 2ca2 0196 adiw r24,1 |
4738 2ca4 9093 0000 sts (Poti3)+1,r25 |
4739 2ca8 8093 0000 sts Poti3,r24 |
4740 2cac 5C01 movw r10,r24 |
4741 .L419: |
4742 2cae 8091 0000 lds r24,ParamSet+7 |
4743 2cb2 E82F mov r30,r24 |
4744 2cb4 F0E0 ldi r31,lo8(0) |
4745 2cb6 EE0F lsl r30 |
4746 2cb8 FF1F rol r31 |
4747 2cba E050 subi r30,lo8(-(PPM_in)) |
4748 2cbc F040 sbci r31,hi8(-(PPM_in)) |
4749 2cbe 8081 ld r24,Z |
4750 2cc0 9181 ldd r25,Z+1 |
4751 2cc2 A091 0000 lds r26,Poti4 |
4752 2cc6 B091 0000 lds r27,(Poti4)+1 |
4753 2cca 8359 subi r24,lo8(-(109)) |
4754 2ccc 9F4F sbci r25,hi8(-(109)) |
4755 2cce 8A17 cp r24,r26 |
4756 2cd0 9B07 cpc r25,r27 |
4757 2cd2 04F4 brge .+4 |
4758 2cd4 0C94 0000 jmp .L420 |
4759 2cd8 CD01 movw r24,r26 |
4760 2cda 0196 adiw r24,1 |
4761 2cdc 9093 0000 sts (Poti4)+1,r25 |
4762 2ce0 8093 0000 sts Poti4,r24 |
4763 2ce4 DC01 movw r26,r24 |
4764 .L421: |
4765 2ce6 8091 0000 lds r24,PPM_in+18 |
4766 2cea 9091 0000 lds r25,(PPM_in+18)+1 |
4767 2cee E091 0000 lds r30,Poti5 |
4768 2cf2 F091 0000 lds r31,(Poti5)+1 |
4769 2cf6 8359 subi r24,lo8(-(109)) |
4770 2cf8 9F4F sbci r25,hi8(-(109)) |
4771 2cfa 8E17 cp r24,r30 |
4772 2cfc 9F07 cpc r25,r31 |
4773 2cfe 04F4 brge .+4 |
4774 2d00 0C94 0000 jmp .L422 |
4775 2d04 CF01 movw r24,r30 |
4776 2d06 0196 adiw r24,1 |
4777 2d08 9093 0000 sts (Poti5)+1,r25 |
4778 2d0c 8093 0000 sts Poti5,r24 |
4779 2d10 FC01 movw r30,r24 |
4780 .L423: |
4781 2d12 8091 0000 lds r24,PPM_in+20 |
4782 2d16 9091 0000 lds r25,(PPM_in+20)+1 |
4783 2d1a 6091 0000 lds r22,Poti6 |
4784 2d1e 7091 0000 lds r23,(Poti6)+1 |
4785 2d22 8359 subi r24,lo8(-(109)) |
4786 2d24 9F4F sbci r25,hi8(-(109)) |
4787 2d26 8617 cp r24,r22 |
4788 2d28 9707 cpc r25,r23 |
4789 2d2a 04F4 brge .+4 |
4790 2d2c 0C94 0000 jmp .L424 |
4791 2d30 CB01 movw r24,r22 |
4792 2d32 0196 adiw r24,1 |
4793 2d34 9093 0000 sts (Poti6)+1,r25 |
4794 2d38 8093 0000 sts Poti6,r24 |
4795 2d3c BC01 movw r22,r24 |
4796 .L425: |
4797 2d3e 8091 0000 lds r24,PPM_in+22 |
4798 2d42 9091 0000 lds r25,(PPM_in+22)+1 |
4799 2d46 4091 0000 lds r20,Poti7 |
4800 2d4a 5091 0000 lds r21,(Poti7)+1 |
4801 2d4e 8359 subi r24,lo8(-(109)) |
4802 2d50 9F4F sbci r25,hi8(-(109)) |
4803 2d52 8417 cp r24,r20 |
4804 2d54 9507 cpc r25,r21 |
4805 2d56 04F4 brge .+4 |
4806 2d58 0C94 0000 jmp .L426 |
4807 2d5c CA01 movw r24,r20 |
4808 2d5e 0196 adiw r24,1 |
4809 2d60 9093 0000 sts (Poti7)+1,r25 |
4810 2d64 8093 0000 sts Poti7,r24 |
4811 2d68 AC01 movw r20,r24 |
4812 .L427: |
4813 2d6a 8091 0000 lds r24,PPM_in+24 |
4814 2d6e 9091 0000 lds r25,(PPM_in+24)+1 |
4815 2d72 2091 0000 lds r18,Poti8 |
4816 2d76 3091 0000 lds r19,(Poti8)+1 |
4817 2d7a 8359 subi r24,lo8(-(109)) |
4818 2d7c 9F4F sbci r25,hi8(-(109)) |
4819 2d7e 8217 cp r24,r18 |
4820 2d80 9307 cpc r25,r19 |
4821 2d82 04F4 brge .+4 |
4822 2d84 0C94 0000 jmp .L428 |
4823 2d88 C901 movw r24,r18 |
4824 2d8a 0196 adiw r24,1 |
4825 2d8c 9093 0000 sts (Poti8)+1,r25 |
4826 2d90 8093 0000 sts Poti8,r24 |
4827 2d94 9C01 movw r18,r24 |
4828 .L429: |
4829 2d96 F7FE sbrs r15,7 |
4830 2d98 00C0 rjmp .+4 |
4831 2d9a 0C94 0000 jmp .L769 |
4832 2d9e 8FEF ldi r24,lo8(255) |
4833 2da0 E816 cp r14,r24 |
4834 2da2 F104 cpc r15,__zero_reg__ |
4835 2da4 01F0 breq .L431 |
4836 2da6 04F0 brlt .L431 |
4837 2da8 8FEF ldi r24,lo8(255) |
4838 2daa 90E0 ldi r25,hi8(255) |
4839 2dac 9093 0000 sts (Poti1)+1,r25 |
4840 2db0 8093 0000 sts Poti1,r24 |
4841 .L431: |
4842 2db4 17FF sbrs r17,7 |
4843 2db6 00C0 rjmp .+4 |
4844 2db8 0C94 0000 jmp .L770 |
4845 2dbc 0F3F cpi r16,255 |
4846 2dbe 1105 cpc r17,__zero_reg__ |
4847 2dc0 01F0 breq .L433 |
4848 2dc2 04F0 brlt .L433 |
4849 2dc4 8FEF ldi r24,lo8(255) |
4850 2dc6 90E0 ldi r25,hi8(255) |
4851 2dc8 9093 0000 sts (Poti2)+1,r25 |
4852 2dcc 8093 0000 sts Poti2,r24 |
4853 .L433: |
4854 2dd0 B7FE sbrs r11,7 |
4855 2dd2 00C0 rjmp .+4 |
4856 2dd4 0C94 0000 jmp .L771 |
4857 2dd8 9FEF ldi r25,lo8(255) |
4858 2dda A916 cp r10,r25 |
4859 2ddc B104 cpc r11,__zero_reg__ |
4860 2dde 01F0 breq .L435 |
4861 2de0 04F0 brlt .L435 |
4862 2de2 8FEF ldi r24,lo8(255) |
4863 2de4 90E0 ldi r25,hi8(255) |
4864 2de6 9093 0000 sts (Poti3)+1,r25 |
4865 2dea 8093 0000 sts Poti3,r24 |
4866 .L435: |
4867 2dee B7FF sbrs r27,7 |
4868 2df0 00C0 rjmp .+4 |
4869 2df2 0C94 0000 jmp .L772 |
4870 2df6 AF3F cpi r26,255 |
4871 2df8 B105 cpc r27,__zero_reg__ |
4872 2dfa 01F0 breq .L437 |
4873 2dfc 04F0 brlt .L437 |
4874 2dfe 8FEF ldi r24,lo8(255) |
4875 2e00 90E0 ldi r25,hi8(255) |
4876 2e02 9093 0000 sts (Poti4)+1,r25 |
4877 2e06 8093 0000 sts Poti4,r24 |
4878 .L437: |
4879 2e0a F7FF sbrs r31,7 |
4880 2e0c 00C0 rjmp .+4 |
4881 2e0e 0C94 0000 jmp .L773 |
4882 2e12 EF3F cpi r30,255 |
4883 2e14 F105 cpc r31,__zero_reg__ |
4884 2e16 01F0 breq .L439 |
4885 2e18 04F0 brlt .L439 |
4886 2e1a 8FEF ldi r24,lo8(255) |
4887 2e1c 90E0 ldi r25,hi8(255) |
4888 2e1e 9093 0000 sts (Poti5)+1,r25 |
4889 2e22 8093 0000 sts Poti5,r24 |
4890 .L439: |
4891 2e26 77FF sbrs r23,7 |
4892 2e28 00C0 rjmp .+4 |
4893 2e2a 0C94 0000 jmp .L774 |
4894 2e2e 6F3F cpi r22,255 |
4895 2e30 7105 cpc r23,__zero_reg__ |
4896 2e32 01F0 breq .L441 |
4897 2e34 04F0 brlt .L441 |
4898 2e36 8FEF ldi r24,lo8(255) |
4899 2e38 90E0 ldi r25,hi8(255) |
4900 2e3a 9093 0000 sts (Poti6)+1,r25 |
4901 2e3e 8093 0000 sts Poti6,r24 |
4902 .L441: |
4903 2e42 57FF sbrs r21,7 |
4904 2e44 00C0 rjmp .+4 |
4905 2e46 0C94 0000 jmp .L775 |
4906 2e4a 4F3F cpi r20,255 |
4907 2e4c 5105 cpc r21,__zero_reg__ |
4908 2e4e 01F0 breq .L443 |
4909 2e50 04F0 brlt .L443 |
4910 2e52 8FEF ldi r24,lo8(255) |
4911 2e54 90E0 ldi r25,hi8(255) |
4912 2e56 9093 0000 sts (Poti7)+1,r25 |
4913 2e5a 8093 0000 sts Poti7,r24 |
4914 .L443: |
4915 2e5e 37FF sbrs r19,7 |
4916 2e60 00C0 rjmp .+4 |
4917 2e62 0C94 0000 jmp .L776 |
4918 2e66 2F3F cpi r18,255 |
4919 2e68 3105 cpc r19,__zero_reg__ |
4920 2e6a 01F0 breq .L445 |
4921 2e6c 04F0 brlt .L445 |
4922 2e6e 8FEF ldi r24,lo8(255) |
4923 2e70 90E0 ldi r25,hi8(255) |
4924 2e72 9093 0000 sts (Poti8)+1,r25 |
4925 2e76 8093 0000 sts Poti8,r24 |
4926 .L445: |
4927 2e7a 8091 0000 lds r24,ParamSet+2 |
4928 2e7e A82F mov r26,r24 |
4929 2e80 B0E0 ldi r27,lo8(0) |
4930 2e82 AA0F lsl r26 |
4931 2e84 BB1F rol r27 |
4932 2e86 FD01 movw r30,r26 |
4933 2e88 E050 subi r30,lo8(-(PPM_in)) |
4934 2e8a F040 sbci r31,hi8(-(PPM_in)) |
4935 2e8c 8081 ld r24,Z |
4936 2e8e 9181 ldd r25,Z+1 |
4937 2e90 8135 cpi r24,81 |
4938 2e92 9105 cpc r25,__zero_reg__ |
4939 2e94 04F4 brge .+4 |
4940 2e96 0C94 0000 jmp .L446 |
4941 2e9a 8091 0000 lds r24,MKFlags |
4942 2e9e 80FF sbrs r24,0 |
4943 2ea0 00C0 rjmp .+4 |
4944 2ea2 0C94 0000 jmp .L446 |
4945 2ea6 8091 0000 lds r24,ParamSet+3 |
4946 2eaa E82F mov r30,r24 |
4947 2eac F0E0 ldi r31,lo8(0) |
4948 2eae EE0F lsl r30 |
4949 2eb0 FF1F rol r31 |
4950 2eb2 E050 subi r30,lo8(-(PPM_in)) |
4951 2eb4 F040 sbci r31,hi8(-(PPM_in)) |
4952 2eb6 8081 ld r24,Z |
4953 2eb8 9181 ldd r25,Z+1 |
4954 2eba 8C34 cpi r24,76 |
4955 2ebc 9105 cpc r25,__zero_reg__ |
4956 2ebe 04F4 brge .+4 |
4957 2ec0 0C94 0000 jmp .L447 |
4958 2ec4 8091 0000 lds r24,delay_neutral.2416 |
4959 2ec8 8F5F subi r24,lo8(-(1)) |
4960 2eca 8093 0000 sts delay_neutral.2416,r24 |
4961 2ece 893C cpi r24,lo8(-55) |
4962 2ed0 00F4 brsh .+4 |
4963 2ed2 0C94 0000 jmp .L446 |
4964 2ed6 1092 0000 sts delay_neutral.2416,__zero_reg__ |
4965 2eda ABE0 ldi r26,lo8(11) |
4966 2edc AD15 cp r26,r13 |
4967 2ede 00F4 brsh .+4 |
4968 2ee0 0C94 0000 jmp .L449 |
4969 2ee4 2998 cbi 37-32,1 |
4970 .L450: |
4971 2ee6 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ |
4972 2eea 1092 0000 sts ModelIsFlying,__zero_reg__ |
4973 2eee 8091 0000 lds r24,ParamSet |
4974 2ef2 482F mov r20,r24 |
4975 2ef4 50E0 ldi r21,lo8(0) |
4976 2ef6 440F lsl r20 |
4977 2ef8 551F rol r21 |
4978 2efa FA01 movw r30,r20 |
4979 2efc E050 subi r30,lo8(-(PPM_in)) |
4980 2efe F040 sbci r31,hi8(-(PPM_in)) |
4981 2f00 8081 ld r24,Z |
4982 2f02 9181 ldd r25,Z+1 |
4983 2f04 8734 cpi r24,71 |
4984 2f06 9105 cpc r25,__zero_reg__ |
4985 2f08 04F4 brge .+4 |
4986 2f0a 0C94 0000 jmp .L451 |
4987 2f0e 8091 0000 lds r24,ParamSet+1 |
4988 2f12 A82F mov r26,r24 |
4989 2f14 B0E0 ldi r27,lo8(0) |
4990 2f16 AA0F lsl r26 |
4991 2f18 BB1F rol r27 |
4992 .L452: |
4993 2f1a FD01 movw r30,r26 |
4994 2f1c E050 subi r30,lo8(-(PPM_in)) |
4995 2f1e F040 sbci r31,hi8(-(PPM_in)) |
4996 2f20 8081 ld r24,Z |
4997 2f22 9181 ldd r25,Z+1 |
4998 2f24 8734 cpi r24,71 |
4999 2f26 9105 cpc r25,__zero_reg__ |
5000 2f28 04F0 brlt .L455 |
5001 2f2a FA01 movw r30,r20 |
5002 2f2c E050 subi r30,lo8(-(PPM_in)) |
5003 2f2e F040 sbci r31,hi8(-(PPM_in)) |
5004 2f30 0190 ld __tmp_reg__,Z+ |
5005 2f32 F081 ld r31,Z |
5006 2f34 E02D mov r30,__tmp_reg__ |
5007 .L455: |
5008 2f36 FD01 movw r30,r26 |
5009 2f38 E050 subi r30,lo8(-(PPM_in)) |
5010 2f3a F040 sbci r31,hi8(-(PPM_in)) |
5011 2f3c 8081 ld r24,Z |
5012 2f3e 9181 ldd r25,Z+1 |
5013 2f40 8734 cpi r24,71 |
5014 2f42 9105 cpc r25,__zero_reg__ |
5015 2f44 04F4 brge .+4 |
5016 2f46 0C94 0000 jmp .L456 |
5017 2f4a FA01 movw r30,r20 |
5018 2f4c E050 subi r30,lo8(-(PPM_in)) |
5019 2f4e F040 sbci r31,hi8(-(PPM_in)) |
5020 2f50 8081 ld r24,Z |
5021 2f52 9181 ldd r25,Z+1 |
5022 2f54 8734 cpi r24,71 |
5023 2f56 9105 cpc r25,__zero_reg__ |
5024 2f58 04F4 brge .+4 |
5025 2f5a 0C94 0000 jmp .L456 |
5026 2f5e 22E0 ldi r18,lo8(2) |
5027 .L457: |
5028 2f60 FD01 movw r30,r26 |
5029 2f62 E050 subi r30,lo8(-(PPM_in)) |
5030 2f64 F040 sbci r31,hi8(-(PPM_in)) |
5031 2f66 8081 ld r24,Z |
5032 2f68 9181 ldd r25,Z+1 |
5033 2f6a 8634 cpi r24,70 |
5034 2f6c 9105 cpc r25,__zero_reg__ |
5035 2f6e 04F4 brge .L458 |
5036 2f70 FA01 movw r30,r20 |
5037 2f72 E050 subi r30,lo8(-(PPM_in)) |
5038 2f74 F040 sbci r31,hi8(-(PPM_in)) |
5039 2f76 8081 ld r24,Z |
5040 2f78 9181 ldd r25,Z+1 |
5041 2f7a 8734 cpi r24,71 |
5042 2f7c 9105 cpc r25,__zero_reg__ |
5043 2f7e 04F0 brlt .L458 |
5044 2f80 23E0 ldi r18,lo8(3) |
5045 .L458: |
5046 2f82 FD01 movw r30,r26 |
5047 2f84 E050 subi r30,lo8(-(PPM_in)) |
5048 2f86 F040 sbci r31,hi8(-(PPM_in)) |
5049 2f88 8081 ld r24,Z |
5050 2f8a 9181 ldd r25,Z+1 |
5051 2f8c 8A5B subi r24,lo8(-70) |
5052 2f8e 9F4F sbci r25,hi8(-70) |
5053 2f90 04F4 brge .L459 |
5054 2f92 FA01 movw r30,r20 |
5055 2f94 E050 subi r30,lo8(-(PPM_in)) |
5056 2f96 F040 sbci r31,hi8(-(PPM_in)) |
5057 2f98 8081 ld r24,Z |
5058 2f9a 9181 ldd r25,Z+1 |
5059 2f9c 8734 cpi r24,71 |
5060 2f9e 9105 cpc r25,__zero_reg__ |
5061 2fa0 04F0 brlt .L459 |
5062 2fa2 24E0 ldi r18,lo8(4) |
5063 .L459: |
5064 2fa4 A050 subi r26,lo8(-(PPM_in)) |
5065 2fa6 B040 sbci r27,hi8(-(PPM_in)) |
5066 2fa8 8D91 ld r24,X+ |
5067 2faa 9C91 ld r25,X |
5068 2fac 8A5B subi r24,lo8(-70) |
5069 2fae 9F4F sbci r25,hi8(-70) |
5070 2fb0 04F4 brge .L460 |
5071 2fb2 4050 subi r20,lo8(-(PPM_in)) |
5072 2fb4 5040 sbci r21,hi8(-(PPM_in)) |
5073 2fb6 DA01 movw r26,r20 |
5074 2fb8 8D91 ld r24,X+ |
5075 2fba 9C91 ld r25,X |
5076 2fbc 8634 cpi r24,70 |
5077 2fbe 9105 cpc r25,__zero_reg__ |
5078 2fc0 04F4 brge .L460 |
5079 2fc2 25E0 ldi r18,lo8(5) |
5080 .L460: |
5081 2fc4 822F mov r24,r18 |
5082 2fc6 0E94 0000 call SetActiveParamSet |
5083 2fca 0E94 0000 call GetActiveParamSet |
5084 2fce 0E94 0000 call ParamSet_ReadFromEEProm |
5085 2fd2 0E94 0000 call Servo_Off |
5086 2fd6 80E0 ldi r24,lo8(0) |
5087 2fd8 0C94 0000 jmp .L726 |
5088 .L504: |
5089 2fdc 8091 0000 lds r24,ParamSet+48 |
5090 2fe0 282F mov r18,r24 |
5091 2fe2 30E0 ldi r19,lo8(0) |
5092 2fe4 8091 0000 lds r24,StickGas |
5093 2fe8 9091 0000 lds r25,(StickGas)+1 |
5094 2fec 2817 cp r18,r24 |
5095 2fee 3907 cpc r19,r25 |
5096 2ff0 04F4 brge .L506 |
5097 2ff2 3093 0000 sts (StickGas)+1,r19 |
5098 2ff6 2093 0000 sts StickGas,r18 |
5099 .L506: |
5100 2ffa 81E0 ldi r24,lo8(1) |
5101 2ffc 8093 0000 sts FunnelCourse,r24 |
5102 3000 00C0 rjmp .L505 |
5103 .L759: |
5104 3002 8091 0000 lds r24,MKFlags |
5105 3006 80FD sbrc r24,0 |
5106 3008 00C0 rjmp .L576 |
5107 300a 0E94 0000 call SetCompassCalState |
5108 300e 9091 0000 lds r25,LoopingNick |
5109 3012 9D8F std Y+29,r25 |
5110 .L728: |
5111 3014 2090 0000 lds r2,GyroYaw |
5112 3018 3090 0000 lds r3,(GyroYaw)+1 |
5113 .L575: |
5114 301c 8091 0000 lds r24,TimerDebugOut.2419 |
5115 3020 8150 subi r24,lo8(-(-1)) |
5116 3022 8093 0000 sts TimerDebugOut.2419,r24 |
5117 3026 8F5F subi r24,lo8(-(1)) |
5118 3028 01F0 breq .+2 |
5119 302a 00C0 rjmp .L593 |
5120 302c 88E1 ldi r24,lo8(24) |
5121 302e 8093 0000 sts TimerDebugOut.2419,r24 |
5122 3032 6091 0000 lds r22,IntegralGyroNick |
5123 3036 7091 0000 lds r23,(IntegralGyroNick)+1 |
5124 303a 8091 0000 lds r24,(IntegralGyroNick)+2 |
5125 303e 9091 0000 lds r25,(IntegralGyroNick)+3 |
5126 3042 2AE0 ldi r18,lo8(10) |
5127 3044 30E0 ldi r19,hi8(10) |
5128 3046 40E0 ldi r20,hlo8(10) |
5129 3048 50E0 ldi r21,hhi8(10) |
5130 304a 0E94 0000 call __mulsi3 |
5131 304e E090 0000 lds r14,ParamSet+23 |
5132 3052 2AE2 ldi r18,lo8(42) |
5133 3054 E29E mul r14,r18 |
5134 3056 7001 movw r14,r0 |
5135 3058 1124 clr r1 |
5136 305a 0027 clr r16 |
5137 305c F7FC sbrc r15,7 |
5138 305e 0095 com r16 |
5139 3060 102F mov r17,r16 |
5140 3062 A801 movw r20,r16 |
5141 3064 9701 movw r18,r14 |
5142 3066 0E94 0000 call __divmodsi4 |
5143 306a 3093 0000 sts (DebugOut+2)+1,r19 |
5144 306e 2093 0000 sts DebugOut+2,r18 |
5145 3072 6091 0000 lds r22,IntegralGyroRoll |
5146 3076 7091 0000 lds r23,(IntegralGyroRoll)+1 |
5147 307a 8091 0000 lds r24,(IntegralGyroRoll)+2 |
5148 307e 9091 0000 lds r25,(IntegralGyroRoll)+3 |
5149 3082 2AE0 ldi r18,lo8(10) |
5150 3084 30E0 ldi r19,hi8(10) |
5151 3086 40E0 ldi r20,hlo8(10) |
5152 3088 50E0 ldi r21,hhi8(10) |
5153 308a 0E94 0000 call __mulsi3 |
5154 308e A801 movw r20,r16 |
5155 3090 9701 movw r18,r14 |
5156 3092 0E94 0000 call __divmodsi4 |
5157 3096 3093 0000 sts (DebugOut+4)+1,r19 |
5158 309a 2093 0000 sts DebugOut+4,r18 |
5159 309e 8091 0000 lds r24,AccNick |
5160 30a2 9091 0000 lds r25,AccNick+1 |
5161 30a6 9C01 movw r18,r24 |
5162 30a8 220F lsl r18 |
5163 30aa 331F rol r19 |
5164 30ac 220F lsl r18 |
5165 30ae 331F rol r19 |
5166 30b0 220F lsl r18 |
5167 30b2 331F rol r19 |
5168 30b4 880F lsl r24 |
5169 30b6 991F rol r25 |
5170 30b8 820F add r24,r18 |
5171 30ba 931F adc r25,r19 |
5172 30bc 6AE2 ldi r22,lo8(42) |
5173 30be 70E0 ldi r23,hi8(42) |
5174 30c0 0E94 0000 call __divmodhi4 |
5175 30c4 7093 0000 sts (DebugOut+6)+1,r23 |
5176 30c8 6093 0000 sts DebugOut+6,r22 |
5177 30cc 8091 0000 lds r24,AccRoll |
5178 30d0 9091 0000 lds r25,AccRoll+1 |
5179 30d4 9C01 movw r18,r24 |
5180 30d6 220F lsl r18 |
5181 30d8 331F rol r19 |
5182 30da 220F lsl r18 |
5183 30dc 331F rol r19 |
5184 30de 220F lsl r18 |
5185 30e0 331F rol r19 |
5186 30e2 880F lsl r24 |
5187 30e4 991F rol r25 |
5188 30e6 820F add r24,r18 |
5189 30e8 931F adc r25,r19 |
5190 30ea 6AE2 ldi r22,lo8(42) |
5191 30ec 70E0 ldi r23,hi8(42) |
5192 30ee 0E94 0000 call __divmodhi4 |
5193 30f2 7093 0000 sts (DebugOut+8)+1,r23 |
5194 30f6 6093 0000 sts DebugOut+8,r22 |
5195 30fa 3092 0000 sts (DebugOut+10)+1,r3 |
5196 30fe 2092 0000 sts DebugOut+10,r2 |
5197 3102 6091 0000 lds r22,ReadingHeight |
5198 3106 7091 0000 lds r23,(ReadingHeight)+1 |
5199 310a 8091 0000 lds r24,(ReadingHeight)+2 |
5200 310e 9091 0000 lds r25,(ReadingHeight)+3 |
5201 3112 25E0 ldi r18,lo8(5) |
5202 3114 30E0 ldi r19,hi8(5) |
5203 3116 40E0 ldi r20,hlo8(5) |
5204 3118 50E0 ldi r21,hhi8(5) |
5205 311a 0E94 0000 call __divmodsi4 |
5206 311e 3093 0000 sts (DebugOut+12)+1,r19 |
5207 3122 2093 0000 sts DebugOut+12,r18 |
5208 3126 6091 0000 lds r22,ReadingIntegralTop |
5209 312a 7091 0000 lds r23,(ReadingIntegralTop)+1 |
5210 312e 8091 0000 lds r24,(ReadingIntegralTop)+2 |
5211 3132 9091 0000 lds r25,(ReadingIntegralTop)+3 |
5212 3136 20E0 ldi r18,lo8(512) |
5213 3138 32E0 ldi r19,hi8(512) |
5214 313a 40E0 ldi r20,hlo8(512) |
5215 313c 50E0 ldi r21,hhi8(512) |
5216 313e 0E94 0000 call __divmodsi4 |
5217 3142 3093 0000 sts (DebugOut+14)+1,r19 |
5218 3146 2093 0000 sts DebugOut+14,r18 |
5219 314a 8091 0000 lds r24,CompassHeading |
5220 314e 9091 0000 lds r25,(CompassHeading)+1 |
5221 3152 9093 0000 sts (DebugOut+18)+1,r25 |
5222 3156 8093 0000 sts DebugOut+18,r24 |
5223 315a 8091 0000 lds r24,UBat |
5224 315e 9091 0000 lds r25,(UBat)+1 |
5225 3162 9093 0000 sts (DebugOut+20)+1,r25 |
5226 3166 8093 0000 sts DebugOut+20,r24 |
5227 316a 8091 0000 lds r24,RC_Quality |
5228 316e 8093 0000 sts DebugOut+22,r24 |
5229 3172 1092 0000 sts DebugOut+23,__zero_reg__ |
5230 3176 6091 0000 lds r22,YawGyroHeading |
5231 317a 7091 0000 lds r23,(YawGyroHeading)+1 |
5232 317e 8091 0000 lds r24,(YawGyroHeading)+2 |
5233 3182 9091 0000 lds r25,(YawGyroHeading)+3 |
5234 3186 A801 movw r20,r16 |
5235 3188 9701 movw r18,r14 |
5236 318a 0E94 0000 call __divmodsi4 |
5237 318e 3093 0000 sts (DebugOut+24)+1,r19 |
5238 3192 2093 0000 sts DebugOut+24,r18 |
5239 3196 8091 0000 lds r24,CompassCalState |
5240 319a 8093 0000 sts DebugOut+40,r24 |
5241 319e 1092 0000 sts DebugOut+41,__zero_reg__ |
5242 31a2 8091 0000 lds r24,ServoNickValue |
5243 31a6 9091 0000 lds r25,(ServoNickValue)+1 |
5244 31aa 9093 0000 sts (DebugOut+42)+1,r25 |
5245 31ae 8093 0000 sts DebugOut+42,r24 |
5246 31b2 8091 0000 lds r24,GPSStickNick |
5247 31b6 9091 0000 lds r25,(GPSStickNick)+1 |
5248 31ba 9093 0000 sts (DebugOut+62)+1,r25 |
5249 31be 8093 0000 sts DebugOut+62,r24 |
5250 31c2 8091 0000 lds r24,GPSStickRoll |
5251 31c6 9091 0000 lds r25,(GPSStickRoll)+1 |
5252 31ca 9093 0000 sts (DebugOut+64)+1,r25 |
5253 31ce 8093 0000 sts DebugOut+64,r24 |
5254 .L593: |
5255 31d2 8090 0000 lds r8,TrimNick |
5256 31d6 9090 0000 lds r9,(TrimNick)+1 |
5257 31da 58E3 ldi r21,lo8(-200) |
5258 31dc 8516 cp r8,r21 |
5259 31de 5FEF ldi r21,hi8(-200) |
5260 31e0 9506 cpc r9,r21 |
5261 31e2 04F0 brlt .+2 |
5262 31e4 00C0 rjmp .L594 |
5263 31e6 88E3 ldi r24,lo8(-200) |
5264 31e8 9FEF ldi r25,hi8(-200) |
5265 31ea 9093 0000 sts (TrimNick)+1,r25 |
5266 31ee 8093 0000 sts TrimNick,r24 |
5267 31f2 F8E3 ldi r31,lo8(-200) |
5268 31f4 8F2E mov r8,r31 |
5269 31f6 FFEF ldi r31,hi8(-200) |
5270 31f8 9F2E mov r9,r31 |
5271 .L595: |
5272 31fa 8091 0000 lds r24,TrimRoll |
5273 31fe 9091 0000 lds r25,(TrimRoll)+1 |
5274 3202 9F8F std Y+31,r25 |
5275 3204 8E8F std Y+30,r24 |
5276 3206 8853 subi r24,lo8(-200) |
5277 3208 9F4F sbci r25,hi8(-200) |
5278 320a 04F0 brlt .+2 |
5279 320c 00C0 rjmp .L596 |
5280 320e 88E3 ldi r24,lo8(-200) |
5281 3210 9FEF ldi r25,hi8(-200) |
5282 3212 9093 0000 sts (TrimRoll)+1,r25 |
5283 3216 8093 0000 sts TrimRoll,r24 |
5284 321a 9F8F std Y+31,r25 |
5285 321c 8E8F std Y+30,r24 |
5286 .L597: |
5287 321e 8091 0000 lds r24,FunnelCourse |
5288 3222 8823 tst r24 |
5289 3224 01F0 breq .L598 |
5290 3226 1092 0000 sts IPartNick.2408,__zero_reg__ |
5291 322a 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ |
5292 322e 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ |
5293 3232 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ |
5294 3236 1092 0000 sts IPartRoll.2409,__zero_reg__ |
5295 323a 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ |
5296 323e 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ |
5297 3242 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ |
5298 .L598: |
5299 3246 FD8D ldd r31,Y+29 |
5300 3248 FF23 tst r31 |
5301 324a 01F4 brne .+2 |
5302 324c 00C0 rjmp .L599 |
5303 324e 4424 clr r4 |
5304 3250 5524 clr r5 |
5305 3252 1C82 std Y+4,__zero_reg__ |
5306 3254 1B82 std Y+3,__zero_reg__ |
5307 .L600: |
5308 3256 A090 0000 lds r10,GyroNick |
5309 325a B090 0000 lds r11,(GyroNick)+1 |
5310 325e BC8A std Y+20,r11 |
5311 3260 AB8A std Y+19,r10 |
5312 3262 8091 0000 lds r24,GyroPFactor |
5313 3266 282F mov r18,r24 |
5314 3268 30E0 ldi r19,lo8(0) |
5315 326a 40E0 ldi r20,lo8(0) |
5316 326c 50E0 ldi r21,hi8(0) |
5317 326e 2F87 std Y+15,r18 |
5318 3270 388B std Y+16,r19 |
5319 3272 498B std Y+17,r20 |
5320 3274 5A8B std Y+18,r21 |
5321 3276 1091 0000 lds r17,LoopingRoll |
5322 327a 1123 tst r17 |
5323 327c 01F4 brne .+2 |
5324 327e 00C0 rjmp .L601 |
5325 3280 1886 std Y+8,__zero_reg__ |
5326 3282 1F82 std Y+7,__zero_reg__ |
5327 3284 1E82 std Y+6,__zero_reg__ |
5328 3286 1D82 std Y+5,__zero_reg__ |
5329 .L602: |
5330 3288 4091 0000 lds r20,GyroRoll |
5331 328c 5091 0000 lds r21,(GyroRoll)+1 |
5332 3290 5E87 std Y+14,r21 |
5333 3292 4D87 std Y+13,r20 |
5334 3294 6090 0000 lds r6,GyroYawPFactor |
5335 3298 7090 0000 lds r7,GyroYawIFactor |
5336 329c 6091 0000 lds r22,IntegralGyroYaw |
5337 32a0 7091 0000 lds r23,(IntegralGyroYaw)+1 |
5338 32a4 8091 0000 lds r24,(IntegralGyroYaw)+2 |
5339 32a8 9091 0000 lds r25,(IntegralGyroYaw)+3 |
5340 32ac 6987 std Y+9,r22 |
5341 32ae 7A87 std Y+10,r23 |
5342 32b0 8B87 std Y+11,r24 |
5343 32b2 9C87 std Y+12,r25 |
5344 32b4 4091 0000 lds r20,StickGas |
5345 32b8 5091 0000 lds r21,(StickGas)+1 |
5346 32bc 2091 0000 lds r18,LowVoltageWarning |
5347 32c0 8091 0000 lds r24,UBat |
5348 32c4 9091 0000 lds r25,(UBat)+1 |
5349 32c8 30E0 ldi r19,lo8(0) |
5350 32ca 2817 cp r18,r24 |
5351 32cc 3907 cpc r19,r25 |
5352 32ce 04F4 brge .L603 |
5353 32d0 6091 0000 lds r22,UBat |
5354 32d4 7091 0000 lds r23,(UBat)+1 |
5355 32d8 429F mul r20,r18 |
5356 32da C001 movw r24,r0 |
5357 32dc 439F mul r20,r19 |
5358 32de 900D add r25,r0 |
5359 32e0 529F mul r21,r18 |
5360 32e2 900D add r25,r0 |
5361 32e4 1124 clr r1 |
5362 32e6 0E94 0000 call __udivmodhi4 |
5363 32ea AB01 movw r20,r22 |
5364 .L603: |
5365 32ec CA01 movw r24,r20 |
5366 32ee 880F lsl r24 |
5367 32f0 991F rol r25 |
5368 32f2 880F lsl r24 |
5369 32f4 991F rol r25 |
5370 32f6 99AF std Y+57,r25 |
5371 32f8 88AF std Y+56,r24 |
5372 32fa 8091 0000 lds r24,ParamSet+8 |
5373 32fe 80FF sbrs r24,0 |
5374 3300 00C0 rjmp .L604 |
5375 3302 1123 tst r17 |
5376 3304 01F4 brne .+2 |
5377 3306 00C0 rjmp .L777 |
5378 .L604: |
5379 3308 8091 0000 lds r24,ParamSet+21 |
5380 330c 282F mov r18,r24 |
5381 330e 30E0 ldi r19,lo8(0) |
5382 3310 265F subi r18,lo8(-(10)) |
5383 3312 3F4F sbci r19,hi8(-(10)) |
5384 3314 5901 movw r10,r18 |
5385 3316 AA0C lsl r10 |
5386 3318 BB1C rol r11 |
5387 331a AA0C lsl r10 |
5388 331c BB1C rol r11 |
5389 331e 2A50 subi r18,lo8(-(-10)) |
5390 3320 3040 sbci r19,hi8(-(-10)) |
5391 3322 C8AC ldd r12,Y+56 |
5392 3324 D9AC ldd r13,Y+57 |
5393 3326 CA14 cp r12,r10 |
5394 3328 DB04 cpc r13,r11 |
5395 332a 04F0 brlt .L669 |
5396 332c 8091 0000 lds r24,ParamSet+22 |
5397 3330 90E0 ldi r25,lo8(0) |
5398 3332 4497 sbiw r24,20 |
5399 3334 880F lsl r24 |
5400 3336 991F rol r25 |
5401 3338 880F lsl r24 |
5402 333a 991F rol r25 |
5403 333c 5601 movw r10,r12 |
5404 333e 8C15 cp r24,r12 |
5405 3340 9D05 cpc r25,r13 |
5406 3342 04F4 brge .+2 |
5407 3344 00C0 rjmp .L778 |
5408 .L669: |
5409 3346 8091 0000 lds r24,MissingMotor |
5410 334a 8823 tst r24 |
5411 334c 01F0 breq .L671 |
5412 334e 8091 0000 lds r24,ModelIsFlying |
5413 3352 9091 0000 lds r25,(ModelIsFlying)+1 |
5414 3356 0297 sbiw r24,2 |
5415 3358 C097 sbiw r24,48 |
5416 335a 00F4 brsh .+2 |
5417 335c 00C0 rjmp .L779 |
5418 .L672: |
5419 335e 5901 movw r10,r18 |
5420 3360 AA0C lsl r10 |
5421 3362 BB1C rol r11 |
5422 3364 AA0C lsl r10 |
5423 3366 BB1C rol r11 |
5424 .L671: |
5425 3368 B092 0000 sts (DebugOut+16)+1,r11 |
5426 336c A092 0000 sts DebugOut+16,r10 |
5427 3370 220C lsl r2 |
5428 3372 331C rol r3 |
5429 3374 B101 movw r22,r2 |
5430 3376 8827 clr r24 |
5431 3378 77FD sbrc r23,7 |
5432 337a 8095 com r24 |
5433 337c 982F mov r25,r24 |
5434 337e 262D mov r18,r6 |
5435 3380 30E0 ldi r19,lo8(0) |
5436 3382 40E0 ldi r20,lo8(0) |
5437 3384 50E0 ldi r21,hi8(0) |
5438 3386 0E94 0000 call __mulsi3 |
5439 338a 20E4 ldi r18,lo8(64) |
5440 338c 30E0 ldi r19,hi8(64) |
5441 338e 40E0 ldi r20,hlo8(64) |
5442 3390 50E0 ldi r21,hhi8(64) |
5443 3392 0E94 0000 call __divmodsi4 |
5444 3396 7901 movw r14,r18 |
5445 3398 8A01 movw r16,r20 |
5446 339a 672D mov r22,r7 |
5447 339c 70E0 ldi r23,lo8(0) |
5448 339e 80E0 ldi r24,lo8(0) |
5449 33a0 90E0 ldi r25,hi8(0) |
5450 33a2 2985 ldd r18,Y+9 |
5451 33a4 3A85 ldd r19,Y+10 |
5452 33a6 4B85 ldd r20,Y+11 |
5453 33a8 5C85 ldd r21,Y+12 |
5454 33aa 0E94 0000 call __mulsi3 |
5455 33ae 20EF ldi r18,lo8(22000) |
5456 33b0 35E5 ldi r19,hi8(22000) |
5457 33b2 40E0 ldi r20,hlo8(22000) |
5458 33b4 50E0 ldi r21,hhi8(22000) |
5459 33b6 0E94 0000 call __divmodsi4 |
5460 33ba C701 movw r24,r14 |
5461 33bc 820F add r24,r18 |
5462 33be 931F adc r25,r19 |
5463 33c0 20E4 ldi r18,hi8(16385) |
5464 33c2 8130 cpi r24,lo8(16385) |
5465 33c4 9207 cpc r25,r18 |
5466 33c6 04F0 brlt .L673 |
5467 33c8 80E0 ldi r24,lo8(16384) |
5468 33ca 90E4 ldi r25,hi8(16384) |
5469 .L673: |
5470 33cc 8C01 movw r16,r24 |
5471 33ce 30EC ldi r19,hi8(-16384) |
5472 33d0 8030 cpi r24,lo8(-16384) |
5473 33d2 9307 cpc r25,r19 |
5474 33d4 04F4 brge .L674 |
5475 33d6 00E0 ldi r16,lo8(-16384) |
5476 33d8 10EC ldi r17,hi8(-16384) |
5477 .L674: |
5478 33da 6091 0000 lds r22,SetPointYaw.2410 |
5479 33de 7091 0000 lds r23,(SetPointYaw.2410)+1 |
5480 33e2 8091 0000 lds r24,(SetPointYaw.2410)+2 |
5481 33e6 9091 0000 lds r25,(SetPointYaw.2410)+3 |
5482 33ea 2CEF ldi r18,lo8(-4) |
5483 33ec 3FEF ldi r19,hi8(-4) |
5484 33ee 4FEF ldi r20,hlo8(-4) |
5485 33f0 5FEF ldi r21,hhi8(-4) |
5486 33f2 0E94 0000 call __mulsi3 |
5487 33f6 7801 movw r14,r16 |
5488 33f8 E60E add r14,r22 |
5489 33fa F71E adc r15,r23 |
5490 33fc 41EA ldi r20,lo8(161) |
5491 33fe A416 cp r10,r20 |
5492 3400 B104 cpc r11,__zero_reg__ |
5493 3402 04F4 brge .+2 |
5494 3404 00C0 rjmp .L675 |
5495 3406 C501 movw r24,r10 |
5496 3408 B7FE sbrs r11,7 |
5497 340a 00C0 rjmp .+4 |
5498 340c 0C94 0000 jmp .L780 |
5499 .L676: |
5500 3410 9595 asr r25 |
5501 3412 8795 ror r24 |
5502 3414 2227 clr r18 |
5503 3416 3327 clr r19 |
5504 3418 281B sub r18,r24 |
5505 341a 390B sbc r19,r25 |
5506 341c E216 cp r14,r18 |
5507 341e F306 cpc r15,r19 |
5508 3420 04F0 brlt .+2 |
5509 3422 00C0 rjmp .L781 |
5510 .L677: |
5511 3424 8091 0000 lds r24,ParamSet+22 |
5512 3428 90E0 ldi r25,lo8(0) |
5513 342a 880F lsl r24 |
5514 342c 991F rol r25 |
5515 342e 880F lsl r24 |
5516 3430 991F rol r25 |
5517 3432 3501 movw r6,r10 |
5518 3434 681A sub r6,r24 |
5519 3436 790A sbc r7,r25 |
5520 3438 2615 cp r18,r6 |
5521 343a 3705 cpc r19,r7 |
5522 343c 04F0 brlt .L681 |
5523 343e 8A19 sub r24,r10 |
5524 3440 9B09 sbc r25,r11 |
5525 3442 3901 movw r6,r18 |
5526 3444 8217 cp r24,r18 |
5527 3446 9307 cpc r25,r19 |
5528 3448 04F4 brge .+2 |
5529 344a 00C0 rjmp .L782 |
5530 .L681: |
5531 344c 6091 0000 lds r22,StickNick |
5532 3450 7091 0000 lds r23,(StickNick)+1 |
5533 3454 79AF std Y+57,r23 |
5534 3456 68AF std Y+56,r22 |
5535 3458 7401 movw r14,r8 |
5536 345a 0027 clr r16 |
5537 345c F7FC sbrc r15,7 |
5538 345e 0095 com r16 |
5539 3460 102F mov r17,r16 |
5540 3462 EE0C lsl r14 |
5541 3464 FF1C rol r15 |
5542 3466 001F rol r16 |
5543 3468 111F rol r17 |
5544 346a EE0C lsl r14 |
5545 346c FF1C rol r15 |
5546 346e 001F rol r16 |
5547 3470 111F rol r17 |
5548 3472 EE0C lsl r14 |
5549 3474 FF1C rol r15 |
5550 3476 001F rol r16 |
5551 3478 111F rol r17 |
5552 347a EE0C lsl r14 |
5553 347c FF1C rol r15 |
5554 347e 001F rol r16 |
5555 3480 111F rol r17 |
5556 3482 EE0C lsl r14 |
5557 3484 FF1C rol r15 |
5558 3486 001F rol r16 |
5559 3488 111F rol r17 |
5560 348a EE0C lsl r14 |
5561 348c FF1C rol r15 |
5562 348e 001F rol r16 |
5563 3490 111F rol r17 |
5564 3492 EE0C lsl r14 |
5565 3494 FF1C rol r15 |
5566 3496 001F rol r16 |
5567 3498 111F rol r17 |
5568 349a 8B89 ldd r24,Y+19 |
5569 349c 9C89 ldd r25,Y+20 |
5570 349e 9C01 movw r18,r24 |
5571 34a0 4427 clr r20 |
5572 34a2 37FD sbrc r19,7 |
5573 34a4 4095 com r20 |
5574 34a6 542F mov r21,r20 |
5575 34a8 6F85 ldd r22,Y+15 |
5576 34aa 7889 ldd r23,Y+16 |
5577 34ac 8989 ldd r24,Y+17 |
5578 34ae 9A89 ldd r25,Y+18 |
5579 34b0 0E94 0000 call __mulsi3 |
5580 34b4 E60E add r14,r22 |
5581 34b6 F71E adc r15,r23 |
5582 34b8 081F adc r16,r24 |
5583 34ba 191F adc r17,r25 |
5584 34bc C801 movw r24,r16 |
5585 34be B701 movw r22,r14 |
5586 34c0 20E4 ldi r18,lo8(64) |
5587 34c2 30E0 ldi r19,hi8(64) |
5588 34c4 40E0 ldi r20,hlo8(64) |
5589 34c6 50E0 ldi r21,hhi8(64) |
5590 34c8 0E94 0000 call __divmodsi4 |
5591 34cc AB81 ldd r26,Y+3 |
5592 34ce BC81 ldd r27,Y+4 |
5593 34d0 2A0F add r18,r26 |
5594 34d2 3B1F adc r19,r27 |
5595 34d4 B0E4 ldi r27,hi8(16385) |
5596 34d6 2130 cpi r18,lo8(16385) |
5597 34d8 3B07 cpc r19,r27 |
5598 34da 04F0 brlt .L683 |
5599 34dc 20E0 ldi r18,lo8(16384) |
5600 34de 30E4 ldi r19,hi8(16384) |
5601 .L683: |
5602 34e0 E0EC ldi r30,hi8(-16384) |
5603 34e2 2030 cpi r18,lo8(-16384) |
5604 34e4 3E07 cpc r19,r30 |
5605 34e6 04F4 brge .L684 |
5606 34e8 20E0 ldi r18,lo8(-16384) |
5607 34ea 30EC ldi r19,hi8(-16384) |
5608 .L684: |
5609 34ec 6901 movw r12,r18 |
5610 34ee 48AD ldd r20,Y+56 |
5611 34f0 59AD ldd r21,Y+57 |
5612 34f2 C41A sub r12,r20 |
5613 34f4 D50A sbc r13,r21 |
5614 34f6 3090 0000 lds r3,GyroIFactor |
5615 34fa 3320 tst r3 |
5616 34fc 01F4 brne .+2 |
5617 34fe 00C0 rjmp .L685 |
5618 3500 441A sub r4,r20 |
5619 3502 550A sbc r5,r21 |
5620 3504 C201 movw r24,r4 |
5621 3506 AA27 clr r26 |
5622 3508 97FD sbrc r25,7 |
5623 350a A095 com r26 |
5624 350c BA2F mov r27,r26 |
5625 350e E090 0000 lds r14,IPartNick.2408 |
5626 3512 F090 0000 lds r15,(IPartNick.2408)+1 |
5627 3516 0091 0000 lds r16,(IPartNick.2408)+2 |
5628 351a 1091 0000 lds r17,(IPartNick.2408)+3 |
5629 351e E80E add r14,r24 |
5630 3520 F91E adc r15,r25 |
5631 3522 0A1F adc r16,r26 |
5632 3524 1B1F adc r17,r27 |
5633 3526 E092 0000 sts IPartNick.2408,r14 |
5634 352a F092 0000 sts (IPartNick.2408)+1,r15 |
5635 352e 0093 0000 sts (IPartNick.2408)+2,r16 |
5636 3532 1093 0000 sts (IPartNick.2408)+3,r17 |
5637 .L686: |
5638 3536 50E0 ldi r21,lo8(-64000) |
5639 3538 E516 cp r14,r21 |
5640 353a 56E0 ldi r21,hi8(-64000) |
5641 353c F506 cpc r15,r21 |
5642 353e 5FEF ldi r21,hlo8(-64000) |
5643 3540 0507 cpc r16,r21 |
5644 3542 5FEF ldi r21,hhi8(-64000) |
5645 3544 1507 cpc r17,r21 |
5646 3546 04F0 brlt .+2 |
5647 3548 00C0 rjmp .L687 |
5648 354a 80E0 ldi r24,lo8(-64000) |
5649 354c 96E0 ldi r25,hi8(-64000) |
5650 354e AFEF ldi r26,hlo8(-64000) |
5651 3550 BFEF ldi r27,hhi8(-64000) |
5652 3552 8093 0000 sts IPartNick.2408,r24 |
5653 3556 9093 0000 sts (IPartNick.2408)+1,r25 |
5654 355a A093 0000 sts (IPartNick.2408)+2,r26 |
5655 355e B093 0000 sts (IPartNick.2408)+3,r27 |
5656 3562 E12C mov r14,__zero_reg__ |
5657 3564 36E0 ldi r19,hi8(-64000) |
5658 3566 F32E mov r15,r19 |
5659 3568 3FEF ldi r19,hlo8(-64000) |
5660 356a 032F mov r16,r19 |
5661 356c 3FEF ldi r19,hhi8(-64000) |
5662 356e 132F mov r17,r19 |
5663 .L688: |
5664 3570 4090 0000 lds r4,Ki |
5665 3574 5090 0000 lds r5,(Ki)+1 |
5666 3578 9201 movw r18,r4 |
5667 357a 4427 clr r20 |
5668 357c 37FD sbrc r19,7 |
5669 357e 4095 com r20 |
5670 3580 542F mov r21,r20 |
5671 3582 C801 movw r24,r16 |
5672 3584 B701 movw r22,r14 |
5673 3586 0E94 0000 call __divmodsi4 |
5674 358a 4601 movw r8,r12 |
5675 358c 820E add r8,r18 |
5676 358e 931E adc r9,r19 |
5677 3590 8091 0000 lds r24,StickRoll |
5678 3594 9091 0000 lds r25,(StickRoll)+1 |
5679 3598 99AF std Y+57,r25 |
5680 359a 88AF std Y+56,r24 |
5681 359c AE8D ldd r26,Y+30 |
5682 359e BF8D ldd r27,Y+31 |
5683 35a0 7D01 movw r14,r26 |
5684 35a2 0027 clr r16 |
5685 35a4 F7FC sbrc r15,7 |
5686 35a6 0095 com r16 |
5687 35a8 102F mov r17,r16 |
5688 35aa EE0C lsl r14 |
5689 35ac FF1C rol r15 |
5690 35ae 001F rol r16 |
5691 35b0 111F rol r17 |
5692 35b2 EE0C lsl r14 |
5693 35b4 FF1C rol r15 |
5694 35b6 001F rol r16 |
5695 35b8 111F rol r17 |
5696 35ba EE0C lsl r14 |
5697 35bc FF1C rol r15 |
5698 35be 001F rol r16 |
5699 35c0 111F rol r17 |
5700 35c2 EE0C lsl r14 |
5701 35c4 FF1C rol r15 |
5702 35c6 001F rol r16 |
5703 35c8 111F rol r17 |
5704 35ca EE0C lsl r14 |
5705 35cc FF1C rol r15 |
5706 35ce 001F rol r16 |
5707 35d0 111F rol r17 |
5708 35d2 EE0C lsl r14 |
5709 35d4 FF1C rol r15 |
5710 35d6 001F rol r16 |
5711 35d8 111F rol r17 |
5712 35da EE0C lsl r14 |
5713 35dc FF1C rol r15 |
5714 35de 001F rol r16 |
5715 35e0 111F rol r17 |
5716 35e2 ED85 ldd r30,Y+13 |
5717 35e4 FE85 ldd r31,Y+14 |
5718 35e6 BF01 movw r22,r30 |
5719 35e8 8827 clr r24 |
5720 35ea 77FD sbrc r23,7 |
5721 35ec 8095 com r24 |
5722 35ee 982F mov r25,r24 |
5723 35f0 2F85 ldd r18,Y+15 |
5724 35f2 3889 ldd r19,Y+16 |
5725 35f4 4989 ldd r20,Y+17 |
5726 35f6 5A89 ldd r21,Y+18 |
5727 35f8 0E94 0000 call __mulsi3 |
5728 35fc E60E add r14,r22 |
5729 35fe F71E adc r15,r23 |
5730 3600 081F adc r16,r24 |
5731 3602 191F adc r17,r25 |
5732 3604 C801 movw r24,r16 |
5733 3606 B701 movw r22,r14 |
5734 3608 20E4 ldi r18,lo8(64) |
5735 360a 30E0 ldi r19,hi8(64) |
5736 360c 40E0 ldi r20,hlo8(64) |
5737 360e 50E0 ldi r21,hhi8(64) |
5738 3610 0E94 0000 call __divmodsi4 |
5739 3614 CD80 ldd r12,Y+5 |
5740 3616 DE80 ldd r13,Y+6 |
5741 3618 2C0D add r18,r12 |
5742 361a 3D1D adc r19,r13 |
5743 361c 40E4 ldi r20,hi8(16385) |
5744 361e 2130 cpi r18,lo8(16385) |
5745 3620 3407 cpc r19,r20 |
5746 3622 04F0 brlt .L689 |
5747 3624 20E0 ldi r18,lo8(16384) |
5748 3626 30E4 ldi r19,hi8(16384) |
5749 .L689: |
5750 3628 50EC ldi r21,hi8(-16384) |
5751 362a 2030 cpi r18,lo8(-16384) |
5752 362c 3507 cpc r19,r21 |
5753 362e 04F4 brge .L690 |
5754 3630 20E0 ldi r18,lo8(-16384) |
5755 3632 30EC ldi r19,hi8(-16384) |
5756 .L690: |
5757 3634 6901 movw r12,r18 |
5758 3636 68AD ldd r22,Y+56 |
5759 3638 79AD ldd r23,Y+57 |
5760 363a C61A sub r12,r22 |
5761 363c D70A sbc r13,r23 |
5762 363e 3320 tst r3 |
5763 3640 01F4 brne .+2 |
5764 3642 00C0 rjmp .L691 |
5765 3644 8F81 ldd r24,Y+7 |
5766 3646 9885 ldd r25,Y+8 |
5767 3648 861B sub r24,r22 |
5768 364a 970B sbc r25,r23 |
5769 364c FC01 movw r30,r24 |
5770 364e CF01 movw r24,r30 |
5771 3650 AA27 clr r26 |
5772 3652 97FD sbrc r25,7 |
5773 3654 A095 com r26 |
5774 3656 BA2F mov r27,r26 |
5775 3658 E090 0000 lds r14,IPartRoll.2409 |
5776 365c F090 0000 lds r15,(IPartRoll.2409)+1 |
5777 3660 0091 0000 lds r16,(IPartRoll.2409)+2 |
5778 3664 1091 0000 lds r17,(IPartRoll.2409)+3 |
5779 3668 E80E add r14,r24 |
5780 366a F91E adc r15,r25 |
5781 366c 0A1F adc r16,r26 |
5782 366e 1B1F adc r17,r27 |
5783 3670 E092 0000 sts IPartRoll.2409,r14 |
5784 3674 F092 0000 sts (IPartRoll.2409)+1,r15 |
5785 3678 0093 0000 sts (IPartRoll.2409)+2,r16 |
5786 367c 1093 0000 sts (IPartRoll.2409)+3,r17 |
5787 .L692: |
5788 3680 F0E0 ldi r31,lo8(-64000) |
5789 3682 EF16 cp r14,r31 |
5790 3684 F6E0 ldi r31,hi8(-64000) |
5791 3686 FF06 cpc r15,r31 |
5792 3688 FFEF ldi r31,hlo8(-64000) |
5793 368a 0F07 cpc r16,r31 |
5794 368c FFEF ldi r31,hhi8(-64000) |
5795 368e 1F07 cpc r17,r31 |
5796 3690 04F0 brlt .+2 |
5797 3692 00C0 rjmp .L693 |
5798 3694 80E0 ldi r24,lo8(-64000) |
5799 3696 96E0 ldi r25,hi8(-64000) |
5800 3698 AFEF ldi r26,hlo8(-64000) |
5801 369a BFEF ldi r27,hhi8(-64000) |
5802 369c 8093 0000 sts IPartRoll.2409,r24 |
5803 36a0 9093 0000 sts (IPartRoll.2409)+1,r25 |
5804 36a4 A093 0000 sts (IPartRoll.2409)+2,r26 |
5805 36a8 B093 0000 sts (IPartRoll.2409)+3,r27 |
5806 36ac E12C mov r14,__zero_reg__ |
5807 36ae F6E0 ldi r31,hi8(-64000) |
5808 36b0 FF2E mov r15,r31 |
5809 36b2 FFEF ldi r31,hlo8(-64000) |
5810 36b4 0F2F mov r16,r31 |
5811 36b6 FFEF ldi r31,hhi8(-64000) |
5812 36b8 1F2F mov r17,r31 |
5813 .L694: |
5814 36ba 9201 movw r18,r4 |
5815 36bc 4427 clr r20 |
5816 36be 37FD sbrc r19,7 |
5817 36c0 4095 com r20 |
5818 36c2 542F mov r21,r20 |
5819 36c4 C801 movw r24,r16 |
5820 36c6 B701 movw r22,r14 |
5821 36c8 0E94 0000 call __divmodsi4 |
5822 36cc 8601 movw r16,r12 |
5823 36ce 020F add r16,r18 |
5824 36d0 131F adc r17,r19 |
5825 36d2 9301 movw r18,r6 |
5826 36d4 77FC sbrc r7,7 |
5827 36d6 00C0 rjmp .L783 |
5828 .L695: |
5829 36d8 3595 asr r19 |
5830 36da 2795 ror r18 |
5831 36dc 2A0D add r18,r10 |
5832 36de 3B1D adc r19,r11 |
5833 36e0 B901 movw r22,r18 |
5834 36e2 8827 clr r24 |
5835 36e4 77FD sbrc r23,7 |
5836 36e6 8095 com r24 |
5837 36e8 982F mov r25,r24 |
5838 36ea 2091 0000 lds r18,FCParam+27 |
5839 36ee 30E0 ldi r19,lo8(0) |
5840 36f0 40E0 ldi r20,lo8(0) |
5841 36f2 50E0 ldi r21,hi8(0) |
5842 36f4 0E94 0000 call __mulsi3 |
5843 36f8 20E4 ldi r18,lo8(64) |
5844 36fa 30E0 ldi r19,hi8(64) |
5845 36fc 40E0 ldi r20,hlo8(64) |
5846 36fe 50E0 ldi r21,hhi8(64) |
5847 3700 0E94 0000 call __divmodsi4 |
5848 3704 B901 movw r22,r18 |
5849 3706 8827 clr r24 |
5850 3708 9927 clr r25 |
5851 370a 821B sub r24,r18 |
5852 370c 930B sbc r25,r19 |
5853 370e 8816 cp r8,r24 |
5854 3710 9906 cpc r9,r25 |
5855 3712 04F0 brlt .+2 |
5856 3714 00C0 rjmp .L696 |
5857 3716 9C01 movw r18,r24 |
5858 .L697: |
5859 3718 0817 cp r16,r24 |
5860 371a 1907 cpc r17,r25 |
5861 371c 04F0 brlt .L700 |
5862 371e CB01 movw r24,r22 |
5863 3720 0617 cp r16,r22 |
5864 3722 1707 cpc r17,r23 |
5865 3724 04F4 brge .+2 |
5866 3726 00C0 rjmp .L784 |
5867 .L700: |
5868 3728 60E0 ldi r22,lo8(Mixer+13) |
5869 372a 262E mov r2,r22 |
5870 372c 60E0 ldi r22,hi8(Mixer+13) |
5871 372e 362E mov r3,r22 |
5872 3730 50E0 ldi r21,lo8(Motor) |
5873 3732 852E mov r8,r21 |
5874 3734 50E0 ldi r21,hi8(Motor) |
5875 3736 952E mov r9,r21 |
5876 3738 40E0 ldi r20,lo8(MotorValue.2421) |
5877 373a C42E mov r12,r20 |
5878 373c 40E0 ldi r20,hi8(MotorValue.2421) |
5879 373e D42E mov r13,r20 |
5880 3740 A301 movw r20,r6 |
5881 3742 6627 clr r22 |
5882 3744 57FD sbrc r21,7 |
5883 3746 6095 com r22 |
5884 3748 762F mov r23,r22 |
5885 374a 4CA7 std Y+44,r20 |
5886 374c 5DA7 std Y+45,r21 |
5887 374e 6EA7 std Y+46,r22 |
5888 3750 7FA7 std Y+47,r23 |
5889 3752 A501 movw r20,r10 |
5890 3754 6627 clr r22 |
5891 3756 57FD sbrc r21,7 |
5892 3758 6095 com r22 |
5893 375a 762F mov r23,r22 |
5894 375c 48AB std Y+48,r20 |
5895 375e 59AB std Y+49,r21 |
5896 3760 6AAB std Y+50,r22 |
5897 3762 7BAB std Y+51,r23 |
5898 3764 A901 movw r20,r18 |
5899 3766 6627 clr r22 |
5900 3768 57FD sbrc r21,7 |
5901 376a 6095 com r22 |
5902 376c 762F mov r23,r22 |
5903 376e 4CAB std Y+52,r20 |
5904 3770 5DAB std Y+53,r21 |
5905 3772 6EAB std Y+54,r22 |
5906 3774 7FAB std Y+55,r23 |
5907 3776 2C01 movw r4,r24 |
5908 3778 6624 clr r6 |
5909 377a 57FC sbrc r5,7 |
5910 377c 6094 com r6 |
5911 377e 762C mov r7,r6 |
5912 3780 00C0 rjmp .L707 |
5913 .L787: |
5914 3782 1396 adiw r26,3 |
5915 3784 2C91 ld r18,X |
5916 3786 3327 clr r19 |
5917 3788 27FD sbrc r18,7 |
5918 378a 3095 com r19 |
5919 378c 432F mov r20,r19 |
5920 378e 532F mov r21,r19 |
5921 3790 6CA5 ldd r22,Y+44 |
5922 3792 7DA5 ldd r23,Y+45 |
5923 3794 8EA5 ldd r24,Y+46 |
5924 3796 9FA5 ldd r25,Y+47 |
5925 3798 0E94 0000 call __mulsi3 |
5926 379c 20E4 ldi r18,lo8(64) |
5927 379e 30E0 ldi r19,hi8(64) |
5928 37a0 40E0 ldi r20,hlo8(64) |
5929 37a2 50E0 ldi r21,hhi8(64) |
5930 37a4 0E94 0000 call __divmodsi4 |
5931 37a8 7901 movw r14,r18 |
5932 37aa 8A01 movw r16,r20 |
5933 37ac 2A2D mov r18,r10 |
5934 37ae 3327 clr r19 |
5935 37b0 27FD sbrc r18,7 |
5936 37b2 3095 com r19 |
5937 37b4 432F mov r20,r19 |
5938 37b6 532F mov r21,r19 |
5939 37b8 68A9 ldd r22,Y+48 |
5940 37ba 79A9 ldd r23,Y+49 |
5941 37bc 8AA9 ldd r24,Y+50 |
5942 37be 9BA9 ldd r25,Y+51 |
5943 37c0 0E94 0000 call __mulsi3 |
5944 37c4 20E4 ldi r18,lo8(64) |
5945 37c6 30E0 ldi r19,hi8(64) |
5946 37c8 40E0 ldi r20,hlo8(64) |
5947 37ca 50E0 ldi r21,hhi8(64) |
5948 37cc 0E94 0000 call __divmodsi4 |
5949 37d0 E20E add r14,r18 |
5950 37d2 F31E adc r15,r19 |
5951 37d4 F101 movw r30,r2 |
5952 37d6 2181 ldd r18,Z+1 |
5953 37d8 3327 clr r19 |
5954 37da 27FD sbrc r18,7 |
5955 37dc 3095 com r19 |
5956 37de 432F mov r20,r19 |
5957 37e0 532F mov r21,r19 |
5958 37e2 6CA9 ldd r22,Y+52 |
5959 37e4 7DA9 ldd r23,Y+53 |
5960 37e6 8EA9 ldd r24,Y+54 |
5961 37e8 9FA9 ldd r25,Y+55 |
5962 37ea 0E94 0000 call __mulsi3 |
5963 37ee 20E4 ldi r18,lo8(64) |
5964 37f0 30E0 ldi r19,hi8(64) |
5965 37f2 40E0 ldi r20,hlo8(64) |
5966 37f4 50E0 ldi r21,hhi8(64) |
5967 37f6 0E94 0000 call __divmodsi4 |
5968 37fa E20E add r14,r18 |
5969 37fc F31E adc r15,r19 |
5970 37fe D101 movw r26,r2 |
5971 3800 1296 adiw r26,2 |
5972 3802 2C91 ld r18,X |
5973 3804 3327 clr r19 |
5974 3806 27FD sbrc r18,7 |
5975 3808 3095 com r19 |
5976 380a 432F mov r20,r19 |
5977 380c 532F mov r21,r19 |
5978 380e C301 movw r24,r6 |
5979 3810 B201 movw r22,r4 |
5980 3812 0E94 0000 call __mulsi3 |
5981 3816 20E4 ldi r18,lo8(64) |
5982 3818 30E0 ldi r19,hi8(64) |
5983 381a 40E0 ldi r20,hlo8(64) |
5984 381c 50E0 ldi r21,hhi8(64) |
5985 381e 0E94 0000 call __divmodsi4 |
5986 3822 E20E add r14,r18 |
5987 3824 F31E adc r15,r19 |
5988 3826 F601 movw r30,r12 |
5989 3828 6081 ld r22,Z |
5990 382a 7181 ldd r23,Z+1 |
5991 382c C701 movw r24,r14 |
5992 382e 0E94 0000 call MotorSmoothing |
5993 3832 D601 movw r26,r12 |
5994 3834 8C93 st X,r24 |
5995 3836 1196 adiw r26,1 |
5996 3838 9C93 st X,r25 |
5997 383a 97FD sbrc r25,7 |
5998 383c 00C0 rjmp .L785 |
5999 .L703: |
6000 383e AC01 movw r20,r24 |
6001 3840 5595 asr r21 |
6002 3842 4795 ror r20 |
6003 3844 5595 asr r21 |
6004 3846 4795 ror r20 |
6005 3848 8091 0000 lds r24,ParamSet+21 |
6006 384c 282F mov r18,r24 |
6007 384e 30E0 ldi r19,lo8(0) |
6008 3850 4217 cp r20,r18 |
6009 3852 5307 cpc r21,r19 |
6010 3854 04F0 brlt .L704 |
6011 3856 8091 0000 lds r24,ParamSet+22 |
6012 385a 90E0 ldi r25,lo8(0) |
6013 385c 9C01 movw r18,r24 |
6014 385e 4217 cp r20,r18 |
6015 3860 5307 cpc r21,r19 |
6016 3862 04F4 brge .L704 |
6017 3864 9A01 movw r18,r20 |
6018 .L704: |
6019 3866 F401 movw r30,r8 |
6020 3868 2083 st Z,r18 |
6021 .L706: |
6022 386a E4E0 ldi r30,lo8(4) |
6023 386c F0E0 ldi r31,hi8(4) |
6024 386e 2E0E add r2,r30 |
6025 3870 3F1E adc r3,r31 |
6026 3872 95E0 ldi r25,lo8(5) |
6027 3874 A92E mov r10,r25 |
6028 3876 B12C mov r11,__zero_reg__ |
6029 3878 8A0C add r8,r10 |
6030 387a 9B1C adc r9,r11 |
6031 387c 22E0 ldi r18,lo8(2) |
6032 387e 30E0 ldi r19,hi8(2) |
6033 3880 C20E add r12,r18 |
6034 3882 D31E adc r13,r19 |
6035 3884 30E0 ldi r19,lo8(Mixer+61) |
6036 3886 2316 cp r2,r19 |
6037 3888 30E0 ldi r19,hi8(Mixer+61) |
6038 388a 3306 cpc r3,r19 |
6039 388c 01F0 breq .L786 |
6040 .L707: |
6041 388e D101 movw r26,r2 |
6042 3890 AC90 ld r10,X |
6043 3892 1A14 cp __zero_reg__,r10 |
6044 3894 04F4 brge .+2 |
6045 3896 00C0 rjmp .L787 |
6046 3898 D401 movw r26,r8 |
6047 389a 1C92 st X,__zero_reg__ |
6048 389c 00C0 rjmp .L706 |
6049 .L785: |
6050 389e 0396 adiw r24,3 |
6051 38a0 00C0 rjmp .L703 |
6052 .L786: |
6053 /* epilogue start */ |
6054 38a2 E996 adiw r28,57 |
6055 38a4 0FB6 in __tmp_reg__,__SREG__ |
6056 38a6 F894 cli |
6057 38a8 DEBF out __SP_H__,r29 |
6058 38aa 0FBE out __SREG__,__tmp_reg__ |
6059 38ac CDBF out __SP_L__,r28 |
6060 38ae CF91 pop r28 |
6061 38b0 DF91 pop r29 |
6062 38b2 1F91 pop r17 |
6063 38b4 0F91 pop r16 |
6064 38b6 FF90 pop r15 |
6065 38b8 EF90 pop r14 |
6066 38ba DF90 pop r13 |
6067 38bc CF90 pop r12 |
6068 38be BF90 pop r11 |
6069 38c0 AF90 pop r10 |
6070 38c2 9F90 pop r9 |
6071 38c4 8F90 pop r8 |
6072 38c6 7F90 pop r7 |
6073 38c8 6F90 pop r6 |
6074 38ca 5F90 pop r5 |
6075 38cc 4F90 pop r4 |
6076 38ce 3F90 pop r3 |
6077 38d0 2F90 pop r2 |
6078 38d2 0895 ret |
6079 .L696: |
6080 38d4 8216 cp r8,r18 |
6081 38d6 9306 cpc r9,r19 |
6082 38d8 04F0 brlt .+2 |
6083 38da 00C0 rjmp .L697 |
6084 38dc 9401 movw r18,r8 |
6085 38de 00C0 rjmp .L697 |
6086 .L693: |
6087 38e0 21E0 ldi r18,lo8(64001) |
6088 38e2 E216 cp r14,r18 |
6089 38e4 2AEF ldi r18,hi8(64001) |
6090 38e6 F206 cpc r15,r18 |
6091 38e8 20E0 ldi r18,hlo8(64001) |
6092 38ea 0207 cpc r16,r18 |
6093 38ec 20E0 ldi r18,hhi8(64001) |
6094 38ee 1207 cpc r17,r18 |
6095 38f0 04F4 brge .+2 |
6096 38f2 00C0 rjmp .L694 |
6097 38f4 80E0 ldi r24,lo8(64000) |
6098 38f6 9AEF ldi r25,hi8(64000) |
6099 38f8 A0E0 ldi r26,hlo8(64000) |
6100 38fa B0E0 ldi r27,hhi8(64000) |
6101 38fc 8093 0000 sts IPartRoll.2409,r24 |
6102 3900 9093 0000 sts (IPartRoll.2409)+1,r25 |
6103 3904 A093 0000 sts (IPartRoll.2409)+2,r26 |
6104 3908 B093 0000 sts (IPartRoll.2409)+3,r27 |
6105 390c E12C mov r14,__zero_reg__ |
6106 390e EAEF ldi r30,hi8(64000) |
6107 3910 FE2E mov r15,r30 |
6108 3912 012D mov r16,__zero_reg__ |
6109 3914 112D mov r17,__zero_reg__ |
6110 3916 00C0 rjmp .L694 |
6111 .L691: |
6112 3918 C601 movw r24,r12 |
6113 391a AA27 clr r26 |
6114 391c 97FD sbrc r25,7 |
6115 391e A095 com r26 |
6116 3920 BA2F mov r27,r26 |
6117 3922 E090 0000 lds r14,IPartRoll.2409 |
6118 3926 F090 0000 lds r15,(IPartRoll.2409)+1 |
6119 392a 0091 0000 lds r16,(IPartRoll.2409)+2 |
6120 392e 1091 0000 lds r17,(IPartRoll.2409)+3 |
6121 3932 E80E add r14,r24 |
6122 3934 F91E adc r15,r25 |
6123 3936 0A1F adc r16,r26 |
6124 3938 1B1F adc r17,r27 |
6125 393a E092 0000 sts IPartRoll.2409,r14 |
6126 393e F092 0000 sts (IPartRoll.2409)+1,r15 |
6127 3942 0093 0000 sts (IPartRoll.2409)+2,r16 |
6128 3946 1093 0000 sts (IPartRoll.2409)+3,r17 |
6129 394a 00C0 rjmp .L692 |
6130 .L687: |
6131 394c 61E0 ldi r22,lo8(64001) |
6132 394e E616 cp r14,r22 |
6133 3950 6AEF ldi r22,hi8(64001) |
6134 3952 F606 cpc r15,r22 |
6135 3954 60E0 ldi r22,hlo8(64001) |
6136 3956 0607 cpc r16,r22 |
6137 3958 60E0 ldi r22,hhi8(64001) |
6138 395a 1607 cpc r17,r22 |
6139 395c 04F4 brge .+2 |
6140 395e 00C0 rjmp .L688 |
6141 3960 80E0 ldi r24,lo8(64000) |
6142 3962 9AEF ldi r25,hi8(64000) |
6143 3964 A0E0 ldi r26,hlo8(64000) |
6144 3966 B0E0 ldi r27,hhi8(64000) |
6145 3968 8093 0000 sts IPartNick.2408,r24 |
6146 396c 9093 0000 sts (IPartNick.2408)+1,r25 |
6147 3970 A093 0000 sts (IPartNick.2408)+2,r26 |
6148 3974 B093 0000 sts (IPartNick.2408)+3,r27 |
6149 3978 E12C mov r14,__zero_reg__ |
6150 397a 2AEF ldi r18,hi8(64000) |
6151 397c F22E mov r15,r18 |
6152 397e 012D mov r16,__zero_reg__ |
6153 3980 112D mov r17,__zero_reg__ |
6154 3982 00C0 rjmp .L688 |
6155 .L685: |
6156 3984 C601 movw r24,r12 |
6157 3986 AA27 clr r26 |
6158 3988 97FD sbrc r25,7 |
6159 398a A095 com r26 |
6160 398c BA2F mov r27,r26 |
6161 398e E090 0000 lds r14,IPartNick.2408 |
6162 3992 F090 0000 lds r15,(IPartNick.2408)+1 |
6163 3996 0091 0000 lds r16,(IPartNick.2408)+2 |
6164 399a 1091 0000 lds r17,(IPartNick.2408)+3 |
6165 399e E80E add r14,r24 |
6166 39a0 F91E adc r15,r25 |
6167 39a2 0A1F adc r16,r26 |
6168 39a4 1B1F adc r17,r27 |
6169 39a6 E092 0000 sts IPartNick.2408,r14 |
6170 39aa F092 0000 sts (IPartNick.2408)+1,r15 |
6171 39ae 0093 0000 sts (IPartNick.2408)+2,r16 |
6172 39b2 1093 0000 sts (IPartNick.2408)+3,r17 |
6173 39b6 00C0 rjmp .L686 |
6174 .L675: |
6175 39b8 C701 movw r24,r14 |
6176 39ba 8135 cpi r24,81 |
6177 39bc 9105 cpc r25,__zero_reg__ |
6178 39be 04F0 brlt .+2 |
6179 39c0 00C0 rjmp .L788 |
6180 39c2 9C01 movw r18,r24 |
6181 39c4 5FEF ldi r21,hi8(-80) |
6182 39c6 803B cpi r24,lo8(-80) |
6183 39c8 9507 cpc r25,r21 |
6184 39ca 04F0 brlt .+2 |
6185 39cc 00C0 rjmp .L677 |
6186 .L790: |
6187 39ce 20EB ldi r18,lo8(-80) |
6188 39d0 3FEF ldi r19,hi8(-80) |
6189 39d2 00C0 rjmp .L677 |
6190 .L601: |
6191 39d4 6091 0000 lds r22,GyroIFactor |
6192 39d8 70E0 ldi r23,lo8(0) |
6193 39da 80E0 ldi r24,lo8(0) |
6194 39dc 90E0 ldi r25,hi8(0) |
6195 39de 2091 0000 lds r18,IntegralGyroRoll |
6196 39e2 3091 0000 lds r19,(IntegralGyroRoll)+1 |
6197 39e6 4091 0000 lds r20,(IntegralGyroRoll)+2 |
6198 39ea 5091 0000 lds r21,(IntegralGyroRoll)+3 |
6199 39ee 0E94 0000 call __mulsi3 |
6200 39f2 28EF ldi r18,lo8(11000) |
6201 39f4 3AE2 ldi r19,hi8(11000) |
6202 39f6 40E0 ldi r20,hlo8(11000) |
6203 39f8 50E0 ldi r21,hhi8(11000) |
6204 39fa 0E94 0000 call __divmodsi4 |
6205 39fe 3887 std Y+8,r19 |
6206 3a00 2F83 std Y+7,r18 |
6207 3a02 3E83 std Y+6,r19 |
6208 3a04 2D83 std Y+5,r18 |
6209 3a06 00C0 rjmp .L602 |
6210 .L599: |
6211 3a08 6091 0000 lds r22,GyroIFactor |
6212 3a0c 70E0 ldi r23,lo8(0) |
6213 3a0e 80E0 ldi r24,lo8(0) |
6214 3a10 90E0 ldi r25,hi8(0) |
6215 3a12 2091 0000 lds r18,IntegralGyroNick |
6216 3a16 3091 0000 lds r19,(IntegralGyroNick)+1 |
6217 3a1a 4091 0000 lds r20,(IntegralGyroNick)+2 |
6218 3a1e 5091 0000 lds r21,(IntegralGyroNick)+3 |
6219 3a22 0E94 0000 call __mulsi3 |
6220 3a26 28EF ldi r18,lo8(11000) |
6221 3a28 3AE2 ldi r19,hi8(11000) |
6222 3a2a 40E0 ldi r20,hlo8(11000) |
6223 3a2c 50E0 ldi r21,hhi8(11000) |
6224 3a2e 0E94 0000 call __divmodsi4 |
6225 3a32 2901 movw r4,r18 |
6226 3a34 3C83 std Y+4,r19 |
6227 3a36 2B83 std Y+3,r18 |
6228 3a38 00C0 rjmp .L600 |
6229 .L596: |
6230 3a3a EE8D ldd r30,Y+30 |
6231 3a3c FF8D ldd r31,Y+31 |
6232 3a3e E93C cpi r30,201 |
6233 3a40 F105 cpc r31,__zero_reg__ |
6234 3a42 04F4 brge .+2 |
6235 3a44 00C0 rjmp .L597 |
6236 3a46 88EC ldi r24,lo8(200) |
6237 3a48 90E0 ldi r25,hi8(200) |
6238 3a4a 9093 0000 sts (TrimRoll)+1,r25 |
6239 3a4e 8093 0000 sts TrimRoll,r24 |
6240 3a52 9F8F std Y+31,r25 |
6241 3a54 8E8F std Y+30,r24 |
6242 3a56 00C0 rjmp .L597 |
6243 .L594: |
6244 3a58 69EC ldi r22,lo8(201) |
6245 3a5a 8616 cp r8,r22 |
6246 3a5c 9104 cpc r9,__zero_reg__ |
6247 3a5e 04F4 brge .+2 |
6248 3a60 00C0 rjmp .L595 |
6249 3a62 88EC ldi r24,lo8(200) |
6250 3a64 90E0 ldi r25,hi8(200) |
6251 3a66 9093 0000 sts (TrimNick)+1,r25 |
6252 3a6a 8093 0000 sts TrimNick,r24 |
6253 3a6e E8EC ldi r30,lo8(200) |
6254 3a70 8E2E mov r8,r30 |
6255 3a72 912C mov r9,__zero_reg__ |
6256 3a74 00C0 rjmp .L595 |
6257 .L573: |
6258 3a76 2155 subi r18,lo8(50001) |
6259 3a78 334C sbci r19,hi8(50001) |
6260 3a7a 4040 sbci r20,hlo8(50001) |
6261 3a7c 5040 sbci r21,hhi8(50001) |
6262 3a7e 04F4 brge .+4 |
6263 3a80 0C94 0000 jmp .L574 |
6264 3a84 80E5 ldi r24,lo8(50000) |
6265 3a86 93EC ldi r25,hi8(50000) |
6266 3a88 A0E0 ldi r26,hlo8(50000) |
6267 3a8a B0E0 ldi r27,hhi8(50000) |
6268 3a8c 8093 0000 sts ReadingIntegralGyroYaw,r24 |
6269 3a90 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 |
6270 3a94 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 |
6271 3a98 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 |
6272 3a9c 0C94 0000 jmp .L574 |
6273 .L740: |
6274 3aa0 9B01 movw r18,r22 |
6275 3aa2 77FD sbrc r23,7 |
6276 3aa4 00C0 rjmp .L789 |
6277 .L484: |
6278 3aa6 C901 movw r24,r18 |
6279 3aa8 9595 asr r25 |
6280 3aaa 8795 ror r24 |
6281 3aac 9595 asr r25 |
6282 3aae 8795 ror r24 |
6283 3ab0 9093 0000 sts (MaxStickNick)+1,r25 |
6284 3ab4 8093 0000 sts MaxStickNick,r24 |
6285 3ab8 8536 cpi r24,101 |
6286 3aba 9105 cpc r25,__zero_reg__ |
6287 3abc 04F4 brge .+4 |
6288 3abe 0C94 0000 jmp .L485 |
6289 3ac2 84E6 ldi r24,lo8(100) |
6290 3ac4 90E0 ldi r25,hi8(100) |
6291 3ac6 9093 0000 sts (MaxStickNick)+1,r25 |
6292 3aca 8093 0000 sts MaxStickNick,r24 |
6293 3ace 0C94 0000 jmp .L485 |
6294 .L737: |
6295 3ad2 1092 0000 sts GyroIFactor,__zero_reg__ |
6296 3ad6 0C94 0000 jmp .L480 |
6297 .L778: |
6298 3ada 5C01 movw r10,r24 |
6299 3adc 00C0 rjmp .L669 |
6300 .L788: |
6301 3ade 80E5 ldi r24,lo8(80) |
6302 3ae0 90E0 ldi r25,hi8(80) |
6303 3ae2 9C01 movw r18,r24 |
6304 3ae4 5FEF ldi r21,hi8(-80) |
6305 3ae6 803B cpi r24,lo8(-80) |
6306 3ae8 9507 cpc r25,r21 |
6307 3aea 04F0 brlt .+2 |
6308 3aec 00C0 rjmp .L677 |
6309 3aee 00C0 rjmp .L790 |
6310 .L784: |
6311 3af0 C801 movw r24,r16 |
6312 3af2 00C0 rjmp .L700 |
6313 .L782: |
6314 3af4 3C01 movw r6,r24 |
6315 3af6 00C0 rjmp .L681 |
6316 .L781: |
6317 3af8 9501 movw r18,r10 |
6318 3afa 3595 asr r19 |
6319 3afc 2795 ror r18 |
6320 3afe E216 cp r14,r18 |
6321 3b00 F306 cpc r15,r19 |
6322 3b02 04F0 brlt .+2 |
6323 3b04 00C0 rjmp .L677 |
6324 3b06 9701 movw r18,r14 |
6325 3b08 00C0 rjmp .L677 |
6326 .L779: |
6327 3b0a 1A14 cp __zero_reg__,r10 |
6328 3b0c 1B04 cpc __zero_reg__,r11 |
6329 3b0e 04F0 brlt .+2 |
6330 3b10 00C0 rjmp .L672 |
6331 3b12 81E0 ldi r24,lo8(1) |
6332 3b14 90E0 ldi r25,hi8(1) |
6333 3b16 9093 0000 sts (ModelIsFlying)+1,r25 |
6334 3b1a 8093 0000 sts ModelIsFlying,r24 |
6335 3b1e 8091 0000 lds r24,ParamSet+21 |
6336 3b22 282F mov r18,r24 |
6337 3b24 30E0 ldi r19,lo8(0) |
6338 3b26 00C0 rjmp .L672 |
6339 .L756: |
6340 3b28 81E0 ldi r24,lo8(1) |
6341 3b2a 90E0 ldi r25,hi8(1) |
6342 3b2c 9093 0000 sts (UpdateCompassCourse.2420)+1,r25 |
6343 3b30 8093 0000 sts UpdateCompassCourse.2420,r24 |
6344 3b34 0C94 0000 jmp .L569 |
6345 .L777: |
6346 3b38 9D8D ldd r25,Y+29 |
6347 3b3a 9923 tst r25 |
6348 3b3c 01F0 breq .+2 |
6349 3b3e 00C0 rjmp .L604 |
6350 3b40 8091 0000 lds r24,HoverGas.2445 |
6351 3b44 9091 0000 lds r25,(HoverGas.2445)+1 |
6352 3b48 9093 0000 sts (DebugOut+44)+1,r25 |
6353 3b4c 8093 0000 sts DebugOut+44,r24 |
6354 3b50 8091 0000 lds r24,ReadingVario |
6355 3b54 9091 0000 lds r25,(ReadingVario)+1 |
6356 3b58 9093 0000 sts (DebugOut+38)+1,r25 |
6357 3b5c 8093 0000 sts DebugOut+38,r24 |
6358 3b60 2091 0000 lds r18,BaroExpandActive.2451 |
6359 3b64 3091 0000 lds r19,(BaroExpandActive.2451)+1 |
6360 3b68 2115 cp r18,__zero_reg__ |
6361 3b6a 3105 cpc r19,__zero_reg__ |
6362 3b6c 01F4 brne .+4 |
6363 3b6e 0C94 0000 jmp .L605 |
6364 3b72 8091 0000 lds r24,ReadingHeight |
6365 3b76 9091 0000 lds r25,(ReadingHeight)+1 |
6366 3b7a A091 0000 lds r26,(ReadingHeight)+2 |
6367 3b7e B091 0000 lds r27,(ReadingHeight)+3 |
6368 3b82 880F lsl r24 |
6369 3b84 991F rol r25 |
6370 3b86 AA1F rol r26 |
6371 3b88 BB1F rol r27 |
6372 3b8a 880F lsl r24 |
6373 3b8c 991F rol r25 |
6374 3b8e AA1F rol r26 |
6375 3b90 BB1F rol r27 |
6376 3b92 880F lsl r24 |
6377 3b94 991F rol r25 |
6378 3b96 AA1F rol r26 |
6379 3b98 BB1F rol r27 |
6380 3b9a 880F lsl r24 |
6381 3b9c 991F rol r25 |
6382 3b9e AA1F rol r26 |
6383 3ba0 BB1F rol r27 |
6384 3ba2 8093 0000 sts SumHeight,r24 |
6385 3ba6 9093 0000 sts (SumHeight)+1,r25 |
6386 3baa A093 0000 sts (SumHeight)+2,r26 |
6387 3bae B093 0000 sts (SumHeight)+3,r27 |
6388 3bb2 1092 0000 sts (ReadingVario)+1,__zero_reg__ |
6389 3bb6 1092 0000 sts ReadingVario,__zero_reg__ |
6390 3bba 2150 subi r18,lo8(-(-1)) |
6391 3bbc 3040 sbci r19,hi8(-(-1)) |
6392 3bbe 3093 0000 sts (BaroExpandActive.2451)+1,r19 |
6393 3bc2 2093 0000 sts BaroExpandActive.2451,r18 |
6394 .L606: |
6395 3bc6 8091 0000 lds r24,ParamSet+8 |
6396 3bca 81FF sbrs r24,1 |
6397 3bcc 00C0 rjmp .L611 |
6398 3bce 8091 0000 lds r24,FCParam+1 |
6399 3bd2 8233 cpi r24,lo8(50) |
6400 3bd4 00F0 brlo .+4 |
6401 3bd6 0C94 0000 jmp .L612 |
6402 3bda 8091 0000 lds r24,delay.2449 |
6403 3bde 8150 subi r24,lo8(-(-1)) |
6404 3be0 8093 0000 sts delay.2449,r24 |
6405 3be4 8F5F subi r24,lo8(-(1)) |
6406 3be6 01F0 breq .+2 |
6407 3be8 00C0 rjmp .L613 |
6408 3bea 8091 0000 lds r24,ReadingHeight |
6409 3bee 9091 0000 lds r25,(ReadingHeight)+1 |
6410 3bf2 A091 0000 lds r26,(ReadingHeight)+2 |
6411 3bf6 B091 0000 lds r27,(ReadingHeight)+3 |
6412 3bfa 8093 0000 sts SetPointHeight,r24 |
6413 3bfe 9093 0000 sts (SetPointHeight)+1,r25 |
6414 3c02 A093 0000 sts (SetPointHeight)+2,r26 |
6415 3c06 B093 0000 sts (SetPointHeight)+3,r27 |
6416 3c0a 1092 0000 sts HCActive.2443,__zero_reg__ |
6417 3c0e 81E0 ldi r24,lo8(1) |
6418 3c10 8093 0000 sts delay.2449,r24 |
6419 3c14 00C0 rjmp .L613 |
6420 .L732: |
6421 3c16 0197 sbiw r24,1 |
6422 3c18 9093 0000 sts (RcLostTimer.2415)+1,r25 |
6423 3c1c 8093 0000 sts RcLostTimer.2415,r24 |
6424 3c20 AAE0 ldi r26,lo8(10) |
6425 3c22 BA2E mov r11,r26 |
6426 3c24 DB14 cp r13,r11 |
6427 3c26 01F0 breq .+4 |
6428 3c28 0C94 0000 jmp .L791 |
6429 .L406: |
6430 3c2c 289A sbi 37-32,0 |
6431 3c2e 0C94 0000 jmp .L408 |
6432 .L754: |
6433 3c32 8091 0000 lds r24,LoopingRoll |
6434 3c36 8823 tst r24 |
6435 3c38 01F0 breq .+4 |
6436 3c3a 0C94 0000 jmp .L530 |
6437 3c3e 8091 0000 lds r24,FunnelCourse |
6438 3c42 8823 tst r24 |
6439 3c44 01F0 breq .+4 |
6440 3c46 0C94 0000 jmp .L530 |
6441 3c4a 8091 0000 lds r24,ParamSet+57 |
6442 3c4e 8823 tst r24 |
6443 3c50 01F4 brne .+4 |
6444 3c52 0C94 0000 jmp .L530 |
6445 3c56 8091 0000 lds r24,MeanIntegralGyroNick |
6446 3c5a 9091 0000 lds r25,(MeanIntegralGyroNick)+1 |
6447 3c5e A091 0000 lds r26,(MeanIntegralGyroNick)+2 |
6448 3c62 B091 0000 lds r27,(MeanIntegralGyroNick)+3 |
6449 3c66 BC01 movw r22,r24 |
6450 3c68 CD01 movw r24,r26 |
6451 3c6a 20E0 ldi r18,lo8(256) |
6452 3c6c 31E0 ldi r19,hi8(256) |
6453 3c6e 40E0 ldi r20,hlo8(256) |
6454 3c70 50E0 ldi r21,hhi8(256) |
6455 3c72 0E94 0000 call __divmodsi4 |
6456 3c76 2D8B std Y+21,r18 |
6457 3c78 3E8B std Y+22,r19 |
6458 3c7a 4F8B std Y+23,r20 |
6459 3c7c 588F std Y+24,r21 |
6460 3c7e 28A3 std Y+32,r18 |
6461 3c80 39A3 std Y+33,r19 |
6462 3c82 4AA3 std Y+34,r20 |
6463 3c84 5BA3 std Y+35,r21 |
6464 3c86 2093 0000 sts MeanIntegralGyroNick,r18 |
6465 3c8a 3093 0000 sts (MeanIntegralGyroNick)+1,r19 |
6466 3c8e 4093 0000 sts (MeanIntegralGyroNick)+2,r20 |
6467 3c92 5093 0000 sts (MeanIntegralGyroNick)+3,r21 |
6468 3c96 2090 0000 lds r2,MeanIntegralGyroRoll |
6469 3c9a 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 |
6470 3c9e 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 |
6471 3ca2 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 |
6472 3ca6 C201 movw r24,r4 |
6473 3ca8 B101 movw r22,r2 |
6474 3caa 20E0 ldi r18,lo8(256) |
6475 3cac 31E0 ldi r19,hi8(256) |
6476 3cae 40E0 ldi r20,hlo8(256) |
6477 3cb0 50E0 ldi r21,hhi8(256) |
6478 3cb2 0E94 0000 call __divmodsi4 |
6479 3cb6 298F std Y+25,r18 |
6480 3cb8 3A8F std Y+26,r19 |
6481 3cba 4B8F std Y+27,r20 |
6482 3cbc 5C8F std Y+28,r21 |
6483 3cbe 1901 movw r2,r18 |
6484 3cc0 2A01 movw r4,r20 |
6485 3cc2 2093 0000 sts MeanIntegralGyroRoll,r18 |
6486 3cc6 3093 0000 sts (MeanIntegralGyroRoll)+1,r19 |
6487 3cca 4093 0000 sts (MeanIntegralGyroRoll)+2,r20 |
6488 3cce 5093 0000 sts (MeanIntegralGyroRoll)+3,r21 |
6489 3cd2 E090 0000 lds r14,ParamSet+23 |
6490 3cd6 FF24 clr r15 |
6491 3cd8 00E0 ldi r16,lo8(0) |
6492 3cda 10E0 ldi r17,hi8(0) |
6493 3cdc 2091 0000 lds r18,MeanAccNick |
6494 3ce0 3091 0000 lds r19,(MeanAccNick)+1 |
6495 3ce4 4091 0000 lds r20,(MeanAccNick)+2 |
6496 3ce8 5091 0000 lds r21,(MeanAccNick)+3 |
6497 3cec C801 movw r24,r16 |
6498 3cee B701 movw r22,r14 |
6499 3cf0 0E94 0000 call __mulsi3 |
6500 3cf4 20E0 ldi r18,lo8(256) |
6501 3cf6 31E0 ldi r19,hi8(256) |
6502 3cf8 40E0 ldi r20,hlo8(256) |
6503 3cfa 50E0 ldi r21,hhi8(256) |
6504 3cfc 0E94 0000 call __divmodsi4 |
6505 3d00 5901 movw r10,r18 |
6506 3d02 6A01 movw r12,r20 |
6507 3d04 2093 0000 sts MeanAccNick,r18 |
6508 3d08 3093 0000 sts (MeanAccNick)+1,r19 |
6509 3d0c 4093 0000 sts (MeanAccNick)+2,r20 |
6510 3d10 5093 0000 sts (MeanAccNick)+3,r21 |
6511 3d14 2091 0000 lds r18,MeanAccRoll |
6512 3d18 3091 0000 lds r19,(MeanAccRoll)+1 |
6513 3d1c 4091 0000 lds r20,(MeanAccRoll)+2 |
6514 3d20 5091 0000 lds r21,(MeanAccRoll)+3 |
6515 3d24 C801 movw r24,r16 |
6516 3d26 B701 movw r22,r14 |
6517 3d28 0E94 0000 call __mulsi3 |
6518 3d2c 20E0 ldi r18,lo8(256) |
6519 3d2e 31E0 ldi r19,hi8(256) |
6520 3d30 40E0 ldi r20,hlo8(256) |
6521 3d32 50E0 ldi r21,hhi8(256) |
6522 3d34 0E94 0000 call __divmodsi4 |
6523 3d38 3901 movw r6,r18 |
6524 3d3a 4A01 movw r8,r20 |
6525 3d3c 2093 0000 sts MeanAccRoll,r18 |
6526 3d40 3093 0000 sts (MeanAccRoll)+1,r19 |
6527 3d44 4093 0000 sts (MeanAccRoll)+2,r20 |
6528 3d48 5093 0000 sts (MeanAccRoll)+3,r21 |
6529 3d4c 6D89 ldd r22,Y+21 |
6530 3d4e 7E89 ldd r23,Y+22 |
6531 3d50 8F89 ldd r24,Y+23 |
6532 3d52 988D ldd r25,Y+24 |
6533 3d54 6A19 sub r22,r10 |
6534 3d56 7B09 sbc r23,r11 |
6535 3d58 8C09 sbc r24,r12 |
6536 3d5a 9D09 sbc r25,r13 |
6537 3d5c 6093 0000 sts IntegralGyroNickError.2411,r22 |
6538 3d60 7093 0000 sts (IntegralGyroNickError.2411)+1,r23 |
6539 3d64 8093 0000 sts (IntegralGyroNickError.2411)+2,r24 |
6540 3d68 9093 0000 sts (IntegralGyroNickError.2411)+3,r25 |
6541 3d6c E090 0000 lds r14,ParamSet+56 |
6542 3d70 FF24 clr r15 |
6543 3d72 00E0 ldi r16,lo8(0) |
6544 3d74 10E0 ldi r17,hi8(0) |
6545 3d76 A801 movw r20,r16 |
6546 3d78 9701 movw r18,r14 |
6547 3d7a 0E94 0000 call __divmodsi4 |
6548 3d7e CA01 movw r24,r20 |
6549 3d80 2093 0000 sts CorrectionNick.2413,r18 |
6550 3d84 3093 0000 sts CorrectionNick.2413+1,r19 |
6551 3d88 4093 0000 sts CorrectionNick.2413+2,r20 |
6552 3d8c 9093 0000 sts CorrectionNick.2413+3,r25 |
6553 3d90 B901 movw r22,r18 |
6554 3d92 20E0 ldi r18,lo8(256) |
6555 3d94 31E0 ldi r19,hi8(256) |
6556 3d96 40E0 ldi r20,hlo8(256) |
6557 3d98 50E0 ldi r21,hhi8(256) |
6558 3d9a 0E94 0000 call __divmodsi4 |
6559 3d9e 3093 0000 sts (AttitudeCorrectionNick)+1,r19 |
6560 3da2 2093 0000 sts AttitudeCorrectionNick,r18 |
6561 3da6 C201 movw r24,r4 |
6562 3da8 B101 movw r22,r2 |
6563 3daa 6619 sub r22,r6 |
6564 3dac 7709 sbc r23,r7 |
6565 3dae 8809 sbc r24,r8 |
6566 3db0 9909 sbc r25,r9 |
6567 3db2 6093 0000 sts IntegralGyroRollError.2412,r22 |
6568 3db6 7093 0000 sts (IntegralGyroRollError.2412)+1,r23 |
6569 3dba 8093 0000 sts (IntegralGyroRollError.2412)+2,r24 |
6570 3dbe 9093 0000 sts (IntegralGyroRollError.2412)+3,r25 |
6571 3dc2 A801 movw r20,r16 |
6572 3dc4 9701 movw r18,r14 |
6573 3dc6 0E94 0000 call __divmodsi4 |
6574 3dca CA01 movw r24,r20 |
6575 3dcc 2093 0000 sts CorrectionRoll.2414,r18 |
6576 3dd0 3093 0000 sts CorrectionRoll.2414+1,r19 |
6577 3dd4 4093 0000 sts CorrectionRoll.2414+2,r20 |
6578 3dd8 9093 0000 sts CorrectionRoll.2414+3,r25 |
6579 3ddc B901 movw r22,r18 |
6580 3dde 20E0 ldi r18,lo8(256) |
6581 3de0 31E0 ldi r19,hi8(256) |
6582 3de2 40E0 ldi r20,hlo8(256) |
6583 3de4 50E0 ldi r21,hhi8(256) |
6584 3de6 0E94 0000 call __divmodsi4 |
6585 3dea 3093 0000 sts (AttitudeCorrectionRoll)+1,r19 |
6586 3dee 2093 0000 sts AttitudeCorrectionRoll,r18 |
6587 3df2 8091 0000 lds r24,MaxStickNick |
6588 3df6 9091 0000 lds r25,(MaxStickNick)+1 |
6589 3dfa 8134 cpi r24,65 |
6590 3dfc 9105 cpc r25,__zero_reg__ |
6591 3dfe 04F4 brge .L531 |
6592 3e00 8091 0000 lds r24,MaxStickRoll |
6593 3e04 9091 0000 lds r25,(MaxStickRoll)+1 |
6594 3e08 8134 cpi r24,65 |
6595 3e0a 9105 cpc r25,__zero_reg__ |
6596 3e0c 04F4 brge .L531 |
6597 3e0e E091 0000 lds r30,ParamSet+3 |
6598 3e12 F0E0 ldi r31,lo8(0) |
6599 3e14 EE0F lsl r30 |
6600 3e16 FF1F rol r31 |
6601 3e18 E050 subi r30,lo8(-(PPM_in)) |
6602 3e1a F040 sbci r31,hi8(-(PPM_in)) |
6603 3e1c 0190 ld __tmp_reg__,Z+ |
6604 3e1e F081 ld r31,Z |
6605 3e20 E02D mov r30,__tmp_reg__ |
6606 3e22 F7FF sbrs r31,7 |
6607 3e24 00C0 rjmp .+4 |
6608 3e26 0C94 0000 jmp .L792 |
6609 .L533: |
6610 3e2a 7A97 sbiw r30,26 |
6611 3e2c 04F0 brlt .L532 |
6612 .L531: |
6613 3e2e 8091 0000 lds r24,FCParam+31 |
6614 3e32 8F3F cpi r24,lo8(-1) |
6615 3e34 01F4 brne .+4 |
6616 3e36 0C94 0000 jmp .L793 |
6617 .L532: |
6618 3e3a 2091 0000 lds r18,IntegralGyroNick2 |
6619 3e3e 3091 0000 lds r19,(IntegralGyroNick2)+1 |
6620 3e42 4091 0000 lds r20,(IntegralGyroNick2)+2 |
6621 3e46 5091 0000 lds r21,(IntegralGyroNick2)+3 |
6622 3e4a A8A4 ldd r10,Y+40 |
6623 3e4c B9A4 ldd r11,Y+41 |
6624 3e4e CAA4 ldd r12,Y+42 |
6625 3e50 DBA4 ldd r13,Y+43 |
6626 3e52 2A19 sub r18,r10 |
6627 3e54 3B09 sbc r19,r11 |
6628 3e56 4C09 sbc r20,r12 |
6629 3e58 5D09 sbc r21,r13 |
6630 3e5a 2093 0000 sts IntegralGyroNickError.2411,r18 |
6631 3e5e 3093 0000 sts (IntegralGyroNickError.2411)+1,r19 |
6632 3e62 4093 0000 sts (IntegralGyroNickError.2411)+2,r20 |
6633 3e66 5093 0000 sts (IntegralGyroNickError.2411)+3,r21 |
6634 3e6a 8091 0000 lds r24,ReadingIntegralGyroNick2 |
6635 3e6e 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 |
6636 3e72 A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 |
6637 3e76 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 |
6638 3e7a 821B sub r24,r18 |
6639 3e7c 930B sbc r25,r19 |
6640 3e7e A40B sbc r26,r20 |
6641 3e80 B50B sbc r27,r21 |
6642 3e82 8093 0000 sts ReadingIntegralGyroNick2,r24 |
6643 3e86 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
6644 3e8a A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
6645 3e8e B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
6646 3e92 2091 0000 lds r18,IntegralGyroRoll2 |
6647 3e96 3091 0000 lds r19,(IntegralGyroRoll2)+1 |
6648 3e9a 4091 0000 lds r20,(IntegralGyroRoll2)+2 |
6649 3e9e 5091 0000 lds r21,(IntegralGyroRoll2)+3 |
6650 3ea2 6CA1 ldd r22,Y+36 |
6651 3ea4 7DA1 ldd r23,Y+37 |
6652 3ea6 8EA1 ldd r24,Y+38 |
6653 3ea8 9FA1 ldd r25,Y+39 |
6654 3eaa 261B sub r18,r22 |
6655 3eac 370B sbc r19,r23 |
6656 3eae 480B sbc r20,r24 |
6657 3eb0 590B sbc r21,r25 |
6658 3eb2 2093 0000 sts IntegralGyroRollError.2412,r18 |
6659 3eb6 3093 0000 sts (IntegralGyroRollError.2412)+1,r19 |
6660 3eba 4093 0000 sts (IntegralGyroRollError.2412)+2,r20 |
6661 3ebe 5093 0000 sts (IntegralGyroRollError.2412)+3,r21 |
6662 3ec2 8091 0000 lds r24,ReadingIntegralGyroRoll2 |
6663 3ec6 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 |
6664 3eca A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 |
6665 3ece B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 |
6666 3ed2 821B sub r24,r18 |
6667 3ed4 930B sbc r25,r19 |
6668 3ed6 A40B sbc r26,r20 |
6669 3ed8 B50B sbc r27,r21 |
6670 3eda 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
6671 3ede 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
6672 3ee2 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
6673 3ee6 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
6674 3eea 8091 0000 lds r24,ParamSet+57 |
6675 3eee 8823 tst r24 |
6676 3ef0 01F0 breq .L536 |
6677 3ef2 2091 0000 lds r18,YawGyroDrift |
6678 3ef6 3091 0000 lds r19,(YawGyroDrift)+1 |
6679 3efa 2138 cpi r18,129 |
6680 3efc 3105 cpc r19,__zero_reg__ |
6681 3efe 04F0 brlt .L537 |
6682 3f00 8091 0000 lds r24,AdBiasGyroYaw |
6683 3f04 9091 0000 lds r25,(AdBiasGyroYaw)+1 |
6684 3f08 0196 adiw r24,1 |
6685 3f0a 9093 0000 sts (AdBiasGyroYaw)+1,r25 |
6686 3f0e 8093 0000 sts AdBiasGyroYaw,r24 |
6687 .L537: |
6688 3f12 2058 subi r18,lo8(-128) |
6689 3f14 3F4F sbci r19,hi8(-128) |
6690 3f16 04F4 brge .L536 |
6691 3f18 8091 0000 lds r24,AdBiasGyroYaw |
6692 3f1c 9091 0000 lds r25,(AdBiasGyroYaw)+1 |
6693 3f20 0197 sbiw r24,1 |
6694 3f22 9093 0000 sts (AdBiasGyroYaw)+1,r25 |
6695 3f26 8093 0000 sts AdBiasGyroYaw,r24 |
6696 .L536: |
6697 3f2a 1092 0000 sts (YawGyroDrift)+1,__zero_reg__ |
6698 3f2e 1092 0000 sts YawGyroDrift,__zero_reg__ |
6699 3f32 81E0 ldi r24,lo8(1) |
6700 3f34 90E0 ldi r25,hi8(1) |
6701 3f36 9093 0000 sts (cnt.2426)+1,r25 |
6702 3f3a 8093 0000 sts cnt.2426,r24 |
6703 3f3e E090 0000 lds r14,IntegralGyroNickError.2411 |
6704 3f42 F090 0000 lds r15,(IntegralGyroNickError.2411)+1 |
6705 3f46 0091 0000 lds r16,(IntegralGyroNickError.2411)+2 |
6706 3f4a 1091 0000 lds r17,(IntegralGyroNickError.2411)+3 |
6707 3f4e 81E0 ldi r24,lo8(513) |
6708 3f50 E816 cp r14,r24 |
6709 3f52 82E0 ldi r24,hi8(513) |
6710 3f54 F806 cpc r15,r24 |
6711 3f56 80E0 ldi r24,hlo8(513) |
6712 3f58 0807 cpc r16,r24 |
6713 3f5a 80E0 ldi r24,hhi8(513) |
6714 3f5c 1807 cpc r17,r24 |
6715 3f5e 04F0 brlt .L538 |
6716 3f60 84E0 ldi r24,lo8(4) |
6717 3f62 90E0 ldi r25,hi8(4) |
6718 3f64 9093 0000 sts (cnt.2426)+1,r25 |
6719 3f68 8093 0000 sts cnt.2426,r24 |
6720 .L538: |
6721 3f6c 1092 0000 sts CorrectionNick.2413,__zero_reg__ |
6722 3f70 1092 0000 sts (CorrectionNick.2413)+1,__zero_reg__ |
6723 3f74 1092 0000 sts (CorrectionNick.2413)+2,__zero_reg__ |
6724 3f78 1092 0000 sts (CorrectionNick.2413)+3,__zero_reg__ |
6725 3f7c 8091 0000 lds r24,MeanIntegralGyroNick_old.2431 |
6726 3f80 9091 0000 lds r25,(MeanIntegralGyroNick_old.2431)+1 |
6727 3f84 A091 0000 lds r26,(MeanIntegralGyroNick_old.2431)+2 |
6728 3f88 B091 0000 lds r27,(MeanIntegralGyroNick_old.2431)+3 |
6729 3f8c AD88 ldd r10,Y+21 |
6730 3f8e BE88 ldd r11,Y+22 |
6731 3f90 CF88 ldd r12,Y+23 |
6732 3f92 D88C ldd r13,Y+24 |
6733 3f94 8A19 sub r24,r10 |
6734 3f96 9B09 sbc r25,r11 |
6735 3f98 AC09 sbc r26,r12 |
6736 3f9a BD09 sbc r27,r13 |
6737 3f9c 815E subi r24,lo8(-(19999)) |
6738 3f9e 914B sbci r25,hi8(-(19999)) |
6739 3fa0 AF4F sbci r26,hlo8(-(19999)) |
6740 3fa2 BF4F sbci r27,hhi8(-(19999)) |
6741 3fa4 8F53 subi r24,lo8(39999) |
6742 3fa6 9C49 sbci r25,hi8(39999) |
6743 3fa8 A040 sbci r26,hlo8(39999) |
6744 3faa B040 sbci r27,hhi8(39999) |
6745 3fac 00F4 brsh .+4 |
6746 3fae 0C94 0000 jmp .L539 |
6747 3fb2 8091 0000 lds r24,FCParam+32 |
6748 3fb6 8931 cpi r24,lo8(25) |
6749 3fb8 04F0 brlt .+4 |
6750 3fba 0C94 0000 jmp .L539 |
6751 3fbe 1092 0000 sts (cnt.2426)+1,__zero_reg__ |
6752 3fc2 1092 0000 sts cnt.2426,__zero_reg__ |
6753 3fc6 88EE ldi r24,lo8(1000) |
6754 3fc8 93E0 ldi r25,hi8(1000) |
6755 3fca 9093 0000 sts (BadCompassHeading)+1,r25 |
6756 3fce 8093 0000 sts BadCompassHeading,r24 |
6757 3fd2 AA24 clr r10 |
6758 3fd4 BB24 clr r11 |
6759 .L548: |
6760 3fd6 8091 0000 lds r24,ParamSet+57 |
6761 3fda 90E0 ldi r25,lo8(0) |
6762 3fdc 8A15 cp r24,r10 |
6763 3fde 9B05 cpc r25,r11 |
6764 3fe0 04F4 brge .L549 |
6765 3fe2 9093 0000 sts (cnt.2426)+1,r25 |
6766 3fe6 8093 0000 sts cnt.2426,r24 |
6767 .L549: |
6768 3fea D090 0000 lds r13,FCParam+32 |
6769 3fee DD20 tst r13 |
6770 3ff0 01F0 breq .L550 |
6771 3ff2 2D2D mov r18,r13 |
6772 3ff4 3327 clr r19 |
6773 3ff6 27FD sbrc r18,7 |
6774 3ff8 3095 com r19 |
6775 3ffa 8091 0000 lds r24,cnt.2426 |
6776 3ffe 9091 0000 lds r25,(cnt.2426)+1 |
6777 4002 2817 cp r18,r24 |
6778 4004 3907 cpc r19,r25 |
6779 4006 04F4 brge .L550 |
6780 4008 3093 0000 sts (cnt.2426)+1,r19 |
6781 400c 2093 0000 sts cnt.2426,r18 |
6782 .L550: |
6783 4010 71E8 ldi r23,lo8(129) |
6784 4012 E716 cp r14,r23 |
6785 4014 F104 cpc r15,__zero_reg__ |
6786 4016 0105 cpc r16,__zero_reg__ |
6787 4018 1105 cpc r17,__zero_reg__ |
6788 401a 04F0 brlt .L551 |
6789 401c 8091 0000 lds r24,BiasHiResGyroNick |
6790 4020 9091 0000 lds r25,(BiasHiResGyroNick)+1 |
6791 4024 2091 0000 lds r18,cnt.2426 |
6792 4028 3091 0000 lds r19,(cnt.2426)+1 |
6793 402c 820F add r24,r18 |
6794 402e 931F adc r25,r19 |
6795 4030 9093 0000 sts (BiasHiResGyroNick)+1,r25 |
6796 4034 8093 0000 sts BiasHiResGyroNick,r24 |
6797 .L551: |
6798 4038 80E8 ldi r24,lo8(-128) |
6799 403a E816 cp r14,r24 |
6800 403c 8FEF ldi r24,hi8(-128) |
6801 403e F806 cpc r15,r24 |
6802 4040 8FEF ldi r24,hlo8(-128) |
6803 4042 0807 cpc r16,r24 |
6804 4044 8FEF ldi r24,hhi8(-128) |
6805 4046 1807 cpc r17,r24 |
6806 4048 04F4 brge .L552 |
6807 404a 8091 0000 lds r24,BiasHiResGyroNick |
6808 404e 9091 0000 lds r25,(BiasHiResGyroNick)+1 |
6809 4052 2091 0000 lds r18,cnt.2426 |
6810 4056 3091 0000 lds r19,(cnt.2426)+1 |
6811 405a 821B sub r24,r18 |
6812 405c 930B sbc r25,r19 |
6813 405e 9093 0000 sts (BiasHiResGyroNick)+1,r25 |
6814 4062 8093 0000 sts BiasHiResGyroNick,r24 |
6815 .L552: |
6816 4066 81E0 ldi r24,lo8(1) |
6817 4068 90E0 ldi r25,hi8(1) |
6818 406a 9093 0000 sts (cnt.2426)+1,r25 |
6819 406e 8093 0000 sts cnt.2426,r24 |
6820 4072 E090 0000 lds r14,IntegralGyroRollError.2412 |
6821 4076 F090 0000 lds r15,(IntegralGyroRollError.2412)+1 |
6822 407a 0091 0000 lds r16,(IntegralGyroRollError.2412)+2 |
6823 407e 1091 0000 lds r17,(IntegralGyroRollError.2412)+3 |
6824 4082 91E0 ldi r25,lo8(513) |
6825 4084 E916 cp r14,r25 |
6826 4086 92E0 ldi r25,hi8(513) |
6827 4088 F906 cpc r15,r25 |
6828 408a 90E0 ldi r25,hlo8(513) |
6829 408c 0907 cpc r16,r25 |
6830 408e 90E0 ldi r25,hhi8(513) |
6831 4090 1907 cpc r17,r25 |
6832 4092 04F0 brlt .L553 |
6833 4094 84E0 ldi r24,lo8(4) |
6834 4096 90E0 ldi r25,hi8(4) |
6835 4098 9093 0000 sts (cnt.2426)+1,r25 |
6836 409c 8093 0000 sts cnt.2426,r24 |
6837 .L553: |
6838 40a0 1092 0000 sts CorrectionRoll.2414,__zero_reg__ |
6839 40a4 1092 0000 sts (CorrectionRoll.2414)+1,__zero_reg__ |
6840 40a8 1092 0000 sts (CorrectionRoll.2414)+2,__zero_reg__ |
6841 40ac 1092 0000 sts (CorrectionRoll.2414)+3,__zero_reg__ |
6842 40b0 8091 0000 lds r24,MeanIntegralGyroRoll_old.2432 |
6843 40b4 9091 0000 lds r25,(MeanIntegralGyroRoll_old.2432)+1 |
6844 40b8 A091 0000 lds r26,(MeanIntegralGyroRoll_old.2432)+2 |
6845 40bc B091 0000 lds r27,(MeanIntegralGyroRoll_old.2432)+3 |
6846 40c0 298D ldd r18,Y+25 |
6847 40c2 3A8D ldd r19,Y+26 |
6848 40c4 4B8D ldd r20,Y+27 |
6849 40c6 5C8D ldd r21,Y+28 |
6850 40c8 821B sub r24,r18 |
6851 40ca 930B sbc r25,r19 |
6852 40cc A40B sbc r26,r20 |
6853 40ce B50B sbc r27,r21 |
6854 40d0 815E subi r24,lo8(-(19999)) |
6855 40d2 914B sbci r25,hi8(-(19999)) |
6856 40d4 AF4F sbci r26,hlo8(-(19999)) |
6857 40d6 BF4F sbci r27,hhi8(-(19999)) |
6858 40d8 8F53 subi r24,lo8(39999) |
6859 40da 9C49 sbci r25,hi8(39999) |
6860 40dc A040 sbci r26,hlo8(39999) |
6861 40de B040 sbci r27,hhi8(39999) |
6862 40e0 00F4 brsh .+4 |
6863 40e2 0C94 0000 jmp .L554 |
6864 40e6 48E1 ldi r20,lo8(24) |
6865 40e8 4D15 cp r20,r13 |
6866 40ea 04F4 brge .+4 |
6867 40ec 0C94 0000 jmp .L554 |
6868 40f0 1092 0000 sts (cnt.2426)+1,__zero_reg__ |
6869 40f4 1092 0000 sts cnt.2426,__zero_reg__ |
6870 40f8 88EE ldi r24,lo8(1000) |
6871 40fa 93E0 ldi r25,hi8(1000) |
6872 40fc 9093 0000 sts (BadCompassHeading)+1,r25 |
6873 4100 8093 0000 sts BadCompassHeading,r24 |
6874 4104 AA24 clr r10 |
6875 4106 BB24 clr r11 |
6876 .L563: |
6877 4108 8091 0000 lds r24,ParamSet+57 |
6878 410c 90E0 ldi r25,lo8(0) |
6879 410e 8A15 cp r24,r10 |
6880 4110 9B05 cpc r25,r11 |
6881 4112 04F4 brge .L564 |
6882 4114 9093 0000 sts (cnt.2426)+1,r25 |
6883 4118 8093 0000 sts cnt.2426,r24 |
6884 .L564: |
6885 411c DD20 tst r13 |
6886 411e 01F0 breq .L565 |
6887 4120 2D2D mov r18,r13 |
6888 4122 3327 clr r19 |
6889 4124 27FD sbrc r18,7 |
6890 4126 3095 com r19 |
6891 4128 8091 0000 lds r24,cnt.2426 |
6892 412c 9091 0000 lds r25,(cnt.2426)+1 |
6893 4130 2817 cp r18,r24 |
6894 4132 3907 cpc r19,r25 |
6895 4134 04F4 brge .L565 |
6896 4136 3093 0000 sts (cnt.2426)+1,r19 |
6897 413a 2093 0000 sts cnt.2426,r18 |
6898 .L565: |
6899 413e F1E8 ldi r31,lo8(129) |
6900 4140 EF16 cp r14,r31 |
6901 4142 F104 cpc r15,__zero_reg__ |
6902 4144 0105 cpc r16,__zero_reg__ |
6903 4146 1105 cpc r17,__zero_reg__ |
6904 4148 04F0 brlt .L566 |
6905 414a 8091 0000 lds r24,BiasHiResGyroRoll |
6906 414e 9091 0000 lds r25,(BiasHiResGyroRoll)+1 |
6907 4152 2091 0000 lds r18,cnt.2426 |
6908 4156 3091 0000 lds r19,(cnt.2426)+1 |
6909 415a 820F add r24,r18 |
6910 415c 931F adc r25,r19 |
6911 415e 9093 0000 sts (BiasHiResGyroRoll)+1,r25 |
6912 4162 8093 0000 sts BiasHiResGyroRoll,r24 |
6913 .L566: |
6914 4166 20E8 ldi r18,lo8(-128) |
6915 4168 E216 cp r14,r18 |
6916 416a 2FEF ldi r18,hi8(-128) |
6917 416c F206 cpc r15,r18 |
6918 416e 2FEF ldi r18,hlo8(-128) |
6919 4170 0207 cpc r16,r18 |
6920 4172 2FEF ldi r18,hhi8(-128) |
6921 4174 1207 cpc r17,r18 |
6922 4176 04F0 brlt .+4 |
6923 4178 0C94 0000 jmp .L567 |
6924 417c 8091 0000 lds r24,BiasHiResGyroRoll |
6925 4180 9091 0000 lds r25,(BiasHiResGyroRoll)+1 |
6926 4184 2091 0000 lds r18,cnt.2426 |
6927 4188 3091 0000 lds r19,(cnt.2426)+1 |
6928 418c 821B sub r24,r18 |
6929 418e 930B sbc r25,r19 |
6930 4190 9093 0000 sts (BiasHiResGyroRoll)+1,r25 |
6931 4194 8093 0000 sts BiasHiResGyroRoll,r24 |
6932 4198 0C94 0000 jmp .L567 |
6933 .L753: |
6934 419c 8091 0000 lds r24,LoopingRoll |
6935 41a0 8823 tst r24 |
6936 41a2 01F0 breq .+4 |
6937 41a4 0C94 0000 jmp .L510 |
6938 .L509: |
6939 41a8 8091 0000 lds r24,AdValueAccZ |
6940 41ac 9091 0000 lds r25,(AdValueAccZ)+1 |
6941 41b0 8150 subi r24,lo8(513) |
6942 41b2 9240 sbci r25,hi8(513) |
6943 41b4 04F4 brge .+2 |
6944 41b6 00C0 rjmp .L794 |
6945 .L511: |
6946 41b8 8091 0000 lds r24,FCParam+31 |
6947 41bc 8F3F cpi r24,lo8(-1) |
6948 41be 01F4 brne .+2 |
6949 41c0 00C0 rjmp .L512 |
6950 41c2 E090 0000 lds r14,ParamSet+23 |
6951 41c6 FF24 clr r15 |
6952 41c8 00E0 ldi r16,lo8(0) |
6953 41ca 10E0 ldi r17,hi8(0) |
6954 41cc A82E mov r10,r24 |
6955 41ce BB24 clr r11 |
6956 41d0 A7FC sbrc r10,7 |
6957 41d2 B094 com r11 |
6958 41d4 CB2C mov r12,r11 |
6959 41d6 DB2C mov r13,r11 |
6960 41d8 68A5 ldd r22,Y+40 |
6961 41da 79A5 ldd r23,Y+41 |
6962 41dc 8AA5 ldd r24,Y+42 |
6963 41de 9BA5 ldd r25,Y+43 |
6964 41e0 A801 movw r20,r16 |
6965 41e2 9701 movw r18,r14 |
6966 41e4 0E94 0000 call __divmodsi4 |
6967 41e8 8091 0000 lds r24,AccNick |
6968 41ec 9091 0000 lds r25,(AccNick)+1 |
6969 41f0 AA27 clr r26 |
6970 41f2 97FD sbrc r25,7 |
6971 41f4 A095 com r26 |
6972 41f6 BA2F mov r27,r26 |
6973 41f8 281B sub r18,r24 |
6974 41fa 390B sbc r19,r25 |
6975 41fc 4A0B sbc r20,r26 |
6976 41fe 5B0B sbc r21,r27 |
6977 4200 CA01 movw r24,r20 |
6978 4202 B901 movw r22,r18 |
6979 4204 A601 movw r20,r12 |
6980 4206 9501 movw r18,r10 |
6981 4208 0E94 0000 call __mulsi3 |
6982 420c 20E0 ldi r18,lo8(512) |
6983 420e 32E0 ldi r19,hi8(512) |
6984 4210 40E0 ldi r20,hlo8(512) |
6985 4212 50E0 ldi r21,hhi8(512) |
6986 4214 0E94 0000 call __divmodsi4 |
6987 4218 522E mov r5,r18 |
6988 421a 432E mov r4,r19 |
6989 421c 342E mov r3,r20 |
6990 421e 252E mov r2,r21 |
6991 4220 822F mov r24,r18 |
6992 4222 932F mov r25,r19 |
6993 4224 A42F mov r26,r20 |
6994 4226 B52F mov r27,r21 |
6995 4228 3C01 movw r6,r24 |
6996 422a 4D01 movw r8,r26 |
6997 422c 6CA1 ldd r22,Y+36 |
6998 422e 7DA1 ldd r23,Y+37 |
6999 4230 8EA1 ldd r24,Y+38 |
7000 4232 9FA1 ldd r25,Y+39 |
7001 4234 A801 movw r20,r16 |
7002 4236 9701 movw r18,r14 |
7003 4238 0E94 0000 call __divmodsi4 |
7004 423c 8091 0000 lds r24,AccRoll |
7005 4240 9091 0000 lds r25,(AccRoll)+1 |
7006 4244 AA27 clr r26 |
7007 4246 97FD sbrc r25,7 |
7008 4248 A095 com r26 |
7009 424a BA2F mov r27,r26 |
7010 424c 281B sub r18,r24 |
7011 424e 390B sbc r19,r25 |
7012 4250 4A0B sbc r20,r26 |
7013 4252 5B0B sbc r21,r27 |
7014 4254 CA01 movw r24,r20 |
7015 4256 B901 movw r22,r18 |
7016 4258 A601 movw r20,r12 |
7017 425a 9501 movw r18,r10 |
7018 425c 0E94 0000 call __mulsi3 |
7019 4260 20E0 ldi r18,lo8(512) |
7020 4262 32E0 ldi r19,hi8(512) |
7021 4264 40E0 ldi r20,hlo8(512) |
7022 4266 50E0 ldi r21,hhi8(512) |
7023 4268 0E94 0000 call __divmodsi4 |
7024 426c 122F mov r17,r18 |
7025 426e 032F mov r16,r19 |
7026 4270 F42E mov r15,r20 |
7027 4272 E52E mov r14,r21 |
7028 4274 822F mov r24,r18 |
7029 4276 932F mov r25,r19 |
7030 4278 A42F mov r26,r20 |
7031 427a B52F mov r27,r21 |
7032 427c 5C01 movw r10,r24 |
7033 427e 6D01 movw r12,r26 |
7034 4280 8091 0000 lds r24,MaxStickNick |
7035 4284 9091 0000 lds r25,(MaxStickNick)+1 |
7036 4288 8134 cpi r24,65 |
7037 428a 9105 cpc r25,__zero_reg__ |
7038 428c 04F0 brlt .+2 |
7039 428e 00C0 rjmp .L513 |
7040 4290 8091 0000 lds r24,MaxStickRoll |
7041 4294 9091 0000 lds r25,(MaxStickRoll)+1 |
7042 4298 8134 cpi r24,65 |
7043 429a 9105 cpc r25,__zero_reg__ |
7044 429c 04F0 brlt .+2 |
7045 429e 00C0 rjmp .L513 |
7046 .L514: |
7047 42a0 E091 0000 lds r30,ParamSet+3 |
7048 42a4 F0E0 ldi r31,lo8(0) |
7049 42a6 EE0F lsl r30 |
7050 42a8 FF1F rol r31 |
7051 42aa E050 subi r30,lo8(-(PPM_in)) |
7052 42ac F040 sbci r31,hi8(-(PPM_in)) |
7053 42ae 0190 ld __tmp_reg__,Z+ |
7054 42b0 F081 ld r31,Z |
7055 42b2 E02D mov r30,__tmp_reg__ |
7056 42b4 F7FF sbrs r31,7 |
7057 42b6 00C0 rjmp .+4 |
7058 42b8 0C94 0000 jmp .L795 |
7059 .L516: |
7060 42bc 7A97 sbiw r30,26 |
7061 42be 04F0 brlt .L515 |
7062 42c0 C401 movw r24,r8 |
7063 42c2 B301 movw r22,r6 |
7064 42c4 23E0 ldi r18,lo8(3) |
7065 42c6 30E0 ldi r19,hi8(3) |
7066 42c8 40E0 ldi r20,hlo8(3) |
7067 42ca 50E0 ldi r21,hhi8(3) |
7068 42cc 0E94 0000 call __divmodsi4 |
7069 42d0 C901 movw r24,r18 |
7070 42d2 DA01 movw r26,r20 |
7071 42d4 3C01 movw r6,r24 |
7072 42d6 4D01 movw r8,r26 |
7073 42d8 C601 movw r24,r12 |
7074 42da B501 movw r22,r10 |
7075 42dc 23E0 ldi r18,lo8(3) |
7076 42de 30E0 ldi r19,hi8(3) |
7077 42e0 40E0 ldi r20,hlo8(3) |
7078 42e2 50E0 ldi r21,hhi8(3) |
7079 42e4 0E94 0000 call __divmodsi4 |
7080 42e8 C901 movw r24,r18 |
7081 42ea DA01 movw r26,r20 |
7082 42ec 5C01 movw r10,r24 |
7083 42ee 6D01 movw r12,r26 |
7084 .L515: |
7085 42f0 8091 0000 lds r24,FCParam+33 |
7086 42f4 9927 clr r25 |
7087 42f6 87FD sbrc r24,7 |
7088 42f8 9095 com r25 |
7089 42fa A92F mov r26,r25 |
7090 42fc B92F mov r27,r25 |
7091 42fe 2227 clr r18 |
7092 4300 3327 clr r19 |
7093 4302 A901 movw r20,r18 |
7094 4304 281B sub r18,r24 |
7095 4306 390B sbc r19,r25 |
7096 4308 4A0B sbc r20,r26 |
7097 430a 5B0B sbc r21,r27 |
7098 430c 6216 cp r6,r18 |
7099 430e 7306 cpc r7,r19 |
7100 4310 8406 cpc r8,r20 |
7101 4312 9506 cpc r9,r21 |
7102 4314 04F0 brlt .+2 |
7103 4316 00C0 rjmp .L517 |
7104 4318 7901 movw r14,r18 |
7105 431a 8A01 movw r16,r20 |
7106 .L518: |
7107 431c A216 cp r10,r18 |
7108 431e B306 cpc r11,r19 |
7109 4320 C406 cpc r12,r20 |
7110 4322 D506 cpc r13,r21 |
7111 4324 04F0 brlt .+2 |
7112 4326 00C0 rjmp .L520 |
7113 4328 5901 movw r10,r18 |
7114 432a 6A01 movw r12,r20 |
7115 .L521: |
7116 432c 8091 0000 lds r24,ReadingIntegralGyroNick |
7117 4330 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 |
7118 4334 A091 0000 lds r26,(ReadingIntegralGyroNick)+2 |
7119 4338 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 |
7120 433c 8E19 sub r24,r14 |
7121 433e 9F09 sbc r25,r15 |
7122 4340 A00B sbc r26,r16 |
7123 4342 B10B sbc r27,r17 |
7124 4344 8093 0000 sts ReadingIntegralGyroNick,r24 |
7125 4348 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 |
7126 434c A093 0000 sts (ReadingIntegralGyroNick)+2,r26 |
7127 4350 B093 0000 sts (ReadingIntegralGyroNick)+3,r27 |
7128 4354 8091 0000 lds r24,ReadingIntegralGyroRoll |
7129 4358 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 |
7130 435c A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 |
7131 4360 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 |
7132 4364 8A19 sub r24,r10 |
7133 4366 9B09 sbc r25,r11 |
7134 4368 AC09 sbc r26,r12 |
7135 436a BD09 sbc r27,r13 |
7136 436c 8093 0000 sts ReadingIntegralGyroRoll,r24 |
7137 4370 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 |
7138 4374 A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 |
7139 4378 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 |
7140 437c 0C94 0000 jmp .L510 |
7141 .L486: |
7142 4380 4150 subi r20,lo8(-(-1)) |
7143 4382 5040 sbci r21,hi8(-(-1)) |
7144 4384 5093 0000 sts (MaxStickRoll)+1,r21 |
7145 4388 4093 0000 sts MaxStickRoll,r20 |
7146 438c 0C94 0000 jmp .L490 |
7147 .L468: |
7148 4390 1092 0000 sts delay_startmotors.2417,__zero_reg__ |
7149 .L469: |
7150 4394 E091 0000 lds r30,ParamSet+3 |
7151 4398 F0E0 ldi r31,lo8(0) |
7152 439a EE0F lsl r30 |
7153 439c FF1F rol r31 |
7154 439e E050 subi r30,lo8(-(PPM_in)) |
7155 43a0 F040 sbci r31,hi8(-(PPM_in)) |
7156 43a2 8081 ld r24,Z |
7157 43a4 9181 ldd r25,Z+1 |
7158 43a6 8C34 cpi r24,76 |
7159 43a8 9105 cpc r25,__zero_reg__ |
7160 43aa 04F4 brge .+2 |
7161 43ac 00C0 rjmp .L470 |
7162 43ae 8091 0000 lds r24,delay_stopmotors.2418 |
7163 43b2 8F5F subi r24,lo8(-(1)) |
7164 43b4 8093 0000 sts delay_stopmotors.2418,r24 |
7165 43b8 893C cpi r24,lo8(-55) |
7166 43ba 00F4 brsh .+4 |
7167 43bc 0C94 0000 jmp .L410 |
7168 43c0 88EC ldi r24,lo8(-56) |
7169 43c2 8093 0000 sts delay_stopmotors.2418,r24 |
7170 43c6 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ |
7171 43ca 1092 0000 sts ModelIsFlying,__zero_reg__ |
7172 .L727: |
7173 43ce 8091 0000 lds r24,MKFlags |
7174 43d2 8E7F andi r24,lo8(-2) |
7175 43d4 8093 0000 sts MKFlags,r24 |
7176 43d8 0C94 0000 jmp .L410 |
7177 .L582: |
7178 43dc 6B01 movw r12,r22 |
7179 43de CC0C lsl r12 |
7180 43e0 DD1C rol r13 |
7181 43e2 CC0C lsl r12 |
7182 43e4 DD1C rol r13 |
7183 43e6 CC0C lsl r12 |
7184 43e8 DD1C rol r13 |
7185 43ea 0C94 0000 jmp .L584 |
7186 .L755: |
7187 43ee 8827 clr r24 |
7188 43f0 9927 clr r25 |
7189 43f2 8E19 sub r24,r14 |
7190 43f4 9F09 sbc r25,r15 |
7191 43f6 0C94 0000 jmp .L570 |
7192 .L783: |
7193 43fa 2227 clr r18 |
7194 43fc 3327 clr r19 |
7195 43fe 2619 sub r18,r6 |
7196 4400 3709 sbc r19,r7 |
7197 4402 00C0 rjmp .L695 |
7198 .L758: |
7199 4404 6D5F subi r22,lo8(-(3)) |
7200 4406 7F4F sbci r23,hi8(-(3)) |
7201 4408 0C94 0000 jmp .L572 |
7202 .L757: |
7203 440c EE27 clr r30 |
7204 440e FF27 clr r31 |
7205 4410 EE19 sub r30,r14 |
7206 4412 FF09 sbc r31,r15 |
7207 4414 0C94 0000 jmp .L571 |
7208 .L736: |
7209 4418 1092 0000 sts (StickGas)+1,__zero_reg__ |
7210 441c 1092 0000 sts StickGas,__zero_reg__ |
7211 4420 8091 0000 lds r24,ParamSet+8 |
7212 4424 82FD sbrc r24,2 |
7213 4426 00C0 rjmp .+4 |
7214 4428 0C94 0000 jmp .L480 |
7215 442c 00C0 rjmp .L737 |
7216 .L735: |
7217 442e 2250 subi r18,lo8(-(-2)) |
7218 4430 3040 sbci r19,hi8(-(-2)) |
7219 4432 3093 0000 sts (StickYaw)+1,r19 |
7220 4436 2093 0000 sts StickYaw,r18 |
7221 443a 0C94 0000 jmp .L475 |
7222 .L744: |
7223 443e 84E6 ldi r24,lo8(100) |
7224 4440 90E0 ldi r25,hi8(100) |
7225 4442 9093 0000 sts (MaxStickRoll)+1,r25 |
7226 4446 8093 0000 sts MaxStickRoll,r24 |
7227 444a 0C94 0000 jmp .L490 |
7228 .L503: |
7229 444e 1092 0000 sts LoopingNick,__zero_reg__ |
7230 4452 0C94 0000 jmp .L472 |
7231 .L500: |
7232 4456 1092 0000 sts LoopingRoll,__zero_reg__ |
7233 445a 0C94 0000 jmp .L501 |
7234 .L751: |
7235 445e 81E0 ldi r24,lo8(1) |
7236 4460 8093 0000 sts LoopingDown,r24 |
7237 4464 0C94 0000 jmp .L498 |
7238 .L749: |
7239 4468 81E0 ldi r24,lo8(1) |
7240 446a 8093 0000 sts LoopingTop,r24 |
7241 446e 8091 0000 lds r24,ParamSet+49 |
7242 4472 482F mov r20,r24 |
7243 4474 50E0 ldi r21,lo8(0) |
7244 4476 61E0 ldi r22,lo8(1) |
7245 4478 0C94 0000 jmp .L496 |
7246 .L747: |
7247 447c 81E0 ldi r24,lo8(1) |
7248 447e 8093 0000 sts LoopingRight,r24 |
7249 4482 8091 0000 lds r24,ParamSet+49 |
7250 4486 482F mov r20,r24 |
7251 4488 50E0 ldi r21,lo8(0) |
7252 448a 0C94 0000 jmp .L494 |
7253 .L745: |
7254 448e 81E0 ldi r24,lo8(1) |
7255 4490 8093 0000 sts LoopingLeft,r24 |
7256 4494 71E0 ldi r23,lo8(1) |
7257 4496 0C94 0000 jmp .L492 |
7258 .L588: |
7259 449a 0894 sec |
7260 449c 4108 sbc r4,__zero_reg__ |
7261 449e 5108 sbc r5,__zero_reg__ |
7262 44a0 5092 0000 sts (BadCompassHeading)+1,r5 |
7263 44a4 4092 0000 sts BadCompassHeading,r4 |
7264 44a8 0C94 0000 jmp .L575 |
7265 .L794: |
7266 44ac 8091 0000 lds r24,MKFlags |
7267 44b0 80FD sbrc r24,0 |
7268 44b2 00C0 rjmp .+4 |
7269 44b4 0C94 0000 jmp .L510 |
7270 44b8 00C0 rjmp .L511 |
7271 .L748: |
7272 44ba 1092 0000 sts LoopingRight,__zero_reg__ |
7273 44be 8091 0000 lds r24,ParamSet+49 |
7274 44c2 482F mov r20,r24 |
7275 44c4 50E0 ldi r21,lo8(0) |
7276 44c6 0C94 0000 jmp .L494 |
7277 .L746: |
7278 44ca 1092 0000 sts LoopingLeft,__zero_reg__ |
7279 44ce 8091 0000 lds r24,ParamSet+1 |
7280 44d2 A82F mov r26,r24 |
7281 44d4 B0E0 ldi r27,lo8(0) |
7282 44d6 8091 0000 lds r24,ParamSet+49 |
7283 44da 482F mov r20,r24 |
7284 44dc 50E0 ldi r21,lo8(0) |
7285 44de AA0F lsl r26 |
7286 44e0 BB1F rol r27 |
7287 44e2 70E0 ldi r23,lo8(0) |
7288 44e4 0C94 0000 jmp .L492 |
7289 .L752: |
7290 44e8 1092 0000 sts LoopingDown,__zero_reg__ |
7291 44ec 0C94 0000 jmp .L498 |
7292 .L750: |
7293 44f0 1092 0000 sts LoopingTop,__zero_reg__ |
7294 44f4 8091 0000 lds r24,ParamSet |
7295 44f8 A82F mov r26,r24 |
7296 44fa B0E0 ldi r27,lo8(0) |
7297 44fc 8091 0000 lds r24,ParamSet+49 |
7298 4500 482F mov r20,r24 |
7299 4502 50E0 ldi r21,lo8(0) |
7300 4504 AA0F lsl r26 |
7301 4506 BB1F rol r27 |
7302 4508 60E0 ldi r22,lo8(0) |
7303 450a 0C94 0000 jmp .L496 |
7304 .L412: |
7305 450e 8091 0000 lds r24,MKFlags |
7306 4512 8260 ori r24,lo8(2) |
7307 4514 8093 0000 sts MKFlags,r24 |
7308 4518 0C94 0000 jmp .L413 |
7309 .L780: |
7310 451c 0196 adiw r24,1 |
7311 451e 0C94 0000 jmp .L676 |
7312 .L764: |
7313 4522 8091 0000 lds r24,YawGyroDrift |
7314 4526 9091 0000 lds r25,(YawGyroDrift)+1 |
7315 452a 860F add r24,r22 |
7316 452c 971F adc r25,r23 |
7317 452e 9093 0000 sts (YawGyroDrift)+1,r25 |
7318 4532 8093 0000 sts YawGyroDrift,r24 |
7319 4536 8091 0000 lds r24,UpdateCompassCourse.2420 |
7320 453a 9091 0000 lds r25,(UpdateCompassCourse.2420)+1 |
7321 453e 892B or r24,r25 |
7322 4540 01F4 brne .+4 |
7323 4542 0C94 0000 jmp .L585 |
7324 4546 E090 0000 lds r14,ParamSet+23 |
7325 454a 8AE2 ldi r24,lo8(42) |
7326 454c E89E mul r14,r24 |
7327 454e 7001 movw r14,r0 |
7328 4550 1124 clr r1 |
7329 4552 0027 clr r16 |
7330 4554 F7FC sbrc r15,7 |
7331 4556 0095 com r16 |
7332 4558 102F mov r17,r16 |
7333 455a 9501 movw r18,r10 |
7334 455c 4427 clr r20 |
7335 455e 37FD sbrc r19,7 |
7336 4560 4095 com r20 |
7337 4562 542F mov r21,r20 |
7338 4564 C801 movw r24,r16 |
7339 4566 B701 movw r22,r14 |
7340 4568 0E94 0000 call __mulsi3 |
7341 456c 6093 0000 sts YawGyroHeading,r22 |
7342 4570 7093 0000 sts (YawGyroHeading)+1,r23 |
7343 4574 8093 0000 sts (YawGyroHeading)+2,r24 |
7344 4578 9093 0000 sts (YawGyroHeading)+3,r25 |
7345 457c A801 movw r20,r16 |
7346 457e 9701 movw r18,r14 |
7347 4580 0E94 0000 call __divmodsi4 |
7348 4584 3093 0000 sts (CompassCourse)+1,r19 |
7349 4588 2093 0000 sts CompassCourse,r18 |
7350 458c 1092 0000 sts (UpdateCompassCourse.2420)+1,__zero_reg__ |
7351 4590 1092 0000 sts UpdateCompassCourse.2420,__zero_reg__ |
7352 4594 0C94 0000 jmp .L585 |
7353 .L513: |
7354 4598 652D mov r22,r5 |
7355 459a 742D mov r23,r4 |
7356 459c 832D mov r24,r3 |
7357 459e 922D mov r25,r2 |
7358 45a0 22E0 ldi r18,lo8(2) |
7359 45a2 30E0 ldi r19,hi8(2) |
7360 45a4 40E0 ldi r20,hlo8(2) |
7361 45a6 50E0 ldi r21,hhi8(2) |
7362 45a8 0E94 0000 call __divmodsi4 |
7363 45ac C901 movw r24,r18 |
7364 45ae DA01 movw r26,r20 |
7365 45b0 3C01 movw r6,r24 |
7366 45b2 4D01 movw r8,r26 |
7367 45b4 612F mov r22,r17 |
7368 45b6 702F mov r23,r16 |
7369 45b8 8F2D mov r24,r15 |
7370 45ba 9E2D mov r25,r14 |
7371 45bc 22E0 ldi r18,lo8(2) |
7372 45be 30E0 ldi r19,hi8(2) |
7373 45c0 40E0 ldi r20,hlo8(2) |
7374 45c2 50E0 ldi r21,hhi8(2) |
7375 45c4 0E94 0000 call __divmodsi4 |
7376 45c8 C901 movw r24,r18 |
7377 45ca DA01 movw r26,r20 |
7378 45cc 5C01 movw r10,r24 |
7379 45ce 6D01 movw r12,r26 |
7380 45d0 00C0 rjmp .L514 |
7381 .L451: |
7382 45d2 8091 0000 lds r24,ParamSet+1 |
7383 45d6 A82F mov r26,r24 |
7384 45d8 B0E0 ldi r27,lo8(0) |
7385 45da AA0F lsl r26 |
7386 45dc BB1F rol r27 |
7387 45de FD01 movw r30,r26 |
7388 45e0 E050 subi r30,lo8(-(PPM_in)) |
7389 45e2 F040 sbci r31,hi8(-(PPM_in)) |
7390 45e4 0190 ld __tmp_reg__,Z+ |
7391 45e6 F081 ld r31,Z |
7392 45e8 E02D mov r30,__tmp_reg__ |
7393 45ea F7FF sbrs r31,7 |
7394 45ec 00C0 rjmp .+4 |
7395 45ee 0C94 0000 jmp .L796 |
7396 .L454: |
7397 45f2 E734 cpi r30,71 |
7398 45f4 F105 cpc r31,__zero_reg__ |
7399 45f6 04F0 brlt .+4 |
7400 45f8 0C94 0000 jmp .L452 |
7401 45fc 8091 0000 lds r24,ParamSet+8 |
7402 4600 8872 andi r24,lo8(40) |
7403 4602 01F4 brne .+4 |
7404 4604 0C94 0000 jmp .L461 |
7405 4608 A050 subi r26,lo8(-(PPM_in)) |
7406 460a B040 sbci r27,hi8(-(PPM_in)) |
7407 460c 8D91 ld r24,X+ |
7408 460e 9C91 ld r25,X |
7409 4610 97FF sbrs r25,7 |
7410 4612 00C0 rjmp .+4 |
7411 4614 0C94 0000 jmp .L797 |
7412 .L463: |
7413 4618 4E97 sbiw r24,30 |
7414 461a 04F0 brlt .+4 |
7415 461c 0C94 0000 jmp .L461 |
7416 4620 4050 subi r20,lo8(-(PPM_in)) |
7417 4622 5040 sbci r21,hi8(-(PPM_in)) |
7418 4624 FA01 movw r30,r20 |
7419 4626 8081 ld r24,Z |
7420 4628 9181 ldd r25,Z+1 |
7421 462a 8A5B subi r24,lo8(-70) |
7422 462c 9F4F sbci r25,hi8(-70) |
7423 462e 04F0 brlt .+4 |
7424 4630 0C94 0000 jmp .L461 |
7425 4634 81E0 ldi r24,lo8(1) |
7426 4636 8093 0000 sts CompassCalState,r24 |
7427 463a 88EE ldi r24,lo8(1000) |
7428 463c 93E0 ldi r25,hi8(1000) |
7429 463e 9093 0000 sts (BeepTime)+1,r25 |
7430 4642 8093 0000 sts BeepTime,r24 |
7431 4646 8091 0000 lds r24,ParamSet+2 |
7432 464a A82F mov r26,r24 |
7433 464c B0E0 ldi r27,lo8(0) |
7434 464e AA0F lsl r26 |
7435 4650 BB1F rol r27 |
7436 .L446: |
7437 4652 A050 subi r26,lo8(-(PPM_in)) |
7438 4654 B040 sbci r27,hi8(-(PPM_in)) |
7439 4656 8D91 ld r24,X+ |
7440 4658 9C91 ld r25,X |
7441 465a 8B5A subi r24,lo8(-85) |
7442 465c 9F4F sbci r25,hi8(-85) |
7443 465e 04F0 brlt .+4 |
7444 4660 0C94 0000 jmp .L410 |
7445 4664 E091 0000 lds r30,ParamSet+3 |
7446 4668 F0E0 ldi r31,lo8(0) |
7447 466a EE0F lsl r30 |
7448 466c FF1F rol r31 |
7449 466e E050 subi r30,lo8(-(PPM_in)) |
7450 4670 F040 sbci r31,hi8(-(PPM_in)) |
7451 4672 8081 ld r24,Z |
7452 4674 9181 ldd r25,Z+1 |
7453 4676 855B subi r24,lo8(-75) |
7454 4678 9F4F sbci r25,hi8(-75) |
7455 467a 04F0 brlt .+2 |
7456 467c 00C0 rjmp .L468 |
7457 467e 8091 0000 lds r24,delay_startmotors.2417 |
7458 4682 8F5F subi r24,lo8(-(1)) |
7459 4684 8093 0000 sts delay_startmotors.2417,r24 |
7460 4688 893C cpi r24,lo8(-55) |
7461 468a 00F4 brsh .+2 |
7462 468c 00C0 rjmp .L469 |
7463 468e 88EC ldi r24,lo8(-56) |
7464 4690 8093 0000 sts delay_startmotors.2417,r24 |
7465 4694 81E0 ldi r24,lo8(1) |
7466 4696 90E0 ldi r25,hi8(1) |
7467 4698 9093 0000 sts (ModelIsFlying)+1,r25 |
7468 469c 8093 0000 sts ModelIsFlying,r24 |
7469 46a0 8091 0000 lds r24,MKFlags |
7470 46a4 8960 ori r24,lo8(9) |
7471 46a6 8093 0000 sts MKFlags,r24 |
7472 46aa 1092 0000 sts SetPointYaw.2410,__zero_reg__ |
7473 46ae 1092 0000 sts (SetPointYaw.2410)+1,__zero_reg__ |
7474 46b2 1092 0000 sts (SetPointYaw.2410)+2,__zero_reg__ |
7475 46b6 1092 0000 sts (SetPointYaw.2410)+3,__zero_reg__ |
7476 46ba 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ |
7477 46be 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ |
7478 46c2 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ |
7479 46c6 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ |
7480 46ca 6091 0000 lds r22,AccNick |
7481 46ce 7091 0000 lds r23,(AccNick)+1 |
7482 46d2 8827 clr r24 |
7483 46d4 77FD sbrc r23,7 |
7484 46d6 8095 com r24 |
7485 46d8 982F mov r25,r24 |
7486 46da E090 0000 lds r14,ParamSet+23 |
7487 46de FF24 clr r15 |
7488 46e0 00E0 ldi r16,lo8(0) |
7489 46e2 10E0 ldi r17,hi8(0) |
7490 46e4 A801 movw r20,r16 |
7491 46e6 9701 movw r18,r14 |
7492 46e8 0E94 0000 call __mulsi3 |
7493 46ec 6093 0000 sts ReadingIntegralGyroNick,r22 |
7494 46f0 7093 0000 sts (ReadingIntegralGyroNick)+1,r23 |
7495 46f4 8093 0000 sts (ReadingIntegralGyroNick)+2,r24 |
7496 46f8 9093 0000 sts (ReadingIntegralGyroNick)+3,r25 |
7497 46fc 6091 0000 lds r22,AccRoll |
7498 4700 7091 0000 lds r23,(AccRoll)+1 |
7499 4704 8827 clr r24 |
7500 4706 77FD sbrc r23,7 |
7501 4708 8095 com r24 |
7502 470a 982F mov r25,r24 |
7503 470c 0E94 0000 call __mulsi3 |
7504 4710 6093 0000 sts ReadingIntegralGyroRoll,r22 |
7505 4714 7093 0000 sts (ReadingIntegralGyroRoll)+1,r23 |
7506 4718 8093 0000 sts (ReadingIntegralGyroRoll)+2,r24 |
7507 471c 9093 0000 sts (ReadingIntegralGyroRoll)+3,r25 |
7508 4720 8091 0000 lds r24,IntegralGyroNick |
7509 4724 9091 0000 lds r25,(IntegralGyroNick)+1 |
7510 4728 A091 0000 lds r26,(IntegralGyroNick)+2 |
7511 472c B091 0000 lds r27,(IntegralGyroNick)+3 |
7512 4730 8093 0000 sts ReadingIntegralGyroNick2,r24 |
7513 4734 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 |
7514 4738 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 |
7515 473c B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 |
7516 4740 8091 0000 lds r24,IntegralGyroRoll |
7517 4744 9091 0000 lds r25,(IntegralGyroRoll)+1 |
7518 4748 A091 0000 lds r26,(IntegralGyroRoll)+2 |
7519 474c B091 0000 lds r27,(IntegralGyroRoll)+3 |
7520 4750 8093 0000 sts ReadingIntegralGyroRoll2,r24 |
7521 4754 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 |
7522 4758 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 |
7523 475c B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 |
7524 4760 1092 0000 sts IPartNick.2408,__zero_reg__ |
7525 4764 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ |
7526 4768 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ |
7527 476c 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ |
7528 4770 1092 0000 sts IPartRoll.2409,__zero_reg__ |
7529 4774 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ |
7530 4778 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ |
7531 477c 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ |
7532 4780 00C0 rjmp .L469 |
7533 .L739: |
7534 4782 3095 com r19 |
7535 4784 2195 neg r18 |
7536 4786 3F4F sbci r19,lo8(-1) |
7537 4788 0C94 0000 jmp .L483 |
7538 .L742: |
7539 478c 3095 com r19 |
7540 478e 2195 neg r18 |
7541 4790 3F4F sbci r19,lo8(-1) |
7542 4792 0C94 0000 jmp .L488 |
7543 .L741: |
7544 4796 2D5F subi r18,lo8(-(3)) |
7545 4798 3F4F sbci r19,hi8(-(3)) |
7546 479a 0C94 0000 jmp .L487 |
7547 .L733: |
7548 479e 4D5F subi r20,lo8(-(3)) |
7549 47a0 5F4F sbci r21,hi8(-(3)) |
7550 47a2 0C94 0000 jmp .L473 |
7551 .L734: |
7552 47a6 4D5F subi r20,lo8(-(3)) |
7553 47a8 5F4F sbci r21,hi8(-(3)) |
7554 47aa 0C94 0000 jmp .L474 |
7555 .L738: |
7556 47ae 2D5F subi r18,lo8(-(3)) |
7557 47b0 3F4F sbci r19,hi8(-(3)) |
7558 47b2 0C94 0000 jmp .L482 |
7559 .L762: |
7560 47b6 60E0 ldi r22,lo8(0) |
7561 47b8 70E0 ldi r23,hi8(0) |
7562 47ba 0C94 0000 jmp .L581 |
7563 .L587: |
7564 47be 84EF ldi r24,lo8(500) |
7565 47c0 91E0 ldi r25,hi8(500) |
7566 47c2 9093 0000 sts (BadCompassHeading)+1,r25 |
7567 47c6 8093 0000 sts BadCompassHeading,r24 |
7568 47ca 0C94 0000 jmp .L575 |
7569 .L428: |
7570 47ce 8091 0000 lds r24,PPM_in+24 |
7571 47d2 9091 0000 lds r25,(PPM_in+24)+1 |
7572 47d6 8259 subi r24,lo8(-(110)) |
7573 47d8 9F4F sbci r25,hi8(-(110)) |
7574 47da 8217 cp r24,r18 |
7575 47dc 9307 cpc r25,r19 |
7576 47de 04F0 brlt .+4 |
7577 47e0 0C94 0000 jmp .L429 |
7578 47e4 2115 cp r18,__zero_reg__ |
7579 47e6 3105 cpc r19,__zero_reg__ |
7580 47e8 01F4 brne .+4 |
7581 47ea 0C94 0000 jmp .L429 |
7582 47ee C901 movw r24,r18 |
7583 47f0 0197 sbiw r24,1 |
7584 47f2 9093 0000 sts (Poti8)+1,r25 |
7585 47f6 8093 0000 sts Poti8,r24 |
7586 47fa 9C01 movw r18,r24 |
7587 47fc 0C94 0000 jmp .L429 |
7588 .L426: |
7589 4800 8091 0000 lds r24,PPM_in+22 |
7590 4804 9091 0000 lds r25,(PPM_in+22)+1 |
7591 4808 8259 subi r24,lo8(-(110)) |
7592 480a 9F4F sbci r25,hi8(-(110)) |
7593 480c 8417 cp r24,r20 |
7594 480e 9507 cpc r25,r21 |
7595 4810 04F0 brlt .+4 |
7596 4812 0C94 0000 jmp .L427 |
7597 4816 4115 cp r20,__zero_reg__ |
7598 4818 5105 cpc r21,__zero_reg__ |
7599 481a 01F4 brne .+4 |
7600 481c 0C94 0000 jmp .L427 |
7601 4820 CA01 movw r24,r20 |
7602 4822 0197 sbiw r24,1 |
7603 4824 9093 0000 sts (Poti7)+1,r25 |
7604 4828 8093 0000 sts Poti7,r24 |
7605 482c AC01 movw r20,r24 |
7606 482e 0C94 0000 jmp .L427 |
7607 .L424: |
7608 4832 8091 0000 lds r24,PPM_in+20 |
7609 4836 9091 0000 lds r25,(PPM_in+20)+1 |
7610 483a 8259 subi r24,lo8(-(110)) |
7611 483c 9F4F sbci r25,hi8(-(110)) |
7612 483e 8617 cp r24,r22 |
7613 4840 9707 cpc r25,r23 |
7614 4842 04F0 brlt .+4 |
7615 4844 0C94 0000 jmp .L425 |
7616 4848 6115 cp r22,__zero_reg__ |
7617 484a 7105 cpc r23,__zero_reg__ |
7618 484c 01F4 brne .+4 |
7619 484e 0C94 0000 jmp .L425 |
7620 4852 CB01 movw r24,r22 |
7621 4854 0197 sbiw r24,1 |
7622 4856 9093 0000 sts (Poti6)+1,r25 |
7623 485a 8093 0000 sts Poti6,r24 |
7624 485e BC01 movw r22,r24 |
7625 4860 0C94 0000 jmp .L425 |
7626 .L422: |
7627 4864 8091 0000 lds r24,PPM_in+18 |
7628 4868 9091 0000 lds r25,(PPM_in+18)+1 |
7629 486c 8259 subi r24,lo8(-(110)) |
7630 486e 9F4F sbci r25,hi8(-(110)) |
7631 4870 8E17 cp r24,r30 |
7632 4872 9F07 cpc r25,r31 |
7633 4874 04F0 brlt .+4 |
7634 4876 0C94 0000 jmp .L423 |
7635 487a 3097 sbiw r30,0 |
7636 487c 01F4 brne .+4 |
7637 487e 0C94 0000 jmp .L423 |
7638 4882 CF01 movw r24,r30 |
7639 4884 0197 sbiw r24,1 |
7640 4886 9093 0000 sts (Poti5)+1,r25 |
7641 488a 8093 0000 sts Poti5,r24 |
7642 488e FC01 movw r30,r24 |
7643 4890 0C94 0000 jmp .L423 |
7644 .L420: |
7645 4894 8081 ld r24,Z |
7646 4896 9181 ldd r25,Z+1 |
7647 4898 8259 subi r24,lo8(-(110)) |
7648 489a 9F4F sbci r25,hi8(-(110)) |
7649 489c 8A17 cp r24,r26 |
7650 489e 9B07 cpc r25,r27 |
7651 48a0 04F0 brlt .+4 |
7652 48a2 0C94 0000 jmp .L421 |
7653 48a6 1097 sbiw r26,0 |
7654 48a8 01F4 brne .+4 |
7655 48aa 0C94 0000 jmp .L421 |
7656 48ae CD01 movw r24,r26 |
7657 48b0 0197 sbiw r24,1 |
7658 48b2 9093 0000 sts (Poti4)+1,r25 |
7659 48b6 8093 0000 sts Poti4,r24 |
7660 48ba DC01 movw r26,r24 |
7661 48bc 0C94 0000 jmp .L421 |
7662 .L418: |
7663 48c0 8081 ld r24,Z |
7664 48c2 9181 ldd r25,Z+1 |
7665 48c4 8259 subi r24,lo8(-(110)) |
7666 48c6 9F4F sbci r25,hi8(-(110)) |
7667 48c8 8A15 cp r24,r10 |
7668 48ca 9B05 cpc r25,r11 |
7669 48cc 04F0 brlt .+4 |
7670 48ce 0C94 0000 jmp .L419 |
7671 48d2 A114 cp r10,__zero_reg__ |
7672 48d4 B104 cpc r11,__zero_reg__ |
7673 48d6 01F4 brne .+4 |
7674 48d8 0C94 0000 jmp .L419 |
7675 48dc C501 movw r24,r10 |
7676 48de 0197 sbiw r24,1 |
7677 48e0 9093 0000 sts (Poti3)+1,r25 |
7678 48e4 8093 0000 sts Poti3,r24 |
7679 48e8 5C01 movw r10,r24 |
7680 48ea 0C94 0000 jmp .L419 |
7681 .L416: |
7682 48ee 8081 ld r24,Z |
7683 48f0 9181 ldd r25,Z+1 |
7684 48f2 8259 subi r24,lo8(-(110)) |
7685 48f4 9F4F sbci r25,hi8(-(110)) |
7686 48f6 8017 cp r24,r16 |
7687 48f8 9107 cpc r25,r17 |
7688 48fa 04F0 brlt .+4 |
7689 48fc 0C94 0000 jmp .L417 |
7690 4900 0115 cp r16,__zero_reg__ |
7691 4902 1105 cpc r17,__zero_reg__ |
7692 4904 01F4 brne .+4 |
7693 4906 0C94 0000 jmp .L417 |
7694 490a C801 movw r24,r16 |
7695 490c 0197 sbiw r24,1 |
7696 490e 9093 0000 sts (Poti2)+1,r25 |
7697 4912 8093 0000 sts Poti2,r24 |
7698 4916 8C01 movw r16,r24 |
7699 4918 0C94 0000 jmp .L417 |
7700 .L414: |
7701 491c 8081 ld r24,Z |
7702 491e 9181 ldd r25,Z+1 |
7703 4920 8259 subi r24,lo8(-(110)) |
7704 4922 9F4F sbci r25,hi8(-(110)) |
7705 4924 8E15 cp r24,r14 |
7706 4926 9F05 cpc r25,r15 |
7707 4928 04F0 brlt .+4 |
7708 492a 0C94 0000 jmp .L415 |
7709 492e E114 cp r14,__zero_reg__ |
7710 4930 F104 cpc r15,__zero_reg__ |
7711 4932 01F4 brne .+4 |
7712 4934 0C94 0000 jmp .L415 |
7713 4938 C701 movw r24,r14 |
7714 493a 0197 sbiw r24,1 |
7715 493c 9093 0000 sts (Poti1)+1,r25 |
7716 4940 8093 0000 sts Poti1,r24 |
7717 4944 7C01 movw r14,r24 |
7718 4946 0C94 0000 jmp .L415 |
7719 .L765: |
7720 494a 4F96 adiw r24,31 |
7721 494c 0C94 0000 jmp .L586 |
7722 .L763: |
7723 4950 8827 clr r24 |
7724 4952 9927 clr r25 |
7725 4954 8219 sub r24,r2 |
7726 4956 9309 sbc r25,r3 |
7727 4958 0C94 0000 jmp .L583 |
7728 .L761: |
7729 495c 9094 com r9 |
7730 495e 8194 neg r8 |
7731 4960 9108 sbc r9,__zero_reg__ |
7732 4962 9394 inc r9 |
7733 4964 0C94 0000 jmp .L578 |
7734 .L760: |
7735 4968 1095 com r17 |
7736 496a 0195 neg r16 |
7737 496c 1F4F sbci r17,lo8(-1) |
7738 496e 0C94 0000 jmp .L577 |
7739 .L477: |
7740 4972 1092 0000 sts (StickYaw)+1,__zero_reg__ |
7741 4976 1092 0000 sts StickYaw,__zero_reg__ |
7742 497a 0C94 0000 jmp .L475 |
7743 .L776: |
7744 497e 1092 0000 sts (Poti8)+1,__zero_reg__ |
7745 4982 1092 0000 sts Poti8,__zero_reg__ |
7746 4986 0C94 0000 jmp .L445 |
7747 .L775: |
7748 498a 1092 0000 sts (Poti7)+1,__zero_reg__ |
7749 498e 1092 0000 sts Poti7,__zero_reg__ |
7750 4992 0C94 0000 jmp .L443 |
7751 .L774: |
7752 4996 1092 0000 sts (Poti6)+1,__zero_reg__ |
7753 499a 1092 0000 sts Poti6,__zero_reg__ |
7754 499e 0C94 0000 jmp .L441 |
7755 .L773: |
7756 49a2 1092 0000 sts (Poti5)+1,__zero_reg__ |
7757 49a6 1092 0000 sts Poti5,__zero_reg__ |
7758 49aa 0C94 0000 jmp .L439 |
7759 .L772: |
7760 49ae 1092 0000 sts (Poti4)+1,__zero_reg__ |
7761 49b2 1092 0000 sts Poti4,__zero_reg__ |
7762 49b6 0C94 0000 jmp .L437 |
7763 .L771: |
7764 49ba 1092 0000 sts (Poti3)+1,__zero_reg__ |
7765 49be 1092 0000 sts Poti3,__zero_reg__ |
7766 49c2 0C94 0000 jmp .L435 |
7767 .L770: |
7768 49c6 1092 0000 sts (Poti2)+1,__zero_reg__ |
7769 49ca 1092 0000 sts Poti2,__zero_reg__ |
7770 49ce 0C94 0000 jmp .L433 |
7771 .L769: |
7772 49d2 1092 0000 sts (Poti1)+1,__zero_reg__ |
7773 49d6 1092 0000 sts Poti1,__zero_reg__ |
7774 49da 0C94 0000 jmp .L431 |
7775 .L590: |
7776 49de 4427 clr r20 |
7777 49e0 5527 clr r21 |
7778 49e2 481B sub r20,r24 |
7779 49e4 590B sbc r21,r25 |
7780 49e6 B901 movw r22,r18 |
7781 49e8 2417 cp r18,r20 |
7782 49ea 3507 cpc r19,r21 |
7783 49ec 04F0 brlt .+4 |
7784 49ee 0C94 0000 jmp .L591 |
7785 49f2 BA01 movw r22,r20 |
7786 49f4 0C94 0000 jmp .L591 |
7787 .L743: |
7788 49f8 3095 com r19 |
7789 49fa 2195 neg r18 |
7790 49fc 3F4F sbci r19,lo8(-1) |
7791 49fe 0C94 0000 jmp .L489 |
7792 .L789: |
7793 4a02 3095 com r19 |
7794 4a04 2195 neg r18 |
7795 4a06 3F4F sbci r19,lo8(-1) |
7796 4a08 00C0 rjmp .L484 |
7797 .L520: |
7798 4a0a 8A15 cp r24,r10 |
7799 4a0c 9B05 cpc r25,r11 |
7800 4a0e AC05 cpc r26,r12 |
7801 4a10 BD05 cpc r27,r13 |
7802 4a12 04F0 brlt .+2 |
7803 4a14 00C0 rjmp .L521 |
7804 4a16 5C01 movw r10,r24 |
7805 4a18 6D01 movw r12,r26 |
7806 4a1a 00C0 rjmp .L521 |
7807 .L517: |
7808 4a1c 8401 movw r16,r8 |
7809 4a1e 7301 movw r14,r6 |
7810 4a20 8615 cp r24,r6 |
7811 4a22 9705 cpc r25,r7 |
7812 4a24 A805 cpc r26,r8 |
7813 4a26 B905 cpc r27,r9 |
7814 4a28 04F0 brlt .+2 |
7815 4a2a 00C0 rjmp .L518 |
7816 4a2c 7C01 movw r14,r24 |
7817 4a2e 8D01 movw r16,r26 |
7818 4a30 00C0 rjmp .L518 |
7819 .L611: |
7820 4a32 8091 0000 lds r24,BaroFlags.2450 |
7821 4a36 90E0 ldi r25,lo8(0) |
7822 4a38 8370 andi r24,lo8(3) |
7823 4a3a 9070 andi r25,hi8(3) |
7824 4a3c 892B or r24,r25 |
7825 4a3e 01F4 brne .+2 |
7826 4a40 00C0 rjmp .L798 |
7827 .L614: |
7828 4a42 81E0 ldi r24,lo8(1) |
7829 4a44 8093 0000 sts HCActive.2443,r24 |
7830 .L613: |
7831 4a48 E090 0000 lds r14,ParamSet+23 |
7832 4a4c 8AE2 ldi r24,lo8(42) |
7833 4a4e E89E mul r14,r24 |
7834 4a50 7001 movw r14,r0 |
7835 4a52 1124 clr r1 |
7836 4a54 0027 clr r16 |
7837 4a56 F7FC sbrc r15,7 |
7838 4a58 0095 com r16 |
7839 4a5a 102F mov r17,r16 |
7840 4a5c 6091 0000 lds r22,IntegralGyroNick |
7841 4a60 7091 0000 lds r23,(IntegralGyroNick)+1 |
7842 4a64 8091 0000 lds r24,(IntegralGyroNick)+2 |
7843 4a68 9091 0000 lds r25,(IntegralGyroNick)+3 |
7844 4a6c A801 movw r20,r16 |
7845 4a6e 9701 movw r18,r14 |
7846 4a70 0E94 0000 call __divmodsi4 |
7847 4a74 5901 movw r10,r18 |
7848 4a76 6A01 movw r12,r20 |
7849 4a78 6091 0000 lds r22,IntegralGyroRoll |
7850 4a7c 7091 0000 lds r23,(IntegralGyroRoll)+1 |
7851 4a80 8091 0000 lds r24,(IntegralGyroRoll)+2 |
7852 4a84 9091 0000 lds r25,(IntegralGyroRoll)+3 |
7853 4a88 A801 movw r20,r16 |
7854 4a8a 9701 movw r18,r14 |
7855 4a8c 0E94 0000 call __divmodsi4 |
7856 4a90 C501 movw r24,r10 |
7857 4a92 B901 movw r22,r18 |
7858 4a94 0E94 0000 call ihypot |
7859 4a98 8D33 cpi r24,61 |
7860 4a9a 9105 cpc r25,__zero_reg__ |
7861 4a9c 04F0 brlt .L615 |
7862 4a9e 8CE3 ldi r24,lo8(60) |
7863 4aa0 90E0 ldi r25,hi8(60) |
7864 .L615: |
7865 4aa2 0E94 0000 call c_cos_8192 |
7866 4aa6 9A83 std Y+2,r25 |
7867 4aa8 8983 std Y+1,r24 |
7868 4aaa 8091 0000 lds r24,HCActive.2443 |
7869 4aae 8823 tst r24 |
7870 4ab0 01F4 brne .+2 |
7871 4ab2 00C0 rjmp .L616 |
7872 4ab4 8091 0000 lds r24,MKFlags |
7873 4ab8 84FD sbrc r24,4 |
7874 4aba 00C0 rjmp .L616 |
7875 4abc 8091 0000 lds r24,ParamSet+88 |
7876 4ac0 80FD sbrc r24,0 |
7877 4ac2 00C0 rjmp .L617 |
7878 4ac4 8091 0000 lds r24,ParamSet+8 |
7879 4ac8 81FF sbrs r24,1 |
7880 4aca 00C0 rjmp .L617 |
7881 4acc 8091 0000 lds r24,MKFlags |
7882 4ad0 81FF sbrs r24,1 |
7883 4ad2 00C0 rjmp .L620 |
7884 4ad4 6091 0000 lds r22,StickGasHover.2444 |
7885 4ad8 7091 0000 lds r23,(StickGasHover.2444)+1 |
7886 4adc 4091 0000 lds r20,StickGas |
7887 4ae0 5091 0000 lds r21,(StickGas)+1 |
7888 4ae4 CB01 movw r24,r22 |
7889 4ae6 0F96 adiw r24,15 |
7890 4ae8 8417 cp r24,r20 |
7891 4aea 9507 cpc r25,r21 |
7892 4aec 04F0 brlt .+2 |
7893 4aee 00C0 rjmp .L621 |
7894 4af0 8091 0000 lds r24,BaroFlags.2450 |
7895 4af4 80FD sbrc r24,0 |
7896 4af6 00C0 rjmp .L621 |
7897 4af8 2091 0000 lds r18,HeightTrimmingFlag.2452 |
7898 4afc 21FF sbrs r18,1 |
7899 4afe 00C0 rjmp .L622 |
7900 4b00 2D7F andi r18,lo8(-3) |
7901 4b02 2093 0000 sts HeightTrimmingFlag.2452,r18 |
7902 4b06 8091 0000 lds r24,ReadingHeight |
7903 4b0a 9091 0000 lds r25,(ReadingHeight)+1 |
7904 4b0e A091 0000 lds r26,(ReadingHeight)+2 |
7905 4b12 B091 0000 lds r27,(ReadingHeight)+3 |
7906 4b16 8093 0000 sts SetPointHeight,r24 |
7907 4b1a 9093 0000 sts (SetPointHeight)+1,r25 |
7908 4b1e A093 0000 sts (SetPointHeight)+2,r26 |
7909 4b22 B093 0000 sts (SetPointHeight)+3,r27 |
7910 .L622: |
7911 4b26 2160 ori r18,lo8(1) |
7912 4b28 2093 0000 sts HeightTrimmingFlag.2452,r18 |
7913 4b2c CA01 movw r24,r20 |
7914 4b2e 0F97 sbiw r24,15 |
7915 4b30 861B sub r24,r22 |
7916 4b32 970B sbc r25,r23 |
7917 4b34 97FD sbrc r25,7 |
7918 4b36 00C0 rjmp .L799 |
7919 .L623: |
7920 4b38 2091 0000 lds r18,HeightTrimming.2442 |
7921 4b3c 3091 0000 lds r19,(HeightTrimming.2442)+1 |
7922 4b40 280F add r18,r24 |
7923 4b42 391F adc r19,r25 |
7924 4b44 3093 0000 sts (HeightTrimming.2442)+1,r19 |
7925 4b48 2093 0000 sts HeightTrimming.2442,r18 |
7926 .L624: |
7927 4b4c C901 movw r24,r18 |
7928 4b4e 37FF sbrs r19,7 |
7929 4b50 00C0 rjmp .L629 |
7930 4b52 8827 clr r24 |
7931 4b54 9927 clr r25 |
7932 4b56 821B sub r24,r18 |
7933 4b58 930B sbc r25,r19 |
7934 .L629: |
7935 4b5a 8150 subi r24,lo8(513) |
7936 4b5c 9240 sbci r25,hi8(513) |
7937 4b5e 04F4 brge .+2 |
7938 4b60 00C0 rjmp .L628 |
7939 4b62 8091 0000 lds r24,ParamSet+13 |
7940 4b66 90E0 ldi r25,lo8(0) |
7941 4b68 5C01 movw r10,r24 |
7942 4b6a A29E mul r10,r18 |
7943 4b6c C001 movw r24,r0 |
7944 4b6e A39E mul r10,r19 |
7945 4b70 900D add r25,r0 |
7946 4b72 B29E mul r11,r18 |
7947 4b74 900D add r25,r0 |
7948 4b76 1124 clr r1 |
7949 4b78 60E0 ldi r22,lo8(1280) |
7950 4b7a 75E0 ldi r23,hi8(1280) |
7951 4b7c 0E94 0000 call __divmodhi4 |
7952 4b80 8827 clr r24 |
7953 4b82 77FD sbrc r23,7 |
7954 4b84 8095 com r24 |
7955 4b86 982F mov r25,r24 |
7956 4b88 2091 0000 lds r18,SetPointHeight |
7957 4b8c 3091 0000 lds r19,(SetPointHeight)+1 |
7958 4b90 4091 0000 lds r20,(SetPointHeight)+2 |
7959 4b94 5091 0000 lds r21,(SetPointHeight)+3 |
7960 4b98 260F add r18,r22 |
7961 4b9a 371F adc r19,r23 |
7962 4b9c 481F adc r20,r24 |
7963 4b9e 591F adc r21,r25 |
7964 4ba0 2093 0000 sts SetPointHeight,r18 |
7965 4ba4 3093 0000 sts (SetPointHeight)+1,r19 |
7966 4ba8 4093 0000 sts (SetPointHeight)+2,r20 |
7967 4bac 5093 0000 sts (SetPointHeight)+3,r21 |
7968 4bb0 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ |
7969 4bb4 1092 0000 sts HeightTrimming.2442,__zero_reg__ |
7970 4bb8 8091 0000 lds r24,ParamSet+88 |
7971 4bbc 81FF sbrs r24,1 |
7972 4bbe 00C0 rjmp .L630 |
7973 4bc0 8BE4 ldi r24,lo8(75) |
7974 4bc2 90E0 ldi r25,hi8(75) |
7975 4bc4 9093 0000 sts (BeepTime)+1,r25 |
7976 4bc8 8093 0000 sts BeepTime,r24 |
7977 .L630: |
7978 4bcc 8091 0000 lds r24,ParamSet+17 |
7979 4bd0 8823 tst r24 |
7980 4bd2 01F4 brne .L628 |
7981 4bd4 8091 0000 lds r24,HoverGas.2445 |
7982 4bd8 9091 0000 lds r25,HoverGas.2445+1 |
7983 4bdc 9C01 movw r18,r24 |
7984 4bde 37FD sbrc r19,7 |
7985 4be0 00C0 rjmp .L800 |
7986 .L631: |
7987 4be2 3595 asr r19 |
7988 4be4 2795 ror r18 |
7989 4be6 3595 asr r19 |
7990 4be8 2795 ror r18 |
7991 4bea 3093 0000 sts (StickGasHover.2444)+1,r19 |
7992 4bee 2093 0000 sts StickGasHover.2444,r18 |
7993 4bf2 8091 0000 lds r24,UBat |
7994 4bf6 9091 0000 lds r25,(UBat)+1 |
7995 4bfa 6091 0000 lds r22,LowVoltageWarning |
7996 4bfe 6C01 movw r12,r24 |
7997 4c00 2C9D mul r18,r12 |
7998 4c02 C001 movw r24,r0 |
7999 4c04 2D9D mul r18,r13 |
8000 4c06 900D add r25,r0 |
8001 4c08 3C9D mul r19,r12 |
8002 4c0a 900D add r25,r0 |
8003 4c0c 1124 clr r1 |
8004 4c0e 70E0 ldi r23,lo8(0) |
8005 4c10 0E94 0000 call __divmodhi4 |
8006 4c14 7093 0000 sts (StickGasHover.2444)+1,r23 |
8007 4c18 6093 0000 sts StickGasHover.2444,r22 |
8008 4c1c 6634 cpi r22,70 |
8009 4c1e 7105 cpc r23,__zero_reg__ |
8010 4c20 04F0 brlt .+2 |
8011 4c22 00C0 rjmp .L632 |
8012 4c24 86E4 ldi r24,lo8(70) |
8013 4c26 90E0 ldi r25,hi8(70) |
8014 4c28 9093 0000 sts (StickGasHover.2444)+1,r25 |
8015 4c2c 8093 0000 sts StickGasHover.2444,r24 |
8016 .L628: |
8017 4c30 8091 0000 lds r24,BaroExpandActive.2451 |
8018 4c34 9091 0000 lds r25,(BaroExpandActive.2451)+1 |
8019 4c38 892B or r24,r25 |
8020 4c3a 01F0 breq .L633 |
8021 4c3c 8091 0000 lds r24,ReadingHeight |
8022 4c40 9091 0000 lds r25,(ReadingHeight)+1 |
8023 4c44 A091 0000 lds r26,(ReadingHeight)+2 |
8024 4c48 B091 0000 lds r27,(ReadingHeight)+3 |
8025 4c4c 8093 0000 sts SetPointHeight,r24 |
8026 4c50 9093 0000 sts (SetPointHeight)+1,r25 |
8027 4c54 A093 0000 sts (SetPointHeight)+2,r26 |
8028 4c58 B093 0000 sts (SetPointHeight)+3,r27 |
8029 .L633: |
8030 4c5c 0091 0000 lds r16,HoverGas.2445 |
8031 4c60 1091 0000 lds r17,(HoverGas.2445)+1 |
8032 .L619: |
8033 4c64 8091 0000 lds r24,ReadingHeight |
8034 4c68 9091 0000 lds r25,(ReadingHeight)+1 |
8035 4c6c A091 0000 lds r26,(ReadingHeight)+2 |
8036 4c70 B091 0000 lds r27,(ReadingHeight)+3 |
8037 4c74 2091 0000 lds r18,SetPointHeight |
8038 4c78 3091 0000 lds r19,(SetPointHeight)+1 |
8039 4c7c 4091 0000 lds r20,(SetPointHeight)+2 |
8040 4c80 5091 0000 lds r21,(SetPointHeight)+3 |
8041 4c84 2817 cp r18,r24 |
8042 4c86 3907 cpc r19,r25 |
8043 4c88 4A07 cpc r20,r26 |
8044 4c8a 5B07 cpc r21,r27 |
8045 4c8c 04F0 brlt .L635 |
8046 4c8e 8091 0000 lds r24,ParamSet+88 |
8047 4c92 80FD sbrc r24,0 |
8048 4c94 00C0 rjmp .L636 |
8049 .L635: |
8050 4c96 8091 0000 lds r24,BaroExpandActive.2451 |
8051 4c9a 9091 0000 lds r25,(BaroExpandActive.2451)+1 |
8052 4c9e 892B or r24,r25 |
8053 4ca0 01F4 brne .+2 |
8054 4ca2 00C0 rjmp .L637 |
8055 4ca4 0091 0000 lds r16,HoverGas.2445 |
8056 4ca8 1091 0000 lds r17,(HoverGas.2445)+1 |
8057 .L638: |
8058 4cac 6091 0000 lds r22,NCGpsZ |
8059 4cb0 7727 clr r23 |
8060 4cb2 67FD sbrc r22,7 |
8061 4cb4 7095 com r23 |
8062 4cb6 872F mov r24,r23 |
8063 4cb8 972F mov r25,r23 |
8064 4cba 2091 0000 lds r18,ParamSet+16 |
8065 4cbe 30E0 ldi r19,lo8(0) |
8066 4cc0 40E0 ldi r20,lo8(0) |
8067 4cc2 50E0 ldi r21,hi8(0) |
8068 4cc4 0E94 0000 call __mulsi3 |
8069 4cc8 20E8 ldi r18,lo8(128) |
8070 4cca 30E0 ldi r19,hi8(128) |
8071 4ccc 40E0 ldi r20,hlo8(128) |
8072 4cce 50E0 ldi r21,hhi8(128) |
8073 4cd0 0E94 0000 call __divmodsi4 |
8074 4cd4 A901 movw r20,r18 |
8075 4cd6 C901 movw r24,r18 |
8076 4cd8 2FEF ldi r18,hi8(-127) |
8077 4cda 4138 cpi r20,lo8(-127) |
8078 4cdc 5207 cpc r21,r18 |
8079 4cde 04F4 brge .L652 |
8080 4ce0 81E8 ldi r24,lo8(-127) |
8081 4ce2 9FEF ldi r25,hi8(-127) |
8082 .L652: |
8083 4ce4 081B sub r16,r24 |
8084 4ce6 190B sbc r17,r25 |
8085 4ce8 B801 movw r22,r16 |
8086 4cea 8827 clr r24 |
8087 4cec 77FD sbrc r23,7 |
8088 4cee 8095 com r24 |
8089 4cf0 982F mov r25,r24 |
8090 4cf2 EDE0 ldi r30,13 |
8091 4cf4 660F 1: lsl r22 |
8092 4cf6 771F rol r23 |
8093 4cf8 881F rol r24 |
8094 4cfa 991F rol r25 |
8095 4cfc EA95 dec r30 |
8096 4cfe 01F4 brne 1b |
8097 4d00 A981 ldd r26,Y+1 |
8098 4d02 BA81 ldd r27,Y+2 |
8099 4d04 9D01 movw r18,r26 |
8100 4d06 4427 clr r20 |
8101 4d08 37FD sbrc r19,7 |
8102 4d0a 4095 com r20 |
8103 4d0c 542F mov r21,r20 |
8104 4d0e 0E94 0000 call __divmodsi4 |
8105 4d12 8091 0000 lds r24,FilterHCGas.2441 |
8106 4d16 9091 0000 lds r25,(FilterHCGas.2441)+1 |
8107 4d1a AC01 movw r20,r24 |
8108 4d1c 440F lsl r20 |
8109 4d1e 551F rol r21 |
8110 4d20 480F add r20,r24 |
8111 4d22 591F adc r21,r25 |
8112 4d24 420F add r20,r18 |
8113 4d26 531F adc r21,r19 |
8114 4d28 57FD sbrc r21,7 |
8115 4d2a 00C0 rjmp .L801 |
8116 .L653: |
8117 4d2c 5595 asr r21 |
8118 4d2e 4795 ror r20 |
8119 4d30 5595 asr r21 |
8120 4d32 4795 ror r20 |
8121 4d34 5093 0000 sts (FilterHCGas.2441)+1,r21 |
8122 4d38 4093 0000 sts FilterHCGas.2441,r20 |
8123 4d3c 8091 0000 lds r24,ParamSet+9 |
8124 4d40 282F mov r18,r24 |
8125 4d42 30E0 ldi r19,lo8(0) |
8126 4d44 220F lsl r18 |
8127 4d46 331F rol r19 |
8128 4d48 220F lsl r18 |
8129 4d4a 331F rol r19 |
8130 4d4c 4217 cp r20,r18 |
8131 4d4e 5307 cpc r21,r19 |
8132 4d50 04F0 brlt .L731 |
8133 4d52 8091 0000 lds r24,ParamSet+22 |
8134 4d56 282F mov r18,r24 |
8135 4d58 30E0 ldi r19,lo8(0) |
8136 4d5a 2451 subi r18,lo8(-(-20)) |
8137 4d5c 3040 sbci r19,hi8(-(-20)) |
8138 4d5e 220F lsl r18 |
8139 4d60 331F rol r19 |
8140 4d62 220F lsl r18 |
8141 4d64 331F rol r19 |
8142 4d66 2417 cp r18,r20 |
8143 4d68 3507 cpc r19,r21 |
8144 4d6a 04F4 brge .L655 |
8145 .L731: |
8146 4d6c 3093 0000 sts (FilterHCGas.2441)+1,r19 |
8147 4d70 2093 0000 sts FilterHCGas.2441,r18 |
8148 .L655: |
8149 4d74 8091 0000 lds r24,ParamSet+88 |
8150 4d78 80FD sbrc r24,0 |
8151 4d7a 00C0 rjmp .L725 |
8152 4d7c 8091 0000 lds r24,FilterHCGas.2441 |
8153 4d80 9091 0000 lds r25,(FilterHCGas.2441)+1 |
8154 .L656: |
8155 4d84 99AF std Y+57,r25 |
8156 4d86 88AF std Y+56,r24 |
8157 4d88 00C0 rjmp .L636 |
8158 .L616: |
8159 4d8a 8091 0000 lds r24,ParamSet+17 |
8160 4d8e 8823 tst r24 |
8161 4d90 01F0 breq .+2 |
8162 4d92 00C0 rjmp .L802 |
8163 4d94 8091 0000 lds r24,HoverGas.2445 |
8164 4d98 9091 0000 lds r25,HoverGas.2445+1 |
8165 4d9c 9C01 movw r18,r24 |
8166 4d9e 37FD sbrc r19,7 |
8167 4da0 00C0 rjmp .L803 |
8168 .L659: |
8169 4da2 3595 asr r19 |
8170 4da4 2795 ror r18 |
8171 4da6 3595 asr r19 |
8172 4da8 2795 ror r18 |
8173 4daa 3093 0000 sts (StickGasHover.2444)+1,r19 |
8174 4dae 2093 0000 sts StickGasHover.2444,r18 |
8175 4db2 8091 0000 lds r24,UBat |
8176 4db6 9091 0000 lds r25,(UBat)+1 |
8177 4dba 6091 0000 lds r22,LowVoltageWarning |
8178 4dbe 5C01 movw r10,r24 |
8179 4dc0 2A9D mul r18,r10 |
8180 4dc2 C001 movw r24,r0 |
8181 4dc4 2B9D mul r18,r11 |
8182 4dc6 900D add r25,r0 |
8183 4dc8 3A9D mul r19,r10 |
8184 4dca 900D add r25,r0 |
8185 4dcc 1124 clr r1 |
8186 4dce 70E0 ldi r23,lo8(0) |
8187 4dd0 0E94 0000 call __divmodhi4 |
8188 4dd4 7093 0000 sts (StickGasHover.2444)+1,r23 |
8189 4dd8 6093 0000 sts StickGasHover.2444,r22 |
8190 .L658: |
8191 4ddc 8091 0000 lds r24,StickGasHover.2444 |
8192 4de0 9091 0000 lds r25,(StickGasHover.2444)+1 |
8193 4de4 8634 cpi r24,70 |
8194 4de6 9105 cpc r25,__zero_reg__ |
8195 4de8 04F0 brlt .+2 |
8196 4dea 00C0 rjmp .L660 |
8197 4dec 86E4 ldi r24,lo8(70) |
8198 4dee 90E0 ldi r25,hi8(70) |
8199 4df0 9093 0000 sts (StickGasHover.2444)+1,r25 |
8200 4df4 8093 0000 sts StickGasHover.2444,r24 |
8201 .L661: |
8202 4df8 C8AC ldd r12,Y+56 |
8203 4dfa D9AC ldd r13,Y+57 |
8204 4dfc D092 0000 sts (FilterHCGas.2441)+1,r13 |
8205 4e00 C092 0000 sts FilterHCGas.2441,r12 |
8206 .L636: |
8207 4e04 8091 0000 lds r24,MKFlags |
8208 4e08 81FD sbrc r24,1 |
8209 4e0a 00C0 rjmp .+4 |
8210 4e0c 0C94 0000 jmp .L604 |
8211 4e10 8091 0000 lds r24,MKFlags |
8212 4e14 84FF sbrs r24,4 |
8213 4e16 00C0 rjmp .+4 |
8214 4e18 0C94 0000 jmp .L604 |
8215 4e1c 8091 0000 lds r24,HoverGasFilter.2448 |
8216 4e20 9091 0000 lds r25,(HoverGasFilter.2448)+1 |
8217 4e24 A091 0000 lds r26,(HoverGasFilter.2448)+2 |
8218 4e28 B091 0000 lds r27,(HoverGasFilter.2448)+3 |
8219 4e2c 0097 sbiw r24,0 |
8220 4e2e A105 cpc r26,__zero_reg__ |
8221 4e30 B105 cpc r27,__zero_reg__ |
8222 4e32 01F4 brne .L662 |
8223 4e34 28AD ldd r18,Y+56 |
8224 4e36 39AD ldd r19,Y+57 |
8225 4e38 C901 movw r24,r18 |
8226 4e3a AA27 clr r26 |
8227 4e3c 97FD sbrc r25,7 |
8228 4e3e A095 com r26 |
8229 4e40 BA2F mov r27,r26 |
8230 4e42 1CE0 ldi r17,12 |
8231 4e44 880F 1: lsl r24 |
8232 4e46 991F rol r25 |
8233 4e48 AA1F rol r26 |
8234 4e4a BB1F rol r27 |
8235 4e4c 1A95 dec r17 |
8236 4e4e 01F4 brne 1b |
8237 4e50 8093 0000 sts HoverGasFilter.2448,r24 |
8238 4e54 9093 0000 sts (HoverGasFilter.2448)+1,r25 |
8239 4e58 A093 0000 sts (HoverGasFilter.2448)+2,r26 |
8240 4e5c B093 0000 sts (HoverGasFilter.2448)+3,r27 |
8241 .L662: |
8242 4e60 8091 0000 lds r24,ReadingVario |
8243 4e64 9091 0000 lds r25,(ReadingVario)+1 |
8244 4e68 97FD sbrc r25,7 |
8245 4e6a 00C0 rjmp .L804 |
8246 .L663: |
8247 4e6c 8436 cpi r24,100 |
8248 4e6e 9105 cpc r25,__zero_reg__ |
8249 4e70 04F0 brlt .+4 |
8250 4e72 0C94 0000 jmp .L604 |
8251 4e76 48AD ldd r20,Y+56 |
8252 4e78 59AD ldd r21,Y+57 |
8253 4e7a BA01 movw r22,r20 |
8254 4e7c 8827 clr r24 |
8255 4e7e 77FD sbrc r23,7 |
8256 4e80 8095 com r24 |
8257 4e82 982F mov r25,r24 |
8258 4e84 A981 ldd r26,Y+1 |
8259 4e86 BA81 ldd r27,Y+2 |
8260 4e88 9D01 movw r18,r26 |
8261 4e8a 4427 clr r20 |
8262 4e8c 37FD sbrc r19,7 |
8263 4e8e 4095 com r20 |
8264 4e90 542F mov r21,r20 |
8265 4e92 0E94 0000 call __mulsi3 |
8266 4e96 20E0 ldi r18,lo8(8192) |
8267 4e98 30E2 ldi r19,hi8(8192) |
8268 4e9a 40E0 ldi r20,hlo8(8192) |
8269 4e9c 50E0 ldi r21,hhi8(8192) |
8270 4e9e 0E94 0000 call __divmodsi4 |
8271 4ea2 7901 movw r14,r18 |
8272 4ea4 8A01 movw r16,r20 |
8273 4ea6 8091 0000 lds r24,ModelIsFlying |
8274 4eaa 9091 0000 lds r25,(ModelIsFlying)+1 |
8275 4eae B7E0 ldi r27,hi8(2000) |
8276 4eb0 803D cpi r24,lo8(2000) |
8277 4eb2 9B07 cpc r25,r27 |
8278 4eb4 00F0 brlo .+2 |
8279 4eb6 00C0 rjmp .L664 |
8280 4eb8 2091 0000 lds r18,HoverGasFilter.2448 |
8281 4ebc 3091 0000 lds r19,(HoverGasFilter.2448)+1 |
8282 4ec0 4091 0000 lds r20,(HoverGasFilter.2448)+2 |
8283 4ec4 5091 0000 lds r21,(HoverGasFilter.2448)+3 |
8284 4ec8 DA01 movw r26,r20 |
8285 4eca C901 movw r24,r18 |
8286 4ecc F9E0 ldi r31,9 |
8287 4ece B695 1: lsr r27 |
8288 4ed0 A795 ror r26 |
8289 4ed2 9795 ror r25 |
8290 4ed4 8795 ror r24 |
8291 4ed6 FA95 dec r31 |
8292 4ed8 01F4 brne 1b |
8293 4eda 5901 movw r10,r18 |
8294 4edc 6A01 movw r12,r20 |
8295 4ede A81A sub r10,r24 |
8296 4ee0 B90A sbc r11,r25 |
8297 4ee2 CA0A sbc r12,r26 |
8298 4ee4 DB0A sbc r13,r27 |
8299 4ee6 A801 movw r20,r16 |
8300 4ee8 9701 movw r18,r14 |
8301 4eea 220F lsl r18 |
8302 4eec 331F rol r19 |
8303 4eee 441F rol r20 |
8304 4ef0 551F rol r21 |
8305 4ef2 220F lsl r18 |
8306 4ef4 331F rol r19 |
8307 4ef6 441F rol r20 |
8308 4ef8 551F rol r21 |
8309 4efa 220F lsl r18 |
8310 4efc 331F rol r19 |
8311 4efe 441F rol r20 |
8312 4f00 551F rol r21 |
8313 4f02 2A0D add r18,r10 |
8314 4f04 3B1D adc r19,r11 |
8315 4f06 4C1D adc r20,r12 |
8316 4f08 5D1D adc r21,r13 |
8317 4f0a 2093 0000 sts HoverGasFilter.2448,r18 |
8318 4f0e 3093 0000 sts (HoverGasFilter.2448)+1,r19 |
8319 4f12 4093 0000 sts (HoverGasFilter.2448)+2,r20 |
8320 4f16 5093 0000 sts (HoverGasFilter.2448)+3,r21 |
8321 .L665: |
8322 4f1a DA01 movw r26,r20 |
8323 4f1c C901 movw r24,r18 |
8324 4f1e 2CE0 ldi r18,12 |
8325 4f20 B695 1: lsr r27 |
8326 4f22 A795 ror r26 |
8327 4f24 9795 ror r25 |
8328 4f26 8795 ror r24 |
8329 4f28 2A95 dec r18 |
8330 4f2a 01F4 brne 1b |
8331 4f2c FC01 movw r30,r24 |
8332 4f2e 9093 0000 sts (HoverGas.2445)+1,r25 |
8333 4f32 8093 0000 sts HoverGas.2445,r24 |
8334 4f36 6091 0000 lds r22,ParamSet+15 |
8335 4f3a 6623 tst r22 |
8336 4f3c 01F4 brne .+2 |
8337 4f3e 00C0 rjmp .L668 |
8338 4f40 70E0 ldi r23,lo8(0) |
8339 4f42 0E94 0000 call __divmodhi4 |
8340 4f46 CB01 movw r24,r22 |
8341 4f48 9F01 movw r18,r30 |
8342 4f4a 261B sub r18,r22 |
8343 4f4c 370B sbc r19,r23 |
8344 4f4e 3093 0000 sts (HoverGasMin.2446)+1,r19 |
8345 4f52 2093 0000 sts HoverGasMin.2446,r18 |
8346 4f56 8E0F add r24,r30 |
8347 4f58 9F1F adc r25,r31 |
8348 4f5a 9093 0000 sts (HoverGasMax.2447)+1,r25 |
8349 4f5e 8093 0000 sts HoverGasMax.2447,r24 |
8350 4f62 0C94 0000 jmp .L604 |
8351 .L605: |
8352 4f66 8091 0000 lds r24,AdAirPressure |
8353 4f6a 9091 0000 lds r25,(AdAirPressure)+1 |
8354 4f6e 8C59 subi r24,lo8(924) |
8355 4f70 9340 sbci r25,hi8(924) |
8356 4f72 04F0 brlt .+2 |
8357 4f74 00C0 rjmp .L805 |
8358 4f76 8091 0000 lds r24,AdAirPressure |
8359 4f7a 9091 0000 lds r25,(AdAirPressure)+1 |
8360 4f7e 8436 cpi r24,100 |
8361 4f80 9105 cpc r25,__zero_reg__ |
8362 4f82 04F0 brlt .+2 |
8363 4f84 00C0 rjmp .L609 |
8364 4f86 87B5 in r24,71-32 |
8365 4f88 8B30 cpi r24,lo8(11) |
8366 4f8a 00F4 brsh .+2 |
8367 4f8c 00C0 rjmp .L610 |
8368 4f8e 8091 0000 lds r24,ExpandBaro |
8369 4f92 8F5F subi r24,lo8(-(1)) |
8370 .L729: |
8371 4f94 8093 0000 sts ExpandBaro,r24 |
8372 4f98 26EF ldi r18,lo8(-10) |
8373 4f9a 8202 muls r24,r18 |
8374 4f9c C001 movw r24,r0 |
8375 4f9e 1124 clr r1 |
8376 4fa0 2091 0000 lds r18,PressureSensorOffset |
8377 4fa4 280F add r18,r24 |
8378 4fa6 27BD out 71-32,r18 |
8379 4fa8 8CE2 ldi r24,lo8(300) |
8380 4faa 91E0 ldi r25,hi8(300) |
8381 4fac 9093 0000 sts (BeepTime)+1,r25 |
8382 4fb0 8093 0000 sts BeepTime,r24 |
8383 4fb4 8EE5 ldi r24,lo8(350) |
8384 4fb6 91E0 ldi r25,hi8(350) |
8385 4fb8 9093 0000 sts (BaroExpandActive.2451)+1,r25 |
8386 4fbc 8093 0000 sts BaroExpandActive.2451,r24 |
8387 4fc0 0C94 0000 jmp .L606 |
8388 .L470: |
8389 4fc4 1092 0000 sts delay_stopmotors.2418,__zero_reg__ |
8390 4fc8 0C94 0000 jmp .L410 |
8391 .L512: |
8392 4fcc E090 0000 lds r14,ParamSet+23 |
8393 4fd0 FF24 clr r15 |
8394 4fd2 00E0 ldi r16,lo8(0) |
8395 4fd4 10E0 ldi r17,hi8(0) |
8396 4fd6 68A5 ldd r22,Y+40 |
8397 4fd8 79A5 ldd r23,Y+41 |
8398 4fda 8AA5 ldd r24,Y+42 |
8399 4fdc 9BA5 ldd r25,Y+43 |
8400 4fde A801 movw r20,r16 |
8401 4fe0 9701 movw r18,r14 |
8402 4fe2 0E94 0000 call __divmodsi4 |
8403 4fe6 8091 0000 lds r24,AccNick |
8404 4fea 9091 0000 lds r25,(AccNick)+1 |
8405 4fee AA27 clr r26 |
8406 4ff0 97FD sbrc r25,7 |
8407 4ff2 A095 com r26 |
8408 4ff4 BA2F mov r27,r26 |
8409 4ff6 281B sub r18,r24 |
8410 4ff8 390B sbc r19,r25 |
8411 4ffa 4A0B sbc r20,r26 |
8412 4ffc 5B0B sbc r21,r27 |
8413 4ffe CA01 movw r24,r20 |
8414 5000 B901 movw r22,r18 |
8415 5002 20E1 ldi r18,lo8(16) |
8416 5004 30E0 ldi r19,hi8(16) |
8417 5006 40E0 ldi r20,hlo8(16) |
8418 5008 50E0 ldi r21,hhi8(16) |
8419 500a 0E94 0000 call __divmodsi4 |
8420 500e 522E mov r5,r18 |
8421 5010 432E mov r4,r19 |
8422 5012 342E mov r3,r20 |
8423 5014 252E mov r2,r21 |
8424 5016 4201 movw r8,r4 |
8425 5018 3101 movw r6,r2 |
8426 501a 6CA1 ldd r22,Y+36 |
8427 501c 7DA1 ldd r23,Y+37 |
8428 501e 8EA1 ldd r24,Y+38 |
8429 5020 9FA1 ldd r25,Y+39 |
8430 5022 A801 movw r20,r16 |
8431 5024 9701 movw r18,r14 |
8432 5026 0E94 0000 call __divmodsi4 |
8433 502a 8091 0000 lds r24,AccRoll |
8434 502e 9091 0000 lds r25,(AccRoll)+1 |
8435 5032 AA27 clr r26 |
8436 5034 97FD sbrc r25,7 |
8437 5036 A095 com r26 |
8438 5038 BA2F mov r27,r26 |
8439 503a 281B sub r18,r24 |
8440 503c 390B sbc r19,r25 |
8441 503e 4A0B sbc r20,r26 |
8442 5040 5B0B sbc r21,r27 |
8443 5042 CA01 movw r24,r20 |
8444 5044 B901 movw r22,r18 |
8445 5046 20E1 ldi r18,lo8(16) |
8446 5048 30E0 ldi r19,hi8(16) |
8447 504a 40E0 ldi r20,hlo8(16) |
8448 504c 50E0 ldi r21,hhi8(16) |
8449 504e 0E94 0000 call __divmodsi4 |
8450 5052 122F mov r17,r18 |
8451 5054 032F mov r16,r19 |
8452 5056 F42E mov r15,r20 |
8453 5058 E52E mov r14,r21 |
8454 505a 822F mov r24,r18 |
8455 505c 932F mov r25,r19 |
8456 505e A42F mov r26,r20 |
8457 5060 B52F mov r27,r21 |
8458 5062 5C01 movw r10,r24 |
8459 5064 6D01 movw r12,r26 |
8460 5066 8091 0000 lds r24,MaxStickNick |
8461 506a 9091 0000 lds r25,(MaxStickNick)+1 |
8462 506e 8134 cpi r24,65 |
8463 5070 9105 cpc r25,__zero_reg__ |
8464 5072 04F4 brge .+2 |
8465 5074 00C0 rjmp .L806 |
8466 .L522: |
8467 5076 652D mov r22,r5 |
8468 5078 742D mov r23,r4 |
8469 507a 832D mov r24,r3 |
8470 507c 922D mov r25,r2 |
8471 507e 23E0 ldi r18,lo8(3) |
8472 5080 30E0 ldi r19,hi8(3) |
8473 5082 40E0 ldi r20,hlo8(3) |
8474 5084 50E0 ldi r21,hhi8(3) |
8475 5086 0E94 0000 call __divmodsi4 |
8476 508a 922E mov r9,r18 |
8477 508c 832E mov r8,r19 |
8478 508e 742E mov r7,r20 |
8479 5090 652E mov r6,r21 |
8480 5092 612F mov r22,r17 |
8481 5094 702F mov r23,r16 |
8482 5096 8F2D mov r24,r15 |
8483 5098 9E2D mov r25,r14 |
8484 509a 23E0 ldi r18,lo8(3) |
8485 509c 30E0 ldi r19,hi8(3) |
8486 509e 40E0 ldi r20,hlo8(3) |
8487 50a0 50E0 ldi r21,hhi8(3) |
8488 50a2 0E94 0000 call __divmodsi4 |
8489 50a6 C901 movw r24,r18 |
8490 50a8 DA01 movw r26,r20 |
8491 50aa 5C01 movw r10,r24 |
8492 50ac 6D01 movw r12,r26 |
8493 .L523: |
8494 50ae E091 0000 lds r30,ParamSet+3 |
8495 50b2 F0E0 ldi r31,lo8(0) |
8496 50b4 EE0F lsl r30 |
8497 50b6 FF1F rol r31 |
8498 50b8 E050 subi r30,lo8(-(PPM_in)) |
8499 50ba F040 sbci r31,hi8(-(PPM_in)) |
8500 50bc 0190 ld __tmp_reg__,Z+ |
8501 50be F081 ld r31,Z |
8502 50c0 E02D mov r30,__tmp_reg__ |
8503 50c2 F7FD sbrc r31,7 |
8504 50c4 00C0 rjmp .L807 |
8505 .L525: |
8506 50c6 7A97 sbiw r30,26 |
8507 50c8 04F0 brlt .L524 |
8508 50ca 692D mov r22,r9 |
8509 50cc 782D mov r23,r8 |
8510 50ce 872D mov r24,r7 |
8511 50d0 962D mov r25,r6 |
8512 50d2 23E0 ldi r18,lo8(3) |
8513 50d4 30E0 ldi r19,hi8(3) |
8514 50d6 40E0 ldi r20,hlo8(3) |
8515 50d8 50E0 ldi r21,hhi8(3) |
8516 50da 0E94 0000 call __divmodsi4 |
8517 50de 922E mov r9,r18 |
8518 50e0 832E mov r8,r19 |
8519 50e2 742E mov r7,r20 |
8520 50e4 652E mov r6,r21 |
8521 50e6 C601 movw r24,r12 |
8522 50e8 B501 movw r22,r10 |
8523 50ea 23E0 ldi r18,lo8(3) |
8524 50ec 30E0 ldi r19,hi8(3) |
8525 50ee 40E0 ldi r20,hlo8(3) |
8526 50f0 50E0 ldi r21,hhi8(3) |
8527 50f2 0E94 0000 call __divmodsi4 |
8528 50f6 C901 movw r24,r18 |
8529 50f8 DA01 movw r26,r20 |
8530 50fa 5C01 movw r10,r24 |
8531 50fc 6D01 movw r12,r26 |
8532 .L524: |
8533 50fe 292D mov r18,r9 |
8534 5100 382D mov r19,r8 |
8535 5102 472D mov r20,r7 |
8536 5104 562D mov r21,r6 |
8537 5106 C901 movw r24,r18 |
8538 5108 DA01 movw r26,r20 |
8539 510a BC01 movw r22,r24 |
8540 510c CD01 movw r24,r26 |
8541 510e 6132 cpi r22,lo8(33) |
8542 5110 7105 cpc r23,__zero_reg__ |
8543 5112 8105 cpc r24,__zero_reg__ |
8544 5114 9105 cpc r25,__zero_reg__ |
8545 5116 04F0 brlt .L526 |
8546 5118 60E2 ldi r22,lo8(32) |
8547 511a 70E0 ldi r23,hi8(32) |
8548 511c 80E0 ldi r24,hlo8(32) |
8549 511e 90E0 ldi r25,hhi8(32) |
8550 .L526: |
8551 5120 7B01 movw r14,r22 |
8552 5122 8C01 movw r16,r24 |
8553 5124 603E cpi r22,lo8(-32) |
8554 5126 3FEF ldi r19,hi8(-32) |
8555 5128 7307 cpc r23,r19 |
8556 512a 3FEF ldi r19,hlo8(-32) |
8557 512c 8307 cpc r24,r19 |
8558 512e 3FEF ldi r19,hhi8(-32) |
8559 5130 9307 cpc r25,r19 |
8560 5132 04F4 brge .L527 |
8561 5134 90EE ldi r25,lo8(-32) |
8562 5136 E92E mov r14,r25 |
8563 5138 9FEF ldi r25,hi8(-32) |
8564 513a F92E mov r15,r25 |
8565 513c 9FEF ldi r25,hlo8(-32) |
8566 513e 092F mov r16,r25 |
8567 5140 9FEF ldi r25,hhi8(-32) |
8568 5142 192F mov r17,r25 |
8569 .L527: |
8570 5144 40EE ldi r20,lo8(-32) |
8571 5146 A416 cp r10,r20 |
8572 5148 4FEF ldi r20,hi8(-32) |
8573 514a B406 cpc r11,r20 |
8574 514c 4FEF ldi r20,hlo8(-32) |
8575 514e C406 cpc r12,r20 |
8576 5150 4FEF ldi r20,hhi8(-32) |
8577 5152 D406 cpc r13,r20 |
8578 5154 04F0 brlt .+2 |
8579 5156 00C0 rjmp .L528 |
8580 5158 80EE ldi r24,lo8(-32) |
8581 515a A82E mov r10,r24 |
8582 515c 8FEF ldi r24,hi8(-32) |
8583 515e B82E mov r11,r24 |
8584 5160 8FEF ldi r24,hlo8(-32) |
8585 5162 C82E mov r12,r24 |
8586 5164 8FEF ldi r24,hhi8(-32) |
8587 5166 D82E mov r13,r24 |
8588 5168 00C0 rjmp .L521 |
8589 .L802: |
8590 516a 8093 0000 sts StickGasHover.2444,r24 |
8591 516e 1092 0000 sts StickGasHover.2444+1,__zero_reg__ |
8592 5172 00C0 rjmp .L658 |
8593 .L798: |
8594 5174 8091 0000 lds r24,FCParam+1 |
8595 5178 2091 0000 lds r18,ExternHeightValue |
8596 517c 3091 0000 lds r19,(ExternHeightValue)+1 |
8597 5180 280F add r18,r24 |
8598 5182 311D adc r19,__zero_reg__ |
8599 5184 8091 0000 lds r24,ParamSet+13 |
8600 5188 90E0 ldi r25,lo8(0) |
8601 518a FC01 movw r30,r24 |
8602 518c 2E9F mul r18,r30 |
8603 518e C001 movw r24,r0 |
8604 5190 2F9F mul r18,r31 |
8605 5192 900D add r25,r0 |
8606 5194 3E9F mul r19,r30 |
8607 5196 900D add r25,r0 |
8608 5198 1124 clr r1 |
8609 519a AA27 clr r26 |
8610 519c 97FD sbrc r25,7 |
8611 519e A095 com r26 |
8612 51a0 BA2F mov r27,r26 |
8613 51a2 8093 0000 sts SetPointHeight,r24 |
8614 51a6 9093 0000 sts (SetPointHeight)+1,r25 |
8615 51aa A093 0000 sts (SetPointHeight)+2,r26 |
8616 51ae B093 0000 sts (SetPointHeight)+3,r27 |
8617 51b2 00C0 rjmp .L614 |
8618 .L612: |
8619 51b4 81E0 ldi r24,lo8(1) |
8620 51b6 8093 0000 sts HCActive.2443,r24 |
8621 51ba 88EC ldi r24,lo8(-56) |
8622 51bc 8093 0000 sts delay.2449,r24 |
8623 51c0 00C0 rjmp .L613 |
8624 .L447: |
8625 51c2 8081 ld r24,Z |
8626 51c4 9181 ldd r25,Z+1 |
8627 51c6 855B subi r24,lo8(-75) |
8628 51c8 9F4F sbci r25,hi8(-75) |
8629 51ca 04F0 brlt .+2 |
8630 51cc 00C0 rjmp .L464 |
8631 51ce 8091 0000 lds r24,delay_neutral.2416 |
8632 51d2 8F5F subi r24,lo8(-(1)) |
8633 51d4 8093 0000 sts delay_neutral.2416,r24 |
8634 51d8 893C cpi r24,lo8(-55) |
8635 51da 00F4 brsh .+2 |
8636 51dc 00C0 rjmp .L446 |
8637 51de 1092 0000 sts delay_neutral.2416,__zero_reg__ |
8638 51e2 3BE0 ldi r19,lo8(11) |
8639 51e4 3D15 cp r19,r13 |
8640 51e6 00F4 brsh .+2 |
8641 51e8 00C0 rjmp .L466 |
8642 51ea 2998 cbi 37-32,1 |
8643 .L467: |
8644 51ec 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ |
8645 51f0 1092 0000 sts ModelIsFlying,__zero_reg__ |
8646 51f4 0E94 0000 call Servo_Off |
8647 51f8 81E0 ldi r24,lo8(1) |
8648 .L726: |
8649 51fa 0E94 0000 call SetNeutral |
8650 51fe 0E94 0000 call Servo_On |
8651 5202 0E94 0000 call GetActiveParamSet |
8652 5206 68E7 ldi r22,lo8(120) |
8653 5208 70E0 ldi r23,hi8(120) |
8654 520a 0E94 0000 call Beep |
8655 520e 8091 0000 lds r24,ParamSet+2 |
8656 5212 A82F mov r26,r24 |
8657 5214 B0E0 ldi r27,lo8(0) |
8658 5216 AA0F lsl r26 |
8659 5218 BB1F rol r27 |
8660 521a 00C0 rjmp .L446 |
8661 .L805: |
8662 521c 87B5 in r24,71-32 |
8663 521e 853F cpi r24,lo8(-11) |
8664 5220 00F0 brlo .+2 |
8665 5222 00C0 rjmp .L608 |
8666 5224 8091 0000 lds r24,ExpandBaro |
8667 5228 8150 subi r24,lo8(-(-1)) |
8668 522a 00C0 rjmp .L729 |
8669 .L806: |
8670 522c 8091 0000 lds r24,MaxStickRoll |
8671 5230 9091 0000 lds r25,(MaxStickRoll)+1 |
8672 5234 8134 cpi r24,65 |
8673 5236 9105 cpc r25,__zero_reg__ |
8674 5238 04F4 brge .+2 |
8675 523a 00C0 rjmp .L523 |
8676 523c 00C0 rjmp .L522 |
8677 .L660: |
8678 523e 8739 cpi r24,151 |
8679 5240 9105 cpc r25,__zero_reg__ |
8680 5242 04F4 brge .+2 |
8681 5244 00C0 rjmp .L661 |
8682 5246 86E9 ldi r24,lo8(150) |
8683 5248 90E0 ldi r25,hi8(150) |
8684 524a 9093 0000 sts (StickGasHover.2444)+1,r25 |
8685 524e 8093 0000 sts StickGasHover.2444,r24 |
8686 5252 00C0 rjmp .L661 |
8687 .L768: |
8688 5254 81E0 ldi r24,lo8(1) |
8689 5256 90E0 ldi r25,hi8(1) |
8690 5258 9093 0000 sts (UpdateCompassCourse.2420)+1,r25 |
8691 525c 8093 0000 sts UpdateCompassCourse.2420,r24 |
8692 5260 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ |
8693 5264 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ |
8694 5268 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ |
8695 526c 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ |
8696 5270 1092 0000 sts SetPointYaw.2410,__zero_reg__ |
8697 5274 1092 0000 sts (SetPointYaw.2410)+1,__zero_reg__ |
8698 5278 1092 0000 sts (SetPointYaw.2410)+2,__zero_reg__ |
8699 527c 1092 0000 sts (SetPointYaw.2410)+3,__zero_reg__ |
8700 5280 0C94 0000 jmp .L413 |
8701 .L766: |
8702 5284 27E0 ldi r18,lo8(7) |
8703 5286 30E0 ldi r19,hi8(7) |
8704 5288 420E add r4,r18 |
8705 528a 531E adc r5,r19 |
8706 528c 0C94 0000 jmp .L589 |
8707 .L528: |
8708 5290 51E2 ldi r21,lo8(33) |
8709 5292 A516 cp r10,r21 |
8710 5294 B104 cpc r11,__zero_reg__ |
8711 5296 C104 cpc r12,__zero_reg__ |
8712 5298 D104 cpc r13,__zero_reg__ |
8713 529a 04F4 brge .+2 |
8714 529c 00C0 rjmp .L521 |
8715 529e B0E2 ldi r27,lo8(32) |
8716 52a0 AB2E mov r10,r27 |
8717 52a2 B12C mov r11,__zero_reg__ |
8718 52a4 C12C mov r12,__zero_reg__ |
8719 52a6 D12C mov r13,__zero_reg__ |
8720 52a8 00C0 rjmp .L521 |
8721 .L795: |
8722 52aa F095 com r31 |
8723 52ac E195 neg r30 |
8724 52ae FF4F sbci r31,lo8(-1) |
8725 52b0 0C94 0000 jmp .L516 |
8726 .L554: |
8727 52b4 51E0 ldi r21,lo8(4097) |
8728 52b6 E516 cp r14,r21 |
8729 52b8 50E1 ldi r21,hi8(4097) |
8730 52ba F506 cpc r15,r21 |
8731 52bc 50E0 ldi r21,hlo8(4097) |
8732 52be 0507 cpc r16,r21 |
8733 52c0 50E0 ldi r21,hhi8(4097) |
8734 52c2 1507 cpc r17,r21 |
8735 52c4 04F4 brge .+2 |
8736 52c6 00C0 rjmp .L556 |
8737 52c8 8091 0000 lds r24,last_r_p.2429 |
8738 52cc 8823 tst r24 |
8739 52ce 01F4 brne .+2 |
8740 52d0 00C0 rjmp .L557 |
8741 52d2 D801 movw r26,r16 |
8742 52d4 C701 movw r24,r14 |
8743 52d6 39E0 ldi r19,9 |
8744 52d8 B595 1: asr r27 |
8745 52da A795 ror r26 |
8746 52dc 9795 ror r25 |
8747 52de 8795 ror r24 |
8748 52e0 3A95 dec r19 |
8749 52e2 01F4 brne 1b |
8750 52e4 A090 0000 lds r10,cnt.2426 |
8751 52e8 B090 0000 lds r11,(cnt.2426)+1 |
8752 52ec A80E add r10,r24 |
8753 52ee B91E adc r11,r25 |
8754 52f0 B092 0000 sts (cnt.2426)+1,r11 |
8755 52f4 A092 0000 sts cnt.2426,r10 |
8756 52f8 C801 movw r24,r16 |
8757 52fa B701 movw r22,r14 |
8758 52fc 28E0 ldi r18,lo8(8) |
8759 52fe 30E0 ldi r19,hi8(8) |
8760 5300 40E0 ldi r20,hlo8(8) |
8761 5302 50E0 ldi r21,hhi8(8) |
8762 5304 0E94 0000 call __divmodsi4 |
8763 5308 C901 movw r24,r18 |
8764 530a DA01 movw r26,r20 |
8765 530c 8093 0000 sts CorrectionRoll.2414,r24 |
8766 5310 9093 0000 sts (CorrectionRoll.2414)+1,r25 |
8767 5314 A093 0000 sts (CorrectionRoll.2414)+2,r26 |
8768 5318 B093 0000 sts (CorrectionRoll.2414)+3,r27 |
8769 531c 8958 subi r24,lo8(5001) |
8770 531e 9341 sbci r25,hi8(5001) |
8771 5320 A040 sbci r26,hlo8(5001) |
8772 5322 B040 sbci r27,hhi8(5001) |
8773 5324 04F0 brlt .L558 |
8774 5326 88E8 ldi r24,lo8(5000) |
8775 5328 93E1 ldi r25,hi8(5000) |
8776 532a A0E0 ldi r26,hlo8(5000) |
8777 532c B0E0 ldi r27,hhi8(5000) |
8778 532e 8093 0000 sts CorrectionRoll.2414,r24 |
8779 5332 9093 0000 sts (CorrectionRoll.2414)+1,r25 |
8780 5336 A093 0000 sts (CorrectionRoll.2414)+2,r26 |
8781 533a B093 0000 sts (CorrectionRoll.2414)+3,r27 |
8782 .L558: |
8783 533e 6091 0000 lds r22,CorrectionRoll.2414 |
8784 5342 7091 0000 lds r23,(CorrectionRoll.2414)+1 |
8785 5346 8091 0000 lds r24,(CorrectionRoll.2414)+2 |
8786 534a 9091 0000 lds r25,(CorrectionRoll.2414)+3 |
8787 534e 20E0 ldi r18,lo8(256) |
8788 5350 31E0 ldi r19,hi8(256) |
8789 5352 40E0 ldi r20,hlo8(256) |
8790 5354 50E0 ldi r21,hhi8(256) |
8791 5356 0E94 0000 call __divmodsi4 |
8792 535a 8091 0000 lds r24,AttitudeCorrectionRoll |
8793 535e 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 |
8794 5362 820F add r24,r18 |
8795 5364 931F adc r25,r19 |
8796 5366 9093 0000 sts (AttitudeCorrectionRoll)+1,r25 |
8797 536a 8093 0000 sts AttitudeCorrectionRoll,r24 |
8798 .L559: |
8799 536e 70E0 ldi r23,lo8(-4096) |
8800 5370 E716 cp r14,r23 |
8801 5372 70EF ldi r23,hi8(-4096) |
8802 5374 F706 cpc r15,r23 |
8803 5376 7FEF ldi r23,hlo8(-4096) |
8804 5378 0707 cpc r16,r23 |
8805 537a 7FEF ldi r23,hhi8(-4096) |
8806 537c 1707 cpc r17,r23 |
8807 537e 04F0 brlt .+2 |
8808 5380 00C0 rjmp .L560 |
8809 5382 8091 0000 lds r24,last_r_n.2430 |
8810 5386 8823 tst r24 |
8811 5388 01F4 brne .+2 |
8812 538a 00C0 rjmp .L561 |
8813 538c 8827 clr r24 |
8814 538e 9927 clr r25 |
8815 5390 DC01 movw r26,r24 |
8816 5392 8E19 sub r24,r14 |
8817 5394 9F09 sbc r25,r15 |
8818 5396 A00B sbc r26,r16 |
8819 5398 B10B sbc r27,r17 |
8820 539a 29E0 ldi r18,9 |
8821 539c B595 1: asr r27 |
8822 539e A795 ror r26 |
8823 53a0 9795 ror r25 |
8824 53a2 8795 ror r24 |
8825 53a4 2A95 dec r18 |
8826 53a6 01F4 brne 1b |
8827 53a8 A80E add r10,r24 |
8828 53aa B91E adc r11,r25 |
8829 53ac B092 0000 sts (cnt.2426)+1,r11 |
8830 53b0 A092 0000 sts cnt.2426,r10 |
8831 53b4 C801 movw r24,r16 |
8832 53b6 B701 movw r22,r14 |
8833 53b8 28E0 ldi r18,lo8(8) |
8834 53ba 30E0 ldi r19,hi8(8) |
8835 53bc 40E0 ldi r20,hlo8(8) |
8836 53be 50E0 ldi r21,hhi8(8) |
8837 53c0 0E94 0000 call __divmodsi4 |
8838 53c4 C901 movw r24,r18 |
8839 53c6 DA01 movw r26,r20 |
8840 53c8 8093 0000 sts CorrectionRoll.2414,r24 |
8841 53cc 9093 0000 sts (CorrectionRoll.2414)+1,r25 |
8842 53d0 A093 0000 sts (CorrectionRoll.2414)+2,r26 |
8843 53d4 B093 0000 sts (CorrectionRoll.2414)+3,r27 |
8844 53d8 8857 subi r24,lo8(-5000) |
8845 53da 9C4E sbci r25,hi8(-5000) |
8846 53dc AF4F sbci r26,hlo8(-5000) |
8847 53de BF4F sbci r27,hhi8(-5000) |
8848 53e0 04F4 brge .L562 |
8849 53e2 88E7 ldi r24,lo8(-5000) |
8850 53e4 9CEE ldi r25,hi8(-5000) |
8851 53e6 AFEF ldi r26,hlo8(-5000) |
8852 53e8 BFEF ldi r27,hhi8(-5000) |
8853 53ea 8093 0000 sts CorrectionRoll.2414,r24 |
8854 53ee 9093 0000 sts (CorrectionRoll.2414)+1,r25 |
8855 53f2 A093 0000 sts (CorrectionRoll.2414)+2,r26 |
8856 53f6 B093 0000 sts (CorrectionRoll.2414)+3,r27 |
8857 .L562: |
8858 53fa 6091 0000 lds r22,CorrectionRoll.2414 |
8859 53fe 7091 0000 lds r23,(CorrectionRoll.2414)+1 |
8860 5402 8091 0000 lds r24,(CorrectionRoll.2414)+2 |
8861 5406 9091 0000 lds r25,(CorrectionRoll.2414)+3 |
8862 540a 20E0 ldi r18,lo8(256) |
8863 540c 31E0 ldi r19,hi8(256) |
8864 540e 40E0 ldi r20,hlo8(256) |
8865 5410 50E0 ldi r21,hhi8(256) |
8866 5412 0E94 0000 call __divmodsi4 |
8867 5416 8091 0000 lds r24,AttitudeCorrectionRoll |
8868 541a 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 |
8869 541e 820F add r24,r18 |
8870 5420 931F adc r25,r19 |
8871 5422 9093 0000 sts (AttitudeCorrectionRoll)+1,r25 |
8872 5426 8093 0000 sts AttitudeCorrectionRoll,r24 |
8873 542a 0C94 0000 jmp .L563 |
8874 .L539: |
8875 542e 31E0 ldi r19,lo8(4097) |
8876 5430 E316 cp r14,r19 |
8877 5432 30E1 ldi r19,hi8(4097) |
8878 5434 F306 cpc r15,r19 |
8879 5436 30E0 ldi r19,hlo8(4097) |
8880 5438 0307 cpc r16,r19 |
8881 543a 30E0 ldi r19,hhi8(4097) |
8882 543c 1307 cpc r17,r19 |
8883 543e 04F4 brge .+2 |
8884 5440 00C0 rjmp .L541 |
8885 5442 8091 0000 lds r24,last_n_p.2427 |
8886 5446 8823 tst r24 |
8887 5448 01F4 brne .+2 |
8888 544a 00C0 rjmp .L542 |
8889 544c D801 movw r26,r16 |
8890 544e C701 movw r24,r14 |
8891 5450 59E0 ldi r21,9 |
8892 5452 B595 1: asr r27 |
8893 5454 A795 ror r26 |
8894 5456 9795 ror r25 |
8895 5458 8795 ror r24 |
8896 545a 5A95 dec r21 |
8897 545c 01F4 brne 1b |
8898 545e A090 0000 lds r10,cnt.2426 |
8899 5462 B090 0000 lds r11,(cnt.2426)+1 |
8900 5466 A80E add r10,r24 |
8901 5468 B91E adc r11,r25 |
8902 546a B092 0000 sts (cnt.2426)+1,r11 |
8903 546e A092 0000 sts cnt.2426,r10 |
8904 5472 C801 movw r24,r16 |
8905 5474 B701 movw r22,r14 |
8906 5476 28E0 ldi r18,lo8(8) |
8907 5478 30E0 ldi r19,hi8(8) |
8908 547a 40E0 ldi r20,hlo8(8) |
8909 547c 50E0 ldi r21,hhi8(8) |
8910 547e 0E94 0000 call __divmodsi4 |
8911 5482 C901 movw r24,r18 |
8912 5484 DA01 movw r26,r20 |
8913 5486 8093 0000 sts CorrectionNick.2413,r24 |
8914 548a 9093 0000 sts (CorrectionNick.2413)+1,r25 |
8915 548e A093 0000 sts (CorrectionNick.2413)+2,r26 |
8916 5492 B093 0000 sts (CorrectionNick.2413)+3,r27 |
8917 5496 8958 subi r24,lo8(5001) |
8918 5498 9341 sbci r25,hi8(5001) |
8919 549a A040 sbci r26,hlo8(5001) |
8920 549c B040 sbci r27,hhi8(5001) |
8921 549e 04F0 brlt .L543 |
8922 54a0 88E8 ldi r24,lo8(5000) |
8923 54a2 93E1 ldi r25,hi8(5000) |
8924 54a4 A0E0 ldi r26,hlo8(5000) |
8925 54a6 B0E0 ldi r27,hhi8(5000) |
8926 54a8 8093 0000 sts CorrectionNick.2413,r24 |
8927 54ac 9093 0000 sts (CorrectionNick.2413)+1,r25 |
8928 54b0 A093 0000 sts (CorrectionNick.2413)+2,r26 |
8929 54b4 B093 0000 sts (CorrectionNick.2413)+3,r27 |
8930 .L543: |
8931 54b8 6091 0000 lds r22,CorrectionNick.2413 |
8932 54bc 7091 0000 lds r23,(CorrectionNick.2413)+1 |
8933 54c0 8091 0000 lds r24,(CorrectionNick.2413)+2 |
8934 54c4 9091 0000 lds r25,(CorrectionNick.2413)+3 |
8935 54c8 20E0 ldi r18,lo8(256) |
8936 54ca 31E0 ldi r19,hi8(256) |
8937 54cc 40E0 ldi r20,hlo8(256) |
8938 54ce 50E0 ldi r21,hhi8(256) |
8939 54d0 0E94 0000 call __divmodsi4 |
8940 54d4 8091 0000 lds r24,AttitudeCorrectionNick |
8941 54d8 9091 0000 lds r25,(AttitudeCorrectionNick)+1 |
8942 54dc 820F add r24,r18 |
8943 54de 931F adc r25,r19 |
8944 54e0 9093 0000 sts (AttitudeCorrectionNick)+1,r25 |
8945 54e4 8093 0000 sts AttitudeCorrectionNick,r24 |
8946 .L544: |
8947 54e8 50E0 ldi r21,lo8(-4096) |
8948 54ea E516 cp r14,r21 |
8949 54ec 50EF ldi r21,hi8(-4096) |
8950 54ee F506 cpc r15,r21 |
8951 54f0 5FEF ldi r21,hlo8(-4096) |
8952 54f2 0507 cpc r16,r21 |
8953 54f4 5FEF ldi r21,hhi8(-4096) |
8954 54f6 1507 cpc r17,r21 |
8955 54f8 04F0 brlt .+2 |
8956 54fa 00C0 rjmp .L545 |
8957 54fc 8091 0000 lds r24,last_n_n.2428 |
8958 5500 8823 tst r24 |
8959 5502 01F4 brne .+2 |
8960 5504 00C0 rjmp .L546 |
8961 5506 8827 clr r24 |
8962 5508 9927 clr r25 |
8963 550a DC01 movw r26,r24 |
8964 550c 8E19 sub r24,r14 |
8965 550e 9F09 sbc r25,r15 |
8966 5510 A00B sbc r26,r16 |
8967 5512 B10B sbc r27,r17 |
8968 5514 49E0 ldi r20,9 |
8969 5516 B595 1: asr r27 |
8970 5518 A795 ror r26 |
8971 551a 9795 ror r25 |
8972 551c 8795 ror r24 |
8973 551e 4A95 dec r20 |
8974 5520 01F4 brne 1b |
8975 5522 A80E add r10,r24 |
8976 5524 B91E adc r11,r25 |
8977 5526 B092 0000 sts (cnt.2426)+1,r11 |
8978 552a A092 0000 sts cnt.2426,r10 |
8979 552e C801 movw r24,r16 |
8980 5530 B701 movw r22,r14 |
8981 5532 28E0 ldi r18,lo8(8) |
8982 5534 30E0 ldi r19,hi8(8) |
8983 5536 40E0 ldi r20,hlo8(8) |
8984 5538 50E0 ldi r21,hhi8(8) |
8985 553a 0E94 0000 call __divmodsi4 |
8986 553e C901 movw r24,r18 |
8987 5540 DA01 movw r26,r20 |
8988 5542 8093 0000 sts CorrectionNick.2413,r24 |
8989 5546 9093 0000 sts (CorrectionNick.2413)+1,r25 |
8990 554a A093 0000 sts (CorrectionNick.2413)+2,r26 |
8991 554e B093 0000 sts (CorrectionNick.2413)+3,r27 |
8992 5552 8857 subi r24,lo8(-5000) |
8993 5554 9C4E sbci r25,hi8(-5000) |
8994 5556 AF4F sbci r26,hlo8(-5000) |
8995 5558 BF4F sbci r27,hhi8(-5000) |
8996 555a 04F4 brge .L547 |
8997 555c 88E7 ldi r24,lo8(-5000) |
8998 555e 9CEE ldi r25,hi8(-5000) |
8999 5560 AFEF ldi r26,hlo8(-5000) |
9000 5562 BFEF ldi r27,hhi8(-5000) |
9001 5564 8093 0000 sts CorrectionNick.2413,r24 |
9002 5568 9093 0000 sts (CorrectionNick.2413)+1,r25 |
9003 556c A093 0000 sts (CorrectionNick.2413)+2,r26 |
9004 5570 B093 0000 sts (CorrectionNick.2413)+3,r27 |
9005 .L547: |
9006 5574 6091 0000 lds r22,CorrectionNick.2413 |
9007 5578 7091 0000 lds r23,(CorrectionNick.2413)+1 |
9008 557c 8091 0000 lds r24,(CorrectionNick.2413)+2 |
9009 5580 9091 0000 lds r25,(CorrectionNick.2413)+3 |
9010 5584 20E0 ldi r18,lo8(256) |
9011 5586 31E0 ldi r19,hi8(256) |
9012 5588 40E0 ldi r20,hlo8(256) |
9013 558a 50E0 ldi r21,hhi8(256) |
9014 558c 0E94 0000 call __divmodsi4 |
9015 5590 8091 0000 lds r24,AttitudeCorrectionNick |
9016 5594 9091 0000 lds r25,(AttitudeCorrectionNick)+1 |
9017 5598 820F add r24,r18 |
9018 559a 931F adc r25,r19 |
9019 559c 9093 0000 sts (AttitudeCorrectionNick)+1,r25 |
9020 55a0 8093 0000 sts AttitudeCorrectionNick,r24 |
9021 55a4 0C94 0000 jmp .L548 |
9022 .L617: |
9023 55a8 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ |
9024 55ac 1092 0000 sts HeightTrimming.2442,__zero_reg__ |
9025 55b0 08AD ldd r16,Y+56 |
9026 55b2 19AD ldd r17,Y+57 |
9027 55b4 00C0 rjmp .L619 |
9028 .L608: |
9029 55b6 8091 0000 lds r24,BaroFlags.2450 |
9030 55ba 8260 ori r24,lo8(2) |
9031 55bc 8093 0000 sts BaroFlags.2450,r24 |
9032 55c0 0C94 0000 jmp .L606 |
9033 .L609: |
9034 55c4 8091 0000 lds r24,BaroFlags.2450 |
9035 55c8 8C7F andi r24,lo8(-4) |
9036 55ca 8093 0000 sts BaroFlags.2450,r24 |
9037 55ce 0C94 0000 jmp .L606 |
9038 .L464: |
9039 55d2 1092 0000 sts delay_neutral.2416,__zero_reg__ |
9040 55d6 00C0 rjmp .L446 |
9041 .L637: |
9042 55d8 8091 0000 lds r24,ReadingHeight |
9043 55dc 9091 0000 lds r25,(ReadingHeight)+1 |
9044 55e0 A091 0000 lds r26,(ReadingHeight)+2 |
9045 55e4 B091 0000 lds r27,(ReadingHeight)+3 |
9046 55e8 5C01 movw r10,r24 |
9047 55ea A21A sub r10,r18 |
9048 55ec B30A sbc r11,r19 |
9049 55ee F090 0000 lds r15,FCParam+2 |
9050 55f2 8091 0000 lds r24,ReadingVario |
9051 55f6 9091 0000 lds r25,(ReadingVario)+1 |
9052 55fa 97FD sbrc r25,7 |
9053 55fc 00C0 rjmp .L808 |
9054 .L639: |
9055 55fe 9595 asr r25 |
9056 5600 8795 ror r24 |
9057 5602 9595 asr r25 |
9058 5604 8795 ror r24 |
9059 5606 9595 asr r25 |
9060 5608 8795 ror r24 |
9061 560a 8930 cpi r24,9 |
9062 560c 9105 cpc r25,__zero_reg__ |
9063 560e 04F0 brlt .+2 |
9064 5610 00C0 rjmp .L809 |
9065 5612 1816 cp __zero_reg__,r24 |
9066 5614 1906 cpc __zero_reg__,r25 |
9067 5616 04F4 brge .+2 |
9068 5618 00C0 rjmp .L641 |
9069 561a 889F mul r24,r24 |
9070 561c A001 movw r20,r0 |
9071 561e 899F mul r24,r25 |
9072 5620 500D add r21,r0 |
9073 5622 989F mul r25,r24 |
9074 5624 500D add r21,r0 |
9075 5626 1124 clr r1 |
9076 5628 57FF sbrs r21,7 |
9077 562a 00C0 rjmp .L644 |
9078 562c 4D5F subi r20,lo8(-(3)) |
9079 562e 5F4F sbci r21,hi8(-(3)) |
9080 .L644: |
9081 5630 5595 asr r21 |
9082 5632 4795 ror r20 |
9083 5634 5595 asr r21 |
9084 5636 4795 ror r20 |
9085 5638 2091 0000 lds r18,ReadingVario |
9086 563c 3091 0000 lds r19,(ReadingVario)+1 |
9087 5640 241B sub r18,r20 |
9088 5642 350B sbc r19,r21 |
9089 .L643: |
9090 5644 B901 movw r22,r18 |
9091 5646 8827 clr r24 |
9092 5648 77FD sbrc r23,7 |
9093 564a 8095 com r24 |
9094 564c 982F mov r25,r24 |
9095 564e 2091 0000 lds r18,FCParam |
9096 5652 30E0 ldi r19,lo8(0) |
9097 5654 40E0 ldi r20,lo8(0) |
9098 5656 50E0 ldi r21,hi8(0) |
9099 5658 0E94 0000 call __mulsi3 |
9100 565c 20E8 ldi r18,lo8(128) |
9101 565e 30E0 ldi r19,hi8(128) |
9102 5660 40E0 ldi r20,hlo8(128) |
9103 5662 50E0 ldi r21,hhi8(128) |
9104 5664 0E94 0000 call __divmodsi4 |
9105 5668 A901 movw r20,r18 |
9106 566a 2F3F cpi r18,255 |
9107 566c 3105 cpc r19,__zero_reg__ |
9108 566e 01F0 breq .L645 |
9109 5670 04F0 brlt .L645 |
9110 5672 4FEF ldi r20,lo8(255) |
9111 5674 50E0 ldi r21,hi8(255) |
9112 .L645: |
9113 5676 8F2D mov r24,r15 |
9114 5678 90E0 ldi r25,lo8(0) |
9115 567a A89E mul r10,r24 |
9116 567c 9001 movw r18,r0 |
9117 567e A99E mul r10,r25 |
9118 5680 300D add r19,r0 |
9119 5682 B89E mul r11,r24 |
9120 5684 300D add r19,r0 |
9121 5686 1124 clr r1 |
9122 5688 37FD sbrc r19,7 |
9123 568a 00C0 rjmp .L810 |
9124 .L646: |
9125 568c 3595 asr r19 |
9126 568e 2795 ror r18 |
9127 5690 3595 asr r19 |
9128 5692 2795 ror r18 |
9129 5694 3595 asr r19 |
9130 5696 2795 ror r18 |
9131 5698 3595 asr r19 |
9132 569a 2795 ror r18 |
9133 569c B801 movw r22,r16 |
9134 569e 621B sub r22,r18 |
9135 56a0 730B sbc r23,r19 |
9136 56a2 9B01 movw r18,r22 |
9137 56a4 CA01 movw r24,r20 |
9138 56a6 7FEF ldi r23,hi8(-127) |
9139 56a8 4138 cpi r20,lo8(-127) |
9140 56aa 5707 cpc r21,r23 |
9141 56ac 04F4 brge .L647 |
9142 56ae 81E8 ldi r24,lo8(-127) |
9143 56b0 9FEF ldi r25,hi8(-127) |
9144 .L647: |
9145 56b2 6901 movw r12,r18 |
9146 56b4 C81A sub r12,r24 |
9147 56b6 D90A sbc r13,r25 |
9148 56b8 6091 0000 lds r22,ReadingIntegralTop |
9149 56bc 7091 0000 lds r23,(ReadingIntegralTop)+1 |
9150 56c0 8091 0000 lds r24,(ReadingIntegralTop)+2 |
9151 56c4 9091 0000 lds r25,(ReadingIntegralTop)+3 |
9152 56c8 E090 0000 lds r14,FCParam+3 |
9153 56cc FF24 clr r15 |
9154 56ce 00E0 ldi r16,lo8(0) |
9155 56d0 10E0 ldi r17,hi8(0) |
9156 56d2 20E8 ldi r18,lo8(128) |
9157 56d4 30E0 ldi r19,hi8(128) |
9158 56d6 40E0 ldi r20,hlo8(128) |
9159 56d8 50E0 ldi r21,hhi8(128) |
9160 56da 0E94 0000 call __divmodsi4 |
9161 56de C801 movw r24,r16 |
9162 56e0 B701 movw r22,r14 |
9163 56e2 0E94 0000 call __mulsi3 |
9164 56e6 20E2 ldi r18,lo8(32) |
9165 56e8 30E0 ldi r19,hi8(32) |
9166 56ea 40E0 ldi r20,hlo8(32) |
9167 56ec 50E0 ldi r21,hhi8(32) |
9168 56ee 0E94 0000 call __divmodsi4 |
9169 56f2 C901 movw r24,r18 |
9170 56f4 2F3F cpi r18,255 |
9171 56f6 3105 cpc r19,__zero_reg__ |
9172 56f8 01F0 breq .L648 |
9173 56fa 04F0 brlt .L648 |
9174 56fc 8FEF ldi r24,lo8(255) |
9175 56fe 90E0 ldi r25,hi8(255) |
9176 .L648: |
9177 5700 AFEF ldi r26,hi8(-127) |
9178 5702 8138 cpi r24,lo8(-127) |
9179 5704 9A07 cpc r25,r26 |
9180 5706 04F4 brge .L649 |
9181 5708 81E8 ldi r24,lo8(-127) |
9182 570a 9FEF ldi r25,hi8(-127) |
9183 .L649: |
9184 570c 9601 movw r18,r12 |
9185 570e 281B sub r18,r24 |
9186 5710 390B sbc r19,r25 |
9187 5712 E5E9 ldi r30,lo8(149) |
9188 5714 F0E0 ldi r31,hi8(149) |
9189 5716 AE0E add r10,r30 |
9190 5718 BF1E adc r11,r31 |
9191 571a FBE2 ldi r31,lo8(299) |
9192 571c AF16 cp r10,r31 |
9193 571e F1E0 ldi r31,hi8(299) |
9194 5720 BF06 cpc r11,r31 |
9195 5722 00F0 brlo .+2 |
9196 5724 00C0 rjmp .L650 |
9197 5726 8091 0000 lds r24,HeightTrimming.2442 |
9198 572a 9091 0000 lds r25,(HeightTrimming.2442)+1 |
9199 572e 892B or r24,r25 |
9200 5730 01F0 breq .+2 |
9201 5732 00C0 rjmp .L650 |
9202 5734 8091 0000 lds r24,HoverGas.2445 |
9203 5738 9091 0000 lds r25,(HoverGas.2445)+1 |
9204 573c 1816 cp __zero_reg__,r24 |
9205 573e 1906 cpc __zero_reg__,r25 |
9206 5740 04F0 brlt .+2 |
9207 5742 00C0 rjmp .L650 |
9208 5744 0091 0000 lds r16,HoverGasMin.2446 |
9209 5748 1091 0000 lds r17,(HoverGasMin.2446)+1 |
9210 574c 2017 cp r18,r16 |
9211 574e 3107 cpc r19,r17 |
9212 5750 04F4 brge .+2 |
9213 5752 00C0 rjmp .L638 |
9214 5754 8091 0000 lds r24,HoverGasMax.2447 |
9215 5758 9091 0000 lds r25,(HoverGasMax.2447)+1 |
9216 575c 8901 movw r16,r18 |
9217 575e 8217 cp r24,r18 |
9218 5760 9307 cpc r25,r19 |
9219 5762 04F0 brlt .+2 |
9220 5764 00C0 rjmp .L638 |
9221 5766 8C01 movw r16,r24 |
9222 5768 00C0 rjmp .L638 |
9223 .L725: |
9224 576a 8091 0000 lds r24,FilterHCGas.2441 |
9225 576e 9091 0000 lds r25,(FilterHCGas.2441)+1 |
9226 5772 E8AD ldd r30,Y+56 |
9227 5774 F9AD ldd r31,Y+57 |
9228 5776 E817 cp r30,r24 |
9229 5778 F907 cpc r31,r25 |
9230 577a 04F0 brlt .+2 |
9231 577c 00C0 rjmp .L656 |
9232 577e F093 0000 sts (FilterHCGas.2441)+1,r31 |
9233 5782 E093 0000 sts FilterHCGas.2441,r30 |
9234 5786 CF01 movw r24,r30 |
9235 5788 00C0 rjmp .L656 |
9236 .L545: |
9237 578a 1092 0000 sts last_n_n.2428,__zero_reg__ |
9238 578e 0C94 0000 jmp .L548 |
9239 .L541: |
9240 5792 1092 0000 sts last_n_p.2427,__zero_reg__ |
9241 5796 A090 0000 lds r10,cnt.2426 |
9242 579a B090 0000 lds r11,(cnt.2426)+1 |
9243 579e 00C0 rjmp .L544 |
9244 .L556: |
9245 57a0 1092 0000 sts last_r_p.2429,__zero_reg__ |
9246 57a4 A090 0000 lds r10,cnt.2426 |
9247 57a8 B090 0000 lds r11,(cnt.2426)+1 |
9248 57ac 00C0 rjmp .L559 |
9249 .L560: |
9250 57ae 1092 0000 sts last_r_n.2430,__zero_reg__ |
9251 57b2 0C94 0000 jmp .L563 |
9252 .L807: |
9253 57b6 F095 com r31 |
9254 57b8 E195 neg r30 |
9255 57ba FF4F sbci r31,lo8(-1) |
9256 57bc 00C0 rjmp .L525 |
9257 .L803: |
9258 57be 2D5F subi r18,lo8(-(3)) |
9259 57c0 3F4F sbci r19,hi8(-(3)) |
9260 57c2 00C0 rjmp .L659 |
9261 .L793: |
9262 57c4 8091 0000 lds r24,AttitudeCorrectionNick |
9263 57c8 9091 0000 lds r25,AttitudeCorrectionNick+1 |
9264 57cc 9C01 movw r18,r24 |
9265 57ce 37FD sbrc r19,7 |
9266 57d0 00C0 rjmp .L811 |
9267 .L534: |
9268 57d2 3595 asr r19 |
9269 57d4 2795 ror r18 |
9270 57d6 3093 0000 sts (AttitudeCorrectionNick)+1,r19 |
9271 57da 2093 0000 sts AttitudeCorrectionNick,r18 |
9272 57de 8091 0000 lds r24,AttitudeCorrectionRoll |
9273 57e2 9091 0000 lds r25,AttitudeCorrectionRoll+1 |
9274 57e6 9C01 movw r18,r24 |
9275 57e8 37FD sbrc r19,7 |
9276 57ea 00C0 rjmp .L812 |
9277 .L535: |
9278 57ec 3595 asr r19 |
9279 57ee 2795 ror r18 |
9280 57f0 3093 0000 sts (AttitudeCorrectionRoll)+1,r19 |
9281 57f4 2093 0000 sts AttitudeCorrectionRoll,r18 |
9282 57f8 0C94 0000 jmp .L532 |
9283 .L650: |
9284 57fc 8901 movw r16,r18 |
9285 57fe 00C0 rjmp .L638 |
9286 .L449: |
9287 5800 299A sbi 37-32,1 |
9288 5802 0C94 0000 jmp .L450 |
9289 .L668: |
9290 5806 1092 0000 sts (HoverGasMin.2446)+1,__zero_reg__ |
9291 580a 1092 0000 sts HoverGasMin.2446,__zero_reg__ |
9292 580e 8FEF ldi r24,lo8(1023) |
9293 5810 93E0 ldi r25,hi8(1023) |
9294 5812 9093 0000 sts (HoverGasMax.2447)+1,r25 |
9295 5816 8093 0000 sts HoverGasMax.2447,r24 |
9296 581a 0C94 0000 jmp .L604 |
9297 .L456: |
9298 581e 21E0 ldi r18,lo8(1) |
9299 5820 0C94 0000 jmp .L457 |
9300 .L664: |
9301 5824 2FE0 ldi r18,hi8(4000) |
9302 5826 803A cpi r24,lo8(4000) |
9303 5828 9207 cpc r25,r18 |
9304 582a 00F0 brlo .+2 |
9305 582c 00C0 rjmp .L666 |
9306 582e 2091 0000 lds r18,HoverGasFilter.2448 |
9307 5832 3091 0000 lds r19,(HoverGasFilter.2448)+1 |
9308 5836 4091 0000 lds r20,(HoverGasFilter.2448)+2 |
9309 583a 5091 0000 lds r21,(HoverGasFilter.2448)+3 |
9310 583e DA01 movw r26,r20 |
9311 5840 C901 movw r24,r18 |
9312 5842 6AE0 ldi r22,10 |
9313 5844 B695 1: lsr r27 |
9314 5846 A795 ror r26 |
9315 5848 9795 ror r25 |
9316 584a 8795 ror r24 |
9317 584c 6A95 dec r22 |
9318 584e 01F4 brne 1b |
9319 5850 5901 movw r10,r18 |
9320 5852 6A01 movw r12,r20 |
9321 5854 A81A sub r10,r24 |
9322 5856 B90A sbc r11,r25 |
9323 5858 CA0A sbc r12,r26 |
9324 585a DB0A sbc r13,r27 |
9325 585c A801 movw r20,r16 |
9326 585e 9701 movw r18,r14 |
9327 5860 220F lsl r18 |
9328 5862 331F rol r19 |
9329 5864 441F rol r20 |
9330 5866 551F rol r21 |
9331 5868 220F lsl r18 |
9332 586a 331F rol r19 |
9333 586c 441F rol r20 |
9334 586e 551F rol r21 |
9335 5870 2A0D add r18,r10 |
9336 5872 3B1D adc r19,r11 |
9337 5874 4C1D adc r20,r12 |
9338 5876 5D1D adc r21,r13 |
9339 5878 2093 0000 sts HoverGasFilter.2448,r18 |
9340 587c 3093 0000 sts (HoverGasFilter.2448)+1,r19 |
9341 5880 4093 0000 sts (HoverGasFilter.2448)+2,r20 |
9342 5884 5093 0000 sts (HoverGasFilter.2448)+3,r21 |
9343 5888 00C0 rjmp .L665 |
9344 .L809: |
9345 588a 88E0 ldi r24,lo8(8) |
9346 588c 90E0 ldi r25,hi8(8) |
9347 .L641: |
9348 588e 889F mul r24,r24 |
9349 5890 9001 movw r18,r0 |
9350 5892 899F mul r24,r25 |
9351 5894 300D add r19,r0 |
9352 5896 989F mul r25,r24 |
9353 5898 300D add r19,r0 |
9354 589a 1124 clr r1 |
9355 589c 3595 asr r19 |
9356 589e 2795 ror r18 |
9357 58a0 3595 asr r19 |
9358 58a2 2795 ror r18 |
9359 58a4 8091 0000 lds r24,ReadingVario |
9360 58a8 9091 0000 lds r25,(ReadingVario)+1 |
9361 58ac 280F add r18,r24 |
9362 58ae 391F adc r19,r25 |
9363 58b0 00C0 rjmp .L643 |
9364 .L610: |
9365 58b2 8091 0000 lds r24,BaroFlags.2450 |
9366 58b6 8160 ori r24,lo8(1) |
9367 58b8 8093 0000 sts BaroFlags.2450,r24 |
9368 58bc 0C94 0000 jmp .L606 |
9369 .L542: |
9370 58c0 81E0 ldi r24,lo8(1) |
9371 58c2 8093 0000 sts last_n_p.2427,r24 |
9372 58c6 A090 0000 lds r10,cnt.2426 |
9373 58ca B090 0000 lds r11,(cnt.2426)+1 |
9374 58ce 00C0 rjmp .L544 |
9375 .L561: |
9376 58d0 81E0 ldi r24,lo8(1) |
9377 58d2 8093 0000 sts last_r_n.2430,r24 |
9378 58d6 0C94 0000 jmp .L563 |
9379 .L804: |
9380 58da 9095 com r25 |
9381 58dc 8195 neg r24 |
9382 58de 9F4F sbci r25,lo8(-1) |
9383 58e0 00C0 rjmp .L663 |
9384 .L557: |
9385 58e2 81E0 ldi r24,lo8(1) |
9386 58e4 8093 0000 sts last_r_p.2429,r24 |
9387 58e8 A090 0000 lds r10,cnt.2426 |
9388 58ec B090 0000 lds r11,(cnt.2426)+1 |
9389 58f0 00C0 rjmp .L559 |
9390 .L546: |
9391 58f2 81E0 ldi r24,lo8(1) |
9392 58f4 8093 0000 sts last_n_n.2428,r24 |
9393 58f8 0C94 0000 jmp .L548 |
9394 .L801: |
9395 58fc 4D5F subi r20,lo8(-(3)) |
9396 58fe 5F4F sbci r21,hi8(-(3)) |
9397 5900 00C0 rjmp .L653 |
9398 .L466: |
9399 5902 299A sbi 37-32,1 |
9400 5904 00C0 rjmp .L467 |
9401 .L620: |
9402 5906 8091 0000 lds r24,ReadingHeight |
9403 590a 9091 0000 lds r25,(ReadingHeight)+1 |
9404 590e A091 0000 lds r26,(ReadingHeight)+2 |
9405 5912 B091 0000 lds r27,(ReadingHeight)+3 |
9406 5916 8059 subi r24,lo8(-(-400)) |
9407 5918 9140 sbci r25,hi8(-(-400)) |
9408 591a A040 sbci r26,hlo8(-(-400)) |
9409 591c B040 sbci r27,hhi8(-(-400)) |
9410 591e 8093 0000 sts SetPointHeight,r24 |
9411 5922 9093 0000 sts (SetPointHeight)+1,r25 |
9412 5926 A093 0000 sts (SetPointHeight)+2,r26 |
9413 592a B093 0000 sts (SetPointHeight)+3,r27 |
9414 592e 8091 0000 lds r24,ParamSet+17 |
9415 5932 8823 tst r24 |
9416 5934 01F4 brne .+2 |
9417 5936 00C0 rjmp .L634 |
9418 5938 8093 0000 sts StickGasHover.2444,r24 |
9419 593c 1092 0000 sts StickGasHover.2444+1,__zero_reg__ |
9420 5940 00C0 rjmp .L633 |
9421 .L666: |
9422 5942 8054 subi r24,lo8(8000) |
9423 5944 9F41 sbci r25,hi8(8000) |
9424 5946 00F0 brlo .+2 |
9425 5948 00C0 rjmp .L667 |
9426 594a 2091 0000 lds r18,HoverGasFilter.2448 |
9427 594e 3091 0000 lds r19,(HoverGasFilter.2448)+1 |
9428 5952 4091 0000 lds r20,(HoverGasFilter.2448)+2 |
9429 5956 5091 0000 lds r21,(HoverGasFilter.2448)+3 |
9430 595a DA01 movw r26,r20 |
9431 595c C901 movw r24,r18 |
9432 595e EBE0 ldi r30,11 |
9433 5960 B695 1: lsr r27 |
9434 5962 A795 ror r26 |
9435 5964 9795 ror r25 |
9436 5966 8795 ror r24 |
9437 5968 EA95 dec r30 |
9438 596a 01F4 brne 1b |
9439 596c 5901 movw r10,r18 |
9440 596e 6A01 movw r12,r20 |
9441 5970 A81A sub r10,r24 |
9442 5972 B90A sbc r11,r25 |
9443 5974 CA0A sbc r12,r26 |
9444 5976 DB0A sbc r13,r27 |
9445 5978 A801 movw r20,r16 |
9446 597a 9701 movw r18,r14 |
9447 597c 220F lsl r18 |
9448 597e 331F rol r19 |
9449 5980 441F rol r20 |
9450 5982 551F rol r21 |
9451 5984 2A0D add r18,r10 |
9452 5986 3B1D adc r19,r11 |
9453 5988 4C1D adc r20,r12 |
9454 598a 5D1D adc r21,r13 |
9455 598c 2093 0000 sts HoverGasFilter.2448,r18 |
9456 5990 3093 0000 sts (HoverGasFilter.2448)+1,r19 |
9457 5994 4093 0000 sts (HoverGasFilter.2448)+2,r20 |
9458 5998 5093 0000 sts (HoverGasFilter.2448)+3,r21 |
9459 599c 00C0 rjmp .L665 |
9460 .L621: |
9461 599e CB01 movw r24,r22 |
9462 59a0 0F97 sbiw r24,15 |
9463 59a2 4817 cp r20,r24 |
9464 59a4 5907 cpc r21,r25 |
9465 59a6 04F4 brge .L625 |
9466 59a8 8091 0000 lds r24,BaroFlags.2450 |
9467 59ac 81FD sbrc r24,1 |
9468 59ae 00C0 rjmp .L625 |
9469 59b0 2091 0000 lds r18,HeightTrimmingFlag.2452 |
9470 59b4 20FF sbrs r18,0 |
9471 59b6 00C0 rjmp .L626 |
9472 59b8 2E7F andi r18,lo8(-2) |
9473 59ba 2093 0000 sts HeightTrimmingFlag.2452,r18 |
9474 59be 8091 0000 lds r24,ReadingHeight |
9475 59c2 9091 0000 lds r25,(ReadingHeight)+1 |
9476 59c6 A091 0000 lds r26,(ReadingHeight)+2 |
9477 59ca B091 0000 lds r27,(ReadingHeight)+3 |
9478 59ce 8093 0000 sts SetPointHeight,r24 |
9479 59d2 9093 0000 sts (SetPointHeight)+1,r25 |
9480 59d6 A093 0000 sts (SetPointHeight)+2,r26 |
9481 59da B093 0000 sts (SetPointHeight)+3,r27 |
9482 .L626: |
9483 59de 2260 ori r18,lo8(2) |
9484 59e0 2093 0000 sts HeightTrimmingFlag.2452,r18 |
9485 59e4 CA01 movw r24,r20 |
9486 59e6 0F96 adiw r24,15 |
9487 59e8 861B sub r24,r22 |
9488 59ea 970B sbc r25,r23 |
9489 59ec 97FD sbrc r25,7 |
9490 59ee 00C0 rjmp .L813 |
9491 .L627: |
9492 59f0 2091 0000 lds r18,HeightTrimming.2442 |
9493 59f4 3091 0000 lds r19,(HeightTrimming.2442)+1 |
9494 59f8 281B sub r18,r24 |
9495 59fa 390B sbc r19,r25 |
9496 59fc 3093 0000 sts (HeightTrimming.2442)+1,r19 |
9497 5a00 2093 0000 sts HeightTrimming.2442,r18 |
9498 5a04 00C0 rjmp .L624 |
9499 .L625: |
9500 5a06 2091 0000 lds r18,HeightTrimmingFlag.2452 |
9501 5a0a 822F mov r24,r18 |
9502 5a0c 90E0 ldi r25,lo8(0) |
9503 5a0e 8370 andi r24,lo8(3) |
9504 5a10 9070 andi r25,hi8(3) |
9505 5a12 892B or r24,r25 |
9506 5a14 01F0 breq .+2 |
9507 5a16 00C0 rjmp .L723 |
9508 5a18 2091 0000 lds r18,HeightTrimming.2442 |
9509 5a1c 3091 0000 lds r19,(HeightTrimming.2442)+1 |
9510 5a20 00C0 rjmp .L624 |
9511 .L667: |
9512 5a22 2091 0000 lds r18,HoverGasFilter.2448 |
9513 5a26 3091 0000 lds r19,(HoverGasFilter.2448)+1 |
9514 5a2a 4091 0000 lds r20,(HoverGasFilter.2448)+2 |
9515 5a2e 5091 0000 lds r21,(HoverGasFilter.2448)+3 |
9516 5a32 DA01 movw r26,r20 |
9517 5a34 C901 movw r24,r18 |
9518 5a36 6CE0 ldi r22,12 |
9519 5a38 B695 1: lsr r27 |
9520 5a3a A795 ror r26 |
9521 5a3c 9795 ror r25 |
9522 5a3e 8795 ror r24 |
9523 5a40 6A95 dec r22 |
9524 5a42 01F4 brne 1b |
9525 5a44 281B sub r18,r24 |
9526 5a46 390B sbc r19,r25 |
9527 5a48 4A0B sbc r20,r26 |
9528 5a4a 5B0B sbc r21,r27 |
9529 5a4c 2E0D add r18,r14 |
9530 5a4e 3F1D adc r19,r15 |
9531 5a50 401F adc r20,r16 |
9532 5a52 511F adc r21,r17 |
9533 5a54 2093 0000 sts HoverGasFilter.2448,r18 |
9534 5a58 3093 0000 sts (HoverGasFilter.2448)+1,r19 |
9535 5a5c 4093 0000 sts (HoverGasFilter.2448)+2,r20 |
9536 5a60 5093 0000 sts (HoverGasFilter.2448)+3,r21 |
9537 5a64 00C0 rjmp .L665 |
9538 .L808: |
9539 5a66 0796 adiw r24,7 |
9540 5a68 00C0 rjmp .L639 |
9541 .L461: |
9542 5a6a 0E94 0000 call GetActiveParamSet |
9543 5a6e 0E94 0000 call ParamSet_ReadFromEEProm |
9544 5a72 0E94 0000 call Servo_Off |
9545 5a76 80E0 ldi r24,lo8(0) |
9546 5a78 00C0 rjmp .L726 |
9547 .L810: |
9548 5a7a 215F subi r18,lo8(-(15)) |
9549 5a7c 3F4F sbci r19,hi8(-(15)) |
9550 5a7e 00C0 rjmp .L646 |
9551 .L634: |
9552 5a80 88E7 ldi r24,lo8(120) |
9553 5a82 90E0 ldi r25,hi8(120) |
9554 5a84 9093 0000 sts (StickGasHover.2444)+1,r25 |
9555 5a88 8093 0000 sts StickGasHover.2444,r24 |
9556 5a8c 00C0 rjmp .L633 |
9557 .L796: |
9558 5a8e F095 com r31 |
9559 5a90 E195 neg r30 |
9560 5a92 FF4F sbci r31,lo8(-1) |
9561 5a94 0C94 0000 jmp .L454 |
9562 .L812: |
9563 5a98 2F5F subi r18,lo8(-(1)) |
9564 5a9a 3F4F sbci r19,hi8(-(1)) |
9565 5a9c 00C0 rjmp .L535 |
9566 .L811: |
9567 5a9e 2F5F subi r18,lo8(-(1)) |
9568 5aa0 3F4F sbci r19,hi8(-(1)) |
9569 5aa2 00C0 rjmp .L534 |
9570 .L792: |
9571 5aa4 F095 com r31 |
9572 5aa6 E195 neg r30 |
9573 5aa8 FF4F sbci r31,lo8(-1) |
9574 5aaa 0C94 0000 jmp .L533 |
9575 .L723: |
9576 5aae 2C7F andi r18,lo8(-4) |
9577 5ab0 2093 0000 sts HeightTrimmingFlag.2452,r18 |
9578 5ab4 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ |
9579 5ab8 1092 0000 sts HeightTrimming.2442,__zero_reg__ |
9580 5abc 8091 0000 lds r24,ReadingHeight |
9581 5ac0 9091 0000 lds r25,(ReadingHeight)+1 |
9582 5ac4 A091 0000 lds r26,(ReadingHeight)+2 |
9583 5ac8 B091 0000 lds r27,(ReadingHeight)+3 |
9584 5acc 8093 0000 sts SetPointHeight,r24 |
9585 5ad0 9093 0000 sts (SetPointHeight)+1,r25 |
9586 5ad4 A093 0000 sts (SetPointHeight)+2,r26 |
9587 5ad8 B093 0000 sts (SetPointHeight)+3,r27 |
9588 5adc 8091 0000 lds r24,ParamSet+88 |
9589 5ae0 81FF sbrs r24,1 |
9590 5ae2 00C0 rjmp .L730 |
9591 5ae4 84EF ldi r24,lo8(500) |
9592 5ae6 91E0 ldi r25,hi8(500) |
9593 5ae8 9093 0000 sts (BeepTime)+1,r25 |
9594 5aec 8093 0000 sts BeepTime,r24 |
9595 .L730: |
9596 5af0 20E0 ldi r18,lo8(0) |
9597 5af2 30E0 ldi r19,hi8(0) |
9598 5af4 00C0 rjmp .L624 |
9599 .L813: |
9600 5af6 9095 com r25 |
9601 5af8 8195 neg r24 |
9602 5afa 9F4F sbci r25,lo8(-1) |
9603 5afc 00C0 rjmp .L627 |
9604 .L799: |
9605 5afe 9095 com r25 |
9606 5b00 8195 neg r24 |
9607 5b02 9F4F sbci r25,lo8(-1) |
9608 5b04 00C0 rjmp .L623 |
9609 .L797: |
9610 5b06 9095 com r25 |
9611 5b08 8195 neg r24 |
9612 5b0a 9F4F sbci r25,lo8(-1) |
9613 5b0c 0C94 0000 jmp .L463 |
9614 .L632: |
9615 5b10 6739 cpi r22,151 |
9616 5b12 7105 cpc r23,__zero_reg__ |
9617 5b14 04F4 brge .+2 |
9618 5b16 00C0 rjmp .L628 |
9619 5b18 86E9 ldi r24,lo8(150) |
9620 5b1a 90E0 ldi r25,hi8(150) |
9621 5b1c 9093 0000 sts (StickGasHover.2444)+1,r25 |
9622 5b20 8093 0000 sts StickGasHover.2444,r24 |
9623 5b24 00C0 rjmp .L628 |
9624 .L800: |
9625 5b26 2D5F subi r18,lo8(-(3)) |
9626 5b28 3F4F sbci r19,hi8(-(3)) |
9627 5b2a 00C0 rjmp .L631 |
9629 .global BiasHiResGyroNick |
9630 .global BiasHiResGyroNick |
9631 .section .bss |
9634 BiasHiResGyroNick: |
9635 0000 0000 .skip 2,0 |
9636 .global BiasHiResGyroRoll |
9637 .global BiasHiResGyroRoll |
9640 BiasHiResGyroRoll: |
9641 0002 0000 .skip 2,0 |
9642 .global AdBiasGyroYaw |
9643 .global AdBiasGyroYaw |
9646 AdBiasGyroYaw: |
9647 0004 0000 .skip 2,0 |
9648 .global AdBiasAccNick |
9649 .global AdBiasAccNick |
9652 AdBiasAccNick: |
9653 0006 0000 .skip 2,0 |
9654 .global AdBiasAccRoll |
9655 .global AdBiasAccRoll |
9658 AdBiasAccRoll: |
9659 0008 0000 .skip 2,0 |
9660 .global AdBiasAccTop |
9661 .global AdBiasAccTop |
9664 AdBiasAccTop: |
9665 000a 0000 0000 .skip 4,0 |
9666 .global IntegralGyroNick |
9667 .global IntegralGyroNick |
9670 IntegralGyroNick: |
9671 000e 0000 0000 .skip 4,0 |
9672 .global IntegralGyroNick2 |
9673 .global IntegralGyroNick2 |
9676 IntegralGyroNick2: |
9677 0012 0000 0000 .skip 4,0 |
9678 .global IntegralGyroRoll |
9679 .global IntegralGyroRoll |
9682 IntegralGyroRoll: |
9683 0016 0000 0000 .skip 4,0 |
9684 .global IntegralGyroRoll2 |
9685 .global IntegralGyroRoll2 |
9688 IntegralGyroRoll2: |
9689 001a 0000 0000 .skip 4,0 |
9690 .global IntegralGyroYaw |
9691 .global IntegralGyroYaw |
9694 IntegralGyroYaw: |
9695 001e 0000 0000 .skip 4,0 |
9696 .global ReadingIntegralGyroNick |
9697 .global ReadingIntegralGyroNick |
9700 ReadingIntegralGyroNick: |
9701 0022 0000 0000 .skip 4,0 |
9702 .global ReadingIntegralGyroNick2 |
9703 .global ReadingIntegralGyroNick2 |
9706 ReadingIntegralGyroNick2: |
9707 0026 0000 0000 .skip 4,0 |
9708 .global ReadingIntegralGyroRoll |
9709 .global ReadingIntegralGyroRoll |
9712 ReadingIntegralGyroRoll: |
9713 002a 0000 0000 .skip 4,0 |
9714 .global ReadingIntegralGyroRoll2 |
9715 .global ReadingIntegralGyroRoll2 |
9718 ReadingIntegralGyroRoll2: |
9719 002e 0000 0000 .skip 4,0 |
9720 .global ReadingIntegralGyroYaw |
9721 .global ReadingIntegralGyroYaw |
9724 ReadingIntegralGyroYaw: |
9725 0032 0000 0000 .skip 4,0 |
9726 .global MeanAccNick |
9727 .global MeanAccNick |
9730 MeanAccNick: |
9731 0036 0000 0000 .skip 4,0 |
9732 .global MeanAccRoll |
9733 .global MeanAccRoll |
9736 MeanAccRoll: |
9737 003a 0000 0000 .skip 4,0 |
9738 .global ReadingIntegralTop |
9739 .global ReadingIntegralTop |
9742 ReadingIntegralTop: |
9743 003e 0000 0000 .skip 4,0 |
9744 .global CompassHeading |
9745 .data |
9748 CompassHeading: |
9749 0000 FFFF .word -1 |
9750 .global CompassCourse |
9753 CompassCourse: |
9754 0002 FFFF .word -1 |
9755 .global CompassOffCourse |
9756 .global CompassOffCourse |
9757 .section .bss |
9760 CompassOffCourse: |
9761 0042 0000 .skip 2,0 |
9762 .global CompassCalState |
9763 .global CompassCalState |
9766 CompassCalState: |
9767 0044 00 .skip 1,0 |
9768 .global FunnelCourse |
9769 .global FunnelCourse |
9772 FunnelCourse: |
9773 0045 00 .skip 1,0 |
9774 .global BadCompassHeading |
9775 .data |
9778 BadCompassHeading: |
9779 0004 F401 .word 500 |
9780 .global NaviAccNick |
9781 .global NaviAccNick |
9782 .section .bss |
9785 NaviAccNick: |
9786 0046 0000 .skip 2,0 |
9787 .global NaviAccRoll |
9788 .global NaviAccRoll |
9791 NaviAccRoll: |
9792 0048 0000 .skip 2,0 |
9793 .global NaviCntAcc |
9794 .global NaviCntAcc |
9797 NaviCntAcc: |
9798 004a 0000 .skip 2,0 |
9799 .global ModelIsFlying |
9800 .global ModelIsFlying |
9803 ModelIsFlying: |
9804 004c 0000 .skip 2,0 |
9805 .global MKFlags |
9806 .global MKFlags |
9809 MKFlags: |
9810 004e 00 .skip 1,0 |
9811 .global TurnOver180Nick |
9812 .data |
9815 TurnOver180Nick: |
9816 0006 90 .byte -112 |
9817 0007 D0 .byte -48 |
9818 0008 03 .byte 3 |
9819 0009 00 .byte 0 |
9820 .global TurnOver180Roll |
9823 TurnOver180Roll: |
9824 000a 90 .byte -112 |
9825 000b D0 .byte -48 |
9826 000c 03 .byte 3 |
9827 000d 00 .byte 0 |
9828 .global Ki |
9831 Ki: |
9832 000e 3801 .word 312 |
9833 .global Poti1 |
9834 .global Poti1 |
9835 .section .bss |
9838 Poti1: |
9839 004f 0000 .skip 2,0 |
9840 .global Poti2 |
9841 .global Poti2 |
9844 Poti2: |
9845 0051 0000 .skip 2,0 |
9846 .global Poti3 |
9847 .global Poti3 |
9850 Poti3: |
9851 0053 0000 .skip 2,0 |
9852 .global Poti4 |
9853 .global Poti4 |
9856 Poti4: |
9857 0055 0000 .skip 2,0 |
9858 .global Poti5 |
9859 .global Poti5 |
9862 Poti5: |
9863 0057 0000 .skip 2,0 |
9864 .global Poti6 |
9865 .global Poti6 |
9868 Poti6: |
9869 0059 0000 .skip 2,0 |
9870 .global Poti7 |
9871 .global Poti7 |
9874 Poti7: |
9875 005b 0000 .skip 2,0 |
9876 .global Poti8 |
9877 .global Poti8 |
9880 Poti8: |
9881 005d 0000 .skip 2,0 |
9882 .global RequiredMotors |
9883 .global RequiredMotors |
9886 RequiredMotors: |
9887 005f 00 .skip 1,0 |
9888 .global StickNick |
9889 .global StickNick |
9892 StickNick: |
9893 0060 0000 .skip 2,0 |
9894 .global StickRoll |
9895 .global StickRoll |
9898 StickRoll: |
9899 0062 0000 .skip 2,0 |
9900 .global StickYaw |
9901 .global StickYaw |
9904 StickYaw: |
9905 0064 0000 .skip 2,0 |
9906 .global StickGas |
9907 .global StickGas |
9910 StickGas: |
9911 0066 0000 .skip 2,0 |
9912 .global GPSStickNick |
9913 .global GPSStickNick |
9916 GPSStickNick: |
9917 0068 0000 .skip 2,0 |
9918 .global GPSStickRoll |
9919 .global GPSStickRoll |
9922 GPSStickRoll: |
9923 006a 0000 .skip 2,0 |
9924 .global MaxStickNick |
9925 .global MaxStickNick |
9928 MaxStickNick: |
9929 006c 0000 .skip 2,0 |
9930 .global MaxStickRoll |
9931 .global MaxStickRoll |
9934 MaxStickRoll: |
9935 006e 0000 .skip 2,0 |
9936 .global ExternStickNick |
9937 .global ExternStickNick |
9940 ExternStickNick: |
9941 0070 0000 .skip 2,0 |
9942 .global ExternStickRoll |
9943 .global ExternStickRoll |
9946 ExternStickRoll: |
9947 0072 0000 .skip 2,0 |
9948 .global ExternStickYaw |
9949 .global ExternStickYaw |
9952 ExternStickYaw: |
9953 0074 0000 .skip 2,0 |
9954 .global ExternHeightValue |
9955 .data |
9958 ExternHeightValue: |
9959 0010 ECFF .word -20 |
9960 .global SetPointHeight |
9961 .global SetPointHeight |
9962 .section .bss |
9965 SetPointHeight: |
9966 0076 0000 0000 .skip 4,0 |
9967 .global AttitudeCorrectionRoll |
9968 .global AttitudeCorrectionRoll |
9971 AttitudeCorrectionRoll: |
9972 007a 0000 .skip 2,0 |
9973 .global AttitudeCorrectionNick |
9974 .global AttitudeCorrectionNick |
9977 AttitudeCorrectionNick: |
9978 007c 0000 .skip 2,0 |
9979 .global LoopingNick |
9980 .global LoopingNick |
9983 LoopingNick: |
9984 007e 00 .skip 1,0 |
9985 .global LoopingRoll |
9986 .global LoopingRoll |
9989 LoopingRoll: |
9990 007f 00 .skip 1,0 |
9991 .global LoopingLeft |
9992 .global LoopingLeft |
9995 LoopingLeft: |
9996 0080 00 .skip 1,0 |
9997 .global LoopingRight |
9998 .global LoopingRight |
10001 LoopingRight: |
10002 0081 00 .skip 1,0 |
10003 .global LoopingDown |
10004 .global LoopingDown |
10007 LoopingDown: |
10008 0082 00 .skip 1,0 |
10009 .global LoopingTop |
10010 .global LoopingTop |
10013 LoopingTop: |
10014 0083 00 .skip 1,0 |
10015 .global FCParam |
10016 .data |
10019 FCParam: |
10020 0012 30 .byte 48 |
10021 0013 FB .byte -5 |
10022 0014 10 .byte 16 |
10023 0015 3A .byte 58 |
10024 0016 40 .byte 64 |
10025 0017 40 .byte 64 |
10026 0018 08 .byte 8 |
10027 0019 96 .byte -106 |
10028 001a 96 .byte -106 |
10029 001b 96 .byte -106 |
10030 001c 96 .byte -106 |
10031 001d 02 .byte 2 |
10032 001e 0A .byte 10 |
10033 001f 00 .byte 0 |
10034 0020 00 .byte 0 |
10035 0021 00 .byte 0 |
10036 0022 00 .byte 0 |
10037 0023 00 .byte 0 |
10038 0024 00 .byte 0 |
10039 0025 00 .byte 0 |
10040 0026 00 .byte 0 |
10041 0027 64 .byte 100 |
10042 0028 64 .byte 100 |
10043 0029 46 .byte 70 |
10044 002a 5A .byte 90 |
10045 002b 41 .byte 65 |
10046 002c 40 .byte 64 |
10047 002d 64 .byte 100 |
10048 002e 00 .byte 0 |
10049 002f 00 .byte 0 |
10050 0030 00 .byte 0 |
10051 0031 0000 00 .skip 3,0 |
10052 .lcomm HeightTrimmingFlag.2452,1 |
10053 .lcomm BaroExpandActive.2451,2 |
10054 .lcomm BaroFlags.2450,1 |
10057 delay.2449: |
10058 0034 64 .byte 100 |
10059 .lcomm HoverGasFilter.2448,4 |
10062 HoverGasMax.2447: |
10063 0035 FF03 .word 1023 |
10064 .lcomm HoverGasMin.2446,2 |
10065 .lcomm HoverGas.2445,2 |
10068 StickGasHover.2444: |
10069 0037 7800 .word 120 |
10070 .lcomm HCActive.2443,1 |
10071 .lcomm HeightTrimming.2442,2 |
10072 .lcomm FilterHCGas.2441,2 |
10073 .lcomm MeanIntegralGyroRoll_old.2432,4 |
10074 .lcomm MeanIntegralGyroNick_old.2431,4 |
10075 .lcomm last_r_n.2430,1 |
10076 .lcomm last_r_p.2429,1 |
10077 .lcomm last_n_n.2428,1 |
10078 .lcomm last_n_p.2427,1 |
10079 .lcomm cnt.2426,2 |
10080 .lcomm stick_roll.2425,2 |
10081 .lcomm stick_nick.2424,2 |
10082 .lcomm MotorValue.2421,24 |
10083 .lcomm UpdateCompassCourse.2420,2 |
10084 .lcomm TimerDebugOut.2419,1 |
10085 .lcomm delay_stopmotors.2418,1 |
10086 .lcomm delay_startmotors.2417,1 |
10087 .lcomm delay_neutral.2416,1 |
10088 .lcomm RcLostTimer.2415,2 |
10089 .lcomm CorrectionRoll.2414,4 |
10090 .lcomm CorrectionNick.2413,4 |
10091 .lcomm IntegralGyroRollError.2412,4 |
10092 .lcomm IntegralGyroNickError.2411,4 |
10093 .lcomm SetPointYaw.2410,4 |
10094 .lcomm IPartRoll.2409,4 |
10095 .lcomm IPartNick.2408,4 |
10098 stick.2382: |
10099 0039 01 .byte 1 |
10100 .lcomm Last_GyroNick.2093,2 |
10101 .lcomm Last_GyroRoll.2092,2 |
10102 .section .progmem.data,"a",@progbits |
10105 __c.1965: |
10106 0000 0A0D 4143 .string "\n\rACC not calibrated!\r\n" |
10106 4320 6E6F |
10106 7420 6361 |
10106 6C69 6272 |
10106 6174 6564 |
10107 .comm GyroNick,2,1 |
10108 .comm GyroRoll,2,1 |
10109 .comm GyroYaw,2,1 |
10110 .comm YawGyroHeading,4,1 |
10111 .comm AccNick,2,1 |
10112 .comm AccRoll,2,1 |
10113 .comm AccTop,2,1 |
10114 .comm TrimNick,2,1 |
10115 .comm TrimRoll,2,1 |
10116 .comm MeanIntegralGyroNick,4,1 |
10117 .comm MeanIntegralGyroRoll,4,1 |
10118 .comm YawGyroDrift,2,1 |
10119 .comm GyroPFactor,1,1 |
10120 .comm GyroIFactor,1,1 |
10121 .comm GyroYawPFactor,1,1 |
10122 .comm GyroYawIFactor,1,1 |
10123 .global __do_copy_data |
10124 .global __do_clear_bss |
DEFINED SYMBOLS |
*ABS*:00000000 fc.c |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2 *ABS*:0000003f __SREG__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3 *ABS*:0000003e __SP_H__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:4 *ABS*:0000003d __SP_L__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:5 *ABS*:00000034 __CCP__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:6 *ABS*:00000000 __tmp_reg__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:7 *ABS*:00000001 __zero_reg__ |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:11 .text:00000000 MotorSmoothing |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:43 .text:0000002c Mean |
*COM*:00000002 GyroNick |
*COM*:00000002 GyroRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9646 .bss:00000004 AdBiasGyroYaw |
*COM*:00000002 GyroYaw |
*COM*:00000002 AccNick |
*COM*:00000002 AccRoll |
*COM*:00000002 AccTop |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9730 .bss:00000036 MeanAccNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9736 .bss:0000003a MeanAccRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9785 .bss:00000046 NaviAccNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9791 .bss:00000048 NaviAccRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9797 .bss:0000004a NaviCntAcc |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9700 .bss:00000022 ReadingIntegralGyroNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9712 .bss:0000002a ReadingIntegralGyroRoll |
*COM*:00000004 YawGyroHeading |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9724 .bss:00000032 ReadingIntegralGyroYaw |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9983 .bss:0000007e LoopingNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9989 .bss:0000007f LoopingRoll |
*COM*:00000002 TrimNick |
*COM*:00000002 TrimRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9718 .bss:0000002e ReadingIntegralGyroRoll2 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9971 .bss:0000007a AttitudeCorrectionRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9823 .data:0000000a TurnOver180Roll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9706 .bss:00000026 ReadingIntegralGyroNick2 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9977 .bss:0000007c AttitudeCorrectionNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9815 .data:00000006 TurnOver180Nick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9694 .bss:0000001e IntegralGyroYaw |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9670 .bss:0000000e IntegralGyroNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9682 .bss:00000016 IntegralGyroRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9676 .bss:00000012 IntegralGyroNick2 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9688 .bss:0000001a IntegralGyroRoll2 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10019 .data:00000012 FCParam |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10095 .bss:000000e3 Last_GyroNick.2093 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10100 .bss:000000e5 Last_GyroRoll.2092 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9772 .bss:00000045 FunnelCourse |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:1559 .text:00000e60 ParameterMapping |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9856 .bss:00000055 Poti4 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9831 .data:0000000e Ki |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9838 .bss:0000004f Poti1 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9844 .bss:00000051 Poti2 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9850 .bss:00000053 Poti3 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2620 .text:000017da SendMotorData |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9809 .bss:0000004e MKFlags |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2681 .text:0000185c SetNeutral |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9652 .bss:00000006 AdBiasAccNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9658 .bss:00000008 AdBiasAccRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9664 .bss:0000000a AdBiasAccTop |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9634 .bss:00000000 BiasHiResGyroNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9640 .bss:00000002 BiasHiResGyroRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9742 .bss:0000003e ReadingIntegralTop |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9748 .data:00000000 CompassHeading |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9753 .data:00000002 CompassCourse |
*COM*:00000002 YawGyroDrift |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9958 .data:00000010 ExternHeightValue |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9916 .bss:00000068 GPSStickNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9922 .bss:0000006a GPSStickRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10105 .progmem.data:00000000 __c.1965 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3317 .text:00001eca Beep |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3353 .text:00001f08 SetCompassCalState |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10098 .data:00000039 stick.2382 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9766 .bss:00000044 CompassCalState |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3402 .text:00001f6a MotorControl |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10087 .bss:000000c5 RcLostTimer.2415 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9803 .bss:0000004c ModelIsFlying |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10080 .bss:000000a5 stick_nick.2424 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9892 .bss:00000060 StickNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10079 .bss:000000a3 stick_roll.2425 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9898 .bss:00000062 StickRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9904 .bss:00000064 StickYaw |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9910 .bss:00000066 StickGas |
*COM*:00000001 GyroPFactor |
*COM*:00000001 GyroIFactor |
*COM*:00000001 GyroYawPFactor |
*COM*:00000001 GyroYawIFactor |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9928 .bss:0000006c MaxStickNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9934 .bss:0000006e MaxStickRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9995 .bss:00000080 LoopingLeft |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10001 .bss:00000081 LoopingRight |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10013 .bss:00000083 LoopingTop |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10007 .bss:00000082 LoopingDown |
*COM*:00000004 MeanIntegralGyroNick |
*COM*:00000004 MeanIntegralGyroRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10073 .bss:00000099 MeanIntegralGyroNick_old.2431 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10072 .bss:00000095 MeanIntegralGyroRoll_old.2432 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9778 .data:00000004 BadCompassHeading |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10092 .bss:000000d7 SetPointYaw.2410 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10094 .bss:000000df IPartNick.2408 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10093 .bss:000000db IPartRoll.2409 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9862 .bss:00000057 Poti5 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9868 .bss:00000059 Poti6 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9874 .bss:0000005b Poti7 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9880 .bss:0000005d Poti8 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10086 .bss:000000c4 delay_neutral.2416 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10083 .bss:000000c1 TimerDebugOut.2419 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10081 .bss:000000a7 MotorValue.2421 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10082 .bss:000000bf UpdateCompassCourse.2420 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10064 .bss:0000008e HoverGas.2445 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10052 .bss:00000085 BaroExpandActive.2451 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10057 .data:00000034 delay.2449 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9965 .bss:00000076 SetPointHeight |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10065 .bss:00000090 HCActive.2443 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10091 .bss:000000d3 IntegralGyroNickError.2411 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10089 .bss:000000cb CorrectionNick.2413 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10090 .bss:000000cf IntegralGyroRollError.2412 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10088 .bss:000000c7 CorrectionRoll.2414 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10078 .bss:000000a1 cnt.2426 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10085 .bss:000000c3 delay_startmotors.2417 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10084 .bss:000000c2 delay_stopmotors.2418 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10053 .bss:00000087 BaroFlags.2450 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10068 .data:00000037 StickGasHover.2444 |
.bss:00000084 HeightTrimmingFlag.2452 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10070 .bss:00000091 HeightTrimming.2442 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10071 .bss:00000093 FilterHCGas.2441 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10054 .bss:00000088 HoverGasFilter.2448 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10059 .bss:0000008c HoverGasMin.2446 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10062 .data:00000035 HoverGasMax.2447 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10075 .bss:0000009e last_r_p.2429 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10074 .bss:0000009d last_r_n.2430 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10077 .bss:000000a0 last_n_p.2427 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10076 .bss:0000009f last_n_n.2428 |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9760 .bss:00000042 CompassOffCourse |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9886 .bss:0000005f RequiredMotors |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9940 .bss:00000070 ExternStickNick |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9946 .bss:00000072 ExternStickRoll |
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9952 .bss:00000074 ExternStickYaw |
|
UNDEFINED SYMBOLS |
HiResGyroNick |
FilterHiResGyroNick |
AdValueGyroNick |
BoardRelease |
HiResGyroRoll |
FilterHiResGyroRoll |
AdValueGyroRoll |
AdValueGyroYaw |
AdValueAccNick |
__mulsi3 |
__divmodsi4 |
AdValueAccRoll |
AdValueAccTop |
ADReady |
ParamSet |
RC_Quality |
__divmodhi4 |
MotorTest_Active |
Motor |
MotorTest |
DebugOut |
I2C_Start |
ExpandBaro |
Delay_ms_Mess |
AdAirPressure |
AdValueAccZ |
__floatsisf |
SetParamWord |
__fixunssfsi |
AirPressure |
StartAirPressure |
ReadingVario |
BeepTime |
PPM_in |
SearchAirPressureOffset |
GetParamWord |
_printf_P |
__gtsf2 |
SearchDacGyroOffset |
Delay_ms |
PPM_diff |
NewPpmData |
ExternControl |
MeasurementCounter |
SetActiveParamSet |
GetActiveParamSet |
ParamSet_ReadFromEEProm |
Servo_Off |
ReadingHeight |
UBat |
ServoNickValue |
LowVoltageWarning |
__udivmodhi4 |
MissingMotor |
Mixer |
SumHeight |
ihypot |
c_cos_8192 |
NCGpsZ |
PressureSensorOffset |
Servo_On |
__do_copy_data |
__do_clear_bss |