Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1564 LPDunwell 1
   1               		.file	"fc.c"
2
   2               	__SREG__ = 0x3f
3
   3               	__SP_H__ = 0x3e
4
   4               	__SP_L__ = 0x3d
5
   5               	__CCP__  = 0x34
6
   6               	__tmp_reg__ = 0
7
   7               	__zero_reg__ = 1
8
   8               		.text
9
   9               	.global	MotorSmoothing
10
  11               	MotorSmoothing:
11
  12               	/* prologue: function */
12
  13               	/* frame size = 0 */
13
  14 0000 9B01      		movw r18,r22
14
  15 0002 6817      		cp r22,r24
15
  16 0004 7907      		cpc r23,r25
16
  17 0006 04F0      		brlt .L7
17
  18 0008 BC01      		movw r22,r24
18
  19 000a 660F      		lsl r22
19
  20 000c 771F      		rol r23
20
  21 000e 621B      		sub r22,r18
21
  22 0010 730B      		sbc r23,r19
22
  23 0012 CB01      		movw r24,r22
23
  24               	/* epilogue start */
24
  25 0014 0895      		ret
25
  26               	.L7:
26
  27 0016 680F      		add r22,r24
27
  28 0018 791F      		adc r23,r25
28
  29 001a 77FD      		sbrc r23,7
29
  30 001c 00C0      		rjmp .L8
30
  31               	.L3:
31
  32 001e 7595      		asr r23
32
  33 0020 6795      		ror r22
33
  34 0022 CB01      		movw r24,r22
34
  35 0024 0895      		ret
35
  36               	.L8:
36
  37 0026 6F5F      		subi r22,lo8(-(1))
37
  38 0028 7F4F      		sbci r23,hi8(-(1))
38
  39 002a 00C0      		rjmp .L3
39
  41               	.global	Mean
40
  43               	Mean:
41
  44 002c 2F92      		push r2
42
  45 002e 3F92      		push r3
43
  46 0030 4F92      		push r4
44
  47 0032 5F92      		push r5
45
  48 0034 6F92      		push r6
46
  49 0036 7F92      		push r7
47
  50 0038 8F92      		push r8
48
  51 003a 9F92      		push r9
49
  52 003c AF92      		push r10
50
  53 003e BF92      		push r11
51
  54 0040 CF92      		push r12
52
  55 0042 DF92      		push r13
53
  56 0044 EF92      		push r14
54
  57 0046 FF92      		push r15
55
  58 0048 0F93      		push r16
56
  59 004a 1F93      		push r17
57
  60 004c DF93      		push r29
58
  61 004e CF93      		push r28
59
  62 0050 CDB7      		in r28,__SP_L__
60
  63 0052 DEB7      		in r29,__SP_H__
61
  64 0054 AD97      		sbiw r28,45
62
  65 0056 0FB6      		in __tmp_reg__,__SREG__
63
  66 0058 F894      		cli
64
  67 005a DEBF      		out __SP_H__,r29
65
  68 005c 0FBE      		out __SREG__,__tmp_reg__
66
  69 005e CDBF      		out __SP_L__,r28
67
  70               	/* prologue: function */
68
  71               	/* frame size = 45 */
69
  72 0060 8091 0000 		lds r24,HiResGyroNick
70
  73 0064 9091 0000 		lds r25,(HiResGyroNick)+1
71
  74 0068 97FD      		sbrc r25,7
72
  75 006a 00C0      		rjmp .L67
73
  76               	.L10:
74
  77 006c 9595      		asr r25
75
  78 006e 8795      		ror r24
76
  79 0070 9595      		asr r25
77
  80 0072 8795      		ror r24
78
  81 0074 9595      		asr r25
79
  82 0076 8795      		ror r24
80
  83 0078 9093 0000 		sts (GyroNick)+1,r25
81
  84 007c 8093 0000 		sts GyroNick,r24
82
  85 0080 2091 0000 		lds r18,FilterHiResGyroNick
83
  86 0084 3091 0000 		lds r19,(FilterHiResGyroNick)+1
84
  87 0088 37FD      		sbrc r19,7
85
  88 008a 00C0      		rjmp .L68
86
  89               	.L11:
87
  90 008c C901      		movw r24,r18
88
  91 008e 9595      		asr r25
89
  92 0090 8795      		ror r24
90
  93 0092 9595      		asr r25
91
  94 0094 8795      		ror r24
92
  95 0096 9595      		asr r25
93
  96 0098 8795      		ror r24
94
  97 009a 9D8F      		std Y+29,r25
95
  98 009c 8C8F      		std Y+28,r24
96
  99 009e 8091 0000 		lds r24,AdValueGyroNick
97
 100 00a2 9091 0000 		lds r25,(AdValueGyroNick)+1
98
 101 00a6 0F97      		sbiw r24,15
99
 102 00a8 04F4      		brge .L12
100
 103 00aa 88E1      		ldi r24,lo8(-1000)
101
 104 00ac 9CEF      		ldi r25,hi8(-1000)
102
 105 00ae 9093 0000 		sts (GyroNick)+1,r25
103
 106 00b2 8093 0000 		sts GyroNick,r24
104
 107               	.L12:
105
 108 00b6 8091 0000 		lds r24,AdValueGyroNick
106
 109 00ba 9091 0000 		lds r25,(AdValueGyroNick)+1
107
 110 00be 0797      		sbiw r24,7
108
 111 00c0 04F4      		brge .L13
109
 112 00c2 80E3      		ldi r24,lo8(-2000)
110
 113 00c4 98EF      		ldi r25,hi8(-2000)
111
 114 00c6 9093 0000 		sts (GyroNick)+1,r25
112
 115 00ca 8093 0000 		sts GyroNick,r24
113
 116               	.L13:
114
 117 00ce 4091 0000 		lds r20,BoardRelease
115
 118 00d2 4A30      		cpi r20,lo8(10)
116
 119 00d4 01F4      		brne .+2
117
 120 00d6 00C0      		rjmp .L69
118
 121 00d8 8091 0000 		lds r24,AdValueGyroNick
119
 122 00dc 9091 0000 		lds r25,(AdValueGyroNick)+1
120
 123 00e0 815D      		subi r24,lo8(2001)
121
 124 00e2 9740      		sbci r25,hi8(2001)
122
 125 00e4 04F0      		brlt .L17
123
 126 00e6 88EE      		ldi r24,lo8(1000)
124
 127 00e8 93E0      		ldi r25,hi8(1000)
125
 128 00ea 9093 0000 		sts (GyroNick)+1,r25
126
 129 00ee 8093 0000 		sts GyroNick,r24
127
 130               	.L17:
128
 131 00f2 8091 0000 		lds r24,AdValueGyroNick
129
 132 00f6 9091 0000 		lds r25,(AdValueGyroNick)+1
130
 133 00fa 805E      		subi r24,lo8(2016)
131
 134 00fc 9740      		sbci r25,hi8(2016)
132
 135 00fe 04F0      		brlt .+2
133
 136 0100 00C0      		rjmp .L65
134
 137               	.L16:
135
 138 0102 8091 0000 		lds r24,HiResGyroRoll
136
 139 0106 9091 0000 		lds r25,(HiResGyroRoll)+1
137
 140 010a 97FD      		sbrc r25,7
138
 141 010c 00C0      		rjmp .L70
139
 142               	.L18:
140
 143 010e 9595      		asr r25
141
 144 0110 8795      		ror r24
142
 145 0112 9595      		asr r25
143
 146 0114 8795      		ror r24
144
 147 0116 9595      		asr r25
145
 148 0118 8795      		ror r24
146
 149 011a 9093 0000 		sts (GyroRoll)+1,r25
147
 150 011e 8093 0000 		sts GyroRoll,r24
148
 151 0122 2091 0000 		lds r18,FilterHiResGyroRoll
149
 152 0126 3091 0000 		lds r19,(FilterHiResGyroRoll)+1
150
 153 012a 37FD      		sbrc r19,7
151
 154 012c 00C0      		rjmp .L71
152
 155               	.L19:
153
 156 012e C901      		movw r24,r18
154
 157 0130 9595      		asr r25
155
 158 0132 8795      		ror r24
156
 159 0134 9595      		asr r25
157
 160 0136 8795      		ror r24
158
 161 0138 9595      		asr r25
159
 162 013a 8795      		ror r24
160
 163 013c 9B8F      		std Y+27,r25
161
 164 013e 8A8F      		std Y+26,r24
162
 165 0140 8091 0000 		lds r24,AdValueGyroRoll
163
 166 0144 9091 0000 		lds r25,(AdValueGyroRoll)+1
164
 167 0148 0F97      		sbiw r24,15
165
 168 014a 04F4      		brge .L20
166
 169 014c 88E1      		ldi r24,lo8(-1000)
167
 170 014e 9CEF      		ldi r25,hi8(-1000)
168
 171 0150 9093 0000 		sts (GyroRoll)+1,r25
169
 172 0154 8093 0000 		sts GyroRoll,r24
170
 173               	.L20:
171
 174 0158 8091 0000 		lds r24,AdValueGyroRoll
172
 175 015c 9091 0000 		lds r25,(AdValueGyroRoll)+1
173
 176 0160 0797      		sbiw r24,7
174
 177 0162 04F4      		brge .L21
175
 178 0164 80E3      		ldi r24,lo8(-2000)
176
 179 0166 98EF      		ldi r25,hi8(-2000)
177
 180 0168 9093 0000 		sts (GyroRoll)+1,r25
178
 181 016c 8093 0000 		sts GyroRoll,r24
179
 182               	.L21:
180
 183 0170 4A30      		cpi r20,lo8(10)
181
 184 0172 01F4      		brne .+2
182
 185 0174 00C0      		rjmp .L72
183
 186 0176 8091 0000 		lds r24,AdValueGyroRoll
184
 187 017a 9091 0000 		lds r25,(AdValueGyroRoll)+1
185
 188 017e 815D      		subi r24,lo8(2001)
186
 189 0180 9740      		sbci r25,hi8(2001)
187
 190 0182 04F0      		brlt .L25
188
 191 0184 88EE      		ldi r24,lo8(1000)
189
 192 0186 93E0      		ldi r25,hi8(1000)
190
 193 0188 9093 0000 		sts (GyroRoll)+1,r25
191
 194 018c 8093 0000 		sts GyroRoll,r24
192
 195               	.L25:
193
 196 0190 8091 0000 		lds r24,AdValueGyroRoll
194
 197 0194 9091 0000 		lds r25,(AdValueGyroRoll)+1
195
 198 0198 805E      		subi r24,lo8(2016)
196
 199 019a 9740      		sbci r25,hi8(2016)
197
 200 019c 04F0      		brlt .+2
198
 201 019e 00C0      		rjmp .L66
199
 202               	.L24:
200
 203 01a0 8091 0000 		lds r24,AdValueGyroYaw
201
 204 01a4 9091 0000 		lds r25,(AdValueGyroYaw)+1
202
 205 01a8 2091 0000 		lds r18,AdBiasGyroYaw
203
 206 01ac 3091 0000 		lds r19,(AdBiasGyroYaw)+1
204
 207 01b0 281B      		sub r18,r24
205
 208 01b2 390B      		sbc r19,r25
206
 209 01b4 3DA7      		std Y+45,r19
207
 210 01b6 2CA7      		std Y+44,r18
208
 211 01b8 3093 0000 		sts (GyroYaw)+1,r19
209
 212 01bc 2093 0000 		sts GyroYaw,r18
210
 213 01c0 6091 0000 		lds r22,AdValueAccNick
211
 214 01c4 7091 0000 		lds r23,(AdValueAccNick)+1
212
 215 01c8 8827      		clr r24
213
 216 01ca 77FD      		sbrc r23,7
214
 217 01cc 8095      		com r24
215
 218 01ce 982F      		mov r25,r24
216
 219 01d0 26E0      		ldi r18,lo8(6)
217
 220 01d2 30E0      		ldi r19,hi8(6)
218
 221 01d4 40E0      		ldi r20,hlo8(6)
219
 222 01d6 50E0      		ldi r21,hhi8(6)
220
 223 01d8 0E94 0000 		call __mulsi3
221
 224 01dc 7B01      		movw r14,r22
222
 225 01de 8C01      		movw r16,r24
223
 226 01e0 2091 0000 		lds r18,AccNick
224
 227 01e4 3091 0000 		lds r19,(AccNick)+1
225
 228 01e8 4427      		clr r20
226
 229 01ea 37FD      		sbrc r19,7
227
 230 01ec 4095      		com r20
228
 231 01ee 542F      		mov r21,r20
229
 232 01f0 CA01      		movw r24,r20
230
 233 01f2 B901      		movw r22,r18
231
 234 01f4 660F      		lsl r22
232
 235 01f6 771F      		rol r23
233
 236 01f8 881F      		rol r24
234
 237 01fa 991F      		rol r25
235
 238 01fc 620F      		add r22,r18
236
 239 01fe 731F      		adc r23,r19
237
 240 0200 841F      		adc r24,r20
238
 241 0202 951F      		adc r25,r21
239
 242 0204 6E0D      		add r22,r14
240
 243 0206 7F1D      		adc r23,r15
241
 244 0208 801F      		adc r24,r16
242
 245 020a 911F      		adc r25,r17
243
 246 020c 24E0      		ldi r18,lo8(4)
244
 247 020e 30E0      		ldi r19,hi8(4)
245
 248 0210 40E0      		ldi r20,hlo8(4)
246
 249 0212 50E0      		ldi r21,hhi8(4)
247
 250 0214 0E94 0000 		call __divmodsi4
248
 251 0218 3093 0000 		sts (AccNick)+1,r19
249
 252 021c 2093 0000 		sts AccNick,r18
250
 253 0220 6091 0000 		lds r22,AdValueAccRoll
251
 254 0224 7091 0000 		lds r23,(AdValueAccRoll)+1
252
 255 0228 8827      		clr r24
253
 256 022a 77FD      		sbrc r23,7
254
 257 022c 8095      		com r24
255
 258 022e 982F      		mov r25,r24
256
 259 0230 26E0      		ldi r18,lo8(6)
257
 260 0232 30E0      		ldi r19,hi8(6)
258
 261 0234 40E0      		ldi r20,hlo8(6)
259
 262 0236 50E0      		ldi r21,hhi8(6)
260
 263 0238 0E94 0000 		call __mulsi3
261
 264 023c 7B01      		movw r14,r22
262
 265 023e 8C01      		movw r16,r24
263
 266 0240 2091 0000 		lds r18,AccRoll
264
 267 0244 3091 0000 		lds r19,(AccRoll)+1
265
 268 0248 4427      		clr r20
266
 269 024a 37FD      		sbrc r19,7
267
 270 024c 4095      		com r20
268
 271 024e 542F      		mov r21,r20
269
 272 0250 CA01      		movw r24,r20
270
 273 0252 B901      		movw r22,r18
271
 274 0254 660F      		lsl r22
272
 275 0256 771F      		rol r23
273
 276 0258 881F      		rol r24
274
 277 025a 991F      		rol r25
275
 278 025c 620F      		add r22,r18
276
 279 025e 731F      		adc r23,r19
277
 280 0260 841F      		adc r24,r20
278
 281 0262 951F      		adc r25,r21
279
 282 0264 6E0D      		add r22,r14
280
 283 0266 7F1D      		adc r23,r15
281
 284 0268 801F      		adc r24,r16
282
 285 026a 911F      		adc r25,r17
283
 286 026c 24E0      		ldi r18,lo8(4)
284
 287 026e 30E0      		ldi r19,hi8(4)
285
 288 0270 40E0      		ldi r20,hlo8(4)
286
 289 0272 50E0      		ldi r21,hhi8(4)
287
 290 0274 0E94 0000 		call __divmodsi4
288
 291 0278 3093 0000 		sts (AccRoll)+1,r19
289
 292 027c 2093 0000 		sts AccRoll,r18
290
 293 0280 E090 0000 		lds r14,AdValueAccTop
291
 294 0284 F090 0000 		lds r15,(AdValueAccTop)+1
292
 295 0288 2091 0000 		lds r18,AccTop
293
 296 028c 3091 0000 		lds r19,(AccTop)+1
294
 297 0290 4427      		clr r20
295
 298 0292 37FD      		sbrc r19,7
296
 299 0294 4095      		com r20
297
 300 0296 542F      		mov r21,r20
298
 301 0298 CA01      		movw r24,r20
299
 302 029a B901      		movw r22,r18
300
 303 029c 660F      		lsl r22
301
 304 029e 771F      		rol r23
302
 305 02a0 881F      		rol r24
303
 306 02a2 991F      		rol r25
304
 307 02a4 620F      		add r22,r18
305
 308 02a6 731F      		adc r23,r19
306
 309 02a8 841F      		adc r24,r20
307
 310 02aa 951F      		adc r25,r21
308
 311 02ac 0027      		clr r16
309
 312 02ae F7FC      		sbrc r15,7
310
 313 02b0 0095      		com r16
311
 314 02b2 102F      		mov r17,r16
312
 315 02b4 6E0D      		add r22,r14
313
 316 02b6 7F1D      		adc r23,r15
314
 317 02b8 801F      		adc r24,r16
315
 318 02ba 911F      		adc r25,r17
316
 319 02bc 24E0      		ldi r18,lo8(4)
317
 320 02be 30E0      		ldi r19,hi8(4)
318
 321 02c0 40E0      		ldi r20,hlo8(4)
319
 322 02c2 50E0      		ldi r21,hhi8(4)
320
 323 02c4 0E94 0000 		call __divmodsi4
321
 324 02c8 3093 0000 		sts (AccTop)+1,r19
322
 325 02cc 2093 0000 		sts AccTop,r18
323
 326 02d0 2091 0000 		lds r18,AdValueAccNick
324
 327 02d4 3091 0000 		lds r19,(AdValueAccNick)+1
325
 328 02d8 C901      		movw r24,r18
326
 329 02da 880F      		lsl r24
327
 330 02dc 991F      		rol r25
328
 331 02de 820F      		add r24,r18
329
 332 02e0 931F      		adc r25,r19
330
 333 02e2 880F      		lsl r24
331
 334 02e4 991F      		rol r25
332
 335 02e6 AA27      		clr r26
333
 336 02e8 97FD      		sbrc r25,7
334
 337 02ea A095      		com r26
335
 338 02ec BA2F      		mov r27,r26
336
 339 02ee 2091 0000 		lds r18,MeanAccNick
337
 340 02f2 3091 0000 		lds r19,(MeanAccNick)+1
338
 341 02f6 4091 0000 		lds r20,(MeanAccNick)+2
339
 342 02fa 5091 0000 		lds r21,(MeanAccNick)+3
340
 343 02fe 280F      		add r18,r24
341
 344 0300 391F      		adc r19,r25
342
 345 0302 4A1F      		adc r20,r26
343
 346 0304 5B1F      		adc r21,r27
344
 347 0306 2093 0000 		sts MeanAccNick,r18
345
 348 030a 3093 0000 		sts (MeanAccNick)+1,r19
346
 349 030e 4093 0000 		sts (MeanAccNick)+2,r20
347
 350 0312 5093 0000 		sts (MeanAccNick)+3,r21
348
 351 0316 2091 0000 		lds r18,AdValueAccRoll
349
 352 031a 3091 0000 		lds r19,(AdValueAccRoll)+1
350
 353 031e C901      		movw r24,r18
351
 354 0320 880F      		lsl r24
352
 355 0322 991F      		rol r25
353
 356 0324 820F      		add r24,r18
354
 357 0326 931F      		adc r25,r19
355
 358 0328 880F      		lsl r24
356
 359 032a 991F      		rol r25
357
 360 032c AA27      		clr r26
358
 361 032e 97FD      		sbrc r25,7
359
 362 0330 A095      		com r26
360
 363 0332 BA2F      		mov r27,r26
361
 364 0334 2091 0000 		lds r18,MeanAccRoll
362
 365 0338 3091 0000 		lds r19,(MeanAccRoll)+1
363
 366 033c 4091 0000 		lds r20,(MeanAccRoll)+2
364
 367 0340 5091 0000 		lds r21,(MeanAccRoll)+3
365
 368 0344 280F      		add r18,r24
366
 369 0346 391F      		adc r19,r25
367
 370 0348 4A1F      		adc r20,r26
368
 371 034a 5B1F      		adc r21,r27
369
 372 034c 2093 0000 		sts MeanAccRoll,r18
370
 373 0350 3093 0000 		sts (MeanAccRoll)+1,r19
371
 374 0354 4093 0000 		sts (MeanAccRoll)+2,r20
372
 375 0358 5093 0000 		sts (MeanAccRoll)+3,r21
373
 376 035c 2091 0000 		lds r18,AdValueAccNick
374
 377 0360 3091 0000 		lds r19,(AdValueAccNick)+1
375
 378 0364 8091 0000 		lds r24,NaviAccNick
376
 379 0368 9091 0000 		lds r25,(NaviAccNick)+1
377
 380 036c 820F      		add r24,r18
378
 381 036e 931F      		adc r25,r19
379
 382 0370 9093 0000 		sts (NaviAccNick)+1,r25
380
 383 0374 8093 0000 		sts NaviAccNick,r24
381
 384 0378 2091 0000 		lds r18,AdValueAccRoll
382
 385 037c 3091 0000 		lds r19,(AdValueAccRoll)+1
383
 386 0380 8091 0000 		lds r24,NaviAccRoll
384
 387 0384 9091 0000 		lds r25,(NaviAccRoll)+1
385
 388 0388 820F      		add r24,r18
386
 389 038a 931F      		adc r25,r19
387
 390 038c 9093 0000 		sts (NaviAccRoll)+1,r25
388
 391 0390 8093 0000 		sts NaviAccRoll,r24
389
 392 0394 8091 0000 		lds r24,NaviCntAcc
390
 393 0398 9091 0000 		lds r25,(NaviCntAcc)+1
391
 394 039c 0196      		adiw r24,1
392
 395 039e 9093 0000 		sts (NaviCntAcc)+1,r25
393
 396 03a2 8093 0000 		sts NaviCntAcc,r24
394
 397 03a6 8091 7A00 		lds r24,122
395
 398 03aa 886C      		ori r24,lo8(-56)
396
 399 03ac 8093 7A00 		sts 122,r24
397
 400 03b0 1092 0000 		sts ADReady,__zero_reg__
398
 401 03b4 8091 0000 		lds r24,ReadingIntegralGyroNick
399
 402 03b8 9091 0000 		lds r25,(ReadingIntegralGyroNick)+1
400
 403 03bc A091 0000 		lds r26,(ReadingIntegralGyroNick)+2
401
 404 03c0 B091 0000 		lds r27,(ReadingIntegralGyroNick)+3
402
 405 03c4 8E8B      		std Y+22,r24
403
 406 03c6 9F8B      		std Y+23,r25
404
 407 03c8 A88F      		std Y+24,r26
405
 408 03ca B98F      		std Y+25,r27
406
 409 03cc 8091 0000 		lds r24,ReadingIntegralGyroRoll
407
 410 03d0 9091 0000 		lds r25,(ReadingIntegralGyroRoll)+1
408
 411 03d4 A091 0000 		lds r26,(ReadingIntegralGyroRoll)+2
409
 412 03d8 B091 0000 		lds r27,(ReadingIntegralGyroRoll)+3
410
 413 03dc 8A8B      		std Y+18,r24
411
 414 03de 9B8B      		std Y+19,r25
412
 415 03e0 AC8B      		std Y+20,r26
413
 416 03e2 BD8B      		std Y+21,r27
414
 417 03e4 ACA5      		ldd r26,Y+44
415
 418 03e6 BDA5      		ldd r27,Y+45
416
 419 03e8 3D01      		movw r6,r26
417
 420 03ea 8824      		clr r8
418
 421 03ec 77FC      		sbrc r7,7
419
 422 03ee 8094      		com r8
420
 423 03f0 982C      		mov r9,r8
421
 424 03f2 E090 0000 		lds r14,YawGyroHeading
422
 425 03f6 F090 0000 		lds r15,(YawGyroHeading)+1
423
 426 03fa 0091 0000 		lds r16,(YawGyroHeading)+2
424
 427 03fe 1091 0000 		lds r17,(YawGyroHeading)+3
425
 428 0402 E60C      		add r14,r6
426
 429 0404 F71C      		adc r15,r7
427
 430 0406 081D      		adc r16,r8
428
 431 0408 191D      		adc r17,r9
429
 432 040a ECA2      		std Y+36,r14
430
 433 040c FDA2      		std Y+37,r15
431
 434 040e 0EA3      		std Y+38,r16
432
 435 0410 1FA3      		std Y+39,r17
433
 436 0412 E092 0000 		sts YawGyroHeading,r14
434
 437 0416 F092 0000 		sts (YawGyroHeading)+1,r15
435
 438 041a 0093 0000 		sts (YawGyroHeading)+2,r16
436
 439 041e 1093 0000 		sts (YawGyroHeading)+3,r17
437
 440 0422 2090 0000 		lds r2,ReadingIntegralGyroYaw
438
 441 0426 3090 0000 		lds r3,(ReadingIntegralGyroYaw)+1
439
 442 042a 4090 0000 		lds r4,(ReadingIntegralGyroYaw)+2
440
 443 042e 5090 0000 		lds r5,(ReadingIntegralGyroYaw)+3
441
 444 0432 260C      		add r2,r6
442
 445 0434 371C      		adc r3,r7
443
 446 0436 481C      		adc r4,r8
444
 447 0438 591C      		adc r5,r9
445
 448 043a 28A2      		std Y+32,r2
446
 449 043c 39A2      		std Y+33,r3
447
 450 043e 4AA2      		std Y+34,r4
448
 451 0440 5BA2      		std Y+35,r5
449
 452 0442 2092 0000 		sts ReadingIntegralGyroYaw,r2
450
 453 0446 3092 0000 		sts (ReadingIntegralGyroYaw)+1,r3
451
 454 044a 4092 0000 		sts (ReadingIntegralGyroYaw)+2,r4
452
 455 044e 5092 0000 		sts (ReadingIntegralGyroYaw)+3,r5
453
 456 0452 B091 0000 		lds r27,LoopingNick
454
 457 0456 BF8F      		std Y+31,r27
455
 458 0458 BB23      		tst r27
456
 459 045a 01F4      		brne .L26
457
 460 045c 8091 0000 		lds r24,LoopingRoll
458
 461 0460 8823      		tst r24
459
 462 0462 01F4      		brne .+2
460
 463 0464 00C0      		rjmp .L27
461
 464               	.L26:
462
 465 0466 E091 0000 		lds r30,ParamSet+8
463
 466 046a E98B      		std Y+17,r30
464
 467               	.L28:
465
 468 046c 1092 0000 		sts (TrimNick)+1,__zero_reg__
466
 469 0470 1092 0000 		sts TrimNick,__zero_reg__
467
 470 0474 1092 0000 		sts (TrimRoll)+1,__zero_reg__
468
 471 0478 1092 0000 		sts TrimRoll,__zero_reg__
469
 472 047c 1D86      		std Y+13,__zero_reg__
470
 473 047e 1E86      		std Y+14,__zero_reg__
471
 474 0480 1F86      		std Y+15,__zero_reg__
472
 475 0482 188A      		std Y+16,__zero_reg__
473
 476 0484 1986      		std Y+9,__zero_reg__
474
 477 0486 1A86      		std Y+10,__zero_reg__
475
 478 0488 1B86      		std Y+11,__zero_reg__
476
 479 048a 1C86      		std Y+12,__zero_reg__
477
 480               	.L40:
478
 481 048c 8091 0000 		lds r24,ParamSet+23
479
 482 0490 A82E      		mov r10,r24
480
 483 0492 BB24      		clr r11
481
 484 0494 CC24      		clr r12
482
 485 0496 DD24      		clr r13
483
 486 0498 C601      		movw r24,r12
484
 487 049a B501      		movw r22,r10
485
 488 049c 20E1      		ldi r18,lo8(15120)
486
 489 049e 3BE3      		ldi r19,hi8(15120)
487
 490 04a0 40E0      		ldi r20,hlo8(15120)
488
 491 04a2 50E0      		ldi r21,hhi8(15120)
489
 492 04a4 0E94 0000 		call __mulsi3
490
 493 04a8 3B01      		movw r6,r22
491
 494 04aa 4C01      		movw r8,r24
492
 495 04ac E616      		cp r14,r22
493
 496 04ae F706      		cpc r15,r23
494
 497 04b0 0807      		cpc r16,r24
495
 498 04b2 1907      		cpc r17,r25
496
 499 04b4 04F0      		brlt .L41
497
 500 04b6 C601      		movw r24,r12
498
 501 04b8 B501      		movw r22,r10
499
 502 04ba 20EF      		ldi r18,lo8(-15120)
500
 503 04bc 34EC      		ldi r19,hi8(-15120)
501
 504 04be 4FEF      		ldi r20,hlo8(-15120)
502
 505 04c0 5FEF      		ldi r21,hhi8(-15120)
503
 506 04c2 0E94 0000 		call __mulsi3
504
 507 04c6 6E0D      		add r22,r14
505
 508 04c8 7F1D      		adc r23,r15
506
 509 04ca 801F      		adc r24,r16
507
 510 04cc 911F      		adc r25,r17
508
 511 04ce 6093 0000 		sts YawGyroHeading,r22
509
 512 04d2 7093 0000 		sts (YawGyroHeading)+1,r23
510
 513 04d6 8093 0000 		sts (YawGyroHeading)+2,r24
511
 514 04da 9093 0000 		sts (YawGyroHeading)+3,r25
512
 515 04de 7B01      		movw r14,r22
513
 516 04e0 8C01      		movw r16,r24
514
 517               	.L41:
515
 518 04e2 17FD      		sbrc r17,7
516
 519 04e4 00C0      		rjmp .L73
517
 520               	.L42:
518
 521 04e6 6090 0000 		lds r6,TrimRoll
519
 522 04ea 7090 0000 		lds r7,(TrimRoll)+1
520
 523 04ee 2A8D      		ldd r18,Y+26
521
 524 04f0 3B8D      		ldd r19,Y+27
522
 525 04f2 260D      		add r18,r6
523
 526 04f4 371D      		adc r19,r7
524
 527 04f6 7901      		movw r14,r18
525
 528 04f8 0027      		clr r16
526
 529 04fa F7FC      		sbrc r15,7
527
 530 04fc 0095      		com r16
528
 531 04fe 102F      		mov r17,r16
529
 532 0500 8091 0000 		lds r24,ReadingIntegralGyroRoll2
530
 533 0504 9091 0000 		lds r25,(ReadingIntegralGyroRoll2)+1
531
 534 0508 A091 0000 		lds r26,(ReadingIntegralGyroRoll2)+2
532
 535 050c B091 0000 		lds r27,(ReadingIntegralGyroRoll2)+3
533
 536 0510 8E0D      		add r24,r14
534
 537 0512 9F1D      		adc r25,r15
535
 538 0514 A01F      		adc r26,r16
536
 539 0516 B11F      		adc r27,r17
537
 540 0518 8093 0000 		sts ReadingIntegralGyroRoll2,r24
538
 541 051c 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
539
 542 0520 A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
540
 543 0524 B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
541
 544 0528 8091 0000 		lds r24,AttitudeCorrectionRoll
542
 545 052c 9091 0000 		lds r25,(AttitudeCorrectionRoll)+1
543
 546 0530 281B      		sub r18,r24
544
 547 0532 390B      		sbc r19,r25
545
 548 0534 4427      		clr r20
546
 549 0536 37FD      		sbrc r19,7
547
 550 0538 4095      		com r20
548
 551 053a 542F      		mov r21,r20
549
 552 053c 8A89      		ldd r24,Y+18
550
 553 053e 9B89      		ldd r25,Y+19
551
 554 0540 AC89      		ldd r26,Y+20
552
 555 0542 BD89      		ldd r27,Y+21
553
 556 0544 280F      		add r18,r24
554
 557 0546 391F      		adc r19,r25
555
 558 0548 4A1F      		adc r20,r26
556
 559 054a 5B1F      		adc r21,r27
557
 560 054c 2093 0000 		sts ReadingIntegralGyroRoll,r18
558
 561 0550 3093 0000 		sts (ReadingIntegralGyroRoll)+1,r19
559
 562 0554 4093 0000 		sts (ReadingIntegralGyroRoll)+2,r20
560
 563 0558 5093 0000 		sts (ReadingIntegralGyroRoll)+3,r21
561
 564 055c E090 0000 		lds r14,TurnOver180Roll
562
 565 0560 F090 0000 		lds r15,(TurnOver180Roll)+1
563
 566 0564 0091 0000 		lds r16,(TurnOver180Roll)+2
564
 567 0568 1091 0000 		lds r17,(TurnOver180Roll)+3
565
 568 056c E216      		cp r14,r18
566
 569 056e F306      		cpc r15,r19
567
 570 0570 0407      		cpc r16,r20
568
 571 0572 1507      		cpc r17,r21
569
 572 0574 04F4      		brge .L43
570
 573 0576 80E1      		ldi r24,lo8(10000)
571
 574 0578 97E2      		ldi r25,hi8(10000)
572
 575 057a A0E0      		ldi r26,hlo8(10000)
573
 576 057c B0E0      		ldi r27,hhi8(10000)
574
 577 057e 8E19      		sub r24,r14
575
 578 0580 9F09      		sbc r25,r15
576
 579 0582 A00B      		sbc r26,r16
577
 580 0584 B10B      		sbc r27,r17
578
 581 0586 8093 0000 		sts ReadingIntegralGyroRoll,r24
579
 582 058a 9093 0000 		sts (ReadingIntegralGyroRoll)+1,r25
580
 583 058e A093 0000 		sts (ReadingIntegralGyroRoll)+2,r26
581
 584 0592 B093 0000 		sts (ReadingIntegralGyroRoll)+3,r27
582
 585 0596 8093 0000 		sts ReadingIntegralGyroRoll2,r24
583
 586 059a 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
584
 587 059e A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
585
 588 05a2 B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
586
 589               	.L43:
587
 590 05a6 8827      		clr r24
588
 591 05a8 9927      		clr r25
589
 592 05aa DC01      		movw r26,r24
590
 593 05ac 8E19      		sub r24,r14
591
 594 05ae 9F09      		sbc r25,r15
592
 595 05b0 A00B      		sbc r26,r16
593
 596 05b2 B10B      		sbc r27,r17
594
 597 05b4 A090 0000 		lds r10,ReadingIntegralGyroRoll
595
 598 05b8 B090 0000 		lds r11,(ReadingIntegralGyroRoll)+1
596
 599 05bc C090 0000 		lds r12,(ReadingIntegralGyroRoll)+2
597
 600 05c0 D090 0000 		lds r13,(ReadingIntegralGyroRoll)+3
598
 601 05c4 A816      		cp r10,r24
599
 602 05c6 B906      		cpc r11,r25
600
 603 05c8 CA06      		cpc r12,r26
601
 604 05ca DB06      		cpc r13,r27
602
 605 05cc 04F4      		brge .L44
603
 606 05ce D801      		movw r26,r16
604
 607 05d0 C701      		movw r24,r14
605
 608 05d2 8051      		subi r24,lo8(-(-10000))
606
 609 05d4 9742      		sbci r25,hi8(-(-10000))
607
 610 05d6 A040      		sbci r26,hlo8(-(-10000))
608
 611 05d8 B040      		sbci r27,hhi8(-(-10000))
609
 612 05da 8093 0000 		sts ReadingIntegralGyroRoll,r24
610
 613 05de 9093 0000 		sts (ReadingIntegralGyroRoll)+1,r25
611
 614 05e2 A093 0000 		sts (ReadingIntegralGyroRoll)+2,r26
612
 615 05e6 B093 0000 		sts (ReadingIntegralGyroRoll)+3,r27
613
 616 05ea 8093 0000 		sts ReadingIntegralGyroRoll2,r24
614
 617 05ee 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
615
 618 05f2 A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
616
 619 05f6 B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
617
 620 05fa 5C01      		movw r10,r24
618
 621 05fc 6D01      		movw r12,r26
619
 622               	.L44:
620
 623 05fe 8090 0000 		lds r8,TrimNick
621
 624 0602 9090 0000 		lds r9,(TrimNick)+1
622
 625 0606 2C8D      		ldd r18,Y+28
623
 626 0608 3D8D      		ldd r19,Y+29
624
 627 060a 280D      		add r18,r8
625
 628 060c 391D      		adc r19,r9
626
 629 060e 7901      		movw r14,r18
627
 630 0610 0027      		clr r16
628
 631 0612 F7FC      		sbrc r15,7
629
 632 0614 0095      		com r16
630
 633 0616 102F      		mov r17,r16
631
 634 0618 8091 0000 		lds r24,ReadingIntegralGyroNick2
632
 635 061c 9091 0000 		lds r25,(ReadingIntegralGyroNick2)+1
633
 636 0620 A091 0000 		lds r26,(ReadingIntegralGyroNick2)+2
634
 637 0624 B091 0000 		lds r27,(ReadingIntegralGyroNick2)+3
635
 638 0628 8E0D      		add r24,r14
636
 639 062a 9F1D      		adc r25,r15
637
 640 062c A01F      		adc r26,r16
638
 641 062e B11F      		adc r27,r17
639
 642 0630 8093 0000 		sts ReadingIntegralGyroNick2,r24
640
 643 0634 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
641
 644 0638 A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
642
 645 063c B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
643
 646 0640 8091 0000 		lds r24,AttitudeCorrectionNick
644
 647 0644 9091 0000 		lds r25,(AttitudeCorrectionNick)+1
645
 648 0648 281B      		sub r18,r24
646
 649 064a 390B      		sbc r19,r25
647
 650 064c 4427      		clr r20
648
 651 064e 37FD      		sbrc r19,7
649
 652 0650 4095      		com r20
650
 653 0652 542F      		mov r21,r20
651
 654 0654 8E89      		ldd r24,Y+22
652
 655 0656 9F89      		ldd r25,Y+23
653
 656 0658 A88D      		ldd r26,Y+24
654
 657 065a B98D      		ldd r27,Y+25
655
 658 065c 280F      		add r18,r24
656
 659 065e 391F      		adc r19,r25
657
 660 0660 4A1F      		adc r20,r26
658
 661 0662 5B1F      		adc r21,r27
659
 662 0664 2093 0000 		sts ReadingIntegralGyroNick,r18
660
 663 0668 3093 0000 		sts (ReadingIntegralGyroNick)+1,r19
661
 664 066c 4093 0000 		sts (ReadingIntegralGyroNick)+2,r20
662
 665 0670 5093 0000 		sts (ReadingIntegralGyroNick)+3,r21
663
 666 0674 E090 0000 		lds r14,TurnOver180Nick
664
 667 0678 F090 0000 		lds r15,(TurnOver180Nick)+1
665
 668 067c 0091 0000 		lds r16,(TurnOver180Nick)+2
666
 669 0680 1091 0000 		lds r17,(TurnOver180Nick)+3
667
 670 0684 E216      		cp r14,r18
668
 671 0686 F306      		cpc r15,r19
669
 672 0688 0407      		cpc r16,r20
670
 673 068a 1507      		cpc r17,r21
671
 674 068c 04F4      		brge .L45
672
 675 068e 88EA      		ldi r24,lo8(25000)
673
 676 0690 91E6      		ldi r25,hi8(25000)
674
 677 0692 A0E0      		ldi r26,hlo8(25000)
675
 678 0694 B0E0      		ldi r27,hhi8(25000)
676
 679 0696 8E19      		sub r24,r14
677
 680 0698 9F09      		sbc r25,r15
678
 681 069a A00B      		sbc r26,r16
679
 682 069c B10B      		sbc r27,r17
680
 683 069e 8093 0000 		sts ReadingIntegralGyroNick,r24
681
 684 06a2 9093 0000 		sts (ReadingIntegralGyroNick)+1,r25
682
 685 06a6 A093 0000 		sts (ReadingIntegralGyroNick)+2,r26
683
 686 06aa B093 0000 		sts (ReadingIntegralGyroNick)+3,r27
684
 687 06ae 8093 0000 		sts ReadingIntegralGyroNick2,r24
685
 688 06b2 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
686
 689 06b6 A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
687
 690 06ba B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
688
 691               	.L45:
689
 692 06be 8827      		clr r24
690
 693 06c0 9927      		clr r25
691
 694 06c2 DC01      		movw r26,r24
692
 695 06c4 8E19      		sub r24,r14
693
 696 06c6 9F09      		sbc r25,r15
694
 697 06c8 A00B      		sbc r26,r16
695
 698 06ca B10B      		sbc r27,r17
696
 699 06cc 2091 0000 		lds r18,ReadingIntegralGyroNick
697
 700 06d0 3091 0000 		lds r19,(ReadingIntegralGyroNick)+1
698
 701 06d4 4091 0000 		lds r20,(ReadingIntegralGyroNick)+2
699
 702 06d8 5091 0000 		lds r21,(ReadingIntegralGyroNick)+3
700
 703 06dc 2817      		cp r18,r24
701
 704 06de 3907      		cpc r19,r25
702
 705 06e0 4A07      		cpc r20,r26
703
 706 06e2 5B07      		cpc r21,r27
704
 707 06e4 04F4      		brge .L46
705
 708 06e6 D801      		movw r26,r16
706
 709 06e8 C701      		movw r24,r14
707
 710 06ea 885A      		subi r24,lo8(-(-25000))
708
 711 06ec 9146      		sbci r25,hi8(-(-25000))
709
 712 06ee A040      		sbci r26,hlo8(-(-25000))
710
 713 06f0 B040      		sbci r27,hhi8(-(-25000))
711
 714 06f2 8093 0000 		sts ReadingIntegralGyroNick,r24
712
 715 06f6 9093 0000 		sts (ReadingIntegralGyroNick)+1,r25
713
 716 06fa A093 0000 		sts (ReadingIntegralGyroNick)+2,r26
714
 717 06fe B093 0000 		sts (ReadingIntegralGyroNick)+3,r27
715
 718 0702 8093 0000 		sts ReadingIntegralGyroNick2,r24
716
 719 0706 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
717
 720 070a A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
718
 721 070e B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
719
 722 0712 9C01      		movw r18,r24
720
 723 0714 AD01      		movw r20,r26
721
 724               	.L46:
722
 725 0716 2092 0000 		sts IntegralGyroYaw,r2
723
 726 071a 3092 0000 		sts (IntegralGyroYaw)+1,r3
724
 727 071e 4092 0000 		sts (IntegralGyroYaw)+2,r4
725
 728 0722 5092 0000 		sts (IntegralGyroYaw)+3,r5
726
 729 0726 2093 0000 		sts IntegralGyroNick,r18
727
 730 072a 3093 0000 		sts (IntegralGyroNick)+1,r19
728
 731 072e 4093 0000 		sts (IntegralGyroNick)+2,r20
729
 732 0732 5093 0000 		sts (IntegralGyroNick)+3,r21
730
 733 0736 A092 0000 		sts IntegralGyroRoll,r10
731
 734 073a B092 0000 		sts (IntegralGyroRoll)+1,r11
732
 735 073e C092 0000 		sts (IntegralGyroRoll)+2,r12
733
 736 0742 D092 0000 		sts (IntegralGyroRoll)+3,r13
734
 737 0746 8091 0000 		lds r24,ReadingIntegralGyroNick2
735
 738 074a 9091 0000 		lds r25,(ReadingIntegralGyroNick2)+1
736
 739 074e A091 0000 		lds r26,(ReadingIntegralGyroNick2)+2
737
 740 0752 B091 0000 		lds r27,(ReadingIntegralGyroNick2)+3
738
 741 0756 8093 0000 		sts IntegralGyroNick2,r24
739
 742 075a 9093 0000 		sts (IntegralGyroNick2)+1,r25
740
 743 075e A093 0000 		sts (IntegralGyroNick2)+2,r26
741
 744 0762 B093 0000 		sts (IntegralGyroNick2)+3,r27
742
 745 0766 8091 0000 		lds r24,ReadingIntegralGyroRoll2
743
 746 076a 9091 0000 		lds r25,(ReadingIntegralGyroRoll2)+1
744
 747 076e A091 0000 		lds r26,(ReadingIntegralGyroRoll2)+2
745
 748 0772 B091 0000 		lds r27,(ReadingIntegralGyroRoll2)+3
746
 749 0776 8093 0000 		sts IntegralGyroRoll2,r24
747
 750 077a 9093 0000 		sts (IntegralGyroRoll2)+1,r25
748
 751 077e A093 0000 		sts (IntegralGyroRoll2)+2,r26
749
 752 0782 B093 0000 		sts (IntegralGyroRoll2)+3,r27
750
 753 0786 6091 0000 		lds r22,FCParam+6
751
 754 078a 6623      		tst r22
752
 755 078c 01F4      		brne .+2
753
 756 078e 00C0      		rjmp .L47
754
 757 0790 4091 0000 		lds r20,HiResGyroNick
755
 758 0794 5091 0000 		lds r21,(HiResGyroNick)+1
756
 759 0798 8091 0000 		lds r24,Last_GyroNick.2093
757
 760 079c 9091 0000 		lds r25,(Last_GyroNick.2093)+1
758
 761 07a0 481B      		sub r20,r24
759
 762 07a2 590B      		sbc r21,r25
760
 763 07a4 2091 0000 		lds r18,HiResGyroNick
761
 764 07a8 3091 0000 		lds r19,(HiResGyroNick)+1
762
 765 07ac 280F      		add r18,r24
763
 766 07ae 391F      		adc r19,r25
764
 767 07b0 37FD      		sbrc r19,7
765
 768 07b2 00C0      		rjmp .L74
766
 769               	.L48:
767
 770 07b4 3595      		asr r19
768
 771 07b6 2795      		ror r18
769
 772 07b8 3093 0000 		sts (Last_GyroNick.2093)+1,r19
770
 773 07bc 2093 0000 		sts Last_GyroNick.2093,r18
771
 774 07c0 CA01      		movw r24,r20
772
 775 07c2 4138      		cpi r20,129
773
 776 07c4 5105      		cpc r21,__zero_reg__
774
 777 07c6 04F0      		brlt .+2
775
 778 07c8 00C0      		rjmp .L75
776
 779 07ca E62F      		mov r30,r22
777
 780 07cc F0E0      		ldi r31,lo8(0)
778
 781 07ce AFEF      		ldi r26,hi8(-128)
779
 782 07d0 8038      		cpi r24,lo8(-128)
780
 783 07d2 9A07      		cpc r25,r26
781
 784 07d4 04F4      		brge .+2
782
 785 07d6 00C0      		rjmp .L76
783
 786               	.L50:
784
 787 07d8 8E9F      		mul r24,r30
785
 788 07da B001      		movw r22,r0
786
 789 07dc 8F9F      		mul r24,r31
787
 790 07de 700D      		add r23,r0
788
 791 07e0 9E9F      		mul r25,r30
789
 792 07e2 700D      		add r23,r0
790
 793 07e4 1124      		clr r1
791
 794 07e6 9B01      		movw r18,r22
792
 795 07e8 77FD      		sbrc r23,7
793
 796 07ea 00C0      		rjmp .L77
794
 797               	.L51:
795
 798 07ec 3595      		asr r19
796
 799 07ee 2795      		ror r18
797
 800 07f0 3595      		asr r19
798
 801 07f2 2795      		ror r18
799
 802 07f4 3595      		asr r19
800
 803 07f6 2795      		ror r18
801
 804 07f8 3595      		asr r19
802
 805 07fa 2795      		ror r18
803
 806 07fc 8091 0000 		lds r24,GyroNick
804
 807 0800 9091 0000 		lds r25,(GyroNick)+1
805
 808 0804 820F      		add r24,r18
806
 809 0806 931F      		adc r25,r19
807
 810 0808 9093 0000 		sts (GyroNick)+1,r25
808
 811 080c 8093 0000 		sts GyroNick,r24
809
 812 0810 4091 0000 		lds r20,HiResGyroRoll
810
 813 0814 5091 0000 		lds r21,(HiResGyroRoll)+1
811
 814 0818 8091 0000 		lds r24,Last_GyroRoll.2092
812
 815 081c 9091 0000 		lds r25,(Last_GyroRoll.2092)+1
813
 816 0820 481B      		sub r20,r24
814
 817 0822 590B      		sbc r21,r25
815
 818 0824 2091 0000 		lds r18,HiResGyroRoll
816
 819 0828 3091 0000 		lds r19,(HiResGyroRoll)+1
817
 820 082c 280F      		add r18,r24
818
 821 082e 391F      		adc r19,r25
819
 822 0830 37FD      		sbrc r19,7
820
 823 0832 00C0      		rjmp .L78
821
 824               	.L52:
822
 825 0834 3595      		asr r19
823
 826 0836 2795      		ror r18
824
 827 0838 3093 0000 		sts (Last_GyroRoll.2092)+1,r19
825
 828 083c 2093 0000 		sts Last_GyroRoll.2092,r18
826
 829 0840 CA01      		movw r24,r20
827
 830 0842 4138      		cpi r20,129
828
 831 0844 5105      		cpc r21,__zero_reg__
829
 832 0846 04F0      		brlt .+2
830
 833 0848 00C0      		rjmp .L79
831
 834 084a BFEF      		ldi r27,hi8(-128)
832
 835 084c 8038      		cpi r24,lo8(-128)
833
 836 084e 9B07      		cpc r25,r27
834
 837 0850 04F4      		brge .+2
835
 838 0852 00C0      		rjmp .L80
836
 839               	.L54:
837
 840 0854 8E9F      		mul r24,r30
838
 841 0856 A001      		movw r20,r0
839
 842 0858 8F9F      		mul r24,r31
840
 843 085a 500D      		add r21,r0
841
 844 085c 9E9F      		mul r25,r30
842
 845 085e 500D      		add r21,r0
843
 846 0860 1124      		clr r1
844
 847 0862 9A01      		movw r18,r20
845
 848 0864 57FD      		sbrc r21,7
846
 849 0866 00C0      		rjmp .L81
847
 850               	.L55:
848
 851 0868 3595      		asr r19
849
 852 086a 2795      		ror r18
850
 853 086c 3595      		asr r19
851
 854 086e 2795      		ror r18
852
 855 0870 3595      		asr r19
853
 856 0872 2795      		ror r18
854
 857 0874 3595      		asr r19
855
 858 0876 2795      		ror r18
856
 859 0878 8091 0000 		lds r24,GyroRoll
857
 860 087c 9091 0000 		lds r25,(GyroRoll)+1
858
 861 0880 820F      		add r24,r18
859
 862 0882 931F      		adc r25,r19
860
 863 0884 9093 0000 		sts (GyroRoll)+1,r25
861
 864 0888 8093 0000 		sts GyroRoll,r24
862
 865 088c 8091 0000 		lds r24,HiResGyroNick
863
 866 0890 9091 0000 		lds r25,(HiResGyroNick)+1
864
 867 0894 680F      		add r22,r24
865
 868 0896 791F      		adc r23,r25
866
 869 0898 7093 0000 		sts (HiResGyroNick)+1,r23
867
 870 089c 6093 0000 		sts HiResGyroNick,r22
868
 871 08a0 8091 0000 		lds r24,HiResGyroRoll
869
 872 08a4 9091 0000 		lds r25,(HiResGyroRoll)+1
870
 873 08a8 480F      		add r20,r24
871
 874 08aa 591F      		adc r21,r25
872
 875 08ac 5093 0000 		sts (HiResGyroRoll)+1,r21
873
 876 08b0 4093 0000 		sts HiResGyroRoll,r20
874
 877               	.L47:
875
 878 08b4 EC8D      		ldd r30,Y+28
876
 879 08b6 FD8D      		ldd r31,Y+29
877
 880 08b8 1E16      		cp __zero_reg__,r30
878
 881 08ba 1F06      		cpc __zero_reg__,r31
879
 882 08bc 04F0      		brlt .+2
880
 883 08be 00C0      		rjmp .L56
881
 884 08c0 6D85      		ldd r22,Y+13
882
 885 08c2 7E85      		ldd r23,Y+14
883
 886 08c4 8F85      		ldd r24,Y+15
884
 887 08c6 9889      		ldd r25,Y+16
885
 888 08c8 20E4      		ldi r18,lo8(64)
886
 889 08ca 30E0      		ldi r19,hi8(64)
887
 890 08cc 40E0      		ldi r20,hlo8(64)
888
 891 08ce 50E0      		ldi r21,hhi8(64)
889
 892 08d0 0E94 0000 		call __divmodsi4
890
 893 08d4 820E      		add r8,r18
891
 894 08d6 931E      		adc r9,r19
892
 895 08d8 9092 0000 		sts (TrimNick)+1,r9
893
 896 08dc 8092 0000 		sts TrimNick,r8
894
 897               	.L57:
895
 898 08e0 2A8D      		ldd r18,Y+26
896
 899 08e2 3B8D      		ldd r19,Y+27
897
 900 08e4 1216      		cp __zero_reg__,r18
898
 901 08e6 1306      		cpc __zero_reg__,r19
899
 902 08e8 04F0      		brlt .+2
900
 903 08ea 00C0      		rjmp .L58
901
 904 08ec 6985      		ldd r22,Y+9
902
 905 08ee 7A85      		ldd r23,Y+10
903
 906 08f0 8B85      		ldd r24,Y+11
904
 907 08f2 9C85      		ldd r25,Y+12
905
 908 08f4 20E4      		ldi r18,lo8(64)
906
 909 08f6 30E0      		ldi r19,hi8(64)
907
 910 08f8 40E0      		ldi r20,hlo8(64)
908
 911 08fa 50E0      		ldi r21,hhi8(64)
909
 912 08fc 0E94 0000 		call __divmodsi4
910
 913 0900 620E      		add r6,r18
911
 914 0902 731E      		adc r7,r19
912
 915 0904 7092 0000 		sts (TrimRoll)+1,r7
913
 916 0908 6092 0000 		sts TrimRoll,r6
914
 917               	.L59:
915
 918 090c 3989      		ldd r19,Y+17
916
 919 090e 37FD      		sbrc r19,7
917
 920 0910 00C0      		rjmp .L82
918
 921               	.L64:
919
 922               	/* epilogue start */
920
 923 0912 AD96      		adiw r28,45
921
 924 0914 0FB6      		in __tmp_reg__,__SREG__
922
 925 0916 F894      		cli
923
 926 0918 DEBF      		out __SP_H__,r29
924
 927 091a 0FBE      		out __SREG__,__tmp_reg__
925
 928 091c CDBF      		out __SP_L__,r28
926
 929 091e CF91      		pop r28
927
 930 0920 DF91      		pop r29
928
 931 0922 1F91      		pop r17
929
 932 0924 0F91      		pop r16
930
 933 0926 FF90      		pop r15
931
 934 0928 EF90      		pop r14
932
 935 092a DF90      		pop r13
933
 936 092c CF90      		pop r12
934
 937 092e BF90      		pop r11
935
 938 0930 AF90      		pop r10
936
 939 0932 9F90      		pop r9
937
 940 0934 8F90      		pop r8
938
 941 0936 7F90      		pop r7
939
 942 0938 6F90      		pop r6
940
 943 093a 5F90      		pop r5
941
 944 093c 4F90      		pop r4
942
 945 093e 3F90      		pop r3
943
 946 0940 2F90      		pop r2
944
 947 0942 0895      		ret
945
 948               	.L72:
946
 949 0944 8091 0000 		lds r24,AdValueGyroRoll
947
 950 0948 9091 0000 		lds r25,(AdValueGyroRoll)+1
948
 951 094c 835F      		subi r24,lo8(1011)
949
 952 094e 9340      		sbci r25,hi8(1011)
950
 953 0950 04F0      		brlt .L23
951
 954 0952 88EE      		ldi r24,lo8(1000)
952
 955 0954 93E0      		ldi r25,hi8(1000)
953
 956 0956 9093 0000 		sts (GyroRoll)+1,r25
954
 957 095a 8093 0000 		sts GyroRoll,r24
955
 958               	.L23:
956
 959 095e 8091 0000 		lds r24,AdValueGyroRoll
957
 960 0962 9091 0000 		lds r25,(AdValueGyroRoll)+1
958
 961 0966 8A5F      		subi r24,lo8(1018)
959
 962 0968 9340      		sbci r25,hi8(1018)
960
 963 096a 04F4      		brge .+2
961
 964 096c 00C0      		rjmp .L24
962
 965               	.L66:
963
 966 096e 80ED      		ldi r24,lo8(2000)
964
 967 0970 97E0      		ldi r25,hi8(2000)
965
 968 0972 9093 0000 		sts (GyroRoll)+1,r25
966
 969 0976 8093 0000 		sts GyroRoll,r24
967
 970 097a 00C0      		rjmp .L24
968
 971               	.L69:
969
 972 097c 8091 0000 		lds r24,AdValueGyroNick
970
 973 0980 9091 0000 		lds r25,(AdValueGyroNick)+1
971
 974 0984 835F      		subi r24,lo8(1011)
972
 975 0986 9340      		sbci r25,hi8(1011)
973
 976 0988 04F0      		brlt .L15
974
 977 098a 88EE      		ldi r24,lo8(1000)
975
 978 098c 93E0      		ldi r25,hi8(1000)
976
 979 098e 9093 0000 		sts (GyroNick)+1,r25
977
 980 0992 8093 0000 		sts GyroNick,r24
978
 981               	.L15:
979
 982 0996 8091 0000 		lds r24,AdValueGyroNick
980
 983 099a 9091 0000 		lds r25,(AdValueGyroNick)+1
981
 984 099e 8A5F      		subi r24,lo8(1018)
982
 985 09a0 9340      		sbci r25,hi8(1018)
983
 986 09a2 04F4      		brge .+2
984
 987 09a4 00C0      		rjmp .L16
985
 988               	.L65:
986
 989 09a6 80ED      		ldi r24,lo8(2000)
987
 990 09a8 97E0      		ldi r25,hi8(2000)
988
 991 09aa 9093 0000 		sts (GyroNick)+1,r25
989
 992 09ae 8093 0000 		sts GyroNick,r24
990
 993 09b2 8091 0000 		lds r24,HiResGyroRoll
991
 994 09b6 9091 0000 		lds r25,(HiResGyroRoll)+1
992
 995 09ba 97FF      		sbrs r25,7
993
 996 09bc 00C0      		rjmp .L18
994
 997               	.L70:
995
 998 09be 0796      		adiw r24,7
996
 999 09c0 00C0      		rjmp .L18
997
 1000               	.L80:
998
 1001 09c2 80E8      		ldi r24,lo8(-128)
999
 1002 09c4 9FEF      		ldi r25,hi8(-128)
1000
 1003 09c6 00C0      		rjmp .L54
1001
 1004               	.L79:
1002
 1005 09c8 80E8      		ldi r24,lo8(128)
1003
 1006 09ca 90E0      		ldi r25,hi8(128)
1004
 1007 09cc BFEF      		ldi r27,hi8(-128)
1005
 1008 09ce 8038      		cpi r24,lo8(-128)
1006
 1009 09d0 9B07      		cpc r25,r27
1007
 1010 09d2 04F0      		brlt .+2
1008
 1011 09d4 00C0      		rjmp .L54
1009
 1012 09d6 00C0      		rjmp .L80
1010
 1013               	.L76:
1011
 1014 09d8 80E8      		ldi r24,lo8(-128)
1012
 1015 09da 9FEF      		ldi r25,hi8(-128)
1013
 1016 09dc 00C0      		rjmp .L50
1014
 1017               	.L75:
1015
 1018 09de 80E8      		ldi r24,lo8(128)
1016
 1019 09e0 90E0      		ldi r25,hi8(128)
1017
 1020 09e2 E62F      		mov r30,r22
1018
 1021 09e4 F0E0      		ldi r31,lo8(0)
1019
 1022 09e6 AFEF      		ldi r26,hi8(-128)
1020
 1023 09e8 8038      		cpi r24,lo8(-128)
1021
 1024 09ea 9A07      		cpc r25,r26
1022
 1025 09ec 04F0      		brlt .+2
1023
 1026 09ee 00C0      		rjmp .L50
1024
 1027 09f0 00C0      		rjmp .L76
1025
 1028               	.L27:
1026
 1029 09f2 F091 0000 		lds r31,ParamSet+8
1027
 1030 09f6 F98B      		std Y+17,r31
1028
 1031 09f8 F6FF      		sbrs r31,6
1029
 1032 09fa 00C0      		rjmp .L28
1030
 1033 09fc 2E89      		ldd r18,Y+22
1031
 1034 09fe 3F89      		ldd r19,Y+23
1032
 1035 0a00 488D      		ldd r20,Y+24
1033
 1036 0a02 598D      		ldd r21,Y+25
1034
 1037 0a04 2934      		cpi r18,lo8(93001)
1035
 1038 0a06 6BE6      		ldi r22,hi8(93001)
1036
 1039 0a08 3607      		cpc r19,r22
1037
 1040 0a0a 61E0      		ldi r22,hlo8(93001)
1038
 1041 0a0c 4607      		cpc r20,r22
1039
 1042 0a0e 60E0      		ldi r22,hhi8(93001)
1040
 1043 0a10 5607      		cpc r21,r22
1041
 1044 0a12 04F0      		brlt .L29
1042
 1045 0a14 28E4      		ldi r18,lo8(93000)
1043
 1046 0a16 3BE6      		ldi r19,hi8(93000)
1044
 1047 0a18 41E0      		ldi r20,hlo8(93000)
1045
 1048 0a1a 50E0      		ldi r21,hhi8(93000)
1046
 1049               	.L29:
1047
 1050 0a1c 2D83      		std Y+5,r18
1048
 1051 0a1e 3E83      		std Y+6,r19
1049
 1052 0a20 4F83      		std Y+7,r20
1050
 1053 0a22 5887      		std Y+8,r21
1051
 1054 0a24 285B      		subi r18,lo8(-93000)
1052
 1055 0a26 3449      		sbci r19,hi8(-93000)
1053
 1056 0a28 4E4F      		sbci r20,hlo8(-93000)
1054
 1057 0a2a 5F4F      		sbci r21,hhi8(-93000)
1055
 1058 0a2c 04F4      		brge .L30
1056
 1059 0a2e 88EB      		ldi r24,lo8(-93000)
1057
 1060 0a30 94E9      		ldi r25,hi8(-93000)
1058
 1061 0a32 AEEF      		ldi r26,hlo8(-93000)
1059
 1062 0a34 BFEF      		ldi r27,hhi8(-93000)
1060
 1063 0a36 8D83      		std Y+5,r24
1061
 1064 0a38 9E83      		std Y+6,r25
1062
 1065 0a3a AF83      		std Y+7,r26
1063
 1066 0a3c B887      		std Y+8,r27
1064
 1067               	.L30:
1065
 1068 0a3e 2A89      		ldd r18,Y+18
1066
 1069 0a40 3B89      		ldd r19,Y+19
1067
 1070 0a42 4C89      		ldd r20,Y+20
1068
 1071 0a44 5D89      		ldd r21,Y+21
1069
 1072 0a46 2934      		cpi r18,lo8(93001)
1070
 1073 0a48 9BE6      		ldi r25,hi8(93001)
1071
 1074 0a4a 3907      		cpc r19,r25
1072
 1075 0a4c 91E0      		ldi r25,hlo8(93001)
1073
 1076 0a4e 4907      		cpc r20,r25
1074
 1077 0a50 90E0      		ldi r25,hhi8(93001)
1075
 1078 0a52 5907      		cpc r21,r25
1076
 1079 0a54 04F0      		brlt .L31
1077
 1080 0a56 28E4      		ldi r18,lo8(93000)
1078
 1081 0a58 3BE6      		ldi r19,hi8(93000)
1079
 1082 0a5a 41E0      		ldi r20,hlo8(93000)
1080
 1083 0a5c 50E0      		ldi r21,hhi8(93000)
1081
 1084               	.L31:
1082
 1085 0a5e 2983      		std Y+1,r18
1083
 1086 0a60 3A83      		std Y+2,r19
1084
 1087 0a62 4B83      		std Y+3,r20
1085
 1088 0a64 5C83      		std Y+4,r21
1086
 1089 0a66 285B      		subi r18,lo8(-93000)
1087
 1090 0a68 3449      		sbci r19,hi8(-93000)
1088
 1091 0a6a 4E4F      		sbci r20,hlo8(-93000)
1089
 1092 0a6c 5F4F      		sbci r21,hhi8(-93000)
1090
 1093 0a6e 04F4      		brge .L32
1091
 1094 0a70 88EB      		ldi r24,lo8(-93000)
1092
 1095 0a72 94E9      		ldi r25,hi8(-93000)
1093
 1096 0a74 AEEF      		ldi r26,hlo8(-93000)
1094
 1097 0a76 BFEF      		ldi r27,hhi8(-93000)
1095
 1098 0a78 8983      		std Y+1,r24
1096
 1099 0a7a 9A83      		std Y+2,r25
1097
 1100 0a7c AB83      		std Y+3,r26
1098
 1101 0a7e BC83      		std Y+4,r27
1099
 1102               	.L32:
1100
 1103 0a80 AA8D      		ldd r26,Y+26
1101
 1104 0a82 BB8D      		ldd r27,Y+27
1102
 1105 0a84 9D01      		movw r18,r26
1103
 1106 0a86 4427      		clr r20
1104
 1107 0a88 37FD      		sbrc r19,7
1105
 1108 0a8a 4095      		com r20
1106
 1109 0a8c 542F      		mov r21,r20
1107
 1110 0a8e 6D81      		ldd r22,Y+5
1108
 1111 0a90 7E81      		ldd r23,Y+6
1109
 1112 0a92 8F81      		ldd r24,Y+7
1110
 1113 0a94 9885      		ldd r25,Y+8
1111
 1114 0a96 0E94 0000 		call __mulsi3
1112
 1115 0a9a 20E0      		ldi r18,lo8(2048)
1113
 1116 0a9c 38E0      		ldi r19,hi8(2048)
1114
 1117 0a9e 40E0      		ldi r20,hlo8(2048)
1115
 1118 0aa0 50E0      		ldi r21,hhi8(2048)
1116
 1119 0aa2 0E94 0000 		call __divmodsi4
1117
 1120 0aa6 CA01      		movw r24,r20
1118
 1121 0aa8 A090 0000 		lds r10,FCParam+25
1119
 1122 0aac BB24      		clr r11
1120
 1123 0aae CC24      		clr r12
1121
 1124 0ab0 DD24      		clr r13
1122
 1125 0ab2 B901      		movw r22,r18
1123
 1126 0ab4 A601      		movw r20,r12
1124
 1127 0ab6 9501      		movw r18,r10
1125
 1128 0ab8 0E94 0000 		call __mulsi3
1126
 1129 0abc 20E0      		ldi r18,lo8(4096)
1127
 1130 0abe 30E1      		ldi r19,hi8(4096)
1128
 1131 0ac0 40E0      		ldi r20,hlo8(4096)
1129
 1132 0ac2 50E0      		ldi r21,hhi8(4096)
1130
 1133 0ac4 0E94 0000 		call __divmodsi4
1131
 1134 0ac8 7901      		movw r14,r18
1132
 1135 0aca 8A01      		movw r16,r20
1133
 1136 0acc 2AA7      		std Y+42,r18
1134
 1137 0ace FBA6      		std Y+43,r15
1135
 1138 0ad0 EC8D      		ldd r30,Y+28
1136
 1139 0ad2 FD8D      		ldd r31,Y+29
1137
 1140 0ad4 9F01      		movw r18,r30
1138
 1141 0ad6 4427      		clr r20
1139
 1142 0ad8 37FD      		sbrc r19,7
1140
 1143 0ada 4095      		com r20
1141
 1144 0adc 542F      		mov r21,r20
1142
 1145 0ade 6981      		ldd r22,Y+1
1143
 1146 0ae0 7A81      		ldd r23,Y+2
1144
 1147 0ae2 8B81      		ldd r24,Y+3
1145
 1148 0ae4 9C81      		ldd r25,Y+4
1146
 1149 0ae6 0E94 0000 		call __mulsi3
1147
 1150 0aea 20E0      		ldi r18,lo8(2048)
1148
 1151 0aec 38E0      		ldi r19,hi8(2048)
1149
 1152 0aee 40E0      		ldi r20,hlo8(2048)
1150
 1153 0af0 50E0      		ldi r21,hhi8(2048)
1151
 1154 0af2 0E94 0000 		call __divmodsi4
1152
 1155 0af6 CA01      		movw r24,r20
1153
 1156 0af8 B901      		movw r22,r18
1154
 1157 0afa A601      		movw r20,r12
1155
 1158 0afc 9501      		movw r18,r10
1156
 1159 0afe 0E94 0000 		call __mulsi3
1157
 1160 0b02 20E0      		ldi r18,lo8(4096)
1158
 1161 0b04 30E1      		ldi r19,hi8(4096)
1159
 1162 0b06 40E0      		ldi r20,hlo8(4096)
1160
 1163 0b08 50E0      		ldi r21,hhi8(4096)
1161
 1164 0b0a 0E94 0000 		call __divmodsi4
1162
 1165 0b0e C901      		movw r24,r18
1163
 1166 0b10 DA01      		movw r26,r20
1164
 1167 0b12 28A7      		std Y+40,r18
1165
 1168 0b14 99A7      		std Y+41,r25
1166
 1169 0b16 5C01      		movw r10,r24
1167
 1170 0b18 6D01      		movw r12,r26
1168
 1171 0b1a AE18      		sub r10,r14
1169
 1172 0b1c BF08      		sbc r11,r15
1170
 1173 0b1e C00A      		sbc r12,r16
1171
 1174 0b20 D10A      		sbc r13,r17
1172
 1175 0b22 8CA1      		ldd r24,Y+36
1173
 1176 0b24 9DA1      		ldd r25,Y+37
1174
 1177 0b26 AEA1      		ldd r26,Y+38
1175
 1178 0b28 BFA1      		ldd r27,Y+39
1176
 1179 0b2a 8A0D      		add r24,r10
1177
 1180 0b2c 9B1D      		adc r25,r11
1178
 1181 0b2e AC1D      		adc r26,r12
1179
 1182 0b30 BD1D      		adc r27,r13
1180
 1183 0b32 8093 0000 		sts YawGyroHeading,r24
1181
 1184 0b36 9093 0000 		sts (YawGyroHeading)+1,r25
1182
 1185 0b3a A093 0000 		sts (YawGyroHeading)+2,r26
1183
 1186 0b3e B093 0000 		sts (YawGyroHeading)+3,r27
1184
 1187 0b42 9091 0000 		lds r25,FCParam+26
1185
 1188 0b46 9E8F      		std Y+30,r25
1186
 1189 0b48 9923      		tst r25
1187
 1190 0b4a 01F4      		brne .L33
1188
 1191 0b4c C601      		movw r24,r12
1189
 1192 0b4e B501      		movw r22,r10
1190
 1193 0b50 22E0      		ldi r18,lo8(2)
1191
 1194 0b52 30E0      		ldi r19,hi8(2)
1192
 1195 0b54 40E0      		ldi r20,hlo8(2)
1193
 1196 0b56 50E0      		ldi r21,hhi8(2)
1194
 1197 0b58 0E94 0000 		call __divmodsi4
1195
 1198 0b5c 88A1      		ldd r24,Y+32
1196
 1199 0b5e 99A1      		ldd r25,Y+33
1197
 1200 0b60 AAA1      		ldd r26,Y+34
1198
 1201 0b62 BBA1      		ldd r27,Y+35
1199
 1202 0b64 821B      		sub r24,r18
1200
 1203 0b66 930B      		sbc r25,r19
1201
 1204 0b68 A40B      		sbc r26,r20
1202
 1205 0b6a B50B      		sbc r27,r21
1203
 1206 0b6c 8093 0000 		sts ReadingIntegralGyroYaw,r24
1204
 1207 0b70 9093 0000 		sts (ReadingIntegralGyroYaw)+1,r25
1205
 1208 0b74 A093 0000 		sts (ReadingIntegralGyroYaw)+2,r26
1206
 1209 0b78 B093 0000 		sts (ReadingIntegralGyroYaw)+3,r27
1207
 1210 0b7c 1C01      		movw r2,r24
1208
 1211 0b7e 2D01      		movw r4,r26
1209
 1212               	.L33:
1210
 1213 0b80 A60C      		add r10,r6
1211
 1214 0b82 B71C      		adc r11,r7
1212
 1215 0b84 C81C      		adc r12,r8
1213
 1216 0b86 D91C      		adc r13,r9
1214
 1217 0b88 C601      		movw r24,r12
1215
 1218 0b8a B501      		movw r22,r10
1216
 1219 0b8c 2D81      		ldd r18,Y+5
1217
 1220 0b8e 3E81      		ldd r19,Y+6
1218
 1221 0b90 4F81      		ldd r20,Y+7
1219
 1222 0b92 5885      		ldd r21,Y+8
1220
 1223 0b94 0E94 0000 		call __mulsi3
1221
 1224 0b98 20E0      		ldi r18,lo8(2048)
1222
 1225 0b9a 38E0      		ldi r19,hi8(2048)
1223
 1226 0b9c 40E0      		ldi r20,hlo8(2048)
1224
 1227 0b9e 50E0      		ldi r21,hhi8(2048)
1225
 1228 0ba0 0E94 0000 		call __divmodsi4
1226
 1229 0ba4 CA01      		movw r24,r20
1227
 1230 0ba6 E090 0000 		lds r14,FCParam+24
1228
 1231 0baa FF24      		clr r15
1229
 1232 0bac 00E0      		ldi r16,lo8(0)
1230
 1233 0bae 10E0      		ldi r17,hi8(0)
1231
 1234 0bb0 B901      		movw r22,r18
1232
 1235 0bb2 A801      		movw r20,r16
1233
 1236 0bb4 9701      		movw r18,r14
1234
 1237 0bb6 0E94 0000 		call __mulsi3
1235
 1238 0bba 20E0      		ldi r18,lo8(4096)
1236
 1239 0bbc 30E1      		ldi r19,hi8(4096)
1237
 1240 0bbe 40E0      		ldi r20,hlo8(4096)
1238
 1241 0bc0 50E0      		ldi r21,hhi8(4096)
1239
 1242 0bc2 0E94 0000 		call __divmodsi4
1240
 1243 0bc6 3901      		movw r6,r18
1241
 1244 0bc8 4A01      		movw r8,r20
1242
 1245 0bca C601      		movw r24,r12
1243
 1246 0bcc B501      		movw r22,r10
1244
 1247 0bce 2981      		ldd r18,Y+1
1245
 1248 0bd0 3A81      		ldd r19,Y+2
1246
 1249 0bd2 4B81      		ldd r20,Y+3
1247
 1250 0bd4 5C81      		ldd r21,Y+4
1248
 1251 0bd6 0E94 0000 		call __mulsi3
1249
 1252 0bda 20E0      		ldi r18,lo8(2048)
1250
 1253 0bdc 38E0      		ldi r19,hi8(2048)
1251
 1254 0bde 40E0      		ldi r20,hlo8(2048)
1252
 1255 0be0 50E0      		ldi r21,hhi8(2048)
1253
 1256 0be2 0E94 0000 		call __divmodsi4
1254
 1257 0be6 CA01      		movw r24,r20
1255
 1258 0be8 B901      		movw r22,r18
1256
 1259 0bea A801      		movw r20,r16
1257
 1260 0bec 9701      		movw r18,r14
1258
 1261 0bee 0E94 0000 		call __mulsi3
1259
 1262 0bf2 20E0      		ldi r18,lo8(4096)
1260
 1263 0bf4 30E1      		ldi r19,hi8(4096)
1261
 1264 0bf6 40E0      		ldi r20,hlo8(4096)
1262
 1265 0bf8 50E0      		ldi r21,hhi8(4096)
1263
 1266 0bfa 0E94 0000 		call __divmodsi4
1264
 1267 0bfe 7901      		movw r14,r18
1265
 1268 0c00 8A01      		movw r16,r20
1266
 1269 0c02 ACA5      		ldd r26,Y+44
1267
 1270 0c04 BDA5      		ldd r27,Y+45
1268
 1271 0c06 A134      		cpi r26,65
1269
 1272 0c08 B105      		cpc r27,__zero_reg__
1270
 1273 0c0a 04F0      		brlt .L34
1271
 1274 0c0c D401      		movw r26,r8
1272
 1275 0c0e C301      		movw r24,r6
1273
 1276 0c10 97FC      		sbrc r9,7
1274
 1277 0c12 00C0      		rjmp .L83
1275
 1278               	.L36:
1276
 1279 0c14 8138      		cpi r24,lo8(129)
1277
 1280 0c16 9105      		cpc r25,__zero_reg__
1278
 1281 0c18 A105      		cpc r26,__zero_reg__
1279
 1282 0c1a B105      		cpc r27,__zero_reg__
1280
 1283 0c1c 04F0      		brlt .+2
1281
 1284 0c1e 00C0      		rjmp .L35
1282
 1285 0c20 D801      		movw r26,r16
1283
 1286 0c22 C701      		movw r24,r14
1284
 1287 0c24 17FD      		sbrc r17,7
1285
 1288 0c26 00C0      		rjmp .L84
1286
 1289               	.L37:
1287
 1290 0c28 8138      		cpi r24,lo8(129)
1288
 1291 0c2a 9105      		cpc r25,__zero_reg__
1289
 1292 0c2c A105      		cpc r26,__zero_reg__
1290
 1293 0c2e B105      		cpc r27,__zero_reg__
1291
 1294 0c30 04F0      		brlt .+2
1292
 1295 0c32 00C0      		rjmp .L35
1293
 1296               	.L34:
1294
 1297 0c34 C401      		movw r24,r8
1295
 1298 0c36 B301      		movw r22,r6
1296
 1299 0c38 24E6      		ldi r18,lo8(100)
1297
 1300 0c3a 30E0      		ldi r19,hi8(100)
1298
 1301 0c3c 40E0      		ldi r20,hlo8(100)
1299
 1302 0c3e 50E0      		ldi r21,hhi8(100)
1300
 1303 0c40 0E94 0000 		call __divmodsi4
1301
 1304 0c44 2E19      		sub r18,r14
1302
 1305 0c46 3F09      		sbc r19,r15
1303
 1306 0c48 3093 0000 		sts (TrimNick)+1,r19
1304
 1307 0c4c 2093 0000 		sts TrimNick,r18
1305
 1308 0c50 C801      		movw r24,r16
1306
 1309 0c52 B701      		movw r22,r14
1307
 1310 0c54 2CE9      		ldi r18,lo8(-100)
1308
 1311 0c56 3FEF      		ldi r19,hi8(-100)
1309
 1312 0c58 4FEF      		ldi r20,hlo8(-100)
1310
 1313 0c5a 5FEF      		ldi r21,hhi8(-100)
1311
 1314 0c5c 0E94 0000 		call __divmodsi4
1312
 1315 0c60 260D      		add r18,r6
1313
 1316 0c62 371D      		adc r19,r7
1314
 1317 0c64 3093 0000 		sts (TrimRoll)+1,r19
1315
 1318 0c68 2093 0000 		sts TrimRoll,r18
1316
 1319 0c6c BE8D      		ldd r27,Y+30
1317
 1320 0c6e EB2E      		mov r14,r27
1318
 1321 0c70 FF24      		clr r15
1319
 1322 0c72 00E0      		ldi r16,lo8(0)
1320
 1323 0c74 10E0      		ldi r17,hi8(0)
1321
 1324 0c76 28A5      		ldd r18,Y+40
1322
 1325 0c78 39A5      		ldd r19,Y+41
1323
 1326 0c7a C901      		movw r24,r18
1324
 1327 0c7c 9C01      		movw r18,r24
1325
 1328 0c7e 97FD      		sbrc r25,7
1326
 1329 0c80 00C0      		rjmp .L85
1327
 1330               	.L38:
1328
 1331 0c82 B901      		movw r22,r18
1329
 1332 0c84 8827      		clr r24
1330
 1333 0c86 77FD      		sbrc r23,7
1331
 1334 0c88 8095      		com r24
1332
 1335 0c8a 982F      		mov r25,r24
1333
 1336 0c8c A801      		movw r20,r16
1334
 1337 0c8e 9701      		movw r18,r14
1335
 1338 0c90 0E94 0000 		call __mulsi3
1336
 1339 0c94 6D87      		std Y+13,r22
1337
 1340 0c96 7E87      		std Y+14,r23
1338
 1341 0c98 8F87      		std Y+15,r24
1339
 1342 0c9a 988B      		std Y+16,r25
1340
 1343 0c9c 2AA5      		ldd r18,Y+42
1341
 1344 0c9e 3BA5      		ldd r19,Y+43
1342
 1345 0ca0 C901      		movw r24,r18
1343
 1346 0ca2 9C01      		movw r18,r24
1344
 1347 0ca4 97FD      		sbrc r25,7
1345
 1348 0ca6 00C0      		rjmp .L86
1346
 1349               	.L39:
1347
 1350 0ca8 B901      		movw r22,r18
1348
 1351 0caa 8827      		clr r24
1349
 1352 0cac 77FD      		sbrc r23,7
1350
 1353 0cae 8095      		com r24
1351
 1354 0cb0 982F      		mov r25,r24
1352
 1355 0cb2 A801      		movw r20,r16
1353
 1356 0cb4 9701      		movw r18,r14
1354
 1357 0cb6 0E94 0000 		call __mulsi3
1355
 1358 0cba 6987      		std Y+9,r22
1356
 1359 0cbc 7A87      		std Y+10,r23
1357
 1360 0cbe 8B87      		std Y+11,r24
1358
 1361 0cc0 9C87      		std Y+12,r25
1359
 1362 0cc2 E090 0000 		lds r14,YawGyroHeading
1360
 1363 0cc6 F090 0000 		lds r15,(YawGyroHeading)+1
1361
 1364 0cca 0091 0000 		lds r16,(YawGyroHeading)+2
1362
 1365 0cce 1091 0000 		lds r17,(YawGyroHeading)+3
1363
 1366 0cd2 00C0      		rjmp .L40
1364
 1367               	.L82:
1365
 1368 0cd4 6F8D      		ldd r22,Y+31
1366
 1369 0cd6 6623      		tst r22
1367
 1370 0cd8 01F0      		breq .+2
1368
 1371 0cda 00C0      		rjmp .L64
1369
 1372 0cdc 8091 0000 		lds r24,LoopingRoll
1370
 1373 0ce0 8823      		tst r24
1371
 1374 0ce2 01F0      		breq .+2
1372
 1375 0ce4 00C0      		rjmp .L64
1373
 1376 0ce6 8C8D      		ldd r24,Y+28
1374
 1377 0ce8 9D8D      		ldd r25,Y+29
1375
 1378 0cea 8150      		subi r24,lo8(257)
1376
 1379 0cec 9140      		sbci r25,hi8(257)
1377
 1380 0cee 04F0      		brlt .+2
1378
 1381 0cf0 00C0      		rjmp .L87
1379
 1382 0cf2 2C8D      		ldd r18,Y+28
1380
 1383 0cf4 3D8D      		ldd r19,Y+29
1381
 1384 0cf6 6FEF      		ldi r22,hi8(-256)
1382
 1385 0cf8 2030      		cpi r18,lo8(-256)
1383
 1386 0cfa 3607      		cpc r19,r22
1384
 1387 0cfc 04F4      		brge .L62
1385
 1388 0cfe 8091 0000 		lds r24,GyroNick
1386
 1389 0d02 9091 0000 		lds r25,(GyroNick)+1
1387
 1390 0d06 8050      		subi r24,lo8(-(256))
1388
 1391 0d08 9F4F      		sbci r25,hi8(-(256))
1389
 1392 0d0a 820F      		add r24,r18
1390
 1393 0d0c 931F      		adc r25,r19
1391
 1394 0d0e 9093 0000 		sts (GyroNick)+1,r25
1392
 1395 0d12 8093 0000 		sts GyroNick,r24
1393
 1396               	.L62:
1394
 1397 0d16 8A8D      		ldd r24,Y+26
1395
 1398 0d18 9B8D      		ldd r25,Y+27
1396
 1399 0d1a 8150      		subi r24,lo8(257)
1397
 1400 0d1c 9140      		sbci r25,hi8(257)
1398
 1401 0d1e 04F0      		brlt .+2
1399
 1402 0d20 00C0      		rjmp .L88
1400
 1403 0d22 2A8D      		ldd r18,Y+26
1401
 1404 0d24 3B8D      		ldd r19,Y+27
1402
 1405 0d26 6FEF      		ldi r22,hi8(-256)
1403
 1406 0d28 2030      		cpi r18,lo8(-256)
1404
 1407 0d2a 3607      		cpc r19,r22
1405
 1408 0d2c 04F0      		brlt .+2
1406
 1409 0d2e 00C0      		rjmp .L64
1407
 1410 0d30 8091 0000 		lds r24,GyroRoll
1408
 1411 0d34 9091 0000 		lds r25,(GyroRoll)+1
1409
 1412 0d38 8050      		subi r24,lo8(-(256))
1410
 1413 0d3a 9F4F      		sbci r25,hi8(-(256))
1411
 1414 0d3c 820F      		add r24,r18
1412
 1415 0d3e 931F      		adc r25,r19
1413
 1416 0d40 9093 0000 		sts (GyroRoll)+1,r25
1414
 1417 0d44 8093 0000 		sts GyroRoll,r24
1415
 1418 0d48 00C0      		rjmp .L64
1416
 1419               	.L73:
1417
 1420 0d4a E60C      		add r14,r6
1418
 1421 0d4c F71C      		adc r15,r7
1419
 1422 0d4e 081D      		adc r16,r8
1420
 1423 0d50 191D      		adc r17,r9
1421
 1424 0d52 E092 0000 		sts YawGyroHeading,r14
1422
 1425 0d56 F092 0000 		sts (YawGyroHeading)+1,r15
1423
 1426 0d5a 0093 0000 		sts (YawGyroHeading)+2,r16
1424
 1427 0d5e 1093 0000 		sts (YawGyroHeading)+3,r17
1425
 1428 0d62 00C0      		rjmp .L42
1426
 1429               	.L58:
1427
 1430 0d64 6985      		ldd r22,Y+9
1428
 1431 0d66 7A85      		ldd r23,Y+10
1429
 1432 0d68 8B85      		ldd r24,Y+11
1430
 1433 0d6a 9C85      		ldd r25,Y+12
1431
 1434 0d6c 20E4      		ldi r18,lo8(64)
1432
 1435 0d6e 30E0      		ldi r19,hi8(64)
1433
 1436 0d70 40E0      		ldi r20,hlo8(64)
1434
 1437 0d72 50E0      		ldi r21,hhi8(64)
1435
 1438 0d74 0E94 0000 		call __divmodsi4
1436
 1439 0d78 5095      		com r21
1437
 1440 0d7a 4095      		com r20
1438
 1441 0d7c 3095      		com r19
1439
 1442 0d7e 2195      		neg r18
1440
 1443 0d80 3F4F      		sbci r19,lo8(-1)
1441
 1444 0d82 4F4F      		sbci r20,lo8(-1)
1442
 1445 0d84 5F4F      		sbci r21,lo8(-1)
1443
 1446 0d86 620E      		add r6,r18
1444
 1447 0d88 731E      		adc r7,r19
1445
 1448 0d8a 7092 0000 		sts (TrimRoll)+1,r7
1446
 1449 0d8e 6092 0000 		sts TrimRoll,r6
1447
 1450 0d92 00C0      		rjmp .L59
1448
 1451               	.L56:
1449
 1452 0d94 6D85      		ldd r22,Y+13
1450
 1453 0d96 7E85      		ldd r23,Y+14
1451
 1454 0d98 8F85      		ldd r24,Y+15
1452
 1455 0d9a 9889      		ldd r25,Y+16
1453
 1456 0d9c 20E4      		ldi r18,lo8(64)
1454
 1457 0d9e 30E0      		ldi r19,hi8(64)
1455
 1458 0da0 40E0      		ldi r20,hlo8(64)
1456
 1459 0da2 50E0      		ldi r21,hhi8(64)
1457
 1460 0da4 0E94 0000 		call __divmodsi4
1458
 1461 0da8 5095      		com r21
1459
 1462 0daa 4095      		com r20
1460
 1463 0dac 3095      		com r19
1461
 1464 0dae 2195      		neg r18
1462
 1465 0db0 3F4F      		sbci r19,lo8(-1)
1463
 1466 0db2 4F4F      		sbci r20,lo8(-1)
1464
 1467 0db4 5F4F      		sbci r21,lo8(-1)
1465
 1468 0db6 820E      		add r8,r18
1466
 1469 0db8 931E      		adc r9,r19
1467
 1470 0dba 9092 0000 		sts (TrimNick)+1,r9
1468
 1471 0dbe 8092 0000 		sts TrimNick,r8
1469
 1472 0dc2 00C0      		rjmp .L57
1470
 1473               	.L67:
1471
 1474 0dc4 0796      		adiw r24,7
1472
 1475 0dc6 00C0      		rjmp .L10
1473
 1476               	.L71:
1474
 1477 0dc8 295F      		subi r18,lo8(-(7))
1475
 1478 0dca 3F4F      		sbci r19,hi8(-(7))
1476
 1479 0dcc 00C0      		rjmp .L19
1477
 1480               	.L68:
1478
 1481 0dce 295F      		subi r18,lo8(-(7))
1479
 1482 0dd0 3F4F      		sbci r19,hi8(-(7))
1480
 1483 0dd2 00C0      		rjmp .L11
1481
 1484               	.L81:
1482
 1485 0dd4 215F      		subi r18,lo8(-(15))
1483
 1486 0dd6 3F4F      		sbci r19,hi8(-(15))
1484
 1487 0dd8 00C0      		rjmp .L55
1485
 1488               	.L74:
1486
 1489 0dda 2F5F      		subi r18,lo8(-(1))
1487
 1490 0ddc 3F4F      		sbci r19,hi8(-(1))
1488
 1491 0dde 00C0      		rjmp .L48
1489
 1492               	.L77:
1490
 1493 0de0 215F      		subi r18,lo8(-(15))
1491
 1494 0de2 3F4F      		sbci r19,hi8(-(15))
1492
 1495 0de4 00C0      		rjmp .L51
1493
 1496               	.L78:
1494
 1497 0de6 2F5F      		subi r18,lo8(-(1))
1495
 1498 0de8 3F4F      		sbci r19,hi8(-(1))
1496
 1499 0dea 00C0      		rjmp .L52
1497
 1500               	.L87:
1498
 1501 0dec 8091 0000 		lds r24,GyroNick
1499
 1502 0df0 9091 0000 		lds r25,(GyroNick)+1
1500
 1503 0df4 8050      		subi r24,lo8(-(-256))
1501
 1504 0df6 9140      		sbci r25,hi8(-(-256))
1502
 1505 0df8 EC8D      		ldd r30,Y+28
1503
 1506 0dfa FD8D      		ldd r31,Y+29
1504
 1507 0dfc 8E0F      		add r24,r30
1505
 1508 0dfe 9F1F      		adc r25,r31
1506
 1509 0e00 9093 0000 		sts (GyroNick)+1,r25
1507
 1510 0e04 8093 0000 		sts GyroNick,r24
1508
 1511 0e08 00C0      		rjmp .L62
1509
 1512               	.L35:
1510
 1513 0e0a 81E0      		ldi r24,lo8(1)
1511
 1514 0e0c 8093 0000 		sts FunnelCourse,r24
1512
 1515 0e10 00C0      		rjmp .L34
1513
 1516               	.L83:
1514
 1517 0e12 8827      		clr r24
1515
 1518 0e14 9927      		clr r25
1516
 1519 0e16 DC01      		movw r26,r24
1517
 1520 0e18 8619      		sub r24,r6
1518
 1521 0e1a 9709      		sbc r25,r7
1519
 1522 0e1c A809      		sbc r26,r8
1520
 1523 0e1e B909      		sbc r27,r9
1521
 1524 0e20 00C0      		rjmp .L36
1522
 1525               	.L88:
1523
 1526 0e22 8091 0000 		lds r24,GyroRoll
1524
 1527 0e26 9091 0000 		lds r25,(GyroRoll)+1
1525
 1528 0e2a 8050      		subi r24,lo8(-(-256))
1526
 1529 0e2c 9140      		sbci r25,hi8(-(-256))
1527
 1530 0e2e EA8D      		ldd r30,Y+26
1528
 1531 0e30 FB8D      		ldd r31,Y+27
1529
 1532 0e32 8E0F      		add r24,r30
1530
 1533 0e34 9F1F      		adc r25,r31
1531
 1534 0e36 9093 0000 		sts (GyroRoll)+1,r25
1532
 1535 0e3a 8093 0000 		sts GyroRoll,r24
1533
 1536 0e3e 00C0      		rjmp .L64
1534
 1537               	.L86:
1535
 1538 0e40 3095      		com r19
1536
 1539 0e42 2195      		neg r18
1537
 1540 0e44 3F4F      		sbci r19,lo8(-1)
1538
 1541 0e46 00C0      		rjmp .L39
1539
 1542               	.L85:
1540
 1543 0e48 3095      		com r19
1541
 1544 0e4a 2195      		neg r18
1542
 1545 0e4c 3F4F      		sbci r19,lo8(-1)
1543
 1546 0e4e 00C0      		rjmp .L38
1544
 1547               	.L84:
1545
 1548 0e50 8827      		clr r24
1546
 1549 0e52 9927      		clr r25
1547
 1550 0e54 DC01      		movw r26,r24
1548
 1551 0e56 8E19      		sub r24,r14
1549
 1552 0e58 9F09      		sbc r25,r15
1550
 1553 0e5a A00B      		sbc r26,r16
1551
 1554 0e5c B10B      		sbc r27,r17
1552
 1555 0e5e 00C0      		rjmp .L37
1553
 1557               	.global	ParameterMapping
1554
 1559               	ParameterMapping:
1555
 1560               	/* prologue: function */
1556
 1561               	/* frame size = 0 */
1557
 1562 0e60 8091 0000 		lds r24,RC_Quality
1558
 1563 0e64 813A      		cpi r24,lo8(-95)
1559
 1564 0e66 00F0      		brlo .L253
1560
 1565 0e68 8091 0000 		lds r24,ParamSet+11
1561
 1566 0e6c 8B3F      		cpi r24,lo8(-5)
1562
 1567 0e6e 00F4      		brsh .L261
1563
 1568 0e70 8093 0000 		sts FCParam+1,r24
1564
 1569               	.L93:
1565
 1570 0e74 8091 0000 		lds r24,ParamSet+10
1566
 1571 0e78 8B3F      		cpi r24,lo8(-5)
1567
 1572 0e7a 00F0      		brlo .L96
1568
 1573 0e7c 8B3F      		cpi r24,lo8(-5)
1569
 1574 0e7e 01F4      		brne .+2
1570
 1575 0e80 00C0      		rjmp .L262
1571
 1576 0e82 8C3F      		cpi r24,lo8(-4)
1572
 1577 0e84 01F4      		brne .+2
1573
 1578 0e86 00C0      		rjmp .L263
1574
 1579 0e88 8D3F      		cpi r24,lo8(-3)
1575
 1580 0e8a 01F4      		brne .+2
1576
 1581 0e8c 00C0      		rjmp .L264
1577
 1582 0e8e 8E3F      		cpi r24,lo8(-2)
1578
 1583 0e90 01F4      		brne .L98
1579
 1584 0e92 8091 0000 		lds r24,Poti4
1580
 1585 0e96 8093 0000 		sts FCParam,r24
1581
 1586 0e9a 00C0      		rjmp .L98
1582
 1587               	.L248:
1583
 1588 0e9c 8093 0000 		sts FCParam+28,r24
1584
 1589               	.L250:
1585
 1590 0ea0 6091 0000 		lds r22,FCParam+12
1586
 1591 0ea4 70E0      		ldi r23,lo8(0)
1587
 1592 0ea6 6F5F      		subi r22,lo8(-(1))
1588
 1593 0ea8 7F4F      		sbci r23,hi8(-(1))
1589
 1594 0eaa 8CE3      		ldi r24,lo8(10300)
1590
 1595 0eac 98E2      		ldi r25,hi8(10300)
1591
 1596 0eae 0E94 0000 		call __divmodhi4
1592
 1597 0eb2 7093 0000 		sts (Ki)+1,r23
1593
 1598 0eb6 6093 0000 		sts Ki,r22
1594
 1599               	.L253:
1595
 1600 0eba 0895      		ret
1596
 1601               	.L261:
1597
 1602 0ebc 8B3F      		cpi r24,lo8(-5)
1598
 1603 0ebe 01F4      		brne .+2
1599
 1604 0ec0 00C0      		rjmp .L265
1600
 1605 0ec2 8C3F      		cpi r24,lo8(-4)
1601
 1606 0ec4 01F4      		brne .+2
1602
 1607 0ec6 00C0      		rjmp .L266
1603
 1608 0ec8 8D3F      		cpi r24,lo8(-3)
1604
 1609 0eca 01F4      		brne .+2
1605
 1610 0ecc 00C0      		rjmp .L267
1606
 1611 0ece 8E3F      		cpi r24,lo8(-2)
1607
 1612 0ed0 01F4      		brne .L93
1608
 1613 0ed2 8091 0000 		lds r24,Poti4
1609
 1614 0ed6 8093 0000 		sts FCParam+1,r24
1610
 1615 0eda 00C0      		rjmp .L93
1611
 1616               	.L96:
1612
 1617 0edc 8093 0000 		sts FCParam,r24
1613
 1618               	.L98:
1614
 1619 0ee0 8091 0000 		lds r24,FCParam
1615
 1620 0ee4 8823      		tst r24
1616
 1621 0ee6 01F4      		brne .L101
1617
 1622 0ee8 1092 0000 		sts FCParam,__zero_reg__
1618
 1623               	.L102:
1619
 1624 0eec 8091 0000 		lds r24,ParamSet+12
1620
 1625 0ef0 8B3F      		cpi r24,lo8(-5)
1621
 1626 0ef2 00F0      		brlo .L103
1622
 1627               	.L271:
1623
 1628 0ef4 8B3F      		cpi r24,lo8(-5)
1624
 1629 0ef6 01F4      		brne .+2
1625
 1630 0ef8 00C0      		rjmp .L268
1626
 1631 0efa 8C3F      		cpi r24,lo8(-4)
1627
 1632 0efc 01F4      		brne .+2
1628
 1633 0efe 00C0      		rjmp .L269
1629
 1634 0f00 8D3F      		cpi r24,lo8(-3)
1630
 1635 0f02 01F4      		brne .+2
1631
 1636 0f04 00C0      		rjmp .L270
1632
 1637 0f06 8E3F      		cpi r24,lo8(-2)
1633
 1638 0f08 01F4      		brne .L105
1634
 1639 0f0a 8091 0000 		lds r24,Poti4
1635
 1640 0f0e 8093 0000 		sts FCParam+2,r24
1636
 1641 0f12 00C0      		rjmp .L105
1637
 1642               	.L101:
1638
 1643 0f14 8436      		cpi r24,lo8(100)
1639
 1644 0f16 00F0      		brlo .L102
1640
 1645 0f18 84E6      		ldi r24,lo8(100)
1641
 1646 0f1a 8093 0000 		sts FCParam,r24
1642
 1647 0f1e 8091 0000 		lds r24,ParamSet+12
1643
 1648 0f22 8B3F      		cpi r24,lo8(-5)
1644
 1649 0f24 00F4      		brsh .L271
1645
 1650               	.L103:
1646
 1651 0f26 8093 0000 		sts FCParam+2,r24
1647
 1652               	.L105:
1648
 1653 0f2a 8091 0000 		lds r24,FCParam+2
1649
 1654 0f2e 8823      		tst r24
1650
 1655 0f30 01F4      		brne .L108
1651
 1656 0f32 1092 0000 		sts FCParam+2,__zero_reg__
1652
 1657               	.L109:
1653
 1658 0f36 8091 0000 		lds r24,ParamSet+14
1654
 1659 0f3a 8B3F      		cpi r24,lo8(-5)
1655
 1660 0f3c 00F0      		brlo .L110
1656
 1661               	.L275:
1657
 1662 0f3e 8B3F      		cpi r24,lo8(-5)
1658
 1663 0f40 01F4      		brne .+2
1659
 1664 0f42 00C0      		rjmp .L272
1660
 1665 0f44 8C3F      		cpi r24,lo8(-4)
1661
 1666 0f46 01F4      		brne .+2
1662
 1667 0f48 00C0      		rjmp .L273
1663
 1668 0f4a 8D3F      		cpi r24,lo8(-3)
1664
 1669 0f4c 01F4      		brne .+2
1665
 1670 0f4e 00C0      		rjmp .L274
1666
 1671 0f50 8E3F      		cpi r24,lo8(-2)
1667
 1672 0f52 01F4      		brne .L112
1668
 1673 0f54 8091 0000 		lds r24,Poti4
1669
 1674 0f58 8093 0000 		sts FCParam+3,r24
1670
 1675 0f5c 00C0      		rjmp .L112
1671
 1676               	.L108:
1672
 1677 0f5e 8436      		cpi r24,lo8(100)
1673
 1678 0f60 00F0      		brlo .L109
1674
 1679 0f62 84E6      		ldi r24,lo8(100)
1675
 1680 0f64 8093 0000 		sts FCParam+2,r24
1676
 1681 0f68 8091 0000 		lds r24,ParamSet+14
1677
 1682 0f6c 8B3F      		cpi r24,lo8(-5)
1678
 1683 0f6e 00F4      		brsh .L275
1679
 1684               	.L110:
1680
 1685 0f70 8093 0000 		sts FCParam+3,r24
1681
 1686               	.L112:
1682
 1687 0f74 8091 0000 		lds r24,ParamSet+16
1683
 1688 0f78 8B3F      		cpi r24,lo8(-5)
1684
 1689 0f7a 00F0      		brlo .L115
1685
 1690 0f7c 8B3F      		cpi r24,lo8(-5)
1686
 1691 0f7e 01F4      		brne .+2
1687
 1692 0f80 00C0      		rjmp .L276
1688
 1693 0f82 8C3F      		cpi r24,lo8(-4)
1689
 1694 0f84 01F4      		brne .+2
1690
 1695 0f86 00C0      		rjmp .L277
1691
 1696 0f88 8D3F      		cpi r24,lo8(-3)
1692
 1697 0f8a 01F4      		brne .+2
1693
 1698 0f8c 00C0      		rjmp .L278
1694
 1699 0f8e 8E3F      		cpi r24,lo8(-2)
1695
 1700 0f90 01F4      		brne .L117
1696
 1701 0f92 8091 0000 		lds r24,Poti4
1697
 1702 0f96 8093 0000 		sts FCParam+4,r24
1698
 1703 0f9a 00C0      		rjmp .L117
1699
 1704               	.L115:
1700
 1705 0f9c 8093 0000 		sts FCParam+4,r24
1701
 1706               	.L117:
1702
 1707 0fa0 8091 0000 		lds r24,ParamSet+24
1703
 1708 0fa4 8B3F      		cpi r24,lo8(-5)
1704
 1709 0fa6 00F0      		brlo .L120
1705
 1710 0fa8 8B3F      		cpi r24,lo8(-5)
1706
 1711 0faa 01F4      		brne .+2
1707
 1712 0fac 00C0      		rjmp .L279
1708
 1713 0fae 8C3F      		cpi r24,lo8(-4)
1709
 1714 0fb0 01F4      		brne .+2
1710
 1715 0fb2 00C0      		rjmp .L280
1711
 1716 0fb4 8D3F      		cpi r24,lo8(-3)
1712
 1717 0fb6 01F4      		brne .+2
1713
 1718 0fb8 00C0      		rjmp .L281
1714
 1719 0fba 8E3F      		cpi r24,lo8(-2)
1715
 1720 0fbc 01F4      		brne .L122
1716
 1721 0fbe 8091 0000 		lds r24,Poti4
1717
 1722 0fc2 8093 0000 		sts FCParam+5,r24
1718
 1723 0fc6 00C0      		rjmp .L122
1719
 1724               	.L120:
1720
 1725 0fc8 8093 0000 		sts FCParam+5,r24
1721
 1726               	.L122:
1722
 1727 0fcc 8091 0000 		lds r24,ParamSet+25
1723
 1728 0fd0 8B3F      		cpi r24,lo8(-5)
1724
 1729 0fd2 00F0      		brlo .L125
1725
 1730 0fd4 8B3F      		cpi r24,lo8(-5)
1726
 1731 0fd6 01F4      		brne .+2
1727
 1732 0fd8 00C0      		rjmp .L282
1728
 1733 0fda 8C3F      		cpi r24,lo8(-4)
1729
 1734 0fdc 01F4      		brne .+2
1730
 1735 0fde 00C0      		rjmp .L283
1731
 1736 0fe0 8D3F      		cpi r24,lo8(-3)
1732
 1737 0fe2 01F4      		brne .+2
1733
 1738 0fe4 00C0      		rjmp .L284
1734
 1739 0fe6 8E3F      		cpi r24,lo8(-2)
1735
 1740 0fe8 01F4      		brne .L127
1736
 1741 0fea 8091 0000 		lds r24,Poti4
1737
 1742 0fee 8093 0000 		sts FCParam+7,r24
1738
 1743 0ff2 00C0      		rjmp .L127
1739
 1744               	.L125:
1740
 1745 0ff4 8093 0000 		sts FCParam+7,r24
1741
 1746               	.L127:
1742
 1747 0ff8 8091 0000 		lds r24,FCParam+7
1743
 1748 0ffc 8B30      		cpi r24,lo8(11)
1744
 1749 0ffe 00F4      		brsh .L130
1745
 1750 1000 8AE0      		ldi r24,lo8(10)
1746
 1751 1002 8093 0000 		sts FCParam+7,r24
1747
 1752               	.L131:
1748
 1753 1006 8091 0000 		lds r24,ParamSet+26
1749
 1754 100a 8B3F      		cpi r24,lo8(-5)
1750
 1755 100c 00F0      		brlo .L132
1751
 1756 100e 8B3F      		cpi r24,lo8(-5)
1752
 1757 1010 01F4      		brne .+2
1753
 1758 1012 00C0      		rjmp .L285
1754
 1759 1014 8C3F      		cpi r24,lo8(-4)
1755
 1760 1016 01F4      		brne .+2
1756
 1761 1018 00C0      		rjmp .L286
1757
 1762 101a 8D3F      		cpi r24,lo8(-3)
1758
 1763 101c 01F4      		brne .+2
1759
 1764 101e 00C0      		rjmp .L287
1760
 1765 1020 8E3F      		cpi r24,lo8(-2)
1761
 1766 1022 01F4      		brne .L134
1762
 1767 1024 8091 0000 		lds r24,Poti4
1763
 1768 1028 8093 0000 		sts FCParam+8,r24
1764
 1769 102c 00C0      		rjmp .L134
1765
 1770               	.L130:
1766
 1771 102e 8F3F      		cpi r24,lo8(-1)
1767
 1772 1030 01F4      		brne .L131
1768
 1773 1032 8093 0000 		sts FCParam+7,r24
1769
 1774 1036 00C0      		rjmp .L131
1770
 1775               	.L132:
1771
 1776 1038 8093 0000 		sts FCParam+8,r24
1772
 1777               	.L134:
1773
 1778 103c 8091 0000 		lds r24,ParamSet+27
1774
 1779 1040 8B3F      		cpi r24,lo8(-5)
1775
 1780 1042 00F0      		brlo .L137
1776
 1781 1044 8B3F      		cpi r24,lo8(-5)
1777
 1782 1046 01F4      		brne .+2
1778
 1783 1048 00C0      		rjmp .L288
1779
 1784 104a 8C3F      		cpi r24,lo8(-4)
1780
 1785 104c 01F4      		brne .+2
1781
 1786 104e 00C0      		rjmp .L289
1782
 1787 1050 8D3F      		cpi r24,lo8(-3)
1783
 1788 1052 01F4      		brne .+2
1784
 1789 1054 00C0      		rjmp .L290
1785
 1790 1056 8E3F      		cpi r24,lo8(-2)
1786
 1791 1058 01F4      		brne .L139
1787
 1792 105a 8091 0000 		lds r24,Poti4
1788
 1793 105e 8093 0000 		sts FCParam+6,r24
1789
 1794 1062 00C0      		rjmp .L139
1790
 1795               	.L137:
1791
 1796 1064 8093 0000 		sts FCParam+6,r24
1792
 1797               	.L139:
1793
 1798 1068 8091 0000 		lds r24,ParamSet+28
1794
 1799 106c 8B3F      		cpi r24,lo8(-5)
1795
 1800 106e 00F0      		brlo .L142
1796
 1801 1070 8B3F      		cpi r24,lo8(-5)
1797
 1802 1072 01F4      		brne .+2
1798
 1803 1074 00C0      		rjmp .L291
1799
 1804 1076 8C3F      		cpi r24,lo8(-4)
1800
 1805 1078 01F4      		brne .+2
1801
 1806 107a 00C0      		rjmp .L292
1802
 1807 107c 8D3F      		cpi r24,lo8(-3)
1803
 1808 107e 01F4      		brne .+2
1804
 1809 1080 00C0      		rjmp .L293
1805
 1810 1082 8E3F      		cpi r24,lo8(-2)
1806
 1811 1084 01F4      		brne .L144
1807
 1812 1086 8091 0000 		lds r24,Poti4
1808
 1813 108a 8093 0000 		sts FCParam+9,r24
1809
 1814 108e 00C0      		rjmp .L144
1810
 1815               	.L142:
1811
 1816 1090 8093 0000 		sts FCParam+9,r24
1812
 1817               	.L144:
1813
 1818 1094 8091 0000 		lds r24,FCParam+9
1814
 1819 1098 8B30      		cpi r24,lo8(11)
1815
 1820 109a 00F4      		brsh .L147
1816
 1821 109c 8AE0      		ldi r24,lo8(10)
1817
 1822 109e 8093 0000 		sts FCParam+9,r24
1818
 1823               	.L148:
1819
 1824 10a2 8091 0000 		lds r24,ParamSet+29
1820
 1825 10a6 8B3F      		cpi r24,lo8(-5)
1821
 1826 10a8 00F0      		brlo .L149
1822
 1827 10aa 8B3F      		cpi r24,lo8(-5)
1823
 1828 10ac 01F4      		brne .+2
1824
 1829 10ae 00C0      		rjmp .L294
1825
 1830 10b0 8C3F      		cpi r24,lo8(-4)
1826
 1831 10b2 01F4      		brne .+2
1827
 1832 10b4 00C0      		rjmp .L295
1828
 1833 10b6 8D3F      		cpi r24,lo8(-3)
1829
 1834 10b8 01F4      		brne .+2
1830
 1835 10ba 00C0      		rjmp .L296
1831
 1836 10bc 8E3F      		cpi r24,lo8(-2)
1832
 1837 10be 01F4      		brne .L151
1833
 1838 10c0 8091 0000 		lds r24,Poti4
1834
 1839 10c4 8093 0000 		sts FCParam+10,r24
1835
 1840 10c8 00C0      		rjmp .L151
1836
 1841               	.L147:
1837
 1842 10ca 8F3F      		cpi r24,lo8(-1)
1838
 1843 10cc 01F4      		brne .L148
1839
 1844 10ce 8093 0000 		sts FCParam+9,r24
1840
 1845 10d2 00C0      		rjmp .L148
1841
 1846               	.L149:
1842
 1847 10d4 8093 0000 		sts FCParam+10,r24
1843
 1848               	.L151:
1844
 1849 10d8 8091 0000 		lds r24,ParamSet+34
1845
 1850 10dc 8B3F      		cpi r24,lo8(-5)
1846
 1851 10de 00F0      		brlo .L154
1847
 1852 10e0 8B3F      		cpi r24,lo8(-5)
1848
 1853 10e2 01F4      		brne .+2
1849
 1854 10e4 00C0      		rjmp .L297
1850
 1855 10e6 8C3F      		cpi r24,lo8(-4)
1851
 1856 10e8 01F4      		brne .+2
1852
 1857 10ea 00C0      		rjmp .L298
1853
 1858 10ec 8D3F      		cpi r24,lo8(-3)
1854
 1859 10ee 01F4      		brne .+2
1855
 1860 10f0 00C0      		rjmp .L299
1856
 1861 10f2 8E3F      		cpi r24,lo8(-2)
1857
 1862 10f4 01F4      		brne .L156
1858
 1863 10f6 8091 0000 		lds r24,Poti4
1859
 1864 10fa 8093 0000 		sts FCParam+12,r24
1860
 1865 10fe 00C0      		rjmp .L156
1861
 1866               	.L154:
1862
 1867 1100 8093 0000 		sts FCParam+12,r24
1863
 1868               	.L156:
1864
 1869 1104 8091 0000 		lds r24,ParamSet+35
1865
 1870 1108 8B3F      		cpi r24,lo8(-5)
1866
 1871 110a 00F0      		brlo .L159
1867
 1872 110c 8B3F      		cpi r24,lo8(-5)
1868
 1873 110e 01F4      		brne .+2
1869
 1874 1110 00C0      		rjmp .L300
1870
 1875 1112 8C3F      		cpi r24,lo8(-4)
1871
 1876 1114 01F4      		brne .+2
1872
 1877 1116 00C0      		rjmp .L301
1873
 1878 1118 8D3F      		cpi r24,lo8(-3)
1874
 1879 111a 01F4      		brne .+2
1875
 1880 111c 00C0      		rjmp .L302
1876
 1881 111e 8E3F      		cpi r24,lo8(-2)
1877
 1882 1120 01F4      		brne .L161
1878
 1883 1122 8091 0000 		lds r24,Poti4
1879
 1884 1126 8093 0000 		sts FCParam+13,r24
1880
 1885 112a 00C0      		rjmp .L161
1881
 1886               	.L159:
1882
 1887 112c 8093 0000 		sts FCParam+13,r24
1883
 1888               	.L161:
1884
 1889 1130 8091 0000 		lds r24,ParamSet+36
1885
 1890 1134 8B3F      		cpi r24,lo8(-5)
1886
 1891 1136 00F0      		brlo .L164
1887
 1892 1138 8B3F      		cpi r24,lo8(-5)
1888
 1893 113a 01F4      		brne .+2
1889
 1894 113c 00C0      		rjmp .L303
1890
 1895 113e 8C3F      		cpi r24,lo8(-4)
1891
 1896 1140 01F4      		brne .+2
1892
 1897 1142 00C0      		rjmp .L304
1893
 1898 1144 8D3F      		cpi r24,lo8(-3)
1894
 1899 1146 01F4      		brne .+2
1895
 1900 1148 00C0      		rjmp .L305
1896
 1901 114a 8E3F      		cpi r24,lo8(-2)
1897
 1902 114c 01F4      		brne .L166
1898
 1903 114e 8091 0000 		lds r24,Poti4
1899
 1904 1152 8093 0000 		sts FCParam+14,r24
1900
 1905 1156 00C0      		rjmp .L166
1901
 1906               	.L164:
1902
 1907 1158 8093 0000 		sts FCParam+14,r24
1903
 1908               	.L166:
1904
 1909 115c 8091 0000 		lds r24,ParamSet+37
1905
 1910 1160 8B3F      		cpi r24,lo8(-5)
1906
 1911 1162 00F0      		brlo .L169
1907
 1912 1164 8B3F      		cpi r24,lo8(-5)
1908
 1913 1166 01F4      		brne .+2
1909
 1914 1168 00C0      		rjmp .L306
1910
 1915 116a 8C3F      		cpi r24,lo8(-4)
1911
 1916 116c 01F4      		brne .+2
1912
 1917 116e 00C0      		rjmp .L307
1913
 1918 1170 8D3F      		cpi r24,lo8(-3)
1914
 1919 1172 01F4      		brne .+2
1915
 1920 1174 00C0      		rjmp .L308
1916
 1921 1176 8E3F      		cpi r24,lo8(-2)
1917
 1922 1178 01F4      		brne .L171
1918
 1923 117a 8091 0000 		lds r24,Poti4
1919
 1924 117e 8093 0000 		sts FCParam+15,r24
1920
 1925 1182 00C0      		rjmp .L171
1921
 1926               	.L169:
1922
 1927 1184 8093 0000 		sts FCParam+15,r24
1923
 1928               	.L171:
1924
 1929 1188 8091 0000 		lds r24,ParamSet+38
1925
 1930 118c 8B3F      		cpi r24,lo8(-5)
1926
 1931 118e 00F0      		brlo .L174
1927
 1932 1190 8B3F      		cpi r24,lo8(-5)
1928
 1933 1192 01F4      		brne .+2
1929
 1934 1194 00C0      		rjmp .L309
1930
 1935 1196 8C3F      		cpi r24,lo8(-4)
1931
 1936 1198 01F4      		brne .+2
1932
 1937 119a 00C0      		rjmp .L310
1933
 1938 119c 8D3F      		cpi r24,lo8(-3)
1934
 1939 119e 01F4      		brne .+2
1935
 1940 11a0 00C0      		rjmp .L311
1936
 1941 11a2 8E3F      		cpi r24,lo8(-2)
1937
 1942 11a4 01F4      		brne .L176
1938
 1943 11a6 8091 0000 		lds r24,Poti4
1939
 1944 11aa 8093 0000 		sts FCParam+16,r24
1940
 1945 11ae 00C0      		rjmp .L176
1941
 1946               	.L174:
1942
 1947 11b0 8093 0000 		sts FCParam+16,r24
1943
 1948               	.L176:
1944
 1949 11b4 8091 0000 		lds r24,ParamSet+59
1945
 1950 11b8 8B3F      		cpi r24,lo8(-5)
1946
 1951 11ba 00F0      		brlo .L179
1947
 1952 11bc 8B3F      		cpi r24,lo8(-5)
1948
 1953 11be 01F4      		brne .+2
1949
 1954 11c0 00C0      		rjmp .L312
1950
 1955 11c2 8C3F      		cpi r24,lo8(-4)
1951
 1956 11c4 01F4      		brne .+2
1952
 1957 11c6 00C0      		rjmp .L313
1953
 1958 11c8 8D3F      		cpi r24,lo8(-3)
1954
 1959 11ca 01F4      		brne .+2
1955
 1960 11cc 00C0      		rjmp .L314
1956
 1961 11ce 8E3F      		cpi r24,lo8(-2)
1957
 1962 11d0 01F4      		brne .L181
1958
 1963 11d2 8091 0000 		lds r24,Poti4
1959
 1964 11d6 8093 0000 		sts FCParam+17,r24
1960
 1965 11da 00C0      		rjmp .L181
1961
 1966               	.L179:
1962
 1967 11dc 8093 0000 		sts FCParam+17,r24
1963
 1968               	.L181:
1964
 1969 11e0 8091 0000 		lds r24,ParamSet+60
1965
 1970 11e4 8B3F      		cpi r24,lo8(-5)
1966
 1971 11e6 00F0      		brlo .L184
1967
 1972 11e8 8B3F      		cpi r24,lo8(-5)
1968
 1973 11ea 01F4      		brne .+2
1969
 1974 11ec 00C0      		rjmp .L315
1970
 1975 11ee 8C3F      		cpi r24,lo8(-4)
1971
 1976 11f0 01F4      		brne .+2
1972
 1977 11f2 00C0      		rjmp .L316
1973
 1978 11f4 8D3F      		cpi r24,lo8(-3)
1974
 1979 11f6 01F4      		brne .+2
1975
 1980 11f8 00C0      		rjmp .L317
1976
 1981 11fa 8E3F      		cpi r24,lo8(-2)
1977
 1982 11fc 01F4      		brne .L186
1978
 1983 11fe 8091 0000 		lds r24,Poti4
1979
 1984 1202 8093 0000 		sts FCParam+18,r24
1980
 1985 1206 00C0      		rjmp .L186
1981
 1986               	.L184:
1982
 1987 1208 8093 0000 		sts FCParam+18,r24
1983
 1988               	.L186:
1984
 1989 120c 8091 0000 		lds r24,ParamSet+61
1985
 1990 1210 8B3F      		cpi r24,lo8(-5)
1986
 1991 1212 00F0      		brlo .L189
1987
 1992 1214 8B3F      		cpi r24,lo8(-5)
1988
 1993 1216 01F4      		brne .+2
1989
 1994 1218 00C0      		rjmp .L318
1990
 1995 121a 8C3F      		cpi r24,lo8(-4)
1991
 1996 121c 01F4      		brne .+2
1992
 1997 121e 00C0      		rjmp .L319
1993
 1998 1220 8D3F      		cpi r24,lo8(-3)
1994
 1999 1222 01F4      		brne .+2
1995
 2000 1224 00C0      		rjmp .L320
1996
 2001 1226 8E3F      		cpi r24,lo8(-2)
1997
 2002 1228 01F4      		brne .L191
1998
 2003 122a 8091 0000 		lds r24,Poti4
1999
 2004 122e 8093 0000 		sts FCParam+19,r24
2000
 2005 1232 00C0      		rjmp .L191
2001
 2006               	.L189:
2002
 2007 1234 8093 0000 		sts FCParam+19,r24
2003
 2008               	.L191:
2004
 2009 1238 8091 0000 		lds r24,ParamSet+62
2005
 2010 123c 8B3F      		cpi r24,lo8(-5)
2006
 2011 123e 00F0      		brlo .L194
2007
 2012 1240 8B3F      		cpi r24,lo8(-5)
2008
 2013 1242 01F4      		brne .+2
2009
 2014 1244 00C0      		rjmp .L321
2010
 2015 1246 8C3F      		cpi r24,lo8(-4)
2011
 2016 1248 01F4      		brne .+2
2012
 2017 124a 00C0      		rjmp .L322
2013
 2018 124c 8D3F      		cpi r24,lo8(-3)
2014
 2019 124e 01F4      		brne .+2
2015
 2020 1250 00C0      		rjmp .L323
2016
 2021 1252 8E3F      		cpi r24,lo8(-2)
2017
 2022 1254 01F4      		brne .L196
2018
 2023 1256 8091 0000 		lds r24,Poti4
2019
 2024 125a 8093 0000 		sts FCParam+20,r24
2020
 2025 125e 00C0      		rjmp .L196
2021
 2026               	.L194:
2022
 2027 1260 8093 0000 		sts FCParam+20,r24
2023
 2028               	.L196:
2024
 2029 1264 8091 0000 		lds r24,ParamSet+39
2025
 2030 1268 8B3F      		cpi r24,lo8(-5)
2026
 2031 126a 00F0      		brlo .L199
2027
 2032 126c 8B3F      		cpi r24,lo8(-5)
2028
 2033 126e 01F4      		brne .+2
2029
 2034 1270 00C0      		rjmp .L324
2030
 2035 1272 8C3F      		cpi r24,lo8(-4)
2031
 2036 1274 01F4      		brne .+2
2032
 2037 1276 00C0      		rjmp .L325
2033
 2038 1278 8D3F      		cpi r24,lo8(-3)
2034
 2039 127a 01F4      		brne .+2
2035
 2040 127c 00C0      		rjmp .L326
2036
 2041 127e 8E3F      		cpi r24,lo8(-2)
2037
 2042 1280 01F4      		brne .L201
2038
 2043 1282 8091 0000 		lds r24,Poti4
2039
 2044 1286 8093 0000 		sts FCParam+21,r24
2040
 2045 128a 00C0      		rjmp .L201
2041
 2046               	.L199:
2042
 2047 128c 8093 0000 		sts FCParam+21,r24
2043
 2048               	.L201:
2044
 2049 1290 8091 0000 		lds r24,ParamSet+43
2045
 2050 1294 8B3F      		cpi r24,lo8(-5)
2046
 2051 1296 00F0      		brlo .L204
2047
 2052 1298 8B3F      		cpi r24,lo8(-5)
2048
 2053 129a 01F4      		brne .+2
2049
 2054 129c 00C0      		rjmp .L327
2050
 2055 129e 8C3F      		cpi r24,lo8(-4)
2051
 2056 12a0 01F4      		brne .+2
2052
 2057 12a2 00C0      		rjmp .L328
2053
 2058 12a4 8D3F      		cpi r24,lo8(-3)
2054
 2059 12a6 01F4      		brne .+2
2055
 2060 12a8 00C0      		rjmp .L329
2056
 2061 12aa 8E3F      		cpi r24,lo8(-2)
2057
 2062 12ac 01F4      		brne .L206
2058
 2063 12ae 8091 0000 		lds r24,Poti4
2059
 2064 12b2 8093 0000 		sts FCParam+22,r24
2060
 2065 12b6 00C0      		rjmp .L206
2061
 2066               	.L204:
2062
 2067 12b8 8093 0000 		sts FCParam+22,r24
2063
 2068               	.L206:
2064
 2069 12bc 8091 0000 		lds r24,ParamSet+48
2065
 2070 12c0 8B3F      		cpi r24,lo8(-5)
2066
 2071 12c2 00F0      		brlo .L209
2067
 2072 12c4 8B3F      		cpi r24,lo8(-5)
2068
 2073 12c6 01F4      		brne .+2
2069
 2074 12c8 00C0      		rjmp .L330
2070
 2075 12ca 8C3F      		cpi r24,lo8(-4)
2071
 2076 12cc 01F4      		brne .+2
2072
 2077 12ce 00C0      		rjmp .L331
2073
 2078 12d0 8D3F      		cpi r24,lo8(-3)
2074
 2079 12d2 01F4      		brne .+2
2075
 2080 12d4 00C0      		rjmp .L332
2076
 2081 12d6 8E3F      		cpi r24,lo8(-2)
2077
 2082 12d8 01F4      		brne .L211
2078
 2083 12da 8091 0000 		lds r24,Poti4
2079
 2084 12de 8093 0000 		sts FCParam+23,r24
2080
 2085 12e2 00C0      		rjmp .L211
2081
 2086               	.L209:
2082
 2087 12e4 8093 0000 		sts FCParam+23,r24
2083
 2088               	.L211:
2084
 2089 12e8 8091 0000 		lds r24,ParamSet+51
2085
 2090 12ec 8B3F      		cpi r24,lo8(-5)
2086
 2091 12ee 00F0      		brlo .L214
2087
 2092 12f0 8B3F      		cpi r24,lo8(-5)
2088
 2093 12f2 01F4      		brne .+2
2089
 2094 12f4 00C0      		rjmp .L333
2090
 2095 12f6 8C3F      		cpi r24,lo8(-4)
2091
 2096 12f8 01F4      		brne .+2
2092
 2097 12fa 00C0      		rjmp .L334
2093
 2098 12fc 8D3F      		cpi r24,lo8(-3)
2094
 2099 12fe 01F4      		brne .+2
2095
 2100 1300 00C0      		rjmp .L335
2096
 2101 1302 8E3F      		cpi r24,lo8(-2)
2097
 2102 1304 01F4      		brne .L216
2098
 2103 1306 8091 0000 		lds r24,Poti4
2099
 2104 130a 8093 0000 		sts FCParam+24,r24
2100
 2105 130e 00C0      		rjmp .L216
2101
 2106               	.L214:
2102
 2107 1310 8093 0000 		sts FCParam+24,r24
2103
 2108               	.L216:
2104
 2109 1314 8091 0000 		lds r24,ParamSet+52
2105
 2110 1318 8B3F      		cpi r24,lo8(-5)
2106
 2111 131a 00F0      		brlo .L219
2107
 2112 131c 8B3F      		cpi r24,lo8(-5)
2108
 2113 131e 01F4      		brne .+2
2109
 2114 1320 00C0      		rjmp .L336
2110
 2115 1322 8C3F      		cpi r24,lo8(-4)
2111
 2116 1324 01F4      		brne .+2
2112
 2117 1326 00C0      		rjmp .L337
2113
 2118 1328 8D3F      		cpi r24,lo8(-3)
2114
 2119 132a 01F4      		brne .+2
2115
 2120 132c 00C0      		rjmp .L338
2116
 2121 132e 8E3F      		cpi r24,lo8(-2)
2117
 2122 1330 01F4      		brne .L221
2118
 2123 1332 8091 0000 		lds r24,Poti4
2119
 2124 1336 8093 0000 		sts FCParam+25,r24
2120
 2125 133a 00C0      		rjmp .L221
2121
 2126               	.L219:
2122
 2127 133c 8093 0000 		sts FCParam+25,r24
2123
 2128               	.L221:
2124
 2129 1340 8091 0000 		lds r24,ParamSet+53
2125
 2130 1344 8B3F      		cpi r24,lo8(-5)
2126
 2131 1346 00F0      		brlo .L224
2127
 2132 1348 8B3F      		cpi r24,lo8(-5)
2128
 2133 134a 01F4      		brne .+2
2129
 2134 134c 00C0      		rjmp .L339
2130
 2135 134e 8C3F      		cpi r24,lo8(-4)
2131
 2136 1350 01F4      		brne .+2
2132
 2137 1352 00C0      		rjmp .L340
2133
 2138 1354 8D3F      		cpi r24,lo8(-3)
2134
 2139 1356 01F4      		brne .+2
2135
 2140 1358 00C0      		rjmp .L341
2136
 2141 135a 8E3F      		cpi r24,lo8(-2)
2137
 2142 135c 01F4      		brne .L226
2138
 2143 135e 8091 0000 		lds r24,Poti4
2139
 2144 1362 8093 0000 		sts FCParam+26,r24
2140
 2145 1366 00C0      		rjmp .L226
2141
 2146               	.L224:
2142
 2147 1368 8093 0000 		sts FCParam+26,r24
2143
 2148               	.L226:
2144
 2149 136c 8091 0000 		lds r24,ParamSet+58
2145
 2150 1370 8B3F      		cpi r24,lo8(-5)
2146
 2151 1372 00F0      		brlo .L229
2147
 2152 1374 8B3F      		cpi r24,lo8(-5)
2148
 2153 1376 01F4      		brne .+2
2149
 2154 1378 00C0      		rjmp .L342
2150
 2155 137a 8C3F      		cpi r24,lo8(-4)
2151
 2156 137c 01F4      		brne .+2
2152
 2157 137e 00C0      		rjmp .L343
2153
 2158 1380 8D3F      		cpi r24,lo8(-3)
2154
 2159 1382 01F4      		brne .+2
2155
 2160 1384 00C0      		rjmp .L344
2156
 2161 1386 8E3F      		cpi r24,lo8(-2)
2157
 2162 1388 01F4      		brne .L231
2158
 2163 138a 8091 0000 		lds r24,Poti4
2159
 2164 138e 8093 0000 		sts FCParam+27,r24
2160
 2165 1392 00C0      		rjmp .L231
2161
 2166               	.L229:
2162
 2167 1394 8093 0000 		sts FCParam+27,r24
2163
 2168               	.L231:
2164
 2169 1398 8091 0000 		lds r24,ParamSet+64
2165
 2170 139c 8B3F      		cpi r24,lo8(-5)
2166
 2171 139e 00F0      		brlo .L234
2167
 2172 13a0 8B3F      		cpi r24,lo8(-5)
2168
 2173 13a2 01F4      		brne .+2
2169
 2174 13a4 00C0      		rjmp .L345
2170
 2175 13a6 8C3F      		cpi r24,lo8(-4)
2171
 2176 13a8 01F4      		brne .+2
2172
 2177 13aa 00C0      		rjmp .L346
2173
 2178 13ac 8D3F      		cpi r24,lo8(-3)
2174
 2179 13ae 01F4      		brne .+2
2175
 2180 13b0 00C0      		rjmp .L347
2176
 2181 13b2 8E3F      		cpi r24,lo8(-2)
2177
 2182 13b4 01F4      		brne .L236
2178
 2183 13b6 8091 0000 		lds r24,Poti4
2179
 2184 13ba 8093 0000 		sts FCParam+29,r24
2180
 2185 13be 00C0      		rjmp .L236
2181
 2186               	.L234:
2182
 2187 13c0 8093 0000 		sts FCParam+29,r24
2183
 2188               	.L236:
2184
 2189 13c4 8091 0000 		lds r24,FCParam+29
2185
 2190 13c8 8230      		cpi r24,lo8(2)
2186
 2191 13ca 00F4      		brsh .+2
2187
 2192 13cc 00C0      		rjmp .L348
2188
 2193 13ce 8F3F      		cpi r24,lo8(-1)
2189
 2194 13d0 01F4      		brne .+2
2190
 2195 13d2 00C0      		rjmp .L349
2191
 2196               	.L240:
2192
 2197 13d4 8091 0000 		lds r24,ParamSet+66
2193
 2198 13d8 8B3F      		cpi r24,lo8(-5)
2194
 2199 13da 00F0      		brlo .L241
2195
 2200 13dc 8B3F      		cpi r24,lo8(-5)
2196
 2201 13de 01F4      		brne .+2
2197
 2202 13e0 00C0      		rjmp .L350
2198
 2203 13e2 8C3F      		cpi r24,lo8(-4)
2199
 2204 13e4 01F4      		brne .+2
2200
 2205 13e6 00C0      		rjmp .L351
2201
 2206 13e8 8D3F      		cpi r24,lo8(-3)
2202
 2207 13ea 01F4      		brne .+2
2203
 2208 13ec 00C0      		rjmp .L352
2204
 2209 13ee 8E3F      		cpi r24,lo8(-2)
2205
 2210 13f0 01F4      		brne .L243
2206
 2211 13f2 8091 0000 		lds r24,Poti4
2207
 2212 13f6 8093 0000 		sts FCParam+30,r24
2208
 2213 13fa 00C0      		rjmp .L243
2209
 2214               	.L241:
2210
 2215 13fc 8093 0000 		sts FCParam+30,r24
2211
 2216               	.L243:
2212
 2217 1400 8091 0000 		lds r24,FCParam+30
2213
 2218 1404 8230      		cpi r24,lo8(2)
2214
 2219 1406 00F4      		brsh .+2
2215
 2220 1408 00C0      		rjmp .L353
2216
 2221 140a 8F3F      		cpi r24,lo8(-1)
2217
 2222 140c 01F4      		brne .+2
2218
 2223 140e 00C0      		rjmp .L354
2219
 2224               	.L247:
2220
 2225 1410 8091 0000 		lds r24,ParamSet+85
2221
 2226 1414 8B3F      		cpi r24,lo8(-5)
2222
 2227 1416 00F4      		brsh .+2
2223
 2228 1418 00C0      		rjmp .L248
2224
 2229 141a 8B3F      		cpi r24,lo8(-5)
2225
 2230 141c 01F0      		breq .L355
2226
 2231 141e 8C3F      		cpi r24,lo8(-4)
2227
 2232 1420 01F4      		brne .+2
2228
 2233 1422 00C0      		rjmp .L356
2229
 2234 1424 8D3F      		cpi r24,lo8(-3)
2230
 2235 1426 01F4      		brne .+2
2231
 2236 1428 00C0      		rjmp .L357
2232
 2237 142a 8E3F      		cpi r24,lo8(-2)
2233
 2238 142c 01F0      		breq .+2
2234
 2239 142e 00C0      		rjmp .L250
2235
 2240 1430 8091 0000 		lds r24,Poti4
2236
 2241 1434 8093 0000 		sts FCParam+28,r24
2237
 2242 1438 00C0      		rjmp .L250
2238
 2243               	.L355:
2239
 2244 143a 8091 0000 		lds r24,Poti1
2240
 2245 143e 8093 0000 		sts FCParam+28,r24
2241
 2246 1442 00C0      		rjmp .L250
2242
 2247               	.L276:
2243
 2248 1444 8091 0000 		lds r24,Poti1
2244
 2249 1448 8093 0000 		sts FCParam+4,r24
2245
 2250 144c 00C0      		rjmp .L117
2246
 2251               	.L282:
2247
 2252 144e 8091 0000 		lds r24,Poti1
2248
 2253 1452 8093 0000 		sts FCParam+7,r24
2249
 2254 1456 00C0      		rjmp .L127
2250
 2255               	.L279:
2251
 2256 1458 8091 0000 		lds r24,Poti1
2252
 2257 145c 8093 0000 		sts FCParam+5,r24
2253
 2258 1460 00C0      		rjmp .L122
2254
 2259               	.L265:
2255
 2260 1462 8091 0000 		lds r24,Poti1
2256
 2261 1466 8093 0000 		sts FCParam+1,r24
2257
 2262 146a 00C0      		rjmp .L93
2258
 2263               	.L262:
2259
 2264 146c 8091 0000 		lds r24,Poti1
2260
 2265 1470 8093 0000 		sts FCParam,r24
2261
 2266 1474 00C0      		rjmp .L98
2262
 2267               	.L268:
2263
 2268 1476 8091 0000 		lds r24,Poti1
2264
 2269 147a 8093 0000 		sts FCParam+2,r24
2265
 2270 147e 00C0      		rjmp .L105
2266
 2271               	.L272:
2267
 2272 1480 8091 0000 		lds r24,Poti1
2268
 2273 1484 8093 0000 		sts FCParam+3,r24
2269
 2274 1488 00C0      		rjmp .L112
2270
 2275               	.L288:
2271
 2276 148a 8091 0000 		lds r24,Poti1
2272
 2277 148e 8093 0000 		sts FCParam+6,r24
2273
 2278 1492 00C0      		rjmp .L139
2274
 2279               	.L285:
2275
 2280 1494 8091 0000 		lds r24,Poti1
2276
 2281 1498 8093 0000 		sts FCParam+8,r24
2277
 2282 149c 00C0      		rjmp .L134
2278
 2283               	.L294:
2279
 2284 149e 8091 0000 		lds r24,Poti1
2280
 2285 14a2 8093 0000 		sts FCParam+10,r24
2281
 2286 14a6 00C0      		rjmp .L151
2282
 2287               	.L291:
2283
 2288 14a8 8091 0000 		lds r24,Poti1
2284
 2289 14ac 8093 0000 		sts FCParam+9,r24
2285
 2290 14b0 00C0      		rjmp .L144
2286
 2291               	.L303:
2287
 2292 14b2 8091 0000 		lds r24,Poti1
2288
 2293 14b6 8093 0000 		sts FCParam+14,r24
2289
 2294 14ba 00C0      		rjmp .L166
2290
 2295               	.L300:
2291
 2296 14bc 8091 0000 		lds r24,Poti1
2292
 2297 14c0 8093 0000 		sts FCParam+13,r24
2293
 2298 14c4 00C0      		rjmp .L161
2294
 2299               	.L297:
2295
 2300 14c6 8091 0000 		lds r24,Poti1
2296
 2301 14ca 8093 0000 		sts FCParam+12,r24
2297
 2302 14ce 00C0      		rjmp .L156
2298
 2303               	.L350:
2299
 2304 14d0 8091 0000 		lds r24,Poti1
2300
 2305 14d4 8093 0000 		sts FCParam+30,r24
2301
 2306 14d8 00C0      		rjmp .L243
2302
 2307               	.L345:
2303
 2308 14da 8091 0000 		lds r24,Poti1
2304
 2309 14de 8093 0000 		sts FCParam+29,r24
2305
 2310 14e2 00C0      		rjmp .L236
2306
 2311               	.L342:
2307
 2312 14e4 8091 0000 		lds r24,Poti1
2308
 2313 14e8 8093 0000 		sts FCParam+27,r24
2309
 2314 14ec 00C0      		rjmp .L231
2310
 2315               	.L339:
2311
 2316 14ee 8091 0000 		lds r24,Poti1
2312
 2317 14f2 8093 0000 		sts FCParam+26,r24
2313
 2318 14f6 00C0      		rjmp .L226
2314
 2319               	.L336:
2315
 2320 14f8 8091 0000 		lds r24,Poti1
2316
 2321 14fc 8093 0000 		sts FCParam+25,r24
2317
 2322 1500 00C0      		rjmp .L221
2318
 2323               	.L333:
2319
 2324 1502 8091 0000 		lds r24,Poti1
2320
 2325 1506 8093 0000 		sts FCParam+24,r24
2321
 2326 150a 00C0      		rjmp .L216
2322
 2327               	.L330:
2323
 2328 150c 8091 0000 		lds r24,Poti1
2324
 2329 1510 8093 0000 		sts FCParam+23,r24
2325
 2330 1514 00C0      		rjmp .L211
2326
 2331               	.L327:
2327
 2332 1516 8091 0000 		lds r24,Poti1
2328
 2333 151a 8093 0000 		sts FCParam+22,r24
2329
 2334 151e 00C0      		rjmp .L206
2330
 2335               	.L324:
2331
 2336 1520 8091 0000 		lds r24,Poti1
2332
 2337 1524 8093 0000 		sts FCParam+21,r24
2333
 2338 1528 00C0      		rjmp .L201
2334
 2339               	.L321:
2335
 2340 152a 8091 0000 		lds r24,Poti1
2336
 2341 152e 8093 0000 		sts FCParam+20,r24
2337
 2342 1532 00C0      		rjmp .L196
2338
 2343               	.L318:
2339
 2344 1534 8091 0000 		lds r24,Poti1
2340
 2345 1538 8093 0000 		sts FCParam+19,r24
2341
 2346 153c 00C0      		rjmp .L191
2342
 2347               	.L315:
2343
 2348 153e 8091 0000 		lds r24,Poti1
2344
 2349 1542 8093 0000 		sts FCParam+18,r24
2345
 2350 1546 00C0      		rjmp .L186
2346
 2351               	.L312:
2347
 2352 1548 8091 0000 		lds r24,Poti1
2348
 2353 154c 8093 0000 		sts FCParam+17,r24
2349
 2354 1550 00C0      		rjmp .L181
2350
 2355               	.L309:
2351
 2356 1552 8091 0000 		lds r24,Poti1
2352
 2357 1556 8093 0000 		sts FCParam+16,r24
2353
 2358 155a 00C0      		rjmp .L176
2354
 2359               	.L306:
2355
 2360 155c 8091 0000 		lds r24,Poti1
2356
 2361 1560 8093 0000 		sts FCParam+15,r24
2357
 2362 1564 00C0      		rjmp .L171
2358
 2363               	.L348:
2359
 2364 1566 81E0      		ldi r24,lo8(1)
2360
 2365 1568 8093 0000 		sts FCParam+29,r24
2361
 2366 156c 00C0      		rjmp .L240
2362
 2367               	.L353:
2363
 2368 156e 81E0      		ldi r24,lo8(1)
2364
 2369 1570 8093 0000 		sts FCParam+30,r24
2365
 2370 1574 00C0      		rjmp .L247
2366
 2371               	.L349:
2367
 2372 1576 8093 0000 		sts FCParam+29,r24
2368
 2373 157a 00C0      		rjmp .L240
2369
 2374               	.L354:
2370
 2375 157c 8093 0000 		sts FCParam+30,r24
2371
 2376 1580 00C0      		rjmp .L247
2372
 2377               	.L319:
2373
 2378 1582 8091 0000 		lds r24,Poti2
2374
 2379 1586 8093 0000 		sts FCParam+19,r24
2375
 2380 158a 00C0      		rjmp .L191
2376
 2381               	.L322:
2377
 2382 158c 8091 0000 		lds r24,Poti2
2378
 2383 1590 8093 0000 		sts FCParam+20,r24
2379
 2384 1594 00C0      		rjmp .L196
2380
 2385               	.L310:
2381
 2386 1596 8091 0000 		lds r24,Poti2
2382
 2387 159a 8093 0000 		sts FCParam+16,r24
2383
 2388 159e 00C0      		rjmp .L176
2384
 2389               	.L313:
2385
 2390 15a0 8091 0000 		lds r24,Poti2
2386
 2391 15a4 8093 0000 		sts FCParam+17,r24
2387
 2392 15a8 00C0      		rjmp .L181
2388
 2393               	.L328:
2389
 2394 15aa 8091 0000 		lds r24,Poti2
2390
 2395 15ae 8093 0000 		sts FCParam+22,r24
2391
 2396 15b2 00C0      		rjmp .L206
2392
 2397               	.L325:
2393
 2398 15b4 8091 0000 		lds r24,Poti2
2394
 2399 15b8 8093 0000 		sts FCParam+21,r24
2395
 2400 15bc 00C0      		rjmp .L201
2396
 2401               	.L337:
2397
 2402 15be 8091 0000 		lds r24,Poti2
2398
 2403 15c2 8093 0000 		sts FCParam+25,r24
2399
 2404 15c6 00C0      		rjmp .L221
2400
 2405               	.L351:
2401
 2406 15c8 8091 0000 		lds r24,Poti2
2402
 2407 15cc 8093 0000 		sts FCParam+30,r24
2403
 2408 15d0 00C0      		rjmp .L243
2404
 2409               	.L343:
2405
 2410 15d2 8091 0000 		lds r24,Poti2
2406
 2411 15d6 8093 0000 		sts FCParam+27,r24
2407
 2412 15da 00C0      		rjmp .L231
2408
 2413               	.L346:
2409
 2414 15dc 8091 0000 		lds r24,Poti2
2410
 2415 15e0 8093 0000 		sts FCParam+29,r24
2411
 2416 15e4 00C0      		rjmp .L236
2412
 2417               	.L316:
2413
 2418 15e6 8091 0000 		lds r24,Poti2
2414
 2419 15ea 8093 0000 		sts FCParam+18,r24
2415
 2420 15ee 00C0      		rjmp .L186
2416
 2421               	.L307:
2417
 2422 15f0 8091 0000 		lds r24,Poti2
2418
 2423 15f4 8093 0000 		sts FCParam+15,r24
2419
 2424 15f8 00C0      		rjmp .L171
2420
 2425               	.L334:
2421
 2426 15fa 8091 0000 		lds r24,Poti2
2422
 2427 15fe 8093 0000 		sts FCParam+24,r24
2423
 2428 1602 00C0      		rjmp .L216
2424
 2429               	.L340:
2425
 2430 1604 8091 0000 		lds r24,Poti2
2426
 2431 1608 8093 0000 		sts FCParam+26,r24
2427
 2432 160c 00C0      		rjmp .L226
2428
 2433               	.L286:
2429
 2434 160e 8091 0000 		lds r24,Poti2
2430
 2435 1612 8093 0000 		sts FCParam+8,r24
2431
 2436 1616 00C0      		rjmp .L134
2432
 2437               	.L273:
2433
 2438 1618 8091 0000 		lds r24,Poti2
2434
 2439 161c 8093 0000 		sts FCParam+3,r24
2435
 2440 1620 00C0      		rjmp .L112
2436
 2441               	.L331:
2437
 2442 1622 8091 0000 		lds r24,Poti2
2438
 2443 1626 8093 0000 		sts FCParam+23,r24
2439
 2444 162a 00C0      		rjmp .L211
2440
 2445               	.L356:
2441
 2446 162c 8091 0000 		lds r24,Poti2
2442
 2447 1630 8093 0000 		sts FCParam+28,r24
2443
 2448 1634 00C0      		rjmp .L250
2444
 2449               	.L283:
2445
 2450 1636 8091 0000 		lds r24,Poti2
2446
 2451 163a 8093 0000 		sts FCParam+7,r24
2447
 2452 163e 00C0      		rjmp .L127
2448
 2453               	.L280:
2449
 2454 1640 8091 0000 		lds r24,Poti2
2450
 2455 1644 8093 0000 		sts FCParam+5,r24
2451
 2456 1648 00C0      		rjmp .L122
2452
 2457               	.L269:
2453
 2458 164a 8091 0000 		lds r24,Poti2
2454
 2459 164e 8093 0000 		sts FCParam+2,r24
2455
 2460 1652 00C0      		rjmp .L105
2456
 2461               	.L266:
2457
 2462 1654 8091 0000 		lds r24,Poti2
2458
 2463 1658 8093 0000 		sts FCParam+1,r24
2459
 2464 165c 00C0      		rjmp .L93
2460
 2465               	.L304:
2461
 2466 165e 8091 0000 		lds r24,Poti2
2462
 2467 1662 8093 0000 		sts FCParam+14,r24
2463
 2468 1666 00C0      		rjmp .L166
2464
 2469               	.L301:
2465
 2470 1668 8091 0000 		lds r24,Poti2
2466
 2471 166c 8093 0000 		sts FCParam+13,r24
2467
 2472 1670 00C0      		rjmp .L161
2468
 2473               	.L277:
2469
 2474 1672 8091 0000 		lds r24,Poti2
2470
 2475 1676 8093 0000 		sts FCParam+4,r24
2471
 2476 167a 00C0      		rjmp .L117
2472
 2477               	.L263:
2473
 2478 167c 8091 0000 		lds r24,Poti2
2474
 2479 1680 8093 0000 		sts FCParam,r24
2475
 2480 1684 00C0      		rjmp .L98
2476
 2481               	.L292:
2477
 2482 1686 8091 0000 		lds r24,Poti2
2478
 2483 168a 8093 0000 		sts FCParam+9,r24
2479
 2484 168e 00C0      		rjmp .L144
2480
 2485               	.L289:
2481
 2486 1690 8091 0000 		lds r24,Poti2
2482
 2487 1694 8093 0000 		sts FCParam+6,r24
2483
 2488 1698 00C0      		rjmp .L139
2484
 2489               	.L298:
2485
 2490 169a 8091 0000 		lds r24,Poti2
2486
 2491 169e 8093 0000 		sts FCParam+12,r24
2487
 2492 16a2 00C0      		rjmp .L156
2488
 2493               	.L295:
2489
 2494 16a4 8091 0000 		lds r24,Poti2
2490
 2495 16a8 8093 0000 		sts FCParam+10,r24
2491
 2496 16ac 00C0      		rjmp .L151
2492
 2497               	.L352:
2493
 2498 16ae 8091 0000 		lds r24,Poti3
2494
 2499 16b2 8093 0000 		sts FCParam+30,r24
2495
 2500 16b6 00C0      		rjmp .L243
2496
 2501               	.L357:
2497
 2502 16b8 8091 0000 		lds r24,Poti3
2498
 2503 16bc 8093 0000 		sts FCParam+28,r24
2499
 2504 16c0 00C0      		rjmp .L250
2500
 2505               	.L329:
2501
 2506 16c2 8091 0000 		lds r24,Poti3
2502
 2507 16c6 8093 0000 		sts FCParam+22,r24
2503
 2508 16ca 00C0      		rjmp .L206
2504
 2509               	.L305:
2505
 2510 16cc 8091 0000 		lds r24,Poti3
2506
 2511 16d0 8093 0000 		sts FCParam+14,r24
2507
 2512 16d4 00C0      		rjmp .L166
2508
 2513               	.L341:
2509
 2514 16d6 8091 0000 		lds r24,Poti3
2510
 2515 16da 8093 0000 		sts FCParam+26,r24
2511
 2516 16de 00C0      		rjmp .L226
2512
 2517               	.L284:
2513
 2518 16e0 8091 0000 		lds r24,Poti3
2514
 2519 16e4 8093 0000 		sts FCParam+7,r24
2515
 2520 16e8 00C0      		rjmp .L127
2516
 2521               	.L323:
2517
 2522 16ea 8091 0000 		lds r24,Poti3
2518
 2523 16ee 8093 0000 		sts FCParam+20,r24
2519
 2524 16f2 00C0      		rjmp .L196
2520
 2525               	.L299:
2521
 2526 16f4 8091 0000 		lds r24,Poti3
2522
 2527 16f8 8093 0000 		sts FCParam+12,r24
2523
 2528 16fc 00C0      		rjmp .L156
2524
 2529               	.L335:
2525
 2530 16fe 8091 0000 		lds r24,Poti3
2526
 2531 1702 8093 0000 		sts FCParam+24,r24
2527
 2532 1706 00C0      		rjmp .L216
2528
 2533               	.L278:
2529
 2534 1708 8091 0000 		lds r24,Poti3
2530
 2535 170c 8093 0000 		sts FCParam+4,r24
2531
 2536 1710 00C0      		rjmp .L117
2532
 2537               	.L317:
2533
 2538 1712 8091 0000 		lds r24,Poti3
2534
 2539 1716 8093 0000 		sts FCParam+18,r24
2535
 2540 171a 00C0      		rjmp .L186
2536
 2541               	.L293:
2537
 2542 171c 8091 0000 		lds r24,Poti3
2538
 2543 1720 8093 0000 		sts FCParam+9,r24
2539
 2544 1724 00C0      		rjmp .L144
2540
 2545               	.L347:
2541
 2546 1726 8091 0000 		lds r24,Poti3
2542
 2547 172a 8093 0000 		sts FCParam+29,r24
2543
 2548 172e 00C0      		rjmp .L236
2544
 2549               	.L270:
2545
 2550 1730 8091 0000 		lds r24,Poti3
2546
 2551 1734 8093 0000 		sts FCParam+2,r24
2547
 2552 1738 00C0      		rjmp .L105
2548
 2553               	.L320:
2549
 2554 173a 8091 0000 		lds r24,Poti3
2550
 2555 173e 8093 0000 		sts FCParam+19,r24
2551
 2556 1742 00C0      		rjmp .L191
2552
 2557               	.L296:
2553
 2558 1744 8091 0000 		lds r24,Poti3
2554
 2559 1748 8093 0000 		sts FCParam+10,r24
2555
 2560 174c 00C0      		rjmp .L151
2556
 2561               	.L308:
2557
 2562 174e 8091 0000 		lds r24,Poti3
2558
 2563 1752 8093 0000 		sts FCParam+15,r24
2559
 2564 1756 00C0      		rjmp .L171
2560
 2565               	.L274:
2561
 2566 1758 8091 0000 		lds r24,Poti3
2562
 2567 175c 8093 0000 		sts FCParam+3,r24
2563
 2568 1760 00C0      		rjmp .L112
2564
 2569               	.L314:
2565
 2570 1762 8091 0000 		lds r24,Poti3
2566
 2571 1766 8093 0000 		sts FCParam+17,r24
2567
 2572 176a 00C0      		rjmp .L181
2568
 2573               	.L290:
2569
 2574 176c 8091 0000 		lds r24,Poti3
2570
 2575 1770 8093 0000 		sts FCParam+6,r24
2571
 2576 1774 00C0      		rjmp .L139
2572
 2577               	.L332:
2573
 2578 1776 8091 0000 		lds r24,Poti3
2574
 2579 177a 8093 0000 		sts FCParam+23,r24
2575
 2580 177e 00C0      		rjmp .L211
2576
 2581               	.L264:
2577
 2582 1780 8091 0000 		lds r24,Poti3
2578
 2583 1784 8093 0000 		sts FCParam,r24
2579
 2584 1788 00C0      		rjmp .L98
2580
 2585               	.L326:
2581
 2586 178a 8091 0000 		lds r24,Poti3
2582
 2587 178e 8093 0000 		sts FCParam+21,r24
2583
 2588 1792 00C0      		rjmp .L201
2584
 2589               	.L302:
2585
 2590 1794 8091 0000 		lds r24,Poti3
2586
 2591 1798 8093 0000 		sts FCParam+13,r24
2587
 2592 179c 00C0      		rjmp .L161
2588
 2593               	.L338:
2589
 2594 179e 8091 0000 		lds r24,Poti3
2590
 2595 17a2 8093 0000 		sts FCParam+25,r24
2591
 2596 17a6 00C0      		rjmp .L221
2592
 2597               	.L281:
2593
 2598 17a8 8091 0000 		lds r24,Poti3
2594
 2599 17ac 8093 0000 		sts FCParam+5,r24
2595
 2600 17b0 00C0      		rjmp .L122
2596
 2601               	.L311:
2597
 2602 17b2 8091 0000 		lds r24,Poti3
2598
 2603 17b6 8093 0000 		sts FCParam+16,r24
2599
 2604 17ba 00C0      		rjmp .L176
2600
 2605               	.L287:
2601
 2606 17bc 8091 0000 		lds r24,Poti3
2602
 2607 17c0 8093 0000 		sts FCParam+8,r24
2603
 2608 17c4 00C0      		rjmp .L134
2604
 2609               	.L344:
2605
 2610 17c6 8091 0000 		lds r24,Poti3
2606
 2611 17ca 8093 0000 		sts FCParam+27,r24
2607
 2612 17ce 00C0      		rjmp .L231
2608
 2613               	.L267:
2609
 2614 17d0 8091 0000 		lds r24,Poti3
2610
 2615 17d4 8093 0000 		sts FCParam+1,r24
2611
 2616 17d8 00C0      		rjmp .L93
2612
 2618               	.global	SendMotorData
2613
 2620               	SendMotorData:
2614
 2621               	/* prologue: function */
2615
 2622               	/* frame size = 0 */
2616
 2623 17da 8091 0000 		lds r24,MKFlags
2617
 2624 17de 80FD      		sbrc r24,0
2618
 2625 17e0 00C0      		rjmp .L359
2619
 2626 17e2 8091 0000 		lds r24,MKFlags
2620
 2627 17e6 857F      		andi r24,lo8(-11)
2621
 2628 17e8 8093 0000 		sts MKFlags,r24
2622
 2629 17ec 9091 0000 		lds r25,MotorTest_Active
2623
 2630 17f0 E0E0      		ldi r30,lo8(Motor)
2624
 2631 17f2 F0E0      		ldi r31,hi8(Motor)
2625
 2632 17f4 A0E0      		ldi r26,lo8(MotorTest)
2626
 2633 17f6 B0E0      		ldi r27,hi8(MotorTest)
2627
 2634 17f8 00C0      		rjmp .L362
2628
 2635               	.L367:
2629
 2636 17fa 1082      		st Z,__zero_reg__
2630
 2637 17fc 3596      		adiw r30,5
2631
 2638 17fe 1196      		adiw r26,1
2632
 2639 1800 80E0      		ldi r24,hi8(Motor+60)
2633
 2640 1802 E030      		cpi r30,lo8(Motor+60)
2634
 2641 1804 F807      		cpc r31,r24
2635
 2642 1806 01F0      		breq .L366
2636
 2643               	.L362:
2637
 2644 1808 9923      		tst r25
2638
 2645 180a 01F0      		breq .L367
2639
 2646 180c 8C91      		ld r24,X
2640
 2647 180e 8083      		st Z,r24
2641
 2648 1810 3596      		adiw r30,5
2642
 2649 1812 1196      		adiw r26,1
2643
 2650 1814 80E0      		ldi r24,hi8(Motor+60)
2644
 2651 1816 E030      		cpi r30,lo8(Motor+60)
2645
 2652 1818 F807      		cpc r31,r24
2646
 2653 181a 01F4      		brne .L362
2647
 2654               	.L366:
2648
 2655 181c 9923      		tst r25
2649
 2656 181e 01F0      		breq .L359
2650
 2657 1820 9150      		subi r25,lo8(-(-1))
2651
 2658 1822 9093 0000 		sts MotorTest_Active,r25
2652
 2659               	.L359:
2653
 2660 1826 E0E0      		ldi r30,lo8(DebugOut+26)
2654
 2661 1828 F0E0      		ldi r31,hi8(DebugOut+26)
2655
 2662 182a 8091 0000 		lds r24,Motor
2656
 2663 182e 8093 0000 		sts DebugOut+26,r24
2657
 2664 1832 1092 0000 		sts DebugOut+27,__zero_reg__
2658
 2665 1836 8091 0000 		lds r24,Motor+5
2659
 2666 183a 8093 0000 		sts DebugOut+28,r24
2660
 2667 183e 1382      		std Z+3,__zero_reg__
2661
 2668 1840 8091 0000 		lds r24,Motor+15
2662
 2669 1844 8093 0000 		sts DebugOut+30,r24
2663
 2670 1848 1582      		std Z+5,__zero_reg__
2664
 2671 184a 8091 0000 		lds r24,Motor+10
2665
 2672 184e 8093 0000 		sts DebugOut+32,r24
2666
 2673 1852 1782      		std Z+7,__zero_reg__
2667
 2674 1854 80E0      		ldi r24,lo8(0)
2668
 2675 1856 0E94 0000 		call I2C_Start
2669
 2676               	/* epilogue start */
2670
 2677 185a 0895      		ret
2671
 2679               	.global	SetNeutral
2672
 2681               	SetNeutral:
2673
 2682 185c 2F92      		push r2
2674
 2683 185e 3F92      		push r3
2675
 2684 1860 4F92      		push r4
2676
 2685 1862 5F92      		push r5
2677
 2686 1864 6F92      		push r6
2678
 2687 1866 7F92      		push r7
2679
 2688 1868 8F92      		push r8
2680
 2689 186a 9F92      		push r9
2681
 2690 186c AF92      		push r10
2682
 2691 186e BF92      		push r11
2683
 2692 1870 CF92      		push r12
2684
 2693 1872 DF92      		push r13
2685
 2694 1874 EF92      		push r14
2686
 2695 1876 FF92      		push r15
2687
 2696 1878 0F93      		push r16
2688
 2697 187a 1F93      		push r17
2689
 2698               	/* prologue: function */
2690
 2699               	/* frame size = 0 */
2691
 2700 187c 582E      		mov r5,r24
2692
 2701 187e 1092 0000 		sts (AdBiasAccNick)+1,__zero_reg__
2693
 2702 1882 1092 0000 		sts AdBiasAccNick,__zero_reg__
2694
 2703 1886 1092 0000 		sts (AdBiasAccRoll)+1,__zero_reg__
2695
 2704 188a 1092 0000 		sts AdBiasAccRoll,__zero_reg__
2696
 2705 188e 80E0      		ldi r24,lo8(0x0)
2697
 2706 1890 90E0      		ldi r25,hi8(0x0)
2698
 2707 1892 A0E0      		ldi r26,hlo8(0x0)
2699
 2708 1894 B0E0      		ldi r27,hhi8(0x0)
2700
 2709 1896 8093 0000 		sts AdBiasAccTop,r24
2701
 2710 189a 9093 0000 		sts (AdBiasAccTop)+1,r25
2702
 2711 189e A093 0000 		sts (AdBiasAccTop)+2,r26
2703
 2712 18a2 B093 0000 		sts (AdBiasAccTop)+3,r27
2704
 2713 18a6 1092 0000 		sts (BiasHiResGyroNick)+1,__zero_reg__
2705
 2714 18aa 1092 0000 		sts BiasHiResGyroNick,__zero_reg__
2706
 2715 18ae 1092 0000 		sts (BiasHiResGyroRoll)+1,__zero_reg__
2707
 2716 18b2 1092 0000 		sts BiasHiResGyroRoll,__zero_reg__
2708
 2717 18b6 1092 0000 		sts (AdBiasGyroYaw)+1,__zero_reg__
2709
 2718 18ba 1092 0000 		sts AdBiasGyroYaw,__zero_reg__
2710
 2719 18be 1092 0000 		sts FCParam+24,__zero_reg__
2711
 2720 18c2 1092 0000 		sts FCParam+25,__zero_reg__
2712
 2721 18c6 1092 0000 		sts ExpandBaro,__zero_reg__
2713
 2722 18ca 84E6      		ldi r24,lo8(100)
2714
 2723 18cc 90E0      		ldi r25,hi8(100)
2715
 2724 18ce 0E94 0000 		call Delay_ms_Mess
2716
 2725 18d2 8091 0000 		lds r24,BoardRelease
2717
 2726 18d6 8D30      		cpi r24,lo8(13)
2718
 2727 18d8 01F4      		brne .+2
2719
 2728 18da 00C0      		rjmp .L384
2720
 2729               	.L369:
2721
 2730 18dc 8091 0000 		lds r24,ParamSet+8
2722
 2731 18e0 80FF      		sbrs r24,0
2723
 2732 18e2 00C0      		rjmp .L370
2724
 2733 18e4 8091 0000 		lds r24,AdAirPressure
2725
 2734 18e8 9091 0000 		lds r25,(AdAirPressure)+1
2726
 2735 18ec 875B      		subi r24,lo8(951)
2727
 2736 18ee 9340      		sbci r25,hi8(951)
2728
 2737 18f0 04F0      		brlt .+2
2729
 2738 18f2 00C0      		rjmp .L371
2730
 2739 18f4 8091 0000 		lds r24,AdAirPressure
2731
 2740 18f8 9091 0000 		lds r25,(AdAirPressure)+1
2732
 2741 18fc 8E5E      		subi r24,lo8(750)
2733
 2742 18fe 9240      		sbci r25,hi8(750)
2734
 2743 1900 04F4      		brge .+2
2735
 2744 1902 00C0      		rjmp .L371
2736
 2745               	.L370:
2737
 2746 1904 2224      		clr r2
2738
 2747 1906 EE24      		clr r14
2739
 2748 1908 FF24      		clr r15
2740
 2749 190a 8701      		movw r16,r14
2741
 2750 190c AA24      		clr r10
2742
 2751 190e BB24      		clr r11
2743
 2752 1910 6501      		movw r12,r10
2744
 2753 1912 6624      		clr r6
2745
 2754 1914 7724      		clr r7
2746
 2755 1916 4301      		movw r8,r6
2747
 2756               	.L372:
2748
 2757 1918 8AE0      		ldi r24,lo8(10)
2749
 2758 191a 90E0      		ldi r25,hi8(10)
2750
 2759 191c 0E94 0000 		call Delay_ms_Mess
2751
 2760 1920 8091 0000 		lds r24,AdValueGyroNick
2752
 2761 1924 9091 0000 		lds r25,(AdValueGyroNick)+1
2753
 2762 1928 880F      		lsl r24
2754
 2763 192a 991F      		rol r25
2755
 2764 192c 880F      		lsl r24
2756
 2765 192e 991F      		rol r25
2757
 2766 1930 880F      		lsl r24
2758
 2767 1932 991F      		rol r25
2759
 2768 1934 AA27      		clr r26
2760
 2769 1936 97FD      		sbrc r25,7
2761
 2770 1938 A095      		com r26
2762
 2771 193a BA2F      		mov r27,r26
2763
 2772 193c E80E      		add r14,r24
2764
 2773 193e F91E      		adc r15,r25
2765
 2774 1940 0A1F      		adc r16,r26
2766
 2775 1942 1B1F      		adc r17,r27
2767
 2776 1944 8091 0000 		lds r24,AdValueGyroRoll
2768
 2777 1948 9091 0000 		lds r25,(AdValueGyroRoll)+1
2769
 2778 194c 880F      		lsl r24
2770
 2779 194e 991F      		rol r25
2771
 2780 1950 880F      		lsl r24
2772
 2781 1952 991F      		rol r25
2773
 2782 1954 880F      		lsl r24
2774
 2783 1956 991F      		rol r25
2775
 2784 1958 AA27      		clr r26
2776
 2785 195a 97FD      		sbrc r25,7
2777
 2786 195c A095      		com r26
2778
 2787 195e BA2F      		mov r27,r26
2779
 2788 1960 A80E      		add r10,r24
2780
 2789 1962 B91E      		adc r11,r25
2781
 2790 1964 CA1E      		adc r12,r26
2782
 2791 1966 DB1E      		adc r13,r27
2783
 2792 1968 8091 0000 		lds r24,AdValueGyroYaw
2784
 2793 196c 9091 0000 		lds r25,(AdValueGyroYaw)+1
2785
 2794 1970 AA27      		clr r26
2786
 2795 1972 97FD      		sbrc r25,7
2787
 2796 1974 A095      		com r26
2788
 2797 1976 BA2F      		mov r27,r26
2789
 2798 1978 680E      		add r6,r24
2790
 2799 197a 791E      		adc r7,r25
2791
 2800 197c 8A1E      		adc r8,r26
2792
 2801 197e 9B1E      		adc r9,r27
2793
 2802 1980 2394      		inc r2
2794
 2803 1982 B0E2      		ldi r27,lo8(32)
2795
 2804 1984 2B16      		cp r2,r27
2796
 2805 1986 01F4      		brne .L372
2797
 2806 1988 80E1      		ldi r24,lo8(16)
2798
 2807 198a 90E0      		ldi r25,hi8(16)
2799
 2808 198c A0E0      		ldi r26,hlo8(16)
2800
 2809 198e B0E0      		ldi r27,hhi8(16)
2801
 2810 1990 E80E      		add r14,r24
2802
 2811 1992 F91E      		adc r15,r25
2803
 2812 1994 0A1F      		adc r16,r26
2804
 2813 1996 1B1F      		adc r17,r27
2805
 2814 1998 C801      		movw r24,r16
2806
 2815 199a B701      		movw r22,r14
2807
 2816 199c 20E2      		ldi r18,lo8(32)
2808
 2817 199e 30E0      		ldi r19,hi8(32)
2809
 2818 19a0 40E0      		ldi r20,hlo8(32)
2810
 2819 19a2 50E0      		ldi r21,hhi8(32)
2811
 2820 19a4 0E94 0000 		call __divmodsi4
2812
 2821 19a8 3093 0000 		sts (BiasHiResGyroNick)+1,r19
2813
 2822 19ac 2093 0000 		sts BiasHiResGyroNick,r18
2814
 2823 19b0 80E1      		ldi r24,lo8(16)
2815
 2824 19b2 90E0      		ldi r25,hi8(16)
2816
 2825 19b4 A0E0      		ldi r26,hlo8(16)
2817
 2826 19b6 B0E0      		ldi r27,hhi8(16)
2818
 2827 19b8 A80E      		add r10,r24
2819
 2828 19ba B91E      		adc r11,r25
2820
 2829 19bc CA1E      		adc r12,r26
2821
 2830 19be DB1E      		adc r13,r27
2822
 2831 19c0 C601      		movw r24,r12
2823
 2832 19c2 B501      		movw r22,r10
2824
 2833 19c4 20E2      		ldi r18,lo8(32)
2825
 2834 19c6 30E0      		ldi r19,hi8(32)
2826
 2835 19c8 40E0      		ldi r20,hlo8(32)
2827
 2836 19ca 50E0      		ldi r21,hhi8(32)
2828
 2837 19cc 0E94 0000 		call __divmodsi4
2829
 2838 19d0 3093 0000 		sts (BiasHiResGyroRoll)+1,r19
2830
 2839 19d4 2093 0000 		sts BiasHiResGyroRoll,r18
2831
 2840 19d8 80E1      		ldi r24,lo8(16)
2832
 2841 19da 90E0      		ldi r25,hi8(16)
2833
 2842 19dc A0E0      		ldi r26,hlo8(16)
2834
 2843 19de B0E0      		ldi r27,hhi8(16)
2835
 2844 19e0 680E      		add r6,r24
2836
 2845 19e2 791E      		adc r7,r25
2837
 2846 19e4 8A1E      		adc r8,r26
2838
 2847 19e6 9B1E      		adc r9,r27
2839
 2848 19e8 C401      		movw r24,r8
2840
 2849 19ea B301      		movw r22,r6
2841
 2850 19ec 20E2      		ldi r18,lo8(32)
2842
 2851 19ee 30E0      		ldi r19,hi8(32)
2843
 2852 19f0 40E0      		ldi r20,hlo8(32)
2844
 2853 19f2 50E0      		ldi r21,hhi8(32)
2845
 2854 19f4 0E94 0000 		call __divmodsi4
2846
 2855 19f8 3093 0000 		sts (AdBiasGyroYaw)+1,r19
2847
 2856 19fc 2093 0000 		sts AdBiasGyroYaw,r18
2848
 2857 1a00 5520      		tst r5
2849
 2858 1a02 01F4      		brne .+2
2850
 2859 1a04 00C0      		rjmp .L373
2851
 2860 1a06 2224      		clr r2
2852
 2861 1a08 EE24      		clr r14
2853
 2862 1a0a FF24      		clr r15
2854
 2863 1a0c 8701      		movw r16,r14
2855
 2864 1a0e AA24      		clr r10
2856
 2865 1a10 BB24      		clr r11
2857
 2866 1a12 6501      		movw r12,r10
2858
 2867 1a14 6624      		clr r6
2859
 2868 1a16 7724      		clr r7
2860
 2869 1a18 4301      		movw r8,r6
2861
 2870               	.L374:
2862
 2871 1a1a 8AE0      		ldi r24,lo8(10)
2863
 2872 1a1c 90E0      		ldi r25,hi8(10)
2864
 2873 1a1e 0E94 0000 		call Delay_ms_Mess
2865
 2874 1a22 8091 0000 		lds r24,AdValueAccNick
2866
 2875 1a26 9091 0000 		lds r25,(AdValueAccNick)+1
2867
 2876 1a2a AA27      		clr r26
2868
 2877 1a2c 97FD      		sbrc r25,7
2869
 2878 1a2e A095      		com r26
2870
 2879 1a30 BA2F      		mov r27,r26
2871
 2880 1a32 E80E      		add r14,r24
2872
 2881 1a34 F91E      		adc r15,r25
2873
 2882 1a36 0A1F      		adc r16,r26
2874
 2883 1a38 1B1F      		adc r17,r27
2875
 2884 1a3a 8091 0000 		lds r24,AdValueAccRoll
2876
 2885 1a3e 9091 0000 		lds r25,(AdValueAccRoll)+1
2877
 2886 1a42 AA27      		clr r26
2878
 2887 1a44 97FD      		sbrc r25,7
2879
 2888 1a46 A095      		com r26
2880
 2889 1a48 BA2F      		mov r27,r26
2881
 2890 1a4a A80E      		add r10,r24
2882
 2891 1a4c B91E      		adc r11,r25
2883
 2892 1a4e CA1E      		adc r12,r26
2884
 2893 1a50 DB1E      		adc r13,r27
2885
 2894 1a52 8091 0000 		lds r24,AdValueAccZ
2886
 2895 1a56 9091 0000 		lds r25,(AdValueAccZ)+1
2887
 2896 1a5a AA27      		clr r26
2888
 2897 1a5c 97FD      		sbrc r25,7
2889
 2898 1a5e A095      		com r26
2890
 2899 1a60 BA2F      		mov r27,r26
2891
 2900 1a62 680E      		add r6,r24
2892
 2901 1a64 791E      		adc r7,r25
2893
 2902 1a66 8A1E      		adc r8,r26
2894
 2903 1a68 9B1E      		adc r9,r27
2895
 2904 1a6a 2394      		inc r2
2896
 2905 1a6c 9AE0      		ldi r25,lo8(10)
2897
 2906 1a6e 2916      		cp r2,r25
2898
 2907 1a70 01F4      		brne .L374
2899
 2908 1a72 C701      		movw r24,r14
2900
 2909 1a74 F7FC      		sbrc r15,7
2901
 2910 1a76 00C0      		rjmp .L385
2902
 2911               	.L375:
2903
 2912 1a78 0596      		adiw r24,5
2904
 2913 1a7a 6AE0      		ldi r22,lo8(10)
2905
 2914 1a7c 70E0      		ldi r23,hi8(10)
2906
 2915 1a7e 0E94 0000 		call __divmodhi4
2907
 2916 1a82 162F      		mov r17,r22
2908
 2917 1a84 072F      		mov r16,r23
2909
 2918 1a86 6093 0000 		sts AdBiasAccNick,r22
2910
 2919 1a8a 7093 0000 		sts AdBiasAccNick+1,r23
2911
 2920 1a8e C501      		movw r24,r10
2912
 2921 1a90 B7FC      		sbrc r11,7
2913
 2922 1a92 00C0      		rjmp .L386
2914
 2923               	.L376:
2915
 2924 1a94 0596      		adiw r24,5
2916
 2925 1a96 6AE0      		ldi r22,lo8(10)
2917
 2926 1a98 70E0      		ldi r23,hi8(10)
2918
 2927 1a9a 0E94 0000 		call __divmodhi4
2919
 2928 1a9e 7093 0000 		sts (AdBiasAccRoll)+1,r23
2920
 2929 1aa2 6093 0000 		sts AdBiasAccRoll,r22
2921
 2930 1aa6 C301      		movw r24,r6
2922
 2931 1aa8 77FC      		sbrc r7,7
2923
 2932 1aaa 00C0      		rjmp .L387
2924
 2933               	.L377:
2925
 2934 1aac 0596      		adiw r24,5
2926
 2935 1aae 6AE0      		ldi r22,lo8(10)
2927
 2936 1ab0 70E0      		ldi r23,hi8(10)
2928
 2937 1ab2 0E94 0000 		call __divmodhi4
2929
 2938 1ab6 8827      		clr r24
2930
 2939 1ab8 77FD      		sbrc r23,7
2931
 2940 1aba 8095      		com r24
2932
 2941 1abc 982F      		mov r25,r24
2933
 2942 1abe 0E94 0000 		call __floatsisf
2934
 2943 1ac2 6093 0000 		sts AdBiasAccTop,r22
2935
 2944 1ac6 7093 0000 		sts (AdBiasAccTop)+1,r23
2936
 2945 1aca 8093 0000 		sts (AdBiasAccTop)+2,r24
2937
 2946 1ace 9093 0000 		sts (AdBiasAccTop)+3,r25
2938
 2947 1ad2 84E0      		ldi r24,lo8(4)
2939
 2948 1ad4 90E0      		ldi r25,hi8(4)
2940
 2949 1ad6 612F      		mov r22,r17
2941
 2950 1ad8 702F      		mov r23,r16
2942
 2951 1ada 0E94 0000 		call SetParamWord
2943
 2952 1ade 6091 0000 		lds r22,AdBiasAccRoll
2944
 2953 1ae2 7091 0000 		lds r23,(AdBiasAccRoll)+1
2945
 2954 1ae6 86E0      		ldi r24,lo8(6)
2946
 2955 1ae8 90E0      		ldi r25,hi8(6)
2947
 2956 1aea 0E94 0000 		call SetParamWord
2948
 2957 1aee 6091 0000 		lds r22,AdBiasAccTop
2949
 2958 1af2 7091 0000 		lds r23,(AdBiasAccTop)+1
2950
 2959 1af6 8091 0000 		lds r24,(AdBiasAccTop)+2
2951
 2960 1afa 9091 0000 		lds r25,(AdBiasAccTop)+3
2952
 2961 1afe 0E94 0000 		call __fixunssfsi
2953
 2962 1b02 9B01      		movw r18,r22
2954
 2963 1b04 AC01      		movw r20,r24
2955
 2964 1b06 88E0      		ldi r24,lo8(8)
2956
 2965 1b08 90E0      		ldi r25,hi8(8)
2957
 2966 1b0a B901      		movw r22,r18
2958
 2967 1b0c 0E94 0000 		call SetParamWord
2959
 2968               	.L378:
2960
 2969 1b10 8091 0000 		lds r24,AirPressure
2961
 2970 1b14 9091 0000 		lds r25,(AirPressure)+1
2962
 2971 1b18 A091 0000 		lds r26,(AirPressure)+2
2963
 2972 1b1c B091 0000 		lds r27,(AirPressure)+3
2964
 2973 1b20 8093 0000 		sts StartAirPressure,r24
2965
 2974 1b24 9093 0000 		sts (StartAirPressure)+1,r25
2966
 2975 1b28 A093 0000 		sts (StartAirPressure)+2,r26
2967
 2976 1b2c B093 0000 		sts (StartAirPressure)+3,r27
2968
 2977 1b30 84E6      		ldi r24,lo8(100)
2969
 2978 1b32 90E0      		ldi r25,hi8(100)
2970
 2979 1b34 0E94 0000 		call Delay_ms_Mess
2971
 2980 1b38 1092 0000 		sts (ReadingVario)+1,__zero_reg__
2972
 2981 1b3c 1092 0000 		sts ReadingVario,__zero_reg__
2973
 2982 1b40 6091 0000 		lds r22,AdValueAccNick
2974
 2983 1b44 7091 0000 		lds r23,(AdValueAccNick)+1
2975
 2984 1b48 8827      		clr r24
2976
 2985 1b4a 77FD      		sbrc r23,7
2977
 2986 1b4c 8095      		com r24
2978
 2987 1b4e 982F      		mov r25,r24
2979
 2988 1b50 26E0      		ldi r18,lo8(6)
2980
 2989 1b52 30E0      		ldi r19,hi8(6)
2981
 2990 1b54 40E0      		ldi r20,hlo8(6)
2982
 2991 1b56 50E0      		ldi r21,hhi8(6)
2983
 2992 1b58 0E94 0000 		call __mulsi3
2984
 2993 1b5c 3B01      		movw r6,r22
2985
 2994 1b5e 4C01      		movw r8,r24
2986
 2995 1b60 7093 0000 		sts (AccNick)+1,r23
2987
 2996 1b64 6093 0000 		sts AccNick,r22
2988
 2997 1b68 6091 0000 		lds r22,AdValueAccRoll
2989
 2998 1b6c 7091 0000 		lds r23,(AdValueAccRoll)+1
2990
 2999 1b70 8827      		clr r24
2991
 3000 1b72 77FD      		sbrc r23,7
2992
 3001 1b74 8095      		com r24
2993
 3002 1b76 982F      		mov r25,r24
2994
 3003 1b78 0E94 0000 		call __mulsi3
2995
 3004 1b7c 5B01      		movw r10,r22
2996
 3005 1b7e 6C01      		movw r12,r24
2997
 3006 1b80 7093 0000 		sts (AccRoll)+1,r23
2998
 3007 1b84 6093 0000 		sts AccRoll,r22
2999
 3008 1b88 8091 0000 		lds r24,AdValueAccTop
3000
 3009 1b8c 9091 0000 		lds r25,(AdValueAccTop)+1
3001
 3010 1b90 9093 0000 		sts (AccTop)+1,r25
3002
 3011 1b94 8093 0000 		sts AccTop,r24
3003
 3012 1b98 8091 0000 		lds r24,AdValueAccTop
3004
 3013 1b9c 9091 0000 		lds r25,(AdValueAccTop)+1
3005
 3014 1ba0 982F      		mov r25,r24
3006
 3015 1ba2 8827      		clr r24
3007
 3016 1ba4 990F      		lsl r25
3008
 3017 1ba6 990F      		lsl r25
3009
 3018 1ba8 AA27      		clr r26
3010
 3019 1baa 97FD      		sbrc r25,7
3011
 3020 1bac A095      		com r26
3012
 3021 1bae BA2F      		mov r27,r26
3013
 3022 1bb0 8093 0000 		sts ReadingIntegralTop,r24
3014
 3023 1bb4 9093 0000 		sts (ReadingIntegralTop)+1,r25
3015
 3024 1bb8 A093 0000 		sts (ReadingIntegralTop)+2,r26
3016
 3025 1bbc B093 0000 		sts (ReadingIntegralTop)+3,r27
3017
 3026 1bc0 1092 0000 		sts (GyroNick)+1,__zero_reg__
3018
 3027 1bc4 1092 0000 		sts GyroNick,__zero_reg__
3019
 3028 1bc8 1092 0000 		sts (GyroRoll)+1,__zero_reg__
3020
 3029 1bcc 1092 0000 		sts GyroRoll,__zero_reg__
3021
 3030 1bd0 1092 0000 		sts (GyroYaw)+1,__zero_reg__
3022
 3031 1bd4 1092 0000 		sts GyroYaw,__zero_reg__
3023
 3032 1bd8 E090 0000 		lds r14,ParamSet+23
3024
 3033 1bdc 2E2C      		mov r2,r14
3025
 3034 1bde 3324      		clr r3
3026
 3035 1be0 4424      		clr r4
3027
 3036 1be2 5524      		clr r5
3028
 3037 1be4 8824      		clr r8
3029
 3038 1be6 77FC      		sbrc r7,7
3030
 3039 1be8 8094      		com r8
3031
 3040 1bea 982C      		mov r9,r8
3032
 3041 1bec C401      		movw r24,r8
3033
 3042 1bee B301      		movw r22,r6
3034
 3043 1bf0 A201      		movw r20,r4
3035
 3044 1bf2 9101      		movw r18,r2
3036
 3045 1bf4 0E94 0000 		call __mulsi3
3037
 3046 1bf8 3B01      		movw r6,r22
3038
 3047 1bfa 4C01      		movw r8,r24
3039
 3048 1bfc 6093 0000 		sts IntegralGyroNick,r22
3040
 3049 1c00 7093 0000 		sts (IntegralGyroNick)+1,r23
3041
 3050 1c04 8093 0000 		sts (IntegralGyroNick)+2,r24
3042
 3051 1c08 9093 0000 		sts (IntegralGyroNick)+3,r25
3043
 3052 1c0c CC24      		clr r12
3044
 3053 1c0e B7FC      		sbrc r11,7
3045
 3054 1c10 C094      		com r12
3046
 3055 1c12 DC2C      		mov r13,r12
3047
 3056 1c14 C601      		movw r24,r12
3048
 3057 1c16 B501      		movw r22,r10
3049
 3058 1c18 0E94 0000 		call __mulsi3
3050
 3059 1c1c 6093 0000 		sts IntegralGyroRoll,r22
3051
 3060 1c20 7093 0000 		sts (IntegralGyroRoll)+1,r23
3052
 3061 1c24 8093 0000 		sts (IntegralGyroRoll)+2,r24
3053
 3062 1c28 9093 0000 		sts (IntegralGyroRoll)+3,r25
3054
 3063 1c2c 6092 0000 		sts ReadingIntegralGyroNick2,r6
3055
 3064 1c30 7092 0000 		sts (ReadingIntegralGyroNick2)+1,r7
3056
 3065 1c34 8092 0000 		sts (ReadingIntegralGyroNick2)+2,r8
3057
 3066 1c38 9092 0000 		sts (ReadingIntegralGyroNick2)+3,r9
3058
 3067 1c3c 6093 0000 		sts ReadingIntegralGyroRoll2,r22
3059
 3068 1c40 7093 0000 		sts (ReadingIntegralGyroRoll2)+1,r23
3060
 3069 1c44 8093 0000 		sts (ReadingIntegralGyroRoll2)+2,r24
3061
 3070 1c48 9093 0000 		sts (ReadingIntegralGyroRoll2)+3,r25
3062
 3071 1c4c 1092 0000 		sts ReadingIntegralGyroYaw,__zero_reg__
3063
 3072 1c50 1092 0000 		sts (ReadingIntegralGyroYaw)+1,__zero_reg__
3064
 3073 1c54 1092 0000 		sts (ReadingIntegralGyroYaw)+2,__zero_reg__
3065
 3074 1c58 1092 0000 		sts (ReadingIntegralGyroYaw)+3,__zero_reg__
3066
 3075 1c5c 2091 0000 		lds r18,CompassHeading
3067
 3076 1c60 3091 0000 		lds r19,(CompassHeading)+1
3068
 3077 1c64 3093 0000 		sts (CompassCourse)+1,r19
3069
 3078 1c68 2093 0000 		sts CompassCourse,r18
3070
 3079 1c6c 8AE2      		ldi r24,lo8(42)
3071
 3080 1c6e E89E      		mul r14,r24
3072
 3081 1c70 7001      		movw r14,r0
3073
 3082 1c72 1124      		clr r1
3074
 3083 1c74 0027      		clr r16
3075
 3084 1c76 F7FC      		sbrc r15,7
3076
 3085 1c78 0095      		com r16
3077
 3086 1c7a 102F      		mov r17,r16
3078
 3087 1c7c 4427      		clr r20
3079
 3088 1c7e 37FD      		sbrc r19,7
3080
 3089 1c80 4095      		com r20
3081
 3090 1c82 542F      		mov r21,r20
3082
 3091 1c84 C801      		movw r24,r16
3083
 3092 1c86 B701      		movw r22,r14
3084
 3093 1c88 0E94 0000 		call __mulsi3
3085
 3094 1c8c 6093 0000 		sts YawGyroHeading,r22
3086
 3095 1c90 7093 0000 		sts (YawGyroHeading)+1,r23
3087
 3096 1c94 8093 0000 		sts (YawGyroHeading)+2,r24
3088
 3097 1c98 9093 0000 		sts (YawGyroHeading)+3,r25
3089
 3098 1c9c 1092 0000 		sts (YawGyroDrift)+1,__zero_reg__
3090
 3099 1ca0 1092 0000 		sts YawGyroDrift,__zero_reg__
3091
 3100 1ca4 82E3      		ldi r24,lo8(50)
3092
 3101 1ca6 90E0      		ldi r25,hi8(50)
3093
 3102 1ca8 9093 0000 		sts (BeepTime)+1,r25
3094
 3103 1cac 8093 0000 		sts BeepTime,r24
3095
 3104 1cb0 6091 0000 		lds r22,ParamSet+54
3096
 3105 1cb4 70E0      		ldi r23,lo8(0)
3097
 3106 1cb6 80E0      		ldi r24,lo8(0)
3098
 3107 1cb8 90E0      		ldi r25,hi8(0)
3099
 3108 1cba 24EC      		ldi r18,lo8(2500)
3100
 3109 1cbc 39E0      		ldi r19,hi8(2500)
3101
 3110 1cbe 40E0      		ldi r20,hlo8(2500)
3102
 3111 1cc0 50E0      		ldi r21,hhi8(2500)
3103
 3112 1cc2 0E94 0000 		call __mulsi3
3104
 3113 1cc6 6856      		subi r22,lo8(-(15000))
3105
 3114 1cc8 754C      		sbci r23,hi8(-(15000))
3106
 3115 1cca 8F4F      		sbci r24,hlo8(-(15000))
3107
 3116 1ccc 9F4F      		sbci r25,hhi8(-(15000))
3108
 3117 1cce 6093 0000 		sts TurnOver180Nick,r22
3109
 3118 1cd2 7093 0000 		sts (TurnOver180Nick)+1,r23
3110
 3119 1cd6 8093 0000 		sts (TurnOver180Nick)+2,r24
3111
 3120 1cda 9093 0000 		sts (TurnOver180Nick)+3,r25
3112
 3121 1cde 6091 0000 		lds r22,ParamSet+55
3113
 3122 1ce2 70E0      		ldi r23,lo8(0)
3114
 3123 1ce4 80E0      		ldi r24,lo8(0)
3115
 3124 1ce6 90E0      		ldi r25,hi8(0)
3116
 3125 1ce8 0E94 0000 		call __mulsi3
3117
 3126 1cec 6856      		subi r22,lo8(-(15000))
3118
 3127 1cee 754C      		sbci r23,hi8(-(15000))
3119
 3128 1cf0 8F4F      		sbci r24,hlo8(-(15000))
3120
 3129 1cf2 9F4F      		sbci r25,hhi8(-(15000))
3121
 3130 1cf4 6093 0000 		sts TurnOver180Roll,r22
3122
 3131 1cf8 7093 0000 		sts (TurnOver180Roll)+1,r23
3123
 3132 1cfc 8093 0000 		sts (TurnOver180Roll)+2,r24
3124
 3133 1d00 9093 0000 		sts (TurnOver180Roll)+3,r25
3125
 3134 1d04 1092 0000 		sts (ExternHeightValue)+1,__zero_reg__
3126
 3135 1d08 1092 0000 		sts ExternHeightValue,__zero_reg__
3127
 3136 1d0c 1092 0000 		sts (GPSStickNick)+1,__zero_reg__
3128
 3137 1d10 1092 0000 		sts GPSStickNick,__zero_reg__
3129
 3138 1d14 1092 0000 		sts (GPSStickRoll)+1,__zero_reg__
3130
 3139 1d18 1092 0000 		sts GPSStickRoll,__zero_reg__
3131
 3140 1d1c 8091 0000 		lds r24,MKFlags
3132
 3141 1d20 8460      		ori r24,lo8(4)
3133
 3142 1d22 8093 0000 		sts MKFlags,r24
3134
 3143 1d26 8FEF      		ldi r24,lo8(-1)
3135
 3144 1d28 8093 0000 		sts FCParam+31,r24
3136
 3145 1d2c 1092 0000 		sts FCParam+32,__zero_reg__
3137
 3146 1d30 80E2      		ldi r24,lo8(32)
3138
 3147 1d32 8093 0000 		sts FCParam+33,r24
3139
 3148 1d36 E091 0000 		lds r30,ParamSet+4
3140
 3149 1d3a F0E0      		ldi r31,lo8(0)
3141
 3150 1d3c EE0F      		lsl r30
3142
 3151 1d3e FF1F      		rol r31
3143
 3152 1d40 E050      		subi r30,lo8(-(PPM_in))
3144
 3153 1d42 F040      		sbci r31,hi8(-(PPM_in))
3145
 3154 1d44 8081      		ld r24,Z
3146
 3155 1d46 9181      		ldd r25,Z+1
3147
 3156 1d48 8259      		subi r24,lo8(-(110))
3148
 3157 1d4a 9F4F      		sbci r25,hi8(-(110))
3149
 3158 1d4c 9093 0000 		sts (Poti1)+1,r25
3150
 3159 1d50 8093 0000 		sts Poti1,r24
3151
 3160 1d54 E091 0000 		lds r30,ParamSet+5
3152
 3161 1d58 F0E0      		ldi r31,lo8(0)
3153
 3162 1d5a EE0F      		lsl r30
3154
 3163 1d5c FF1F      		rol r31
3155
 3164 1d5e E050      		subi r30,lo8(-(PPM_in))
3156
 3165 1d60 F040      		sbci r31,hi8(-(PPM_in))
3157
 3166 1d62 8081      		ld r24,Z
3158
 3167 1d64 9181      		ldd r25,Z+1
3159
 3168 1d66 8259      		subi r24,lo8(-(110))
3160
 3169 1d68 9F4F      		sbci r25,hi8(-(110))
3161
 3170 1d6a 9093 0000 		sts (Poti2)+1,r25
3162
 3171 1d6e 8093 0000 		sts Poti2,r24
3163
 3172 1d72 E091 0000 		lds r30,ParamSet+6
3164
 3173 1d76 F0E0      		ldi r31,lo8(0)
3165
 3174 1d78 EE0F      		lsl r30
3166
 3175 1d7a FF1F      		rol r31
3167
 3176 1d7c E050      		subi r30,lo8(-(PPM_in))
3168
 3177 1d7e F040      		sbci r31,hi8(-(PPM_in))
3169
 3178 1d80 8081      		ld r24,Z
3170
 3179 1d82 9181      		ldd r25,Z+1
3171
 3180 1d84 8259      		subi r24,lo8(-(110))
3172
 3181 1d86 9F4F      		sbci r25,hi8(-(110))
3173
 3182 1d88 9093 0000 		sts (Poti3)+1,r25
3174
 3183 1d8c 8093 0000 		sts Poti3,r24
3175
 3184 1d90 E091 0000 		lds r30,ParamSet+7
3176
 3185 1d94 F0E0      		ldi r31,lo8(0)
3177
 3186 1d96 EE0F      		lsl r30
3178
 3187 1d98 FF1F      		rol r31
3179
 3188 1d9a E050      		subi r30,lo8(-(PPM_in))
3180
 3189 1d9c F040      		sbci r31,hi8(-(PPM_in))
3181
 3190 1d9e 8081      		ld r24,Z
3182
 3191 1da0 9181      		ldd r25,Z+1
3183
 3192 1da2 8259      		subi r24,lo8(-(110))
3184
 3193 1da4 9F4F      		sbci r25,hi8(-(110))
3185
 3194 1da6 9093 0000 		sts (Poti4)+1,r25
3186
 3195 1daa 8093 0000 		sts Poti4,r24
3187
 3196 1dae 84E6      		ldi r24,lo8(100)
3188
 3197 1db0 8093 0000 		sts RC_Quality,r24
3189
 3198               	/* epilogue start */
3190
 3199 1db4 1F91      		pop r17
3191
 3200 1db6 0F91      		pop r16
3192
 3201 1db8 FF90      		pop r15
3193
 3202 1dba EF90      		pop r14
3194
 3203 1dbc DF90      		pop r13
3195
 3204 1dbe CF90      		pop r12
3196
 3205 1dc0 BF90      		pop r11
3197
 3206 1dc2 AF90      		pop r10
3198
 3207 1dc4 9F90      		pop r9
3199
 3208 1dc6 8F90      		pop r8
3200
 3209 1dc8 7F90      		pop r7
3201
 3210 1dca 6F90      		pop r6
3202
 3211 1dcc 5F90      		pop r5
3203
 3212 1dce 4F90      		pop r4
3204
 3213 1dd0 3F90      		pop r3
3205
 3214 1dd2 2F90      		pop r2
3206
 3215 1dd4 0895      		ret
3207
 3216               	.L371:
3208
 3217 1dd6 0E94 0000 		call SearchAirPressureOffset
3209
 3218 1dda 00C0      		rjmp .L370
3210
 3219               	.L373:
3211
 3220 1ddc 84E0      		ldi r24,lo8(4)
3212
 3221 1dde 90E0      		ldi r25,hi8(4)
3213
 3222 1de0 0E94 0000 		call GetParamWord
3214
 3223 1de4 9093 0000 		sts (AdBiasAccNick)+1,r25
3215
 3224 1de8 8093 0000 		sts AdBiasAccNick,r24
3216
 3225 1dec 86E0      		ldi r24,lo8(6)
3217
 3226 1dee 90E0      		ldi r25,hi8(6)
3218
 3227 1df0 0E94 0000 		call GetParamWord
3219
 3228 1df4 9093 0000 		sts (AdBiasAccRoll)+1,r25
3220
 3229 1df8 8093 0000 		sts AdBiasAccRoll,r24
3221
 3230 1dfc 88E0      		ldi r24,lo8(8)
3222
 3231 1dfe 90E0      		ldi r25,hi8(8)
3223
 3232 1e00 0E94 0000 		call GetParamWord
3224
 3233 1e04 AA27      		clr r26
3225
 3234 1e06 97FD      		sbrc r25,7
3226
 3235 1e08 A095      		com r26
3227
 3236 1e0a BA2F      		mov r27,r26
3228
 3237 1e0c BC01      		movw r22,r24
3229
 3238 1e0e CD01      		movw r24,r26
3230
 3239 1e10 0E94 0000 		call __floatsisf
3231
 3240 1e14 6093 0000 		sts AdBiasAccTop,r22
3232
 3241 1e18 7093 0000 		sts (AdBiasAccTop)+1,r23
3233
 3242 1e1c 8093 0000 		sts (AdBiasAccTop)+2,r24
3234
 3243 1e20 9093 0000 		sts (AdBiasAccTop)+3,r25
3235
 3244 1e24 8091 0000 		lds r24,AdBiasAccNick
3236
 3245 1e28 9091 0000 		lds r25,(AdBiasAccNick)+1
3237
 3246 1e2c 8150      		subi r24,lo8(2049)
3238
 3247 1e2e 9840      		sbci r25,hi8(2049)
3239
 3248 1e30 04F4      		brge .L379
3240
 3249 1e32 8091 0000 		lds r24,AdBiasAccRoll
3241
 3250 1e36 9091 0000 		lds r25,(AdBiasAccRoll)+1
3242
 3251 1e3a 8150      		subi r24,lo8(2049)
3243
 3252 1e3c 9840      		sbci r25,hi8(2049)
3244
 3253 1e3e 04F0      		brlt .L388
3245
 3254               	.L379:
3246
 3255 1e40 00D0      		rcall .
3247
 3256 1e42 0F92      		push __tmp_reg__
3248
 3257 1e44 EDB7      		in r30,__SP_L__
3249
 3258 1e46 FEB7      		in r31,__SP_H__
3250
 3259 1e48 1182      		std Z+1,__zero_reg__
3251
 3260 1e4a 80E0      		ldi r24,lo8(__c.1965)
3252
 3261 1e4c 90E0      		ldi r25,hi8(__c.1965)
3253
 3262 1e4e 9383      		std Z+3,r25
3254
 3263 1e50 8283      		std Z+2,r24
3255
 3264 1e52 0E94 0000 		call _printf_P
3256
 3265 1e56 80E0      		ldi r24,lo8(1024)
3257
 3266 1e58 94E0      		ldi r25,hi8(1024)
3258
 3267 1e5a 9093 0000 		sts (AdBiasAccNick)+1,r25
3259
 3268 1e5e 8093 0000 		sts AdBiasAccNick,r24
3260
 3269 1e62 9093 0000 		sts (AdBiasAccRoll)+1,r25
3261
 3270 1e66 8093 0000 		sts AdBiasAccRoll,r24
3262
 3271 1e6a 80E0      		ldi r24,lo8(0x44354000)
3263
 3272 1e6c 90E4      		ldi r25,hi8(0x44354000)
3264
 3273 1e6e A5E3      		ldi r26,hlo8(0x44354000)
3265
 3274 1e70 B4E4      		ldi r27,hhi8(0x44354000)
3266
 3275 1e72 8093 0000 		sts AdBiasAccTop,r24
3267
 3276 1e76 9093 0000 		sts (AdBiasAccTop)+1,r25
3268
 3277 1e7a A093 0000 		sts (AdBiasAccTop)+2,r26
3269
 3278 1e7e B093 0000 		sts (AdBiasAccTop)+3,r27
3270
 3279 1e82 0F90      		pop __tmp_reg__
3271
 3280 1e84 0F90      		pop __tmp_reg__
3272
 3281 1e86 0F90      		pop __tmp_reg__
3273
 3282 1e88 00C0      		rjmp .L378
3274
 3283               	.L388:
3275
 3284 1e8a 6091 0000 		lds r22,AdBiasAccTop
3276
 3285 1e8e 7091 0000 		lds r23,(AdBiasAccTop)+1
3277
 3286 1e92 8091 0000 		lds r24,(AdBiasAccTop)+2
3278
 3287 1e96 9091 0000 		lds r25,(AdBiasAccTop)+3
3279
 3288 1e9a 20E0      		ldi r18,lo8(0x44800000)
3280
 3289 1e9c 30E0      		ldi r19,hi8(0x44800000)
3281
 3290 1e9e 40E8      		ldi r20,hlo8(0x44800000)
3282
 3291 1ea0 54E4      		ldi r21,hhi8(0x44800000)
3283
 3292 1ea2 0E94 0000 		call __gtsf2
3284
 3293 1ea6 1816      		cp __zero_reg__,r24
3285
 3294 1ea8 04F0      		brlt .L379
3286
 3295 1eaa 00C0      		rjmp .L378
3287
 3296               	.L384:
3288
 3297 1eac 0E94 0000 		call SearchDacGyroOffset
3289
 3298 1eb0 00C0      		rjmp .L369
3290
 3299               	.L385:
3291
 3300 1eb2 9095      		com r25
3292
 3301 1eb4 8195      		neg r24
3293
 3302 1eb6 9F4F      		sbci r25,lo8(-1)
3294
 3303 1eb8 00C0      		rjmp .L375
3295
 3304               	.L386:
3296
 3305 1eba 9095      		com r25
3297
 3306 1ebc 8195      		neg r24
3298
 3307 1ebe 9F4F      		sbci r25,lo8(-1)
3299
 3308 1ec0 00C0      		rjmp .L376
3300
 3309               	.L387:
3301
 3310 1ec2 9095      		com r25
3302
 3311 1ec4 8195      		neg r24
3303
 3312 1ec6 9F4F      		sbci r25,lo8(-1)
3304
 3313 1ec8 00C0      		rjmp .L377
3305
 3315               	.global	Beep
3306
 3317               	Beep:
3307
 3318 1eca EF92      		push r14
3308
 3319 1ecc FF92      		push r15
3309
 3320 1ece 1F93      		push r17
3310
 3321 1ed0 CF93      		push r28
3311
 3322 1ed2 DF93      		push r29
3312
 3323               	/* prologue: function */
3313
 3324               	/* frame size = 0 */
3314
 3325 1ed4 182F      		mov r17,r24
3315
 3326 1ed6 7B01      		movw r14,r22
3316
 3327 1ed8 8091 0000 		lds r24,MKFlags
3317
 3328 1edc 80FD      		sbrc r24,0
3318
 3329 1ede 00C0      		rjmp .L392
3319
 3330 1ee0 1123      		tst r17
3320
 3331 1ee2 01F0      		breq .L392
3321
 3332 1ee4 EB01      		movw r28,r22
3322
 3333 1ee6 CC0F      		lsl r28
3323
 3334 1ee8 DD1F      		rol r29
3324
 3335               	.L391:
3325
 3336 1eea F092 0000 		sts (BeepTime)+1,r15
3326
 3337 1eee E092 0000 		sts BeepTime,r14
3327
 3338 1ef2 CE01      		movw r24,r28
3328
 3339 1ef4 0E94 0000 		call Delay_ms
3329
 3340 1ef8 1150      		subi r17,lo8(-(-1))
3330
 3341 1efa 01F4      		brne .L391
3331
 3342               	.L392:
3332
 3343               	/* epilogue start */
3333
 3344 1efc DF91      		pop r29
3334
 3345 1efe CF91      		pop r28
3335
 3346 1f00 1F91      		pop r17
3336
 3347 1f02 FF90      		pop r15
3337
 3348 1f04 EF90      		pop r14
3338
 3349 1f06 0895      		ret
3339
 3351               	.global	SetCompassCalState
3340
 3353               	SetCompassCalState:
3341
 3354               	/* prologue: function */
3342
 3355               	/* frame size = 0 */
3343
 3356 1f08 8091 0000 		lds r24,ParamSet
3344
 3357 1f0c A82F      		mov r26,r24
3345
 3358 1f0e B0E0      		ldi r27,lo8(0)
3346
 3359 1f10 AA0F      		lsl r26
3347
 3360 1f12 BB1F      		rol r27
3348
 3361 1f14 FD01      		movw r30,r26
3349
 3362 1f16 E050      		subi r30,lo8(-(PPM_in))
3350
 3363 1f18 F040      		sbci r31,hi8(-(PPM_in))
3351
 3364 1f1a 8081      		ld r24,Z
3352
 3365 1f1c 9181      		ldd r25,Z+1
3353
 3366 1f1e 8D5E      		subi r24,lo8(-19)
3354
 3367 1f20 9F4F      		sbci r25,hi8(-19)
3355
 3368 1f22 04F0      		brlt .L395
3356
 3369 1f24 1092 0000 		sts stick.2382,__zero_reg__
3357
 3370               	.L395:
3358
 3371 1f28 A050      		subi r26,lo8(-(PPM_in))
3359
 3372 1f2a B040      		sbci r27,hi8(-(PPM_in))
3360
 3373 1f2c 8D91      		ld r24,X+
3361
 3374 1f2e 9C91      		ld r25,X
3362
 3375 1f30 8A5B      		subi r24,lo8(-70)
3363
 3376 1f32 9F4F      		sbci r25,hi8(-70)
3364
 3377 1f34 04F4      		brge .L398
3365
 3378 1f36 8091 0000 		lds r24,stick.2382
3366
 3379 1f3a 8823      		tst r24
3367
 3380 1f3c 01F4      		brne .L398
3368
 3381 1f3e 81E0      		ldi r24,lo8(1)
3369
 3382 1f40 8093 0000 		sts stick.2382,r24
3370
 3383 1f44 8091 0000 		lds r24,CompassCalState
3371
 3384 1f48 8F5F      		subi r24,lo8(-(1))
3372
 3385 1f4a 8093 0000 		sts CompassCalState,r24
3373
 3386 1f4e 8530      		cpi r24,lo8(5)
3374
 3387 1f50 00F0      		brlo .L399
3375
 3388 1f52 88EE      		ldi r24,lo8(1000)
3376
 3389 1f54 93E0      		ldi r25,hi8(1000)
3377
 3390 1f56 9093 0000 		sts (BeepTime)+1,r25
3378
 3391 1f5a 8093 0000 		sts BeepTime,r24
3379
 3392               	.L398:
3380
 3393 1f5e 0895      		ret
3381
 3394               	.L399:
3382
 3395 1f60 66E9      		ldi r22,lo8(150)
3383
 3396 1f62 70E0      		ldi r23,hi8(150)
3384
 3397 1f64 0E94 0000 		call Beep
3385
 3398 1f68 0895      		ret
3386
 3400               	.global	MotorControl
3387
 3402               	MotorControl:
3388
 3403 1f6a 2F92      		push r2
3389
 3404 1f6c 3F92      		push r3
3390
 3405 1f6e 4F92      		push r4
3391
 3406 1f70 5F92      		push r5
3392
 3407 1f72 6F92      		push r6
3393
 3408 1f74 7F92      		push r7
3394
 3409 1f76 8F92      		push r8
3395
 3410 1f78 9F92      		push r9
3396
 3411 1f7a AF92      		push r10
3397
 3412 1f7c BF92      		push r11
3398
 3413 1f7e CF92      		push r12
3399
 3414 1f80 DF92      		push r13
3400
 3415 1f82 EF92      		push r14
3401
 3416 1f84 FF92      		push r15
3402
 3417 1f86 0F93      		push r16
3403
 3418 1f88 1F93      		push r17
3404
 3419 1f8a DF93      		push r29
3405
 3420 1f8c CF93      		push r28
3406
 3421 1f8e CDB7      		in r28,__SP_L__
3407
 3422 1f90 DEB7      		in r29,__SP_H__
3408
 3423 1f92 E997      		sbiw r28,57
3409
 3424 1f94 0FB6      		in __tmp_reg__,__SREG__
3410
 3425 1f96 F894      		cli
3411
 3426 1f98 DEBF      		out __SP_H__,r29
3412
 3427 1f9a 0FBE      		out __SREG__,__tmp_reg__
3413
 3428 1f9c CDBF      		out __SP_L__,r28
3414
 3429               	/* prologue: function */
3415
 3430               	/* frame size = 57 */
3416
 3431 1f9e 0E94 0000 		call Mean
3417
 3432 1fa2 D090 0000 		lds r13,BoardRelease
3418
 3433 1fa6 BBE0      		ldi r27,lo8(11)
3419
 3434 1fa8 AB2E      		mov r10,r27
3420
 3435 1faa AD14      		cp r10,r13
3421
 3436 1fac 00F4      		brsh .+2
3422
 3437 1fae 00C0      		rjmp .L401
3423
 3438 1fb0 299A      		sbi 37-32,1
3424
 3439 1fb2 8091 0000 		lds r24,RC_Quality
3425
 3440 1fb6 8436      		cpi r24,lo8(100)
3426
 3441 1fb8 00F0      		brlo .+2
3427
 3442 1fba 00C0      		rjmp .L403
3428
 3443               	.L767:
3429
 3444 1fbc 8091 0000 		lds r24,RcLostTimer.2415
3430
 3445 1fc0 9091 0000 		lds r25,(RcLostTimer.2415)+1
3431
 3446 1fc4 0097      		sbiw r24,0
3432
 3447 1fc6 01F0      		breq .+4
3433
 3448 1fc8 0C94 0000 		jmp .L732
3434
 3449 1fcc 8091 0000 		lds r24,MKFlags
3435
 3450 1fd0 8E7E      		andi r24,lo8(-18)
3436
 3451 1fd2 8093 0000 		sts MKFlags,r24
3437
 3452 1fd6 AAE0      		ldi r26,lo8(10)
3438
 3453 1fd8 BA2E      		mov r11,r26
3439
 3454 1fda DB14      		cp r13,r11
3440
 3455 1fdc 01F4      		brne .+4
3441
 3456 1fde 0C94 0000 		jmp .L406
3442
 3457               	.L791:
3443
 3458 1fe2 F4E1      		ldi r31,lo8(20)
3444
 3459 1fe4 CF2E      		mov r12,r31
3445
 3460 1fe6 DC14      		cp r13,r12
3446
 3461 1fe8 01F4      		brne .+4
3447
 3462 1fea 0C94 0000 		jmp .L406
3448
 3463 1fee 2898      		cbi 37-32,0
3449
 3464               	.L408:
3450
 3465 1ff0 8091 0000 		lds r24,ModelIsFlying
3451
 3466 1ff4 9091 0000 		lds r25,(ModelIsFlying)+1
3452
 3467 1ff8 895E      		subi r24,lo8(1001)
3453
 3468 1ffa 9340      		sbci r25,hi8(1001)
3454
 3469 1ffc 00F4      		brsh .+4
3455
 3470 1ffe 0C94 0000 		jmp .L727
3456
 3471 2002 8091 0000 		lds r24,MKFlags
3457
 3472 2006 8061      		ori r24,lo8(16)
3458
 3473 2008 8093 0000 		sts MKFlags,r24
3459
 3474 200c E091 0000 		lds r30,ParamSet
3460
 3475 2010 2E2F      		mov r18,r30
3461
 3476 2012 30E0      		ldi r19,lo8(0)
3462
 3477 2014 220F      		lsl r18
3463
 3478 2016 331F      		rol r19
3464
 3479 2018 F901      		movw r30,r18
3465
 3480 201a E050      		subi r30,lo8(-(PPM_diff))
3466
 3481 201c F040      		sbci r31,hi8(-(PPM_diff))
3467
 3482 201e 1182      		std Z+1,__zero_reg__
3468
 3483 2020 1082      		st Z,__zero_reg__
3469
 3484 2022 A091 0000 		lds r26,ParamSet+1
3470
 3485 2026 B0E0      		ldi r27,lo8(0)
3471
 3486 2028 AA0F      		lsl r26
3472
 3487 202a BB1F      		rol r27
3473
 3488 202c FD01      		movw r30,r26
3474
 3489 202e E050      		subi r30,lo8(-(PPM_diff))
3475
 3490 2030 F040      		sbci r31,hi8(-(PPM_diff))
3476
 3491 2032 1182      		std Z+1,__zero_reg__
3477
 3492 2034 1082      		st Z,__zero_reg__
3478
 3493 2036 E091 0000 		lds r30,ParamSet+3
3479
 3494 203a 4E2F      		mov r20,r30
3480
 3495 203c 50E0      		ldi r21,lo8(0)
3481
 3496 203e 440F      		lsl r20
3482
 3497 2040 551F      		rol r21
3483
 3498 2042 CA01      		movw r24,r20
3484
 3499 2044 8050      		subi r24,lo8(-(PPM_diff))
3485
 3500 2046 9040      		sbci r25,hi8(-(PPM_diff))
3486
 3501 2048 FC01      		movw r30,r24
3487
 3502 204a 1182      		std Z+1,__zero_reg__
3488
 3503 204c 1082      		st Z,__zero_reg__
3489
 3504 204e 2050      		subi r18,lo8(-(PPM_in))
3490
 3505 2050 3040      		sbci r19,hi8(-(PPM_in))
3491
 3506 2052 F901      		movw r30,r18
3492
 3507 2054 1182      		std Z+1,__zero_reg__
3493
 3508 2056 1082      		st Z,__zero_reg__
3494
 3509 2058 A050      		subi r26,lo8(-(PPM_in))
3495
 3510 205a B040      		sbci r27,hi8(-(PPM_in))
3496
 3511 205c 1196      		adiw r26,1
3497
 3512 205e 1C92      		st X,__zero_reg__
3498
 3513 2060 1E92      		st -X,__zero_reg__
3499
 3514 2062 4050      		subi r20,lo8(-(PPM_in))
3500
 3515 2064 5040      		sbci r21,hi8(-(PPM_in))
3501
 3516 2066 DA01      		movw r26,r20
3502
 3517 2068 1196      		adiw r26,1
3503
 3518 206a 1C92      		st X,__zero_reg__
3504
 3519 206c 1E92      		st -X,__zero_reg__
3505
 3520               	.L410:
3506
 3521 206e 8091 0000 		lds r24,NewPpmData
3507
 3522 2072 8150      		subi r24,lo8(-(-1))
3508
 3523 2074 8093 0000 		sts NewPpmData,r24
3509
 3524 2078 8F5F      		subi r24,lo8(-(1))
3510
 3525 207a 01F0      		breq .L471
3511
 3526 207c 8091 0000 		lds r24,MKFlags
3512
 3527 2080 84FF      		sbrs r24,4
3513
 3528 2082 00C0      		rjmp .L472
3514
 3529               	.L471:
3515
 3530 2084 0E94 0000 		call ParameterMapping
3516
 3531 2088 E091 0000 		lds r30,ParamSet
3517
 3532 208c F0E0      		ldi r31,lo8(0)
3518
 3533 208e EE0F      		lsl r30
3519
 3534 2090 FF1F      		rol r31
3520
 3535 2092 E050      		subi r30,lo8(-(PPM_in))
3521
 3536 2094 F040      		sbci r31,hi8(-(PPM_in))
3522
 3537 2096 2081      		ld r18,Z
3523
 3538 2098 3181      		ldd r19,Z+1
3524
 3539 209a 8091 0000 		lds r24,ParamSet+18
3525
 3540 209e 90E0      		ldi r25,lo8(0)
3526
 3541 20a0 289F      		mul r18,r24
3527
 3542 20a2 A001      		movw r20,r0
3528
 3543 20a4 299F      		mul r18,r25
3529
 3544 20a6 500D      		add r21,r0
3530
 3545 20a8 389F      		mul r19,r24
3531
 3546 20aa 500D      		add r21,r0
3532
 3547 20ac 1124      		clr r1
3533
 3548 20ae 2091 0000 		lds r18,stick_nick.2424
3534
 3549 20b2 3091 0000 		lds r19,(stick_nick.2424)+1
3535
 3550 20b6 C901      		movw r24,r18
3536
 3551 20b8 880F      		lsl r24
3537
 3552 20ba 991F      		rol r25
3538
 3553 20bc 820F      		add r24,r18
3539
 3554 20be 931F      		adc r25,r19
3540
 3555 20c0 480F      		add r20,r24
3541
 3556 20c2 591F      		adc r21,r25
3542
 3557 20c4 57FF      		sbrs r21,7
3543
 3558 20c6 00C0      		rjmp .+4
3544
 3559 20c8 0C94 0000 		jmp .L733
3545
 3560               	.L473:
3546
 3561 20cc 5595      		asr r21
3547
 3562 20ce 4795      		ror r20
3548
 3563 20d0 5595      		asr r21
3549
 3564 20d2 4795      		ror r20
3550
 3565 20d4 5093 0000 		sts (stick_nick.2424)+1,r21
3551
 3566 20d8 4093 0000 		sts stick_nick.2424,r20
3552
 3567 20dc E091 0000 		lds r30,ParamSet
3553
 3568 20e0 F0E0      		ldi r31,lo8(0)
3554
 3569 20e2 EE0F      		lsl r30
3555
 3570 20e4 FF1F      		rol r31
3556
 3571 20e6 E050      		subi r30,lo8(-(PPM_diff))
3557
 3572 20e8 F040      		sbci r31,hi8(-(PPM_diff))
3558
 3573 20ea 2081      		ld r18,Z
3559
 3574 20ec 3181      		ldd r19,Z+1
3560
 3575 20ee 8091 0000 		lds r24,ParamSet+19
3561
 3576 20f2 90E0      		ldi r25,lo8(0)
3562
 3577 20f4 BC01      		movw r22,r24
3563
 3578 20f6 269F      		mul r18,r22
3564
 3579 20f8 C001      		movw r24,r0
3565
 3580 20fa 279F      		mul r18,r23
3566
 3581 20fc 900D      		add r25,r0
3567
 3582 20fe 369F      		mul r19,r22
3568
 3583 2100 900D      		add r25,r0
3569
 3584 2102 1124      		clr r1
3570
 3585 2104 840F      		add r24,r20
3571
 3586 2106 951F      		adc r25,r21
3572
 3587 2108 9093 0000 		sts (stick_nick.2424)+1,r25
3573
 3588 210c 8093 0000 		sts stick_nick.2424,r24
3574
 3589 2110 2091 0000 		lds r18,GPSStickNick
3575
 3590 2114 3091 0000 		lds r19,(GPSStickNick)+1
3576
 3591 2118 821B      		sub r24,r18
3577
 3592 211a 930B      		sbc r25,r19
3578
 3593 211c 9093 0000 		sts (StickNick)+1,r25
3579
 3594 2120 8093 0000 		sts StickNick,r24
3580
 3595 2124 E091 0000 		lds r30,ParamSet+1
3581
 3596 2128 F0E0      		ldi r31,lo8(0)
3582
 3597 212a EE0F      		lsl r30
3583
 3598 212c FF1F      		rol r31
3584
 3599 212e E050      		subi r30,lo8(-(PPM_in))
3585
 3600 2130 F040      		sbci r31,hi8(-(PPM_in))
3586
 3601 2132 2081      		ld r18,Z
3587
 3602 2134 3181      		ldd r19,Z+1
3588
 3603 2136 8091 0000 		lds r24,ParamSet+18
3589
 3604 213a 90E0      		ldi r25,lo8(0)
3590
 3605 213c 289F      		mul r18,r24
3591
 3606 213e A001      		movw r20,r0
3592
 3607 2140 299F      		mul r18,r25
3593
 3608 2142 500D      		add r21,r0
3594
 3609 2144 389F      		mul r19,r24
3595
 3610 2146 500D      		add r21,r0
3596
 3611 2148 1124      		clr r1
3597
 3612 214a 2091 0000 		lds r18,stick_roll.2425
3598
 3613 214e 3091 0000 		lds r19,(stick_roll.2425)+1
3599
 3614 2152 C901      		movw r24,r18
3600
 3615 2154 880F      		lsl r24
3601
 3616 2156 991F      		rol r25
3602
 3617 2158 820F      		add r24,r18
3603
 3618 215a 931F      		adc r25,r19
3604
 3619 215c 480F      		add r20,r24
3605
 3620 215e 591F      		adc r21,r25
3606
 3621 2160 57FF      		sbrs r21,7
3607
 3622 2162 00C0      		rjmp .+4
3608
 3623 2164 0C94 0000 		jmp .L734
3609
 3624               	.L474:
3610
 3625 2168 5595      		asr r21
3611
 3626 216a 4795      		ror r20
3612
 3627 216c 5595      		asr r21
3613
 3628 216e 4795      		ror r20
3614
 3629 2170 5093 0000 		sts (stick_roll.2425)+1,r21
3615
 3630 2174 4093 0000 		sts stick_roll.2425,r20
3616
 3631 2178 E091 0000 		lds r30,ParamSet+1
3617
 3632 217c F0E0      		ldi r31,lo8(0)
3618
 3633 217e EE0F      		lsl r30
3619
 3634 2180 FF1F      		rol r31
3620
 3635 2182 E050      		subi r30,lo8(-(PPM_diff))
3621
 3636 2184 F040      		sbci r31,hi8(-(PPM_diff))
3622
 3637 2186 2081      		ld r18,Z
3623
 3638 2188 3181      		ldd r19,Z+1
3624
 3639 218a 8091 0000 		lds r24,ParamSet+19
3625
 3640 218e 90E0      		ldi r25,lo8(0)
3626
 3641 2190 DC01      		movw r26,r24
3627
 3642 2192 2A9F      		mul r18,r26
3628
 3643 2194 C001      		movw r24,r0
3629
 3644 2196 2B9F      		mul r18,r27
3630
 3645 2198 900D      		add r25,r0
3631
 3646 219a 3A9F      		mul r19,r26
3632
 3647 219c 900D      		add r25,r0
3633
 3648 219e 1124      		clr r1
3634
 3649 21a0 840F      		add r24,r20
3635
 3650 21a2 951F      		adc r25,r21
3636
 3651 21a4 9093 0000 		sts (stick_roll.2425)+1,r25
3637
 3652 21a8 8093 0000 		sts stick_roll.2425,r24
3638
 3653 21ac 2091 0000 		lds r18,GPSStickRoll
3639
 3654 21b0 3091 0000 		lds r19,(GPSStickRoll)+1
3640
 3655 21b4 821B      		sub r24,r18
3641
 3656 21b6 930B      		sbc r25,r19
3642
 3657 21b8 9093 0000 		sts (StickRoll)+1,r25
3643
 3658 21bc 8093 0000 		sts StickRoll,r24
3644
 3659 21c0 E091 0000 		lds r30,ParamSet+3
3645
 3660 21c4 F0E0      		ldi r31,lo8(0)
3646
 3661 21c6 EE0F      		lsl r30
3647
 3662 21c8 FF1F      		rol r31
3648
 3663 21ca E050      		subi r30,lo8(-(PPM_in))
3649
 3664 21cc F040      		sbci r31,hi8(-(PPM_in))
3650
 3665 21ce 8081      		ld r24,Z
3651
 3666 21d0 9181      		ldd r25,Z+1
3652
 3667 21d2 2227      		clr r18
3653
 3668 21d4 3327      		clr r19
3654
 3669 21d6 281B      		sub r18,r24
3655
 3670 21d8 390B      		sbc r19,r25
3656
 3671 21da 3093 0000 		sts (StickYaw)+1,r19
3657
 3672 21de 2093 0000 		sts StickYaw,r18
3658
 3673 21e2 8091 0000 		lds r24,ParamSet+8
3659
 3674 21e6 8872      		andi r24,lo8(40)
3660
 3675 21e8 01F0      		breq .L475
3661
 3676 21ea 2330      		cpi r18,3
3662
 3677 21ec 3105      		cpc r19,__zero_reg__
3663
 3678 21ee 04F0      		brlt .+4
3664
 3679 21f0 0C94 0000 		jmp .L735
3665
 3680 21f4 BFEF      		ldi r27,hi8(-2)
3666
 3681 21f6 2E3F      		cpi r18,lo8(-2)
3667
 3682 21f8 3B07      		cpc r19,r27
3668
 3683 21fa 04F0      		brlt .+4
3669
 3684 21fc 0C94 0000 		jmp .L477
3670
 3685 2200 2E5F      		subi r18,lo8(-(2))
3671
 3686 2202 3F4F      		sbci r19,hi8(-(2))
3672
 3687 2204 3093 0000 		sts (StickYaw)+1,r19
3673
 3688 2208 2093 0000 		sts StickYaw,r18
3674
 3689               	.L475:
3675
 3690 220c E091 0000 		lds r30,ParamSet+2
3676
 3691 2210 F0E0      		ldi r31,lo8(0)
3677
 3692 2212 EE0F      		lsl r30
3678
 3693 2214 FF1F      		rol r31
3679
 3694 2216 E050      		subi r30,lo8(-(PPM_in))
3680
 3695 2218 F040      		sbci r31,hi8(-(PPM_in))
3681
 3696 221a 0190      		ld __tmp_reg__,Z+
3682
 3697 221c F081      		ld r31,Z
3683
 3698 221e E02D      		mov r30,__tmp_reg__
3684
 3699 2220 E858      		subi r30,lo8(-(120))
3685
 3700 2222 FF4F      		sbci r31,hi8(-(120))
3686
 3701 2224 F093 0000 		sts (StickGas)+1,r31
3687
 3702 2228 E093 0000 		sts StickGas,r30
3688
 3703 222c 8091 0000 		lds r24,FCParam+7
3689
 3704 2230 865F      		subi r24,lo8(-(10))
3690
 3705 2232 8093 0000 		sts GyroPFactor,r24
3691
 3706 2236 8091 0000 		lds r24,FCParam+8
3692
 3707 223a 8093 0000 		sts GyroIFactor,r24
3693
 3708 223e 8091 0000 		lds r24,FCParam+9
3694
 3709 2242 865F      		subi r24,lo8(-(10))
3695
 3710 2244 8093 0000 		sts GyroYawPFactor,r24
3696
 3711 2248 8091 0000 		lds r24,FCParam+10
3697
 3712 224c 8093 0000 		sts GyroYawIFactor,r24
3698
 3713 2250 8091 0000 		lds r24,ExternControl+10
3699
 3714 2254 80FF      		sbrs r24,0
3700
 3715 2256 00C0      		rjmp .L478
3701
 3716 2258 8091 0000 		lds r24,FCParam+28
3702
 3717 225c 8138      		cpi r24,lo8(-127)
3703
 3718 225e 00F4      		brsh .+2
3704
 3719 2260 00C0      		rjmp .L478
3705
 3720 2262 2091 0000 		lds r18,ParamSet+18
3706
 3721 2266 30E0      		ldi r19,lo8(0)
3707
 3722 2268 8091 0000 		lds r24,ExternControl+3
3708
 3723 226c 9927      		clr r25
3709
 3724 226e 87FD      		sbrc r24,7
3710
 3725 2270 9095      		com r25
3711
 3726 2272 289F      		mul r18,r24
3712
 3727 2274 A001      		movw r20,r0
3713
 3728 2276 299F      		mul r18,r25
3714
 3729 2278 500D      		add r21,r0
3715
 3730 227a 389F      		mul r19,r24
3716
 3731 227c 500D      		add r21,r0
3717
 3732 227e 1124      		clr r1
3718
 3733 2280 8091 0000 		lds r24,StickNick
3719
 3734 2284 9091 0000 		lds r25,(StickNick)+1
3720
 3735 2288 840F      		add r24,r20
3721
 3736 228a 951F      		adc r25,r21
3722
 3737 228c 9093 0000 		sts (StickNick)+1,r25
3723
 3738 2290 8093 0000 		sts StickNick,r24
3724
 3739 2294 8091 0000 		lds r24,ExternControl+4
3725
 3740 2298 9927      		clr r25
3726
 3741 229a 87FD      		sbrc r24,7
3727
 3742 229c 9095      		com r25
3728
 3743 229e 5901      		movw r10,r18
3729
 3744 22a0 A89E      		mul r10,r24
3730
 3745 22a2 9001      		movw r18,r0
3731
 3746 22a4 A99E      		mul r10,r25
3732
 3747 22a6 300D      		add r19,r0
3733
 3748 22a8 B89E      		mul r11,r24
3734
 3749 22aa 300D      		add r19,r0
3735
 3750 22ac 1124      		clr r1
3736
 3751 22ae 8091 0000 		lds r24,StickRoll
3737
 3752 22b2 9091 0000 		lds r25,(StickRoll)+1
3738
 3753 22b6 820F      		add r24,r18
3739
 3754 22b8 931F      		adc r25,r19
3740
 3755 22ba 9093 0000 		sts (StickRoll)+1,r25
3741
 3756 22be 8093 0000 		sts StickRoll,r24
3742
 3757 22c2 2091 0000 		lds r18,ExternControl+5
3743
 3758 22c6 3327      		clr r19
3744
 3759 22c8 27FD      		sbrc r18,7
3745
 3760 22ca 3095      		com r19
3746
 3761 22cc 8091 0000 		lds r24,StickYaw
3747
 3762 22d0 9091 0000 		lds r25,(StickYaw)+1
3748
 3763 22d4 820F      		add r24,r18
3749
 3764 22d6 931F      		adc r25,r19
3750
 3765 22d8 9093 0000 		sts (StickYaw)+1,r25
3751
 3766 22dc 8093 0000 		sts StickYaw,r24
3752
 3767 22e0 2091 0000 		lds r18,ParamSet+13
3753
 3768 22e4 30E0      		ldi r19,lo8(0)
3754
 3769 22e6 8091 0000 		lds r24,ExternControl+7
3755
 3770 22ea 9927      		clr r25
3756
 3771 22ec 87FD      		sbrc r24,7
3757
 3772 22ee 9095      		com r25
3758
 3773 22f0 6C01      		movw r12,r24
3759
 3774 22f2 2C9D      		mul r18,r12
3760
 3775 22f4 C001      		movw r24,r0
3761
 3776 22f6 2D9D      		mul r18,r13
3762
 3777 22f8 900D      		add r25,r0
3763
 3778 22fa 3C9D      		mul r19,r12
3764
 3779 22fc 900D      		add r25,r0
3765
 3780 22fe 1124      		clr r1
3766
 3781 2300 9093 0000 		sts (ExternHeightValue)+1,r25
3767
 3782 2304 8093 0000 		sts ExternHeightValue,r24
3768
 3783 2308 8091 0000 		lds r24,ExternControl+6
3769
 3784 230c 90E0      		ldi r25,lo8(0)
3770
 3785 230e 8E17      		cp r24,r30
3771
 3786 2310 9F07      		cpc r25,r31
3772
 3787 2312 04F4      		brge .L478
3773
 3788 2314 9093 0000 		sts (StickGas)+1,r25
3774
 3789 2318 8093 0000 		sts StickGas,r24
3775
 3790               	.L478:
3776
 3791 231c 8091 0000 		lds r24,StickGas
3777
 3792 2320 9091 0000 		lds r25,(StickGas)+1
3778
 3793 2324 97FF      		sbrs r25,7
3779
 3794 2326 00C0      		rjmp .+4
3780
 3795 2328 0C94 0000 		jmp .L736
3781
 3796 232c 8091 0000 		lds r24,ParamSet+8
3782
 3797 2330 82FF      		sbrs r24,2
3783
 3798 2332 00C0      		rjmp .+4
3784
 3799 2334 0C94 0000 		jmp .L737
3785
 3800               	.L480:
3786
 3801 2338 6091 0000 		lds r22,StickNick
3787
 3802 233c 7091 0000 		lds r23,StickNick+1
3788
 3803 2340 4091 0000 		lds r20,MaxStickNick
3789
 3804 2344 5091 0000 		lds r21,(MaxStickNick)+1
3790
 3805 2348 CB01      		movw r24,r22
3791
 3806 234a 9C01      		movw r18,r24
3792
 3807 234c 97FF      		sbrs r25,7
3793
 3808 234e 00C0      		rjmp .+4
3794
 3809 2350 0C94 0000 		jmp .L738
3795
 3810               	.L482:
3796
 3811 2354 3595      		asr r19
3797
 3812 2356 2795      		ror r18
3798
 3813 2358 3595      		asr r19
3799
 3814 235a 2795      		ror r18
3800
 3815 235c 37FF      		sbrs r19,7
3801
 3816 235e 00C0      		rjmp .+4
3802
 3817 2360 0C94 0000 		jmp .L739
3803
 3818               	.L483:
3804
 3819 2364 4217      		cp r20,r18
3805
 3820 2366 5307      		cpc r21,r19
3806
 3821 2368 04F4      		brge .+4
3807
 3822 236a 0C94 0000 		jmp .L740
3808
 3823 236e 4150      		subi r20,lo8(-(-1))
3809
 3824 2370 5040      		sbci r21,hi8(-(-1))
3810
 3825 2372 5093 0000 		sts (MaxStickNick)+1,r21
3811
 3826 2376 4093 0000 		sts MaxStickNick,r20
3812
 3827               	.L485:
3813
 3828 237a 6091 0000 		lds r22,StickRoll
3814
 3829 237e 7091 0000 		lds r23,StickRoll+1
3815
 3830 2382 4091 0000 		lds r20,MaxStickRoll
3816
 3831 2386 5091 0000 		lds r21,(MaxStickRoll)+1
3817
 3832 238a CB01      		movw r24,r22
3818
 3833 238c 9C01      		movw r18,r24
3819
 3834 238e 97FF      		sbrs r25,7
3820
 3835 2390 00C0      		rjmp .+4
3821
 3836 2392 0C94 0000 		jmp .L741
3822
 3837               	.L487:
3823
 3838 2396 3595      		asr r19
3824
 3839 2398 2795      		ror r18
3825
 3840 239a 3595      		asr r19
3826
 3841 239c 2795      		ror r18
3827
 3842 239e 37FF      		sbrs r19,7
3828
 3843 23a0 00C0      		rjmp .+4
3829
 3844 23a2 0C94 0000 		jmp .L742
3830
 3845               	.L488:
3831
 3846 23a6 4217      		cp r20,r18
3832
 3847 23a8 5307      		cpc r21,r19
3833
 3848 23aa 04F0      		brlt .+4
3834
 3849 23ac 0C94 0000 		jmp .L486
3835
 3850 23b0 9B01      		movw r18,r22
3836
 3851 23b2 77FF      		sbrs r23,7
3837
 3852 23b4 00C0      		rjmp .+4
3838
 3853 23b6 0C94 0000 		jmp .L743
3839
 3854               	.L489:
3840
 3855 23ba C901      		movw r24,r18
3841
 3856 23bc 9595      		asr r25
3842
 3857 23be 8795      		ror r24
3843
 3858 23c0 9595      		asr r25
3844
 3859 23c2 8795      		ror r24
3845
 3860 23c4 9093 0000 		sts (MaxStickRoll)+1,r25
3846
 3861 23c8 8093 0000 		sts MaxStickRoll,r24
3847
 3862 23cc 8536      		cpi r24,101
3848
 3863 23ce 9105      		cpc r25,__zero_reg__
3849
 3864 23d0 04F0      		brlt .+4
3850
 3865 23d2 0C94 0000 		jmp .L744
3851
 3866               	.L490:
3852
 3867 23d6 8091 0000 		lds r24,ParamSet+1
3853
 3868 23da A82F      		mov r26,r24
3854
 3869 23dc B0E0      		ldi r27,lo8(0)
3855
 3870 23de AA0F      		lsl r26
3856
 3871 23e0 BB1F      		rol r27
3857
 3872 23e2 FD01      		movw r30,r26
3858
 3873 23e4 E050      		subi r30,lo8(-(PPM_in))
3859
 3874 23e6 F040      		sbci r31,hi8(-(PPM_in))
3860
 3875 23e8 2081      		ld r18,Z
3861
 3876 23ea 3181      		ldd r19,Z+1
3862
 3877 23ec 8091 0000 		lds r24,ParamSet+49
3863
 3878 23f0 482F      		mov r20,r24
3864
 3879 23f2 50E0      		ldi r21,lo8(0)
3865
 3880 23f4 4217      		cp r20,r18
3866
 3881 23f6 5307      		cpc r21,r19
3867
 3882 23f8 04F4      		brge .L491
3868
 3883 23fa 8091 0000 		lds r24,ParamSet+86
3869
 3884 23fe 82FF      		sbrs r24,2
3870
 3885 2400 00C0      		rjmp .+4
3871
 3886 2402 0C94 0000 		jmp .L745
3872
 3887               	.L491:
3873
 3888 2406 7091 0000 		lds r23,LoopingLeft
3874
 3889 240a 7723      		tst r23
3875
 3890 240c 01F0      		breq .L492
3876
 3891 240e FD01      		movw r30,r26
3877
 3892 2410 E050      		subi r30,lo8(-(PPM_in))
3878
 3893 2412 F040      		sbci r31,hi8(-(PPM_in))
3879
 3894 2414 2081      		ld r18,Z
3880
 3895 2416 3181      		ldd r19,Z+1
3881
 3896 2418 8091 0000 		lds r24,ParamSet+50
3882
 3897 241c FA01      		movw r30,r20
3883
 3898 241e E81B      		sub r30,r24
3884
 3899 2420 F109      		sbc r31,__zero_reg__
3885
 3900 2422 2E17      		cp r18,r30
3886
 3901 2424 3F07      		cpc r19,r31
3887
 3902 2426 04F4      		brge .+4
3888
 3903 2428 0C94 0000 		jmp .L746
3889
 3904               	.L492:
3890
 3905 242c FD01      		movw r30,r26
3891
 3906 242e E050      		subi r30,lo8(-(PPM_in))
3892
 3907 2430 F040      		sbci r31,hi8(-(PPM_in))
3893
 3908 2432 2081      		ld r18,Z
3894
 3909 2434 3181      		ldd r19,Z+1
3895
 3910 2436 8827      		clr r24
3896
 3911 2438 9927      		clr r25
3897
 3912 243a 841B      		sub r24,r20
3898
 3913 243c 950B      		sbc r25,r21
3899
 3914 243e 2817      		cp r18,r24
3900
 3915 2440 3907      		cpc r19,r25
3901
 3916 2442 04F4      		brge .L493
3902
 3917 2444 8091 0000 		lds r24,ParamSet+86
3903
 3918 2448 83FF      		sbrs r24,3
3904
 3919 244a 00C0      		rjmp .+4
3905
 3920 244c 0C94 0000 		jmp .L747
3906
 3921               	.L493:
3907
 3922 2450 8091 0000 		lds r24,LoopingRight
3908
 3923 2454 8823      		tst r24
3909
 3924 2456 01F0      		breq .L494
3910
 3925 2458 A050      		subi r26,lo8(-(PPM_in))
3911
 3926 245a B040      		sbci r27,hi8(-(PPM_in))
3912
 3927 245c 2D91      		ld r18,X+
3913
 3928 245e 3C91      		ld r19,X
3914
 3929 2460 8091 0000 		lds r24,ParamSet+50
3915
 3930 2464 90E0      		ldi r25,lo8(0)
3916
 3931 2466 841B      		sub r24,r20
3917
 3932 2468 950B      		sbc r25,r21
3918
 3933 246a 8217      		cp r24,r18
3919
 3934 246c 9307      		cpc r25,r19
3920
 3935 246e 04F4      		brge .+4
3921
 3936 2470 0C94 0000 		jmp .L748
3922
 3937               	.L494:
3923
 3938 2474 8091 0000 		lds r24,ParamSet
3924
 3939 2478 A82F      		mov r26,r24
3925
 3940 247a B0E0      		ldi r27,lo8(0)
3926
 3941 247c AA0F      		lsl r26
3927
 3942 247e BB1F      		rol r27
3928
 3943 2480 FD01      		movw r30,r26
3929
 3944 2482 E050      		subi r30,lo8(-(PPM_in))
3930
 3945 2484 F040      		sbci r31,hi8(-(PPM_in))
3931
 3946 2486 8081      		ld r24,Z
3932
 3947 2488 9181      		ldd r25,Z+1
3933
 3948 248a 4817      		cp r20,r24
3934
 3949 248c 5907      		cpc r21,r25
3935
 3950 248e 04F4      		brge .L495
3936
 3951 2490 8091 0000 		lds r24,ParamSet+86
3937
 3952 2494 80FF      		sbrs r24,0
3938
 3953 2496 00C0      		rjmp .+4
3939
 3954 2498 0C94 0000 		jmp .L749
3940
 3955               	.L495:
3941
 3956 249c 6091 0000 		lds r22,LoopingTop
3942
 3957 24a0 6623      		tst r22
3943
 3958 24a2 01F0      		breq .L496
3944
 3959 24a4 FD01      		movw r30,r26
3945
 3960 24a6 E050      		subi r30,lo8(-(PPM_in))
3946
 3961 24a8 F040      		sbci r31,hi8(-(PPM_in))
3947
 3962 24aa 2081      		ld r18,Z
3948
 3963 24ac 3181      		ldd r19,Z+1
3949
 3964 24ae 8091 0000 		lds r24,ParamSet+50
3950
 3965 24b2 5A01      		movw r10,r20
3951
 3966 24b4 A81A      		sub r10,r24
3952
 3967 24b6 B108      		sbc r11,__zero_reg__
3953
 3968 24b8 2A15      		cp r18,r10
3954
 3969 24ba 3B05      		cpc r19,r11
3955
 3970 24bc 04F4      		brge .+4
3956
 3971 24be 0C94 0000 		jmp .L750
3957
 3972               	.L496:
3958
 3973 24c2 FD01      		movw r30,r26
3959
 3974 24c4 E050      		subi r30,lo8(-(PPM_in))
3960
 3975 24c6 F040      		sbci r31,hi8(-(PPM_in))
3961
 3976 24c8 2081      		ld r18,Z
3962
 3977 24ca 3181      		ldd r19,Z+1
3963
 3978 24cc 8827      		clr r24
3964
 3979 24ce 9927      		clr r25
3965
 3980 24d0 841B      		sub r24,r20
3966
 3981 24d2 950B      		sbc r25,r21
3967
 3982 24d4 2817      		cp r18,r24
3968
 3983 24d6 3907      		cpc r19,r25
3969
 3984 24d8 04F4      		brge .L497
3970
 3985 24da 8091 0000 		lds r24,ParamSet+86
3971
 3986 24de 81FF      		sbrs r24,1
3972
 3987 24e0 00C0      		rjmp .+4
3973
 3988 24e2 0C94 0000 		jmp .L751
3974
 3989               	.L497:
3975
 3990 24e6 8091 0000 		lds r24,LoopingDown
3976
 3991 24ea 8823      		tst r24
3977
 3992 24ec 01F0      		breq .L498
3978
 3993 24ee A050      		subi r26,lo8(-(PPM_in))
3979
 3994 24f0 B040      		sbci r27,hi8(-(PPM_in))
3980
 3995 24f2 2D91      		ld r18,X+
3981
 3996 24f4 3C91      		ld r19,X
3982
 3997 24f6 8091 0000 		lds r24,ParamSet+50
3983
 3998 24fa 90E0      		ldi r25,lo8(0)
3984
 3999 24fc 841B      		sub r24,r20
3985
 4000 24fe 950B      		sbc r25,r21
3986
 4001 2500 8217      		cp r24,r18
3987
 4002 2502 9307      		cpc r25,r19
3988
 4003 2504 04F4      		brge .+4
3989
 4004 2506 0C94 0000 		jmp .L752
3990
 4005               	.L498:
3991
 4006 250a 7723      		tst r23
3992
 4007 250c 01F4      		brne .L499
3993
 4008 250e 8091 0000 		lds r24,LoopingRight
3994
 4009 2512 8823      		tst r24
3995
 4010 2514 01F4      		brne .+4
3996
 4011 2516 0C94 0000 		jmp .L500
3997
 4012               	.L499:
3998
 4013 251a 81E0      		ldi r24,lo8(1)
3999
 4014 251c 8093 0000 		sts LoopingRoll,r24
4000
 4015               	.L501:
4001
 4016 2520 6623      		tst r22
4002
 4017 2522 01F4      		brne .L502
4003
 4018 2524 8091 0000 		lds r24,LoopingDown
4004
 4019 2528 8823      		tst r24
4005
 4020 252a 01F4      		brne .+4
4006
 4021 252c 0C94 0000 		jmp .L503
4007
 4022               	.L502:
4008
 4023 2530 81E0      		ldi r24,lo8(1)
4009
 4024 2532 8093 0000 		sts LoopingNick,r24
4010
 4025 2536 1092 0000 		sts LoopingRoll,__zero_reg__
4011
 4026 253a 1092 0000 		sts LoopingLeft,__zero_reg__
4012
 4027 253e 1092 0000 		sts LoopingRight,__zero_reg__
4013
 4028               	.L472:
4014
 4029 2542 8091 0000 		lds r24,LoopingRoll
4015
 4030 2546 8823      		tst r24
4016
 4031 2548 01F0      		breq .+2
4017
 4032 254a 00C0      		rjmp .L504
4018
 4033 254c 8091 0000 		lds r24,LoopingNick
4019
 4034 2550 8823      		tst r24
4020
 4035 2552 01F0      		breq .+2
4021
 4036 2554 00C0      		rjmp .L504
4022
 4037               	.L505:
4023
 4038 2556 8091 0000 		lds r24,MKFlags
4024
 4039 255a 84FF      		sbrs r24,4
4025
 4040 255c 00C0      		rjmp .L507
4026
 4041 255e 1092 0000 		sts (StickYaw)+1,__zero_reg__
4027
 4042 2562 1092 0000 		sts StickYaw,__zero_reg__
4028
 4043 2566 1092 0000 		sts (StickNick)+1,__zero_reg__
4029
 4044 256a 1092 0000 		sts StickNick,__zero_reg__
4030
 4045 256e 1092 0000 		sts (StickRoll)+1,__zero_reg__
4031
 4046 2572 1092 0000 		sts StickRoll,__zero_reg__
4032
 4047 2576 8091 0000 		lds r24,ParamSet+31
4033
 4048 257a 8093 0000 		sts StickGas,r24
4034
 4049 257e 1092 0000 		sts StickGas+1,__zero_reg__
4035
 4050 2582 9AE5      		ldi r25,lo8(90)
4036
 4051 2584 9093 0000 		sts GyroPFactor,r25
4037
 4052 2588 88E7      		ldi r24,lo8(120)
4038
 4053 258a 8093 0000 		sts GyroIFactor,r24
4039
 4054 258e 9093 0000 		sts GyroYawPFactor,r25
4040
 4055 2592 8093 0000 		sts GyroYawIFactor,r24
4041
 4056 2596 1092 0000 		sts LoopingRoll,__zero_reg__
4042
 4057 259a 1092 0000 		sts LoopingNick,__zero_reg__
4043
 4058 259e 1092 0000 		sts (MaxStickNick)+1,__zero_reg__
4044
 4059 25a2 1092 0000 		sts MaxStickNick,__zero_reg__
4045
 4060 25a6 1092 0000 		sts (MaxStickRoll)+1,__zero_reg__
4046
 4061 25aa 1092 0000 		sts MaxStickRoll,__zero_reg__
4047
 4062               	.L507:
4048
 4063 25ae 2091 0000 		lds r18,MeanIntegralGyroNick
4049
 4064 25b2 3091 0000 		lds r19,(MeanIntegralGyroNick)+1
4050
 4065 25b6 4091 0000 		lds r20,(MeanIntegralGyroNick)+2
4051
 4066 25ba 5091 0000 		lds r21,(MeanIntegralGyroNick)+3
4052
 4067 25be 28A3      		std Y+32,r18
4053
 4068 25c0 39A3      		std Y+33,r19
4054
 4069 25c2 4AA3      		std Y+34,r20
4055
 4070 25c4 5BA3      		std Y+35,r21
4056
 4071 25c6 4091 0000 		lds r20,IntegralGyroNick
4057
 4072 25ca 5091 0000 		lds r21,(IntegralGyroNick)+1
4058
 4073 25ce 6091 0000 		lds r22,(IntegralGyroNick)+2
4059
 4074 25d2 7091 0000 		lds r23,(IntegralGyroNick)+3
4060
 4075 25d6 48A7      		std Y+40,r20
4061
 4076 25d8 59A7      		std Y+41,r21
4062
 4077 25da 6AA7      		std Y+42,r22
4063
 4078 25dc 7BA7      		std Y+43,r23
4064
 4079 25de 88A1      		ldd r24,Y+32
4065
 4080 25e0 99A1      		ldd r25,Y+33
4066
 4081 25e2 AAA1      		ldd r26,Y+34
4067
 4082 25e4 BBA1      		ldd r27,Y+35
4068
 4083 25e6 840F      		add r24,r20
4069
 4084 25e8 951F      		adc r25,r21
4070
 4085 25ea A61F      		adc r26,r22
4071
 4086 25ec B71F      		adc r27,r23
4072
 4087 25ee 8093 0000 		sts MeanIntegralGyroNick,r24
4073
 4088 25f2 9093 0000 		sts (MeanIntegralGyroNick)+1,r25
4074
 4089 25f6 A093 0000 		sts (MeanIntegralGyroNick)+2,r26
4075
 4090 25fa B093 0000 		sts (MeanIntegralGyroNick)+3,r27
4076
 4091 25fe 2090 0000 		lds r2,MeanIntegralGyroRoll
4077
 4092 2602 3090 0000 		lds r3,(MeanIntegralGyroRoll)+1
4078
 4093 2606 4090 0000 		lds r4,(MeanIntegralGyroRoll)+2
4079
 4094 260a 5090 0000 		lds r5,(MeanIntegralGyroRoll)+3
4080
 4095 260e A090 0000 		lds r10,IntegralGyroRoll
4081
 4096 2612 B090 0000 		lds r11,(IntegralGyroRoll)+1
4082
 4097 2616 C090 0000 		lds r12,(IntegralGyroRoll)+2
4083
 4098 261a D090 0000 		lds r13,(IntegralGyroRoll)+3
4084
 4099 261e ACA2      		std Y+36,r10
4085
 4100 2620 BDA2      		std Y+37,r11
4086
 4101 2622 CEA2      		std Y+38,r12
4087
 4102 2624 DFA2      		std Y+39,r13
4088
 4103 2626 2A0C      		add r2,r10
4089
 4104 2628 3B1C      		adc r3,r11
4090
 4105 262a 4C1C      		adc r4,r12
4091
 4106 262c 5D1C      		adc r5,r13
4092
 4107 262e 2092 0000 		sts MeanIntegralGyroRoll,r2
4093
 4108 2632 3092 0000 		sts (MeanIntegralGyroRoll)+1,r3
4094
 4109 2636 4092 0000 		sts (MeanIntegralGyroRoll)+2,r4
4095
 4110 263a 5092 0000 		sts (MeanIntegralGyroRoll)+3,r5
4096
 4111 263e B090 0000 		lds r11,LoopingNick
4097
 4112 2642 BD8E      		std Y+29,r11
4098
 4113 2644 BB20      		tst r11
4099
 4114 2646 01F4      		brne .L508
4100
 4115 2648 8091 0000 		lds r24,LoopingRoll
4101
 4116 264c 8823      		tst r24
4102
 4117 264e 01F4      		brne .+4
4103
 4118 2650 0C94 0000 		jmp .L509
4104
 4119               	.L508:
4105
 4120 2654 1092 0000 		sts (MeasurementCounter)+1,__zero_reg__
4106
 4121 2658 1092 0000 		sts MeasurementCounter,__zero_reg__
4107
 4122 265c 1092 0000 		sts MeanAccNick,__zero_reg__
4108
 4123 2660 1092 0000 		sts (MeanAccNick)+1,__zero_reg__
4109
 4124 2664 1092 0000 		sts (MeanAccNick)+2,__zero_reg__
4110
 4125 2668 1092 0000 		sts (MeanAccNick)+3,__zero_reg__
4111
 4126 266c 1092 0000 		sts MeanAccRoll,__zero_reg__
4112
 4127 2670 1092 0000 		sts (MeanAccRoll)+1,__zero_reg__
4113
 4128 2674 1092 0000 		sts (MeanAccRoll)+2,__zero_reg__
4114
 4129 2678 1092 0000 		sts (MeanAccRoll)+3,__zero_reg__
4115
 4130 267c 1092 0000 		sts MeanIntegralGyroNick,__zero_reg__
4116
 4131 2680 1092 0000 		sts (MeanIntegralGyroNick)+1,__zero_reg__
4117
 4132 2684 1092 0000 		sts (MeanIntegralGyroNick)+2,__zero_reg__
4118
 4133 2688 1092 0000 		sts (MeanIntegralGyroNick)+3,__zero_reg__
4119
 4134 268c 1092 0000 		sts MeanIntegralGyroRoll,__zero_reg__
4120
 4135 2690 1092 0000 		sts (MeanIntegralGyroRoll)+1,__zero_reg__
4121
 4136 2694 1092 0000 		sts (MeanIntegralGyroRoll)+2,__zero_reg__
4122
 4137 2698 1092 0000 		sts (MeanIntegralGyroRoll)+3,__zero_reg__
4123
 4138 269c 8091 0000 		lds r24,ReadingIntegralGyroNick
4124
 4139 26a0 9091 0000 		lds r25,(ReadingIntegralGyroNick)+1
4125
 4140 26a4 A091 0000 		lds r26,(ReadingIntegralGyroNick)+2
4126
 4141 26a8 B091 0000 		lds r27,(ReadingIntegralGyroNick)+3
4127
 4142 26ac 8093 0000 		sts ReadingIntegralGyroNick2,r24
4128
 4143 26b0 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
4129
 4144 26b4 A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
4130
 4145 26b8 B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
4131
 4146 26bc 8091 0000 		lds r24,ReadingIntegralGyroRoll
4132
 4147 26c0 9091 0000 		lds r25,(ReadingIntegralGyroRoll)+1
4133
 4148 26c4 A091 0000 		lds r26,(ReadingIntegralGyroRoll)+2
4134
 4149 26c8 B091 0000 		lds r27,(ReadingIntegralGyroRoll)+3
4135
 4150 26cc 8093 0000 		sts ReadingIntegralGyroRoll2,r24
4136
 4151 26d0 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
4137
 4152 26d4 A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
4138
 4153 26d8 B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
4139
 4154 26dc 1092 0000 		sts (AttitudeCorrectionNick)+1,__zero_reg__
4140
 4155 26e0 1092 0000 		sts AttitudeCorrectionNick,__zero_reg__
4141
 4156 26e4 1092 0000 		sts (AttitudeCorrectionRoll)+1,__zero_reg__
4142
 4157 26e8 1092 0000 		sts AttitudeCorrectionRoll,__zero_reg__
4143
 4158 26ec CD8C      		ldd r12,Y+29
4144
 4159 26ee CC20      		tst r12
4145
 4160 26f0 01F4      		brne .+4
4146
 4161 26f2 0C94 0000 		jmp .L753
4147
 4162               	.L510:
4148
 4163 26f6 8091 0000 		lds r24,MeasurementCounter
4149
 4164 26fa 9091 0000 		lds r25,(MeasurementCounter)+1
4150
 4165 26fe 8F3F      		cpi r24,255
4151
 4166 2700 9105      		cpc r25,__zero_reg__
4152
 4167 2702 01F0      		breq .+2
4153
 4168 2704 00F4      		brsh .+2
4154
 4169 2706 00C0      		rjmp .L529
4155
 4170 2708 6D8D      		ldd r22,Y+29
4156
 4171 270a 6623      		tst r22
4157
 4172 270c 01F4      		brne .+4
4158
 4173 270e 0C94 0000 		jmp .L754
4159
 4174               	.L530:
4160
 4175 2712 1092 0000 		sts (AttitudeCorrectionRoll)+1,__zero_reg__
4161
 4176 2716 1092 0000 		sts AttitudeCorrectionRoll,__zero_reg__
4162
 4177 271a 1092 0000 		sts (AttitudeCorrectionNick)+1,__zero_reg__
4163
 4178 271e 1092 0000 		sts AttitudeCorrectionNick,__zero_reg__
4164
 4179 2722 1092 0000 		sts FunnelCourse,__zero_reg__
4165
 4180 2726 4091 0000 		lds r20,MeanIntegralGyroNick
4166
 4181 272a 5091 0000 		lds r21,(MeanIntegralGyroNick)+1
4167
 4182 272e 6091 0000 		lds r22,(MeanIntegralGyroNick)+2
4168
 4183 2732 7091 0000 		lds r23,(MeanIntegralGyroNick)+3
4169
 4184 2736 48A3      		std Y+32,r20
4170
 4185 2738 59A3      		std Y+33,r21
4171
 4186 273a 6AA3      		std Y+34,r22
4172
 4187 273c 7BA3      		std Y+35,r23
4173
 4188 273e 2090 0000 		lds r2,MeanIntegralGyroRoll
4174
 4189 2742 3090 0000 		lds r3,(MeanIntegralGyroRoll)+1
4175
 4190 2746 4090 0000 		lds r4,(MeanIntegralGyroRoll)+2
4176
 4191 274a 5090 0000 		lds r5,(MeanIntegralGyroRoll)+3
4177
 4192               	.L567:
4178
 4193 274e 8091 0000 		lds r24,GyroIFactor
4179
 4194 2752 8823      		tst r24
4180
 4195 2754 01F4      		brne .L568
4181
 4196 2756 1092 0000 		sts (AttitudeCorrectionRoll)+1,__zero_reg__
4182
 4197 275a 1092 0000 		sts AttitudeCorrectionRoll,__zero_reg__
4183
 4198 275e 1092 0000 		sts (AttitudeCorrectionNick)+1,__zero_reg__
4184
 4199 2762 1092 0000 		sts AttitudeCorrectionNick,__zero_reg__
4185
 4200               	.L568:
4186
 4201 2766 68A1      		ldd r22,Y+32
4187
 4202 2768 79A1      		ldd r23,Y+33
4188
 4203 276a 8AA1      		ldd r24,Y+34
4189
 4204 276c 9BA1      		ldd r25,Y+35
4190
 4205 276e 6093 0000 		sts MeanIntegralGyroNick_old.2431,r22
4191
 4206 2772 7093 0000 		sts (MeanIntegralGyroNick_old.2431)+1,r23
4192
 4207 2776 8093 0000 		sts (MeanIntegralGyroNick_old.2431)+2,r24
4193
 4208 277a 9093 0000 		sts (MeanIntegralGyroNick_old.2431)+3,r25
4194
 4209 277e 2092 0000 		sts MeanIntegralGyroRoll_old.2432,r2
4195
 4210 2782 3092 0000 		sts (MeanIntegralGyroRoll_old.2432)+1,r3
4196
 4211 2786 4092 0000 		sts (MeanIntegralGyroRoll_old.2432)+2,r4
4197
 4212 278a 5092 0000 		sts (MeanIntegralGyroRoll_old.2432)+3,r5
4198
 4213 278e 1092 0000 		sts MeanAccNick,__zero_reg__
4199
 4214 2792 1092 0000 		sts (MeanAccNick)+1,__zero_reg__
4200
 4215 2796 1092 0000 		sts (MeanAccNick)+2,__zero_reg__
4201
 4216 279a 1092 0000 		sts (MeanAccNick)+3,__zero_reg__
4202
 4217 279e 1092 0000 		sts MeanAccRoll,__zero_reg__
4203
 4218 27a2 1092 0000 		sts (MeanAccRoll)+1,__zero_reg__
4204
 4219 27a6 1092 0000 		sts (MeanAccRoll)+2,__zero_reg__
4205
 4220 27aa 1092 0000 		sts (MeanAccRoll)+3,__zero_reg__
4206
 4221 27ae 1092 0000 		sts MeanIntegralGyroNick,__zero_reg__
4207
 4222 27b2 1092 0000 		sts (MeanIntegralGyroNick)+1,__zero_reg__
4208
 4223 27b6 1092 0000 		sts (MeanIntegralGyroNick)+2,__zero_reg__
4209
 4224 27ba 1092 0000 		sts (MeanIntegralGyroNick)+3,__zero_reg__
4210
 4225 27be 1092 0000 		sts MeanIntegralGyroRoll,__zero_reg__
4211
 4226 27c2 1092 0000 		sts (MeanIntegralGyroRoll)+1,__zero_reg__
4212
 4227 27c6 1092 0000 		sts (MeanIntegralGyroRoll)+2,__zero_reg__
4213
 4228 27ca 1092 0000 		sts (MeanIntegralGyroRoll)+3,__zero_reg__
4214
 4229 27ce 1092 0000 		sts (MeasurementCounter)+1,__zero_reg__
4215
 4230 27d2 1092 0000 		sts MeasurementCounter,__zero_reg__
4216
 4231               	.L529:
4217
 4232 27d6 E090 0000 		lds r14,StickYaw
4218
 4233 27da F090 0000 		lds r15,(StickYaw)+1
4219
 4234 27de C701      		movw r24,r14
4220
 4235 27e0 F7FE      		sbrs r15,7
4221
 4236 27e2 00C0      		rjmp .+4
4222
 4237 27e4 0C94 0000 		jmp .L755
4223
 4238               	.L570:
4224
 4239 27e8 4097      		sbiw r24,16
4225
 4240 27ea 04F0      		brlt .L569
4226
 4241 27ec 88EE      		ldi r24,lo8(1000)
4227
 4242 27ee 93E0      		ldi r25,hi8(1000)
4228
 4243 27f0 9093 0000 		sts (BadCompassHeading)+1,r25
4229
 4244 27f4 8093 0000 		sts BadCompassHeading,r24
4230
 4245 27f8 8091 0000 		lds r24,ParamSet+8
4231
 4246 27fc 84FD      		sbrc r24,4
4232
 4247 27fe 00C0      		rjmp .+4
4233
 4248 2800 0C94 0000 		jmp .L756
4234
 4249               	.L569:
4235
 4250 2804 1091 0000 		lds r17,ParamSet+20
4236
 4251 2808 B701      		movw r22,r14
4237
 4252 280a 8827      		clr r24
4238
 4253 280c 77FD      		sbrc r23,7
4239
 4254 280e 8095      		com r24
4240
 4255 2810 982F      		mov r25,r24
4241
 4256 2812 212F      		mov r18,r17
4242
 4257 2814 30E0      		ldi r19,lo8(0)
4243
 4258 2816 40E0      		ldi r20,lo8(0)
4244
 4259 2818 50E0      		ldi r21,hi8(0)
4245
 4260 281a 0E94 0000 		call __mulsi3
4246
 4261 281e F701      		movw r30,r14
4247
 4262 2820 F7FE      		sbrs r15,7
4248
 4263 2822 00C0      		rjmp .+4
4249
 4264 2824 0C94 0000 		jmp .L757
4250
 4265               	.L571:
4251
 4266 2828 9F01      		movw r18,r30
4252
 4267 282a 4427      		clr r20
4253
 4268 282c 37FD      		sbrc r19,7
4254
 4269 282e 4095      		com r20
4255
 4270 2830 542F      		mov r21,r20
4256
 4271 2832 0E94 0000 		call __mulsi3
4257
 4272 2836 20E0      		ldi r18,lo8(512)
4258
 4273 2838 32E0      		ldi r19,hi8(512)
4259
 4274 283a 40E0      		ldi r20,hlo8(512)
4260
 4275 283c 50E0      		ldi r21,hhi8(512)
4261
 4276 283e 0E94 0000 		call __divmodsi4
4262
 4277 2842 812F      		mov r24,r17
4263
 4278 2844 90E0      		ldi r25,lo8(0)
4264
 4279 2846 8E9D      		mul r24,r14
4265
 4280 2848 B001      		movw r22,r0
4266
 4281 284a 8F9D      		mul r24,r15
4267
 4282 284c 700D      		add r23,r0
4268
 4283 284e 9E9D      		mul r25,r14
4269
 4284 2850 700D      		add r23,r0
4270
 4285 2852 1124      		clr r1
4271
 4286 2854 77FF      		sbrs r23,7
4272
 4287 2856 00C0      		rjmp .+4
4273
 4288 2858 0C94 0000 		jmp .L758
4274
 4289               	.L572:
4275
 4290 285c 7595      		asr r23
4276
 4291 285e 6795      		ror r22
4277
 4292 2860 7595      		asr r23
4278
 4293 2862 6795      		ror r22
4279
 4294 2864 260F      		add r18,r22
4280
 4295 2866 371F      		adc r19,r23
4281
 4296 2868 C901      		movw r24,r18
4282
 4297 286a AA27      		clr r26
4283
 4298 286c 97FD      		sbrc r25,7
4284
 4299 286e A095      		com r26
4285
 4300 2870 BA2F      		mov r27,r26
4286
 4301 2872 8093 0000 		sts SetPointYaw.2410,r24
4287
 4302 2876 9093 0000 		sts (SetPointYaw.2410)+1,r25
4288
 4303 287a A093 0000 		sts (SetPointYaw.2410)+2,r26
4289
 4304 287e B093 0000 		sts (SetPointYaw.2410)+3,r27
4290
 4305 2882 2091 0000 		lds r18,ReadingIntegralGyroYaw
4291
 4306 2886 3091 0000 		lds r19,(ReadingIntegralGyroYaw)+1
4292
 4307 288a 4091 0000 		lds r20,(ReadingIntegralGyroYaw)+2
4293
 4308 288e 5091 0000 		lds r21,(ReadingIntegralGyroYaw)+3
4294
 4309 2892 281B      		sub r18,r24
4295
 4310 2894 390B      		sbc r19,r25
4296
 4311 2896 4A0B      		sbc r20,r26
4297
 4312 2898 5B0B      		sbc r21,r27
4298
 4313 289a 2093 0000 		sts ReadingIntegralGyroYaw,r18
4299
 4314 289e 3093 0000 		sts (ReadingIntegralGyroYaw)+1,r19
4300
 4315 28a2 4093 0000 		sts (ReadingIntegralGyroYaw)+2,r20
4301
 4316 28a6 5093 0000 		sts (ReadingIntegralGyroYaw)+3,r21
4302
 4317 28aa 203B      		cpi r18,lo8(-50000)
4303
 4318 28ac 7CE3      		ldi r23,hi8(-50000)
4304
 4319 28ae 3707      		cpc r19,r23
4305
 4320 28b0 7FEF      		ldi r23,hlo8(-50000)
4306
 4321 28b2 4707      		cpc r20,r23
4307
 4322 28b4 7FEF      		ldi r23,hhi8(-50000)
4308
 4323 28b6 5707      		cpc r21,r23
4309
 4324 28b8 04F0      		brlt .+4
4310
 4325 28ba 0C94 0000 		jmp .L573
4311
 4326 28be 80EB      		ldi r24,lo8(-50000)
4312
 4327 28c0 9CE3      		ldi r25,hi8(-50000)
4313
 4328 28c2 AFEF      		ldi r26,hlo8(-50000)
4314
 4329 28c4 BFEF      		ldi r27,hhi8(-50000)
4315
 4330 28c6 8093 0000 		sts ReadingIntegralGyroYaw,r24
4316
 4331 28ca 9093 0000 		sts (ReadingIntegralGyroYaw)+1,r25
4317
 4332 28ce A093 0000 		sts (ReadingIntegralGyroYaw)+2,r26
4318
 4333 28d2 B093 0000 		sts (ReadingIntegralGyroYaw)+3,r27
4319
 4334               	.L574:
4320
 4335 28d6 8091 0000 		lds r24,ParamSet+8
4321
 4336 28da 8872      		andi r24,lo8(40)
4322
 4337 28dc 01F4      		brne .+2
4323
 4338 28de 00C0      		rjmp .L728
4324
 4339 28e0 8091 0000 		lds r24,CompassCalState
4325
 4340 28e4 8823      		tst r24
4326
 4341 28e6 01F0      		breq .+2
4327
 4342 28e8 00C0      		rjmp .L759
4328
 4343               	.L576:
4329
 4344 28ea 68A5      		ldd r22,Y+40
4330
 4345 28ec 79A5      		ldd r23,Y+41
4331
 4346 28ee 8AA5      		ldd r24,Y+42
4332
 4347 28f0 9BA5      		ldd r25,Y+43
4333
 4348 28f2 20E0      		ldi r18,lo8(512)
4334
 4349 28f4 32E0      		ldi r19,hi8(512)
4335
 4350 28f6 40E0      		ldi r20,hlo8(512)
4336
 4351 28f8 50E0      		ldi r21,hhi8(512)
4337
 4352 28fa 0E94 0000 		call __divmodsi4
4338
 4353 28fe 8901      		movw r16,r18
4339
 4354 2900 37FF      		sbrs r19,7
4340
 4355 2902 00C0      		rjmp .+4
4341
 4356 2904 0C94 0000 		jmp .L760
4342
 4357               	.L577:
4343
 4358 2908 6CA1      		ldd r22,Y+36
4344
 4359 290a 7DA1      		ldd r23,Y+37
4345
 4360 290c 8EA1      		ldd r24,Y+38
4346
 4361 290e 9FA1      		ldd r25,Y+39
4347
 4362 2910 20E0      		ldi r18,lo8(512)
4348
 4363 2912 32E0      		ldi r19,hi8(512)
4349
 4364 2914 40E0      		ldi r20,hlo8(512)
4350
 4365 2916 50E0      		ldi r21,hhi8(512)
4351
 4366 2918 0E94 0000 		call __divmodsi4
4352
 4367 291c 4901      		movw r8,r18
4353
 4368 291e 37FF      		sbrs r19,7
4354
 4369 2920 00C0      		rjmp .+4
4355
 4370 2922 0C94 0000 		jmp .L761
4356
 4371               	.L578:
4357
 4372 2926 8016      		cp r8,r16
4358
 4373 2928 9106      		cpc r9,r17
4359
 4374 292a 04F4      		brge .L579
4360
 4375 292c 4801      		movw r8,r16
4361
 4376               	.L579:
4362
 4377 292e A090 0000 		lds r10,CompassHeading
4363
 4378 2932 B090 0000 		lds r11,(CompassHeading)+1
4364
 4379 2936 B7FE      		sbrs r11,7
4365
 4380 2938 00C0      		rjmp .+4
4366
 4381 293a 0C94 0000 		jmp .L762
4367
 4382 293e ACE1      		ldi r26,lo8(540)
4368
 4383 2940 B2E0      		ldi r27,hi8(540)
4369
 4384 2942 AA0E      		add r10,r26
4370
 4385 2944 BB1E      		adc r11,r27
4371
 4386 2946 7501      		movw r14,r10
4372
 4387 2948 0027      		clr r16
4373
 4388 294a F7FC      		sbrc r15,7
4374
 4389 294c 0095      		com r16
4375
 4390 294e 102F      		mov r17,r16
4376
 4391 2950 E4EE      		ldi r30,lo8(-540)
4377
 4392 2952 FDEF      		ldi r31,hi8(-540)
4378
 4393 2954 AE0E      		add r10,r30
4379
 4394 2956 BF1E      		adc r11,r31
4380
 4395 2958 2091 0000 		lds r18,ParamSet+23
4381
 4396 295c 8AE2      		ldi r24,lo8(42)
4382
 4397 295e 289F      		mul r18,r24
4383
 4398 2960 9001      		movw r18,r0
4384
 4399 2962 1124      		clr r1
4385
 4400 2964 4427      		clr r20
4386
 4401 2966 37FD      		sbrc r19,7
4387
 4402 2968 4095      		com r20
4388
 4403 296a 542F      		mov r21,r20
4389
 4404 296c 6091 0000 		lds r22,YawGyroHeading
4390
 4405 2970 7091 0000 		lds r23,(YawGyroHeading)+1
4391
 4406 2974 8091 0000 		lds r24,(YawGyroHeading)+2
4392
 4407 2978 9091 0000 		lds r25,(YawGyroHeading)+3
4393
 4408 297c 0E94 0000 		call __divmodsi4
4394
 4409 2980 E21A      		sub r14,r18
4395
 4410 2982 F30A      		sbc r15,r19
4396
 4411 2984 040B      		sbc r16,r20
4397
 4412 2986 150B      		sbc r17,r21
4398
 4413 2988 C801      		movw r24,r16
4399
 4414 298a B701      		movw r22,r14
4400
 4415 298c 28E6      		ldi r18,lo8(360)
4401
 4416 298e 31E0      		ldi r19,hi8(360)
4402
 4417 2990 40E0      		ldi r20,hlo8(360)
4403
 4418 2992 50E0      		ldi r21,hhi8(360)
4404
 4419 2994 0E94 0000 		call __divmodsi4
4405
 4420 2998 645B      		subi r22,lo8(-(-180))
4406
 4421 299a 7040      		sbci r23,hi8(-(-180))
4407
 4422               	.L581:
4408
 4423 299c 2090 0000 		lds r2,GyroYaw
4409
 4424 29a0 3090 0000 		lds r3,(GyroYaw)+1
4410
 4425 29a4 C101      		movw r24,r2
4411
 4426 29a6 37FE      		sbrs r3,7
4412
 4427 29a8 00C0      		rjmp .+4
4413
 4428 29aa 0C94 0000 		jmp .L763
4414
 4429               	.L583:
4415
 4430 29ae 8138      		cpi r24,129
4416
 4431 29b0 9105      		cpc r25,__zero_reg__
4417
 4432 29b2 04F4      		brge .+4
4418
 4433 29b4 0C94 0000 		jmp .L582
4419
 4434 29b8 60E0      		ldi r22,lo8(0)
4420
 4435 29ba 70E0      		ldi r23,hi8(0)
4421
 4436 29bc CC24      		clr r12
4422
 4437 29be DD24      		clr r13
4423
 4438               	.L584:
4424
 4439 29c0 4090 0000 		lds r4,BadCompassHeading
4425
 4440 29c4 5090 0000 		lds r5,(BadCompassHeading)+1
4426
 4441 29c8 4114      		cp r4,__zero_reg__
4427
 4442 29ca 5104      		cpc r5,__zero_reg__
4428
 4443 29cc 01F4      		brne .L585
4429
 4444 29ce F9E1      		ldi r31,lo8(25)
4430
 4445 29d0 8F16      		cp r8,r31
4431
 4446 29d2 9104      		cpc r9,__zero_reg__
4432
 4447 29d4 04F4      		brge .+4
4433
 4448 29d6 0C94 0000 		jmp .L764
4434
 4449               	.L585:
4435
 4450 29da B401      		movw r22,r8
4436
 4451 29dc 7595      		asr r23
4437
 4452 29de 6795      		ror r22
4438
 4453 29e0 7595      		asr r23
4439
 4454 29e2 6795      		ror r22
4440
 4455 29e4 7595      		asr r23
4441
 4456 29e6 6795      		ror r22
4442
 4457 29e8 6F5F      		subi r22,lo8(-(1))
4443
 4458 29ea 7F4F      		sbci r23,hi8(-(1))
4444
 4459 29ec C601      		movw r24,r12
4445
 4460 29ee 0E94 0000 		call __divmodhi4
4446
 4461 29f2 5B01      		movw r10,r22
4447
 4462 29f4 CC24      		clr r12
4448
 4463 29f6 B7FC      		sbrc r11,7
4449
 4464 29f8 C094      		com r12
4450
 4465 29fa DC2C      		mov r13,r12
4451
 4466 29fc 8091 0000 		lds r24,YawGyroHeading
4452
 4467 2a00 9091 0000 		lds r25,(YawGyroHeading)+1
4453
 4468 2a04 A091 0000 		lds r26,(YawGyroHeading)+2
4454
 4469 2a08 B091 0000 		lds r27,(YawGyroHeading)+3
4455
 4470 2a0c A80E      		add r10,r24
4456
 4471 2a0e B91E      		adc r11,r25
4457
 4472 2a10 CA1E      		adc r12,r26
4458
 4473 2a12 DB1E      		adc r13,r27
4459
 4474 2a14 A092 0000 		sts YawGyroHeading,r10
4460
 4475 2a18 B092 0000 		sts (YawGyroHeading)+1,r11
4461
 4476 2a1c C092 0000 		sts (YawGyroHeading)+2,r12
4462
 4477 2a20 D092 0000 		sts (YawGyroHeading)+3,r13
4463
 4478 2a24 8091 0000 		lds r24,FCParam+5
4464
 4479 2a28 682E      		mov r6,r24
4465
 4480 2a2a 7724      		clr r7
4466
 4481 2a2c 869C      		mul r8,r6
4467
 4482 2a2e C001      		movw r24,r0
4468
 4483 2a30 879C      		mul r8,r7
4469
 4484 2a32 900D      		add r25,r0
4470
 4485 2a34 969C      		mul r9,r6
4471
 4486 2a36 900D      		add r25,r0
4472
 4487 2a38 1124      		clr r1
4473
 4488 2a3a 97FF      		sbrs r25,7
4474
 4489 2a3c 00C0      		rjmp .+4
4475
 4490 2a3e 0C94 0000 		jmp .L765
4476
 4491               	.L586:
4477
 4492 2a42 9595      		asr r25
4478
 4493 2a44 8795      		ror r24
4479
 4494 2a46 9595      		asr r25
4480
 4495 2a48 8795      		ror r24
4481
 4496 2a4a 9595      		asr r25
4482
 4497 2a4c 8795      		ror r24
4483
 4498 2a4e 9595      		asr r25
4484
 4499 2a50 8795      		ror r24
4485
 4500 2a52 9595      		asr r25
4486
 4501 2a54 8795      		ror r24
4487
 4502 2a56 4301      		movw r8,r6
4488
 4503 2a58 881A      		sub r8,r24
4489
 4504 2a5a 990A      		sbc r9,r25
4490
 4505 2a5c 97FE      		sbrs r9,7
4491
 4506 2a5e 00C0      		rjmp .+4
4492
 4507 2a60 0C94 0000 		jmp .L587
4493
 4508 2a64 4114      		cp r4,__zero_reg__
4494
 4509 2a66 5104      		cpc r5,__zero_reg__
4495
 4510 2a68 01F0      		breq .+4
4496
 4511 2a6a 0C94 0000 		jmp .L588
4497
 4512 2a6e 4090 0000 		lds r4,MaxStickRoll
4498
 4513 2a72 5090 0000 		lds r5,(MaxStickRoll)+1
4499
 4514 2a76 8091 0000 		lds r24,MaxStickNick
4500
 4515 2a7a 9091 0000 		lds r25,(MaxStickNick)+1
4501
 4516 2a7e 480E      		add r4,r24
4502
 4517 2a80 591E      		adc r5,r25
4503
 4518 2a82 57FE      		sbrs r5,7
4504
 4519 2a84 00C0      		rjmp .+4
4505
 4520 2a86 0C94 0000 		jmp .L766
4506
 4521               	.L589:
4507
 4522 2a8a 5594      		asr r5
4508
 4523 2a8c 4794      		ror r4
4509
 4524 2a8e 5594      		asr r5
4510
 4525 2a90 4794      		ror r4
4511
 4526 2a92 5594      		asr r5
4512
 4527 2a94 4794      		ror r4
4513
 4528 2a96 40E4      		ldi r20,lo8(64)
4514
 4529 2a98 50E0      		ldi r21,hi8(64)
4515
 4530 2a9a 440E      		add r4,r20
4516
 4531 2a9c 551E      		adc r5,r21
4517
 4532 2a9e 8091 0000 		lds r24,CompassCourse
4518
 4533 2aa2 9091 0000 		lds r25,(CompassCourse)+1
4519
 4534 2aa6 AA27      		clr r26
4520
 4535 2aa8 97FD      		sbrc r25,7
4521
 4536 2aaa A095      		com r26
4522
 4537 2aac BA2F      		mov r27,r26
4523
 4538 2aae 7CE1      		ldi r23,lo8(540)
4524
 4539 2ab0 E72E      		mov r14,r23
4525
 4540 2ab2 72E0      		ldi r23,hi8(540)
4526
 4541 2ab4 F72E      		mov r15,r23
4527
 4542 2ab6 012D      		mov r16,__zero_reg__
4528
 4543 2ab8 112D      		mov r17,__zero_reg__
4529
 4544 2aba E81A      		sub r14,r24
4530
 4545 2abc F90A      		sbc r15,r25
4531
 4546 2abe 0A0B      		sbc r16,r26
4532
 4547 2ac0 1B0B      		sbc r17,r27
4533
 4548 2ac2 2091 0000 		lds r18,ParamSet+23
4534
 4549 2ac6 8AE2      		ldi r24,lo8(42)
4535
 4550 2ac8 289F      		mul r18,r24
4536
 4551 2aca 9001      		movw r18,r0
4537
 4552 2acc 1124      		clr r1
4538
 4553 2ace 4427      		clr r20
4539
 4554 2ad0 37FD      		sbrc r19,7
4540
 4555 2ad2 4095      		com r20
4541
 4556 2ad4 542F      		mov r21,r20
4542
 4557 2ad6 C601      		movw r24,r12
4543
 4558 2ad8 B501      		movw r22,r10
4544
 4559 2ada 0E94 0000 		call __divmodsi4
4545
 4560 2ade E20E      		add r14,r18
4546
 4561 2ae0 F31E      		adc r15,r19
4547
 4562 2ae2 041F      		adc r16,r20
4548
 4563 2ae4 151F      		adc r17,r21
4549
 4564 2ae6 C801      		movw r24,r16
4550
 4565 2ae8 B701      		movw r22,r14
4551
 4566 2aea 28E6      		ldi r18,lo8(360)
4552
 4567 2aec 31E0      		ldi r19,hi8(360)
4553
 4568 2aee 40E0      		ldi r20,hlo8(360)
4554
 4569 2af0 50E0      		ldi r21,hhi8(360)
4555
 4570 2af2 0E94 0000 		call __divmodsi4
4556
 4571 2af6 645B      		subi r22,lo8(-(-180))
4557
 4572 2af8 7040      		sbci r23,hi8(-(-180))
4558
 4573 2afa 689D      		mul r22,r8
4559
 4574 2afc C001      		movw r24,r0
4560
 4575 2afe 699D      		mul r22,r9
4561
 4576 2b00 900D      		add r25,r0
4562
 4577 2b02 789D      		mul r23,r8
4563
 4578 2b04 900D      		add r25,r0
4564
 4579 2b06 1124      		clr r1
4565
 4580 2b08 B201      		movw r22,r4
4566
 4581 2b0a 0E94 0000 		call __divmodhi4
4567
 4582 2b0e 9B01      		movw r18,r22
4568
 4583 2b10 C301      		movw r24,r6
4569
 4584 2b12 880F      		lsl r24
4570
 4585 2b14 991F      		rol r25
4571
 4586 2b16 860D      		add r24,r6
4572
 4587 2b18 971D      		adc r25,r7
4573
 4588 2b1a 8217      		cp r24,r18
4574
 4589 2b1c 9307      		cpc r25,r19
4575
 4590 2b1e 04F0      		brlt .+4
4576
 4591 2b20 0C94 0000 		jmp .L590
4577
 4592 2b24 BC01      		movw r22,r24
4578
 4593               	.L591:
4579
 4594 2b26 9B01      		movw r18,r22
4580
 4595 2b28 4427      		clr r20
4581
 4596 2b2a 37FD      		sbrc r19,7
4582
 4597 2b2c 4095      		com r20
4583
 4598 2b2e 542F      		mov r21,r20
4584
 4599 2b30 8091 0000 		lds r24,ReadingIntegralGyroYaw
4585
 4600 2b34 9091 0000 		lds r25,(ReadingIntegralGyroYaw)+1
4586
 4601 2b38 A091 0000 		lds r26,(ReadingIntegralGyroYaw)+2
4587
 4602 2b3c B091 0000 		lds r27,(ReadingIntegralGyroYaw)+3
4588
 4603 2b40 820F      		add r24,r18
4589
 4604 2b42 931F      		adc r25,r19
4590
 4605 2b44 A41F      		adc r26,r20
4591
 4606 2b46 B51F      		adc r27,r21
4592
 4607 2b48 8093 0000 		sts ReadingIntegralGyroYaw,r24
4593
 4608 2b4c 9093 0000 		sts (ReadingIntegralGyroYaw)+1,r25
4594
 4609 2b50 A093 0000 		sts (ReadingIntegralGyroYaw)+2,r26
4595
 4610 2b54 B093 0000 		sts (ReadingIntegralGyroYaw)+3,r27
4596
 4611 2b58 00C0      		rjmp .L575
4597
 4612               	.L401:
4598
 4613 2b5a 2998      		cbi 37-32,1
4599
 4614 2b5c 8091 0000 		lds r24,RC_Quality
4600
 4615 2b60 8436      		cpi r24,lo8(100)
4601
 4616 2b62 00F4      		brsh .+2
4602
 4617 2b64 00C0      		rjmp .L767
4603
 4618               	.L403:
4604
 4619 2b66 8091 0000 		lds r24,RC_Quality
4605
 4620 2b6a 8D38      		cpi r24,lo8(-115)
4606
 4621 2b6c 00F4      		brsh .+2
4607
 4622 2b6e 00C0      		rjmp .L410
4608
 4623 2b70 8091 0000 		lds r24,MKFlags
4609
 4624 2b74 8F7E      		andi r24,lo8(-17)
4610
 4625 2b76 8093 0000 		sts MKFlags,r24
4611
 4626 2b7a 8091 0000 		lds r24,ParamSet+32
4612
 4627 2b7e 22E3      		ldi r18,lo8(50)
4613
 4628 2b80 829F      		mul r24,r18
4614
 4629 2b82 C001      		movw r24,r0
4615
 4630 2b84 1124      		clr r1
4616
 4631 2b86 9093 0000 		sts (RcLostTimer.2415)+1,r25
4617
 4632 2b8a 8093 0000 		sts RcLostTimer.2415,r24
4618
 4633 2b8e 8091 0000 		lds r24,StickGas
4619
 4634 2b92 9091 0000 		lds r25,(StickGas)+1
4620
 4635 2b96 8997      		sbiw r24,41
4621
 4636 2b98 04F0      		brlt .L411
4622
 4637 2b9a 8091 0000 		lds r24,MKFlags
4623
 4638 2b9e 80FF      		sbrs r24,0
4624
 4639 2ba0 00C0      		rjmp .L411
4625
 4640 2ba2 8091 0000 		lds r24,ModelIsFlying
4626
 4641 2ba6 9091 0000 		lds r25,(ModelIsFlying)+1
4627
 4642 2baa BFEF      		ldi r27,hi8(-1)
4628
 4643 2bac 8F3F      		cpi r24,lo8(-1)
4629
 4644 2bae 9B07      		cpc r25,r27
4630
 4645 2bb0 01F4      		brne .+4
4631
 4646 2bb2 0C94 0000 		jmp .L412
4632
 4647 2bb6 0196      		adiw r24,1
4633
 4648 2bb8 9093 0000 		sts (ModelIsFlying)+1,r25
4634
 4649 2bbc 8093 0000 		sts ModelIsFlying,r24
4635
 4650               	.L411:
4636
 4651 2bc0 8091 0000 		lds r24,ModelIsFlying
4637
 4652 2bc4 9091 0000 		lds r25,(ModelIsFlying)+1
4638
 4653 2bc8 8F3F      		cpi r24,255
4639
 4654 2bca 9105      		cpc r25,__zero_reg__
4640
 4655 2bcc 01F0      		breq .+6
4641
 4656 2bce 00F0      		brlo .+4
4642
 4657 2bd0 0C94 0000 		jmp .L412
4643
 4658 2bd4 1092 0000 		sts IPartNick.2408,__zero_reg__
4644
 4659 2bd8 1092 0000 		sts (IPartNick.2408)+1,__zero_reg__
4645
 4660 2bdc 1092 0000 		sts (IPartNick.2408)+2,__zero_reg__
4646
 4661 2be0 1092 0000 		sts (IPartNick.2408)+3,__zero_reg__
4647
 4662 2be4 1092 0000 		sts IPartRoll.2409,__zero_reg__
4648
 4663 2be8 1092 0000 		sts (IPartRoll.2409)+1,__zero_reg__
4649
 4664 2bec 1092 0000 		sts (IPartRoll.2409)+2,__zero_reg__
4650
 4665 2bf0 1092 0000 		sts (IPartRoll.2409)+3,__zero_reg__
4651
 4666 2bf4 1092 0000 		sts (StickYaw)+1,__zero_reg__
4652
 4667 2bf8 1092 0000 		sts StickYaw,__zero_reg__
4653
 4668 2bfc 8A3F      		cpi r24,250
4654
 4669 2bfe 9105      		cpc r25,__zero_reg__
4655
 4670 2c00 01F4      		brne .+4
4656
 4671 2c02 0C94 0000 		jmp .L768
4657
 4672               	.L413:
4658
 4673 2c06 8091 0000 		lds r24,ParamSet+4
4659
 4674 2c0a E82F      		mov r30,r24
4660
 4675 2c0c F0E0      		ldi r31,lo8(0)
4661
 4676 2c0e EE0F      		lsl r30
4662
 4677 2c10 FF1F      		rol r31
4663
 4678 2c12 E050      		subi r30,lo8(-(PPM_in))
4664
 4679 2c14 F040      		sbci r31,hi8(-(PPM_in))
4665
 4680 2c16 8081      		ld r24,Z
4666
 4681 2c18 9181      		ldd r25,Z+1
4667
 4682 2c1a E090 0000 		lds r14,Poti1
4668
 4683 2c1e F090 0000 		lds r15,(Poti1)+1
4669
 4684 2c22 8359      		subi r24,lo8(-(109))
4670
 4685 2c24 9F4F      		sbci r25,hi8(-(109))
4671
 4686 2c26 8E15      		cp r24,r14
4672
 4687 2c28 9F05      		cpc r25,r15
4673
 4688 2c2a 04F4      		brge .+4
4674
 4689 2c2c 0C94 0000 		jmp .L414
4675
 4690 2c30 C701      		movw r24,r14
4676
 4691 2c32 0196      		adiw r24,1
4677
 4692 2c34 9093 0000 		sts (Poti1)+1,r25
4678
 4693 2c38 8093 0000 		sts Poti1,r24
4679
 4694 2c3c 7C01      		movw r14,r24
4680
 4695               	.L415:
4681
 4696 2c3e 8091 0000 		lds r24,ParamSet+5
4682
 4697 2c42 E82F      		mov r30,r24
4683
 4698 2c44 F0E0      		ldi r31,lo8(0)
4684
 4699 2c46 EE0F      		lsl r30
4685
 4700 2c48 FF1F      		rol r31
4686
 4701 2c4a E050      		subi r30,lo8(-(PPM_in))
4687
 4702 2c4c F040      		sbci r31,hi8(-(PPM_in))
4688
 4703 2c4e 8081      		ld r24,Z
4689
 4704 2c50 9181      		ldd r25,Z+1
4690
 4705 2c52 0091 0000 		lds r16,Poti2
4691
 4706 2c56 1091 0000 		lds r17,(Poti2)+1
4692
 4707 2c5a 8359      		subi r24,lo8(-(109))
4693
 4708 2c5c 9F4F      		sbci r25,hi8(-(109))
4694
 4709 2c5e 8017      		cp r24,r16
4695
 4710 2c60 9107      		cpc r25,r17
4696
 4711 2c62 04F4      		brge .+4
4697
 4712 2c64 0C94 0000 		jmp .L416
4698
 4713 2c68 C801      		movw r24,r16
4699
 4714 2c6a 0196      		adiw r24,1
4700
 4715 2c6c 9093 0000 		sts (Poti2)+1,r25
4701
 4716 2c70 8093 0000 		sts Poti2,r24
4702
 4717 2c74 8C01      		movw r16,r24
4703
 4718               	.L417:
4704
 4719 2c76 8091 0000 		lds r24,ParamSet+6
4705
 4720 2c7a E82F      		mov r30,r24
4706
 4721 2c7c F0E0      		ldi r31,lo8(0)
4707
 4722 2c7e EE0F      		lsl r30
4708
 4723 2c80 FF1F      		rol r31
4709
 4724 2c82 E050      		subi r30,lo8(-(PPM_in))
4710
 4725 2c84 F040      		sbci r31,hi8(-(PPM_in))
4711
 4726 2c86 8081      		ld r24,Z
4712
 4727 2c88 9181      		ldd r25,Z+1
4713
 4728 2c8a A090 0000 		lds r10,Poti3
4714
 4729 2c8e B090 0000 		lds r11,(Poti3)+1
4715
 4730 2c92 8359      		subi r24,lo8(-(109))
4716
 4731 2c94 9F4F      		sbci r25,hi8(-(109))
4717
 4732 2c96 8A15      		cp r24,r10
4718
 4733 2c98 9B05      		cpc r25,r11
4719
 4734 2c9a 04F4      		brge .+4
4720
 4735 2c9c 0C94 0000 		jmp .L418
4721
 4736 2ca0 C501      		movw r24,r10
4722
 4737 2ca2 0196      		adiw r24,1
4723
 4738 2ca4 9093 0000 		sts (Poti3)+1,r25
4724
 4739 2ca8 8093 0000 		sts Poti3,r24
4725
 4740 2cac 5C01      		movw r10,r24
4726
 4741               	.L419:
4727
 4742 2cae 8091 0000 		lds r24,ParamSet+7
4728
 4743 2cb2 E82F      		mov r30,r24
4729
 4744 2cb4 F0E0      		ldi r31,lo8(0)
4730
 4745 2cb6 EE0F      		lsl r30
4731
 4746 2cb8 FF1F      		rol r31
4732
 4747 2cba E050      		subi r30,lo8(-(PPM_in))
4733
 4748 2cbc F040      		sbci r31,hi8(-(PPM_in))
4734
 4749 2cbe 8081      		ld r24,Z
4735
 4750 2cc0 9181      		ldd r25,Z+1
4736
 4751 2cc2 A091 0000 		lds r26,Poti4
4737
 4752 2cc6 B091 0000 		lds r27,(Poti4)+1
4738
 4753 2cca 8359      		subi r24,lo8(-(109))
4739
 4754 2ccc 9F4F      		sbci r25,hi8(-(109))
4740
 4755 2cce 8A17      		cp r24,r26
4741
 4756 2cd0 9B07      		cpc r25,r27
4742
 4757 2cd2 04F4      		brge .+4
4743
 4758 2cd4 0C94 0000 		jmp .L420
4744
 4759 2cd8 CD01      		movw r24,r26
4745
 4760 2cda 0196      		adiw r24,1
4746
 4761 2cdc 9093 0000 		sts (Poti4)+1,r25
4747
 4762 2ce0 8093 0000 		sts Poti4,r24
4748
 4763 2ce4 DC01      		movw r26,r24
4749
 4764               	.L421:
4750
 4765 2ce6 8091 0000 		lds r24,PPM_in+18
4751
 4766 2cea 9091 0000 		lds r25,(PPM_in+18)+1
4752
 4767 2cee E091 0000 		lds r30,Poti5
4753
 4768 2cf2 F091 0000 		lds r31,(Poti5)+1
4754
 4769 2cf6 8359      		subi r24,lo8(-(109))
4755
 4770 2cf8 9F4F      		sbci r25,hi8(-(109))
4756
 4771 2cfa 8E17      		cp r24,r30
4757
 4772 2cfc 9F07      		cpc r25,r31
4758
 4773 2cfe 04F4      		brge .+4
4759
 4774 2d00 0C94 0000 		jmp .L422
4760
 4775 2d04 CF01      		movw r24,r30
4761
 4776 2d06 0196      		adiw r24,1
4762
 4777 2d08 9093 0000 		sts (Poti5)+1,r25
4763
 4778 2d0c 8093 0000 		sts Poti5,r24
4764
 4779 2d10 FC01      		movw r30,r24
4765
 4780               	.L423:
4766
 4781 2d12 8091 0000 		lds r24,PPM_in+20
4767
 4782 2d16 9091 0000 		lds r25,(PPM_in+20)+1
4768
 4783 2d1a 6091 0000 		lds r22,Poti6
4769
 4784 2d1e 7091 0000 		lds r23,(Poti6)+1
4770
 4785 2d22 8359      		subi r24,lo8(-(109))
4771
 4786 2d24 9F4F      		sbci r25,hi8(-(109))
4772
 4787 2d26 8617      		cp r24,r22
4773
 4788 2d28 9707      		cpc r25,r23
4774
 4789 2d2a 04F4      		brge .+4
4775
 4790 2d2c 0C94 0000 		jmp .L424
4776
 4791 2d30 CB01      		movw r24,r22
4777
 4792 2d32 0196      		adiw r24,1
4778
 4793 2d34 9093 0000 		sts (Poti6)+1,r25
4779
 4794 2d38 8093 0000 		sts Poti6,r24
4780
 4795 2d3c BC01      		movw r22,r24
4781
 4796               	.L425:
4782
 4797 2d3e 8091 0000 		lds r24,PPM_in+22
4783
 4798 2d42 9091 0000 		lds r25,(PPM_in+22)+1
4784
 4799 2d46 4091 0000 		lds r20,Poti7
4785
 4800 2d4a 5091 0000 		lds r21,(Poti7)+1
4786
 4801 2d4e 8359      		subi r24,lo8(-(109))
4787
 4802 2d50 9F4F      		sbci r25,hi8(-(109))
4788
 4803 2d52 8417      		cp r24,r20
4789
 4804 2d54 9507      		cpc r25,r21
4790
 4805 2d56 04F4      		brge .+4
4791
 4806 2d58 0C94 0000 		jmp .L426
4792
 4807 2d5c CA01      		movw r24,r20
4793
 4808 2d5e 0196      		adiw r24,1
4794
 4809 2d60 9093 0000 		sts (Poti7)+1,r25
4795
 4810 2d64 8093 0000 		sts Poti7,r24
4796
 4811 2d68 AC01      		movw r20,r24
4797
 4812               	.L427:
4798
 4813 2d6a 8091 0000 		lds r24,PPM_in+24
4799
 4814 2d6e 9091 0000 		lds r25,(PPM_in+24)+1
4800
 4815 2d72 2091 0000 		lds r18,Poti8
4801
 4816 2d76 3091 0000 		lds r19,(Poti8)+1
4802
 4817 2d7a 8359      		subi r24,lo8(-(109))
4803
 4818 2d7c 9F4F      		sbci r25,hi8(-(109))
4804
 4819 2d7e 8217      		cp r24,r18
4805
 4820 2d80 9307      		cpc r25,r19
4806
 4821 2d82 04F4      		brge .+4
4807
 4822 2d84 0C94 0000 		jmp .L428
4808
 4823 2d88 C901      		movw r24,r18
4809
 4824 2d8a 0196      		adiw r24,1
4810
 4825 2d8c 9093 0000 		sts (Poti8)+1,r25
4811
 4826 2d90 8093 0000 		sts Poti8,r24
4812
 4827 2d94 9C01      		movw r18,r24
4813
 4828               	.L429:
4814
 4829 2d96 F7FE      		sbrs r15,7
4815
 4830 2d98 00C0      		rjmp .+4
4816
 4831 2d9a 0C94 0000 		jmp .L769
4817
 4832 2d9e 8FEF      		ldi r24,lo8(255)
4818
 4833 2da0 E816      		cp r14,r24
4819
 4834 2da2 F104      		cpc r15,__zero_reg__
4820
 4835 2da4 01F0      		breq .L431
4821
 4836 2da6 04F0      		brlt .L431
4822
 4837 2da8 8FEF      		ldi r24,lo8(255)
4823
 4838 2daa 90E0      		ldi r25,hi8(255)
4824
 4839 2dac 9093 0000 		sts (Poti1)+1,r25
4825
 4840 2db0 8093 0000 		sts Poti1,r24
4826
 4841               	.L431:
4827
 4842 2db4 17FF      		sbrs r17,7
4828
 4843 2db6 00C0      		rjmp .+4
4829
 4844 2db8 0C94 0000 		jmp .L770
4830
 4845 2dbc 0F3F      		cpi r16,255
4831
 4846 2dbe 1105      		cpc r17,__zero_reg__
4832
 4847 2dc0 01F0      		breq .L433
4833
 4848 2dc2 04F0      		brlt .L433
4834
 4849 2dc4 8FEF      		ldi r24,lo8(255)
4835
 4850 2dc6 90E0      		ldi r25,hi8(255)
4836
 4851 2dc8 9093 0000 		sts (Poti2)+1,r25
4837
 4852 2dcc 8093 0000 		sts Poti2,r24
4838
 4853               	.L433:
4839
 4854 2dd0 B7FE      		sbrs r11,7
4840
 4855 2dd2 00C0      		rjmp .+4
4841
 4856 2dd4 0C94 0000 		jmp .L771
4842
 4857 2dd8 9FEF      		ldi r25,lo8(255)
4843
 4858 2dda A916      		cp r10,r25
4844
 4859 2ddc B104      		cpc r11,__zero_reg__
4845
 4860 2dde 01F0      		breq .L435
4846
 4861 2de0 04F0      		brlt .L435
4847
 4862 2de2 8FEF      		ldi r24,lo8(255)
4848
 4863 2de4 90E0      		ldi r25,hi8(255)
4849
 4864 2de6 9093 0000 		sts (Poti3)+1,r25
4850
 4865 2dea 8093 0000 		sts Poti3,r24
4851
 4866               	.L435:
4852
 4867 2dee B7FF      		sbrs r27,7
4853
 4868 2df0 00C0      		rjmp .+4
4854
 4869 2df2 0C94 0000 		jmp .L772
4855
 4870 2df6 AF3F      		cpi r26,255
4856
 4871 2df8 B105      		cpc r27,__zero_reg__
4857
 4872 2dfa 01F0      		breq .L437
4858
 4873 2dfc 04F0      		brlt .L437
4859
 4874 2dfe 8FEF      		ldi r24,lo8(255)
4860
 4875 2e00 90E0      		ldi r25,hi8(255)
4861
 4876 2e02 9093 0000 		sts (Poti4)+1,r25
4862
 4877 2e06 8093 0000 		sts Poti4,r24
4863
 4878               	.L437:
4864
 4879 2e0a F7FF      		sbrs r31,7
4865
 4880 2e0c 00C0      		rjmp .+4
4866
 4881 2e0e 0C94 0000 		jmp .L773
4867
 4882 2e12 EF3F      		cpi r30,255
4868
 4883 2e14 F105      		cpc r31,__zero_reg__
4869
 4884 2e16 01F0      		breq .L439
4870
 4885 2e18 04F0      		brlt .L439
4871
 4886 2e1a 8FEF      		ldi r24,lo8(255)
4872
 4887 2e1c 90E0      		ldi r25,hi8(255)
4873
 4888 2e1e 9093 0000 		sts (Poti5)+1,r25
4874
 4889 2e22 8093 0000 		sts Poti5,r24
4875
 4890               	.L439:
4876
 4891 2e26 77FF      		sbrs r23,7
4877
 4892 2e28 00C0      		rjmp .+4
4878
 4893 2e2a 0C94 0000 		jmp .L774
4879
 4894 2e2e 6F3F      		cpi r22,255
4880
 4895 2e30 7105      		cpc r23,__zero_reg__
4881
 4896 2e32 01F0      		breq .L441
4882
 4897 2e34 04F0      		brlt .L441
4883
 4898 2e36 8FEF      		ldi r24,lo8(255)
4884
 4899 2e38 90E0      		ldi r25,hi8(255)
4885
 4900 2e3a 9093 0000 		sts (Poti6)+1,r25
4886
 4901 2e3e 8093 0000 		sts Poti6,r24
4887
 4902               	.L441:
4888
 4903 2e42 57FF      		sbrs r21,7
4889
 4904 2e44 00C0      		rjmp .+4
4890
 4905 2e46 0C94 0000 		jmp .L775
4891
 4906 2e4a 4F3F      		cpi r20,255
4892
 4907 2e4c 5105      		cpc r21,__zero_reg__
4893
 4908 2e4e 01F0      		breq .L443
4894
 4909 2e50 04F0      		brlt .L443
4895
 4910 2e52 8FEF      		ldi r24,lo8(255)
4896
 4911 2e54 90E0      		ldi r25,hi8(255)
4897
 4912 2e56 9093 0000 		sts (Poti7)+1,r25
4898
 4913 2e5a 8093 0000 		sts Poti7,r24
4899
 4914               	.L443:
4900
 4915 2e5e 37FF      		sbrs r19,7
4901
 4916 2e60 00C0      		rjmp .+4
4902
 4917 2e62 0C94 0000 		jmp .L776
4903
 4918 2e66 2F3F      		cpi r18,255
4904
 4919 2e68 3105      		cpc r19,__zero_reg__
4905
 4920 2e6a 01F0      		breq .L445
4906
 4921 2e6c 04F0      		brlt .L445
4907
 4922 2e6e 8FEF      		ldi r24,lo8(255)
4908
 4923 2e70 90E0      		ldi r25,hi8(255)
4909
 4924 2e72 9093 0000 		sts (Poti8)+1,r25
4910
 4925 2e76 8093 0000 		sts Poti8,r24
4911
 4926               	.L445:
4912
 4927 2e7a 8091 0000 		lds r24,ParamSet+2
4913
 4928 2e7e A82F      		mov r26,r24
4914
 4929 2e80 B0E0      		ldi r27,lo8(0)
4915
 4930 2e82 AA0F      		lsl r26
4916
 4931 2e84 BB1F      		rol r27
4917
 4932 2e86 FD01      		movw r30,r26
4918
 4933 2e88 E050      		subi r30,lo8(-(PPM_in))
4919
 4934 2e8a F040      		sbci r31,hi8(-(PPM_in))
4920
 4935 2e8c 8081      		ld r24,Z
4921
 4936 2e8e 9181      		ldd r25,Z+1
4922
 4937 2e90 8135      		cpi r24,81
4923
 4938 2e92 9105      		cpc r25,__zero_reg__
4924
 4939 2e94 04F4      		brge .+4
4925
 4940 2e96 0C94 0000 		jmp .L446
4926
 4941 2e9a 8091 0000 		lds r24,MKFlags
4927
 4942 2e9e 80FF      		sbrs r24,0
4928
 4943 2ea0 00C0      		rjmp .+4
4929
 4944 2ea2 0C94 0000 		jmp .L446
4930
 4945 2ea6 8091 0000 		lds r24,ParamSet+3
4931
 4946 2eaa E82F      		mov r30,r24
4932
 4947 2eac F0E0      		ldi r31,lo8(0)
4933
 4948 2eae EE0F      		lsl r30
4934
 4949 2eb0 FF1F      		rol r31
4935
 4950 2eb2 E050      		subi r30,lo8(-(PPM_in))
4936
 4951 2eb4 F040      		sbci r31,hi8(-(PPM_in))
4937
 4952 2eb6 8081      		ld r24,Z
4938
 4953 2eb8 9181      		ldd r25,Z+1
4939
 4954 2eba 8C34      		cpi r24,76
4940
 4955 2ebc 9105      		cpc r25,__zero_reg__
4941
 4956 2ebe 04F4      		brge .+4
4942
 4957 2ec0 0C94 0000 		jmp .L447
4943
 4958 2ec4 8091 0000 		lds r24,delay_neutral.2416
4944
 4959 2ec8 8F5F      		subi r24,lo8(-(1))
4945
 4960 2eca 8093 0000 		sts delay_neutral.2416,r24
4946
 4961 2ece 893C      		cpi r24,lo8(-55)
4947
 4962 2ed0 00F4      		brsh .+4
4948
 4963 2ed2 0C94 0000 		jmp .L446
4949
 4964 2ed6 1092 0000 		sts delay_neutral.2416,__zero_reg__
4950
 4965 2eda ABE0      		ldi r26,lo8(11)
4951
 4966 2edc AD15      		cp r26,r13
4952
 4967 2ede 00F4      		brsh .+4
4953
 4968 2ee0 0C94 0000 		jmp .L449
4954
 4969 2ee4 2998      		cbi 37-32,1
4955
 4970               	.L450:
4956
 4971 2ee6 1092 0000 		sts (ModelIsFlying)+1,__zero_reg__
4957
 4972 2eea 1092 0000 		sts ModelIsFlying,__zero_reg__
4958
 4973 2eee 8091 0000 		lds r24,ParamSet
4959
 4974 2ef2 482F      		mov r20,r24
4960
 4975 2ef4 50E0      		ldi r21,lo8(0)
4961
 4976 2ef6 440F      		lsl r20
4962
 4977 2ef8 551F      		rol r21
4963
 4978 2efa FA01      		movw r30,r20
4964
 4979 2efc E050      		subi r30,lo8(-(PPM_in))
4965
 4980 2efe F040      		sbci r31,hi8(-(PPM_in))
4966
 4981 2f00 8081      		ld r24,Z
4967
 4982 2f02 9181      		ldd r25,Z+1
4968
 4983 2f04 8734      		cpi r24,71
4969
 4984 2f06 9105      		cpc r25,__zero_reg__
4970
 4985 2f08 04F4      		brge .+4
4971
 4986 2f0a 0C94 0000 		jmp .L451
4972
 4987 2f0e 8091 0000 		lds r24,ParamSet+1
4973
 4988 2f12 A82F      		mov r26,r24
4974
 4989 2f14 B0E0      		ldi r27,lo8(0)
4975
 4990 2f16 AA0F      		lsl r26
4976
 4991 2f18 BB1F      		rol r27
4977
 4992               	.L452:
4978
 4993 2f1a FD01      		movw r30,r26
4979
 4994 2f1c E050      		subi r30,lo8(-(PPM_in))
4980
 4995 2f1e F040      		sbci r31,hi8(-(PPM_in))
4981
 4996 2f20 8081      		ld r24,Z
4982
 4997 2f22 9181      		ldd r25,Z+1
4983
 4998 2f24 8734      		cpi r24,71
4984
 4999 2f26 9105      		cpc r25,__zero_reg__
4985
 5000 2f28 04F0      		brlt .L455
4986
 5001 2f2a FA01      		movw r30,r20
4987
 5002 2f2c E050      		subi r30,lo8(-(PPM_in))
4988
 5003 2f2e F040      		sbci r31,hi8(-(PPM_in))
4989
 5004 2f30 0190      		ld __tmp_reg__,Z+
4990
 5005 2f32 F081      		ld r31,Z
4991
 5006 2f34 E02D      		mov r30,__tmp_reg__
4992
 5007               	.L455:
4993
 5008 2f36 FD01      		movw r30,r26
4994
 5009 2f38 E050      		subi r30,lo8(-(PPM_in))
4995
 5010 2f3a F040      		sbci r31,hi8(-(PPM_in))
4996
 5011 2f3c 8081      		ld r24,Z
4997
 5012 2f3e 9181      		ldd r25,Z+1
4998
 5013 2f40 8734      		cpi r24,71
4999
 5014 2f42 9105      		cpc r25,__zero_reg__
5000
 5015 2f44 04F4      		brge .+4
5001
 5016 2f46 0C94 0000 		jmp .L456
5002
 5017 2f4a FA01      		movw r30,r20
5003
 5018 2f4c E050      		subi r30,lo8(-(PPM_in))
5004
 5019 2f4e F040      		sbci r31,hi8(-(PPM_in))
5005
 5020 2f50 8081      		ld r24,Z
5006
 5021 2f52 9181      		ldd r25,Z+1
5007
 5022 2f54 8734      		cpi r24,71
5008
 5023 2f56 9105      		cpc r25,__zero_reg__
5009
 5024 2f58 04F4      		brge .+4
5010
 5025 2f5a 0C94 0000 		jmp .L456
5011
 5026 2f5e 22E0      		ldi r18,lo8(2)
5012
 5027               	.L457:
5013
 5028 2f60 FD01      		movw r30,r26
5014
 5029 2f62 E050      		subi r30,lo8(-(PPM_in))
5015
 5030 2f64 F040      		sbci r31,hi8(-(PPM_in))
5016
 5031 2f66 8081      		ld r24,Z
5017
 5032 2f68 9181      		ldd r25,Z+1
5018
 5033 2f6a 8634      		cpi r24,70
5019
 5034 2f6c 9105      		cpc r25,__zero_reg__
5020
 5035 2f6e 04F4      		brge .L458
5021
 5036 2f70 FA01      		movw r30,r20
5022
 5037 2f72 E050      		subi r30,lo8(-(PPM_in))
5023
 5038 2f74 F040      		sbci r31,hi8(-(PPM_in))
5024
 5039 2f76 8081      		ld r24,Z
5025
 5040 2f78 9181      		ldd r25,Z+1
5026
 5041 2f7a 8734      		cpi r24,71
5027
 5042 2f7c 9105      		cpc r25,__zero_reg__
5028
 5043 2f7e 04F0      		brlt .L458
5029
 5044 2f80 23E0      		ldi r18,lo8(3)
5030
 5045               	.L458:
5031
 5046 2f82 FD01      		movw r30,r26
5032
 5047 2f84 E050      		subi r30,lo8(-(PPM_in))
5033
 5048 2f86 F040      		sbci r31,hi8(-(PPM_in))
5034
 5049 2f88 8081      		ld r24,Z
5035
 5050 2f8a 9181      		ldd r25,Z+1
5036
 5051 2f8c 8A5B      		subi r24,lo8(-70)
5037
 5052 2f8e 9F4F      		sbci r25,hi8(-70)
5038
 5053 2f90 04F4      		brge .L459
5039
 5054 2f92 FA01      		movw r30,r20
5040
 5055 2f94 E050      		subi r30,lo8(-(PPM_in))
5041
 5056 2f96 F040      		sbci r31,hi8(-(PPM_in))
5042
 5057 2f98 8081      		ld r24,Z
5043
 5058 2f9a 9181      		ldd r25,Z+1
5044
 5059 2f9c 8734      		cpi r24,71
5045
 5060 2f9e 9105      		cpc r25,__zero_reg__
5046
 5061 2fa0 04F0      		brlt .L459
5047
 5062 2fa2 24E0      		ldi r18,lo8(4)
5048
 5063               	.L459:
5049
 5064 2fa4 A050      		subi r26,lo8(-(PPM_in))
5050
 5065 2fa6 B040      		sbci r27,hi8(-(PPM_in))
5051
 5066 2fa8 8D91      		ld r24,X+
5052
 5067 2faa 9C91      		ld r25,X
5053
 5068 2fac 8A5B      		subi r24,lo8(-70)
5054
 5069 2fae 9F4F      		sbci r25,hi8(-70)
5055
 5070 2fb0 04F4      		brge .L460
5056
 5071 2fb2 4050      		subi r20,lo8(-(PPM_in))
5057
 5072 2fb4 5040      		sbci r21,hi8(-(PPM_in))
5058
 5073 2fb6 DA01      		movw r26,r20
5059
 5074 2fb8 8D91      		ld r24,X+
5060
 5075 2fba 9C91      		ld r25,X
5061
 5076 2fbc 8634      		cpi r24,70
5062
 5077 2fbe 9105      		cpc r25,__zero_reg__
5063
 5078 2fc0 04F4      		brge .L460
5064
 5079 2fc2 25E0      		ldi r18,lo8(5)
5065
 5080               	.L460:
5066
 5081 2fc4 822F      		mov r24,r18
5067
 5082 2fc6 0E94 0000 		call SetActiveParamSet
5068
 5083 2fca 0E94 0000 		call GetActiveParamSet
5069
 5084 2fce 0E94 0000 		call ParamSet_ReadFromEEProm
5070
 5085 2fd2 0E94 0000 		call Servo_Off
5071
 5086 2fd6 80E0      		ldi r24,lo8(0)
5072
 5087 2fd8 0C94 0000 		jmp .L726
5073
 5088               	.L504:
5074
 5089 2fdc 8091 0000 		lds r24,ParamSet+48
5075
 5090 2fe0 282F      		mov r18,r24
5076
 5091 2fe2 30E0      		ldi r19,lo8(0)
5077
 5092 2fe4 8091 0000 		lds r24,StickGas
5078
 5093 2fe8 9091 0000 		lds r25,(StickGas)+1
5079
 5094 2fec 2817      		cp r18,r24
5080
 5095 2fee 3907      		cpc r19,r25
5081
 5096 2ff0 04F4      		brge .L506
5082
 5097 2ff2 3093 0000 		sts (StickGas)+1,r19
5083
 5098 2ff6 2093 0000 		sts StickGas,r18
5084
 5099               	.L506:
5085
 5100 2ffa 81E0      		ldi r24,lo8(1)
5086
 5101 2ffc 8093 0000 		sts FunnelCourse,r24
5087
 5102 3000 00C0      		rjmp .L505
5088
 5103               	.L759:
5089
 5104 3002 8091 0000 		lds r24,MKFlags
5090
 5105 3006 80FD      		sbrc r24,0
5091
 5106 3008 00C0      		rjmp .L576
5092
 5107 300a 0E94 0000 		call SetCompassCalState
5093
 5108 300e 9091 0000 		lds r25,LoopingNick
5094
 5109 3012 9D8F      		std Y+29,r25
5095
 5110               	.L728:
5096
 5111 3014 2090 0000 		lds r2,GyroYaw
5097
 5112 3018 3090 0000 		lds r3,(GyroYaw)+1
5098
 5113               	.L575:
5099
 5114 301c 8091 0000 		lds r24,TimerDebugOut.2419
5100
 5115 3020 8150      		subi r24,lo8(-(-1))
5101
 5116 3022 8093 0000 		sts TimerDebugOut.2419,r24
5102
 5117 3026 8F5F      		subi r24,lo8(-(1))
5103
 5118 3028 01F0      		breq .+2
5104
 5119 302a 00C0      		rjmp .L593
5105
 5120 302c 88E1      		ldi r24,lo8(24)
5106
 5121 302e 8093 0000 		sts TimerDebugOut.2419,r24
5107
 5122 3032 6091 0000 		lds r22,IntegralGyroNick
5108
 5123 3036 7091 0000 		lds r23,(IntegralGyroNick)+1
5109
 5124 303a 8091 0000 		lds r24,(IntegralGyroNick)+2
5110
 5125 303e 9091 0000 		lds r25,(IntegralGyroNick)+3
5111
 5126 3042 2AE0      		ldi r18,lo8(10)
5112
 5127 3044 30E0      		ldi r19,hi8(10)
5113
 5128 3046 40E0      		ldi r20,hlo8(10)
5114
 5129 3048 50E0      		ldi r21,hhi8(10)
5115
 5130 304a 0E94 0000 		call __mulsi3
5116
 5131 304e E090 0000 		lds r14,ParamSet+23
5117
 5132 3052 2AE2      		ldi r18,lo8(42)
5118
 5133 3054 E29E      		mul r14,r18
5119
 5134 3056 7001      		movw r14,r0
5120
 5135 3058 1124      		clr r1
5121
 5136 305a 0027      		clr r16
5122
 5137 305c F7FC      		sbrc r15,7
5123
 5138 305e 0095      		com r16
5124
 5139 3060 102F      		mov r17,r16
5125
 5140 3062 A801      		movw r20,r16
5126
 5141 3064 9701      		movw r18,r14
5127
 5142 3066 0E94 0000 		call __divmodsi4
5128
 5143 306a 3093 0000 		sts (DebugOut+2)+1,r19
5129
 5144 306e 2093 0000 		sts DebugOut+2,r18
5130
 5145 3072 6091 0000 		lds r22,IntegralGyroRoll
5131
 5146 3076 7091 0000 		lds r23,(IntegralGyroRoll)+1
5132
 5147 307a 8091 0000 		lds r24,(IntegralGyroRoll)+2
5133
 5148 307e 9091 0000 		lds r25,(IntegralGyroRoll)+3
5134
 5149 3082 2AE0      		ldi r18,lo8(10)
5135
 5150 3084 30E0      		ldi r19,hi8(10)
5136
 5151 3086 40E0      		ldi r20,hlo8(10)
5137
 5152 3088 50E0      		ldi r21,hhi8(10)
5138
 5153 308a 0E94 0000 		call __mulsi3
5139
 5154 308e A801      		movw r20,r16
5140
 5155 3090 9701      		movw r18,r14
5141
 5156 3092 0E94 0000 		call __divmodsi4
5142
 5157 3096 3093 0000 		sts (DebugOut+4)+1,r19
5143
 5158 309a 2093 0000 		sts DebugOut+4,r18
5144
 5159 309e 8091 0000 		lds r24,AccNick
5145
 5160 30a2 9091 0000 		lds r25,AccNick+1
5146
 5161 30a6 9C01      		movw r18,r24
5147
 5162 30a8 220F      		lsl r18
5148
 5163 30aa 331F      		rol r19
5149
 5164 30ac 220F      		lsl r18
5150
 5165 30ae 331F      		rol r19
5151
 5166 30b0 220F      		lsl r18
5152
 5167 30b2 331F      		rol r19
5153
 5168 30b4 880F      		lsl r24
5154
 5169 30b6 991F      		rol r25
5155
 5170 30b8 820F      		add r24,r18
5156
 5171 30ba 931F      		adc r25,r19
5157
 5172 30bc 6AE2      		ldi r22,lo8(42)
5158
 5173 30be 70E0      		ldi r23,hi8(42)
5159
 5174 30c0 0E94 0000 		call __divmodhi4
5160
 5175 30c4 7093 0000 		sts (DebugOut+6)+1,r23
5161
 5176 30c8 6093 0000 		sts DebugOut+6,r22
5162
 5177 30cc 8091 0000 		lds r24,AccRoll
5163
 5178 30d0 9091 0000 		lds r25,AccRoll+1
5164
 5179 30d4 9C01      		movw r18,r24
5165
 5180 30d6 220F      		lsl r18
5166
 5181 30d8 331F      		rol r19
5167
 5182 30da 220F      		lsl r18
5168
 5183 30dc 331F      		rol r19
5169
 5184 30de 220F      		lsl r18
5170
 5185 30e0 331F      		rol r19
5171
 5186 30e2 880F      		lsl r24
5172
 5187 30e4 991F      		rol r25
5173
 5188 30e6 820F      		add r24,r18
5174
 5189 30e8 931F      		adc r25,r19
5175
 5190 30ea 6AE2      		ldi r22,lo8(42)
5176
 5191 30ec 70E0      		ldi r23,hi8(42)
5177
 5192 30ee 0E94 0000 		call __divmodhi4
5178
 5193 30f2 7093 0000 		sts (DebugOut+8)+1,r23
5179
 5194 30f6 6093 0000 		sts DebugOut+8,r22
5180
 5195 30fa 3092 0000 		sts (DebugOut+10)+1,r3
5181
 5196 30fe 2092 0000 		sts DebugOut+10,r2
5182
 5197 3102 6091 0000 		lds r22,ReadingHeight
5183
 5198 3106 7091 0000 		lds r23,(ReadingHeight)+1
5184
 5199 310a 8091 0000 		lds r24,(ReadingHeight)+2
5185
 5200 310e 9091 0000 		lds r25,(ReadingHeight)+3
5186
 5201 3112 25E0      		ldi r18,lo8(5)
5187
 5202 3114 30E0      		ldi r19,hi8(5)
5188
 5203 3116 40E0      		ldi r20,hlo8(5)
5189
 5204 3118 50E0      		ldi r21,hhi8(5)
5190
 5205 311a 0E94 0000 		call __divmodsi4
5191
 5206 311e 3093 0000 		sts (DebugOut+12)+1,r19
5192
 5207 3122 2093 0000 		sts DebugOut+12,r18
5193
 5208 3126 6091 0000 		lds r22,ReadingIntegralTop
5194
 5209 312a 7091 0000 		lds r23,(ReadingIntegralTop)+1
5195
 5210 312e 8091 0000 		lds r24,(ReadingIntegralTop)+2
5196
 5211 3132 9091 0000 		lds r25,(ReadingIntegralTop)+3
5197
 5212 3136 20E0      		ldi r18,lo8(512)
5198
 5213 3138 32E0      		ldi r19,hi8(512)
5199
 5214 313a 40E0      		ldi r20,hlo8(512)
5200
 5215 313c 50E0      		ldi r21,hhi8(512)
5201
 5216 313e 0E94 0000 		call __divmodsi4
5202
 5217 3142 3093 0000 		sts (DebugOut+14)+1,r19
5203
 5218 3146 2093 0000 		sts DebugOut+14,r18
5204
 5219 314a 8091 0000 		lds r24,CompassHeading
5205
 5220 314e 9091 0000 		lds r25,(CompassHeading)+1
5206
 5221 3152 9093 0000 		sts (DebugOut+18)+1,r25
5207
 5222 3156 8093 0000 		sts DebugOut+18,r24
5208
 5223 315a 8091 0000 		lds r24,UBat
5209
 5224 315e 9091 0000 		lds r25,(UBat)+1
5210
 5225 3162 9093 0000 		sts (DebugOut+20)+1,r25
5211
 5226 3166 8093 0000 		sts DebugOut+20,r24
5212
 5227 316a 8091 0000 		lds r24,RC_Quality
5213
 5228 316e 8093 0000 		sts DebugOut+22,r24
5214
 5229 3172 1092 0000 		sts DebugOut+23,__zero_reg__
5215
 5230 3176 6091 0000 		lds r22,YawGyroHeading
5216
 5231 317a 7091 0000 		lds r23,(YawGyroHeading)+1
5217
 5232 317e 8091 0000 		lds r24,(YawGyroHeading)+2
5218
 5233 3182 9091 0000 		lds r25,(YawGyroHeading)+3
5219
 5234 3186 A801      		movw r20,r16
5220
 5235 3188 9701      		movw r18,r14
5221
 5236 318a 0E94 0000 		call __divmodsi4
5222
 5237 318e 3093 0000 		sts (DebugOut+24)+1,r19
5223
 5238 3192 2093 0000 		sts DebugOut+24,r18
5224
 5239 3196 8091 0000 		lds r24,CompassCalState
5225
 5240 319a 8093 0000 		sts DebugOut+40,r24
5226
 5241 319e 1092 0000 		sts DebugOut+41,__zero_reg__
5227
 5242 31a2 8091 0000 		lds r24,ServoNickValue
5228
 5243 31a6 9091 0000 		lds r25,(ServoNickValue)+1
5229
 5244 31aa 9093 0000 		sts (DebugOut+42)+1,r25
5230
 5245 31ae 8093 0000 		sts DebugOut+42,r24
5231
 5246 31b2 8091 0000 		lds r24,GPSStickNick
5232
 5247 31b6 9091 0000 		lds r25,(GPSStickNick)+1
5233
 5248 31ba 9093 0000 		sts (DebugOut+62)+1,r25
5234
 5249 31be 8093 0000 		sts DebugOut+62,r24
5235
 5250 31c2 8091 0000 		lds r24,GPSStickRoll
5236
 5251 31c6 9091 0000 		lds r25,(GPSStickRoll)+1
5237
 5252 31ca 9093 0000 		sts (DebugOut+64)+1,r25
5238
 5253 31ce 8093 0000 		sts DebugOut+64,r24
5239
 5254               	.L593:
5240
 5255 31d2 8090 0000 		lds r8,TrimNick
5241
 5256 31d6 9090 0000 		lds r9,(TrimNick)+1
5242
 5257 31da 58E3      		ldi r21,lo8(-200)
5243
 5258 31dc 8516      		cp r8,r21
5244
 5259 31de 5FEF      		ldi r21,hi8(-200)
5245
 5260 31e0 9506      		cpc r9,r21
5246
 5261 31e2 04F0      		brlt .+2
5247
 5262 31e4 00C0      		rjmp .L594
5248
 5263 31e6 88E3      		ldi r24,lo8(-200)
5249
 5264 31e8 9FEF      		ldi r25,hi8(-200)
5250
 5265 31ea 9093 0000 		sts (TrimNick)+1,r25
5251
 5266 31ee 8093 0000 		sts TrimNick,r24
5252
 5267 31f2 F8E3      		ldi r31,lo8(-200)
5253
 5268 31f4 8F2E      		mov r8,r31
5254
 5269 31f6 FFEF      		ldi r31,hi8(-200)
5255
 5270 31f8 9F2E      		mov r9,r31
5256
 5271               	.L595:
5257
 5272 31fa 8091 0000 		lds r24,TrimRoll
5258
 5273 31fe 9091 0000 		lds r25,(TrimRoll)+1
5259
 5274 3202 9F8F      		std Y+31,r25
5260
 5275 3204 8E8F      		std Y+30,r24
5261
 5276 3206 8853      		subi r24,lo8(-200)
5262
 5277 3208 9F4F      		sbci r25,hi8(-200)
5263
 5278 320a 04F0      		brlt .+2
5264
 5279 320c 00C0      		rjmp .L596
5265
 5280 320e 88E3      		ldi r24,lo8(-200)
5266
 5281 3210 9FEF      		ldi r25,hi8(-200)
5267
 5282 3212 9093 0000 		sts (TrimRoll)+1,r25
5268
 5283 3216 8093 0000 		sts TrimRoll,r24
5269
 5284 321a 9F8F      		std Y+31,r25
5270
 5285 321c 8E8F      		std Y+30,r24
5271
 5286               	.L597:
5272
 5287 321e 8091 0000 		lds r24,FunnelCourse
5273
 5288 3222 8823      		tst r24
5274
 5289 3224 01F0      		breq .L598
5275
 5290 3226 1092 0000 		sts IPartNick.2408,__zero_reg__
5276
 5291 322a 1092 0000 		sts (IPartNick.2408)+1,__zero_reg__
5277
 5292 322e 1092 0000 		sts (IPartNick.2408)+2,__zero_reg__
5278
 5293 3232 1092 0000 		sts (IPartNick.2408)+3,__zero_reg__
5279
 5294 3236 1092 0000 		sts IPartRoll.2409,__zero_reg__
5280
 5295 323a 1092 0000 		sts (IPartRoll.2409)+1,__zero_reg__
5281
 5296 323e 1092 0000 		sts (IPartRoll.2409)+2,__zero_reg__
5282
 5297 3242 1092 0000 		sts (IPartRoll.2409)+3,__zero_reg__
5283
 5298               	.L598:
5284
 5299 3246 FD8D      		ldd r31,Y+29
5285
 5300 3248 FF23      		tst r31
5286
 5301 324a 01F4      		brne .+2
5287
 5302 324c 00C0      		rjmp .L599
5288
 5303 324e 4424      		clr r4
5289
 5304 3250 5524      		clr r5
5290
 5305 3252 1C82      		std Y+4,__zero_reg__
5291
 5306 3254 1B82      		std Y+3,__zero_reg__
5292
 5307               	.L600:
5293
 5308 3256 A090 0000 		lds r10,GyroNick
5294
 5309 325a B090 0000 		lds r11,(GyroNick)+1
5295
 5310 325e BC8A      		std Y+20,r11
5296
 5311 3260 AB8A      		std Y+19,r10
5297
 5312 3262 8091 0000 		lds r24,GyroPFactor
5298
 5313 3266 282F      		mov r18,r24
5299
 5314 3268 30E0      		ldi r19,lo8(0)
5300
 5315 326a 40E0      		ldi r20,lo8(0)
5301
 5316 326c 50E0      		ldi r21,hi8(0)
5302
 5317 326e 2F87      		std Y+15,r18
5303
 5318 3270 388B      		std Y+16,r19
5304
 5319 3272 498B      		std Y+17,r20
5305
 5320 3274 5A8B      		std Y+18,r21
5306
 5321 3276 1091 0000 		lds r17,LoopingRoll
5307
 5322 327a 1123      		tst r17
5308
 5323 327c 01F4      		brne .+2
5309
 5324 327e 00C0      		rjmp .L601
5310
 5325 3280 1886      		std Y+8,__zero_reg__
5311
 5326 3282 1F82      		std Y+7,__zero_reg__
5312
 5327 3284 1E82      		std Y+6,__zero_reg__
5313
 5328 3286 1D82      		std Y+5,__zero_reg__
5314
 5329               	.L602:
5315
 5330 3288 4091 0000 		lds r20,GyroRoll
5316
 5331 328c 5091 0000 		lds r21,(GyroRoll)+1
5317
 5332 3290 5E87      		std Y+14,r21
5318
 5333 3292 4D87      		std Y+13,r20
5319
 5334 3294 6090 0000 		lds r6,GyroYawPFactor
5320
 5335 3298 7090 0000 		lds r7,GyroYawIFactor
5321
 5336 329c 6091 0000 		lds r22,IntegralGyroYaw
5322
 5337 32a0 7091 0000 		lds r23,(IntegralGyroYaw)+1
5323
 5338 32a4 8091 0000 		lds r24,(IntegralGyroYaw)+2
5324
 5339 32a8 9091 0000 		lds r25,(IntegralGyroYaw)+3
5325
 5340 32ac 6987      		std Y+9,r22
5326
 5341 32ae 7A87      		std Y+10,r23
5327
 5342 32b0 8B87      		std Y+11,r24
5328
 5343 32b2 9C87      		std Y+12,r25
5329
 5344 32b4 4091 0000 		lds r20,StickGas
5330
 5345 32b8 5091 0000 		lds r21,(StickGas)+1
5331
 5346 32bc 2091 0000 		lds r18,LowVoltageWarning
5332
 5347 32c0 8091 0000 		lds r24,UBat
5333
 5348 32c4 9091 0000 		lds r25,(UBat)+1
5334
 5349 32c8 30E0      		ldi r19,lo8(0)
5335
 5350 32ca 2817      		cp r18,r24
5336
 5351 32cc 3907      		cpc r19,r25
5337
 5352 32ce 04F4      		brge .L603
5338
 5353 32d0 6091 0000 		lds r22,UBat
5339
 5354 32d4 7091 0000 		lds r23,(UBat)+1
5340
 5355 32d8 429F      		mul r20,r18
5341
 5356 32da C001      		movw r24,r0
5342
 5357 32dc 439F      		mul r20,r19
5343
 5358 32de 900D      		add r25,r0
5344
 5359 32e0 529F      		mul r21,r18
5345
 5360 32e2 900D      		add r25,r0
5346
 5361 32e4 1124      		clr r1
5347
 5362 32e6 0E94 0000 		call __udivmodhi4
5348
 5363 32ea AB01      		movw r20,r22
5349
 5364               	.L603:
5350
 5365 32ec CA01      		movw r24,r20
5351
 5366 32ee 880F      		lsl r24
5352
 5367 32f0 991F      		rol r25
5353
 5368 32f2 880F      		lsl r24
5354
 5369 32f4 991F      		rol r25
5355
 5370 32f6 99AF      		std Y+57,r25
5356
 5371 32f8 88AF      		std Y+56,r24
5357
 5372 32fa 8091 0000 		lds r24,ParamSet+8
5358
 5373 32fe 80FF      		sbrs r24,0
5359
 5374 3300 00C0      		rjmp .L604
5360
 5375 3302 1123      		tst r17
5361
 5376 3304 01F4      		brne .+2
5362
 5377 3306 00C0      		rjmp .L777
5363
 5378               	.L604:
5364
 5379 3308 8091 0000 		lds r24,ParamSet+21
5365
 5380 330c 282F      		mov r18,r24
5366
 5381 330e 30E0      		ldi r19,lo8(0)
5367
 5382 3310 265F      		subi r18,lo8(-(10))
5368
 5383 3312 3F4F      		sbci r19,hi8(-(10))
5369
 5384 3314 5901      		movw r10,r18
5370
 5385 3316 AA0C      		lsl r10
5371
 5386 3318 BB1C      		rol r11
5372
 5387 331a AA0C      		lsl r10
5373
 5388 331c BB1C      		rol r11
5374
 5389 331e 2A50      		subi r18,lo8(-(-10))
5375
 5390 3320 3040      		sbci r19,hi8(-(-10))
5376
 5391 3322 C8AC      		ldd r12,Y+56
5377
 5392 3324 D9AC      		ldd r13,Y+57
5378
 5393 3326 CA14      		cp r12,r10
5379
 5394 3328 DB04      		cpc r13,r11
5380
 5395 332a 04F0      		brlt .L669
5381
 5396 332c 8091 0000 		lds r24,ParamSet+22
5382
 5397 3330 90E0      		ldi r25,lo8(0)
5383
 5398 3332 4497      		sbiw r24,20
5384
 5399 3334 880F      		lsl r24
5385
 5400 3336 991F      		rol r25
5386
 5401 3338 880F      		lsl r24
5387
 5402 333a 991F      		rol r25
5388
 5403 333c 5601      		movw r10,r12
5389
 5404 333e 8C15      		cp r24,r12
5390
 5405 3340 9D05      		cpc r25,r13
5391
 5406 3342 04F4      		brge .+2
5392
 5407 3344 00C0      		rjmp .L778
5393
 5408               	.L669:
5394
 5409 3346 8091 0000 		lds r24,MissingMotor
5395
 5410 334a 8823      		tst r24
5396
 5411 334c 01F0      		breq .L671
5397
 5412 334e 8091 0000 		lds r24,ModelIsFlying
5398
 5413 3352 9091 0000 		lds r25,(ModelIsFlying)+1
5399
 5414 3356 0297      		sbiw r24,2
5400
 5415 3358 C097      		sbiw r24,48
5401
 5416 335a 00F4      		brsh .+2
5402
 5417 335c 00C0      		rjmp .L779
5403
 5418               	.L672:
5404
 5419 335e 5901      		movw r10,r18
5405
 5420 3360 AA0C      		lsl r10
5406
 5421 3362 BB1C      		rol r11
5407
 5422 3364 AA0C      		lsl r10
5408
 5423 3366 BB1C      		rol r11
5409
 5424               	.L671:
5410
 5425 3368 B092 0000 		sts (DebugOut+16)+1,r11
5411
 5426 336c A092 0000 		sts DebugOut+16,r10
5412
 5427 3370 220C      		lsl r2
5413
 5428 3372 331C      		rol r3
5414
 5429 3374 B101      		movw r22,r2
5415
 5430 3376 8827      		clr r24
5416
 5431 3378 77FD      		sbrc r23,7
5417
 5432 337a 8095      		com r24
5418
 5433 337c 982F      		mov r25,r24
5419
 5434 337e 262D      		mov r18,r6
5420
 5435 3380 30E0      		ldi r19,lo8(0)
5421
 5436 3382 40E0      		ldi r20,lo8(0)
5422
 5437 3384 50E0      		ldi r21,hi8(0)
5423
 5438 3386 0E94 0000 		call __mulsi3
5424
 5439 338a 20E4      		ldi r18,lo8(64)
5425
 5440 338c 30E0      		ldi r19,hi8(64)
5426
 5441 338e 40E0      		ldi r20,hlo8(64)
5427
 5442 3390 50E0      		ldi r21,hhi8(64)
5428
 5443 3392 0E94 0000 		call __divmodsi4
5429
 5444 3396 7901      		movw r14,r18
5430
 5445 3398 8A01      		movw r16,r20
5431
 5446 339a 672D      		mov r22,r7
5432
 5447 339c 70E0      		ldi r23,lo8(0)
5433
 5448 339e 80E0      		ldi r24,lo8(0)
5434
 5449 33a0 90E0      		ldi r25,hi8(0)
5435
 5450 33a2 2985      		ldd r18,Y+9
5436
 5451 33a4 3A85      		ldd r19,Y+10
5437
 5452 33a6 4B85      		ldd r20,Y+11
5438
 5453 33a8 5C85      		ldd r21,Y+12
5439
 5454 33aa 0E94 0000 		call __mulsi3
5440
 5455 33ae 20EF      		ldi r18,lo8(22000)
5441
 5456 33b0 35E5      		ldi r19,hi8(22000)
5442
 5457 33b2 40E0      		ldi r20,hlo8(22000)
5443
 5458 33b4 50E0      		ldi r21,hhi8(22000)
5444
 5459 33b6 0E94 0000 		call __divmodsi4
5445
 5460 33ba C701      		movw r24,r14
5446
 5461 33bc 820F      		add r24,r18
5447
 5462 33be 931F      		adc r25,r19
5448
 5463 33c0 20E4      		ldi r18,hi8(16385)
5449
 5464 33c2 8130      		cpi r24,lo8(16385)
5450
 5465 33c4 9207      		cpc r25,r18
5451
 5466 33c6 04F0      		brlt .L673
5452
 5467 33c8 80E0      		ldi r24,lo8(16384)
5453
 5468 33ca 90E4      		ldi r25,hi8(16384)
5454
 5469               	.L673:
5455
 5470 33cc 8C01      		movw r16,r24
5456
 5471 33ce 30EC      		ldi r19,hi8(-16384)
5457
 5472 33d0 8030      		cpi r24,lo8(-16384)
5458
 5473 33d2 9307      		cpc r25,r19
5459
 5474 33d4 04F4      		brge .L674
5460
 5475 33d6 00E0      		ldi r16,lo8(-16384)
5461
 5476 33d8 10EC      		ldi r17,hi8(-16384)
5462
 5477               	.L674:
5463
 5478 33da 6091 0000 		lds r22,SetPointYaw.2410
5464
 5479 33de 7091 0000 		lds r23,(SetPointYaw.2410)+1
5465
 5480 33e2 8091 0000 		lds r24,(SetPointYaw.2410)+2
5466
 5481 33e6 9091 0000 		lds r25,(SetPointYaw.2410)+3
5467
 5482 33ea 2CEF      		ldi r18,lo8(-4)
5468
 5483 33ec 3FEF      		ldi r19,hi8(-4)
5469
 5484 33ee 4FEF      		ldi r20,hlo8(-4)
5470
 5485 33f0 5FEF      		ldi r21,hhi8(-4)
5471
 5486 33f2 0E94 0000 		call __mulsi3
5472
 5487 33f6 7801      		movw r14,r16
5473
 5488 33f8 E60E      		add r14,r22
5474
 5489 33fa F71E      		adc r15,r23
5475
 5490 33fc 41EA      		ldi r20,lo8(161)
5476
 5491 33fe A416      		cp r10,r20
5477
 5492 3400 B104      		cpc r11,__zero_reg__
5478
 5493 3402 04F4      		brge .+2
5479
 5494 3404 00C0      		rjmp .L675
5480
 5495 3406 C501      		movw r24,r10
5481
 5496 3408 B7FE      		sbrs r11,7
5482
 5497 340a 00C0      		rjmp .+4
5483
 5498 340c 0C94 0000 		jmp .L780
5484
 5499               	.L676:
5485
 5500 3410 9595      		asr r25
5486
 5501 3412 8795      		ror r24
5487
 5502 3414 2227      		clr r18
5488
 5503 3416 3327      		clr r19
5489
 5504 3418 281B      		sub r18,r24
5490
 5505 341a 390B      		sbc r19,r25
5491
 5506 341c E216      		cp r14,r18
5492
 5507 341e F306      		cpc r15,r19
5493
 5508 3420 04F0      		brlt .+2
5494
 5509 3422 00C0      		rjmp .L781
5495
 5510               	.L677:
5496
 5511 3424 8091 0000 		lds r24,ParamSet+22
5497
 5512 3428 90E0      		ldi r25,lo8(0)
5498
 5513 342a 880F      		lsl r24
5499
 5514 342c 991F      		rol r25
5500
 5515 342e 880F      		lsl r24
5501
 5516 3430 991F      		rol r25
5502
 5517 3432 3501      		movw r6,r10
5503
 5518 3434 681A      		sub r6,r24
5504
 5519 3436 790A      		sbc r7,r25
5505
 5520 3438 2615      		cp r18,r6
5506
 5521 343a 3705      		cpc r19,r7
5507
 5522 343c 04F0      		brlt .L681
5508
 5523 343e 8A19      		sub r24,r10
5509
 5524 3440 9B09      		sbc r25,r11
5510
 5525 3442 3901      		movw r6,r18
5511
 5526 3444 8217      		cp r24,r18
5512
 5527 3446 9307      		cpc r25,r19
5513
 5528 3448 04F4      		brge .+2
5514
 5529 344a 00C0      		rjmp .L782
5515
 5530               	.L681:
5516
 5531 344c 6091 0000 		lds r22,StickNick
5517
 5532 3450 7091 0000 		lds r23,(StickNick)+1
5518
 5533 3454 79AF      		std Y+57,r23
5519
 5534 3456 68AF      		std Y+56,r22
5520
 5535 3458 7401      		movw r14,r8
5521
 5536 345a 0027      		clr r16
5522
 5537 345c F7FC      		sbrc r15,7
5523
 5538 345e 0095      		com r16
5524
 5539 3460 102F      		mov r17,r16
5525
 5540 3462 EE0C      		lsl r14
5526
 5541 3464 FF1C      		rol r15
5527
 5542 3466 001F      		rol r16
5528
 5543 3468 111F      		rol r17
5529
 5544 346a EE0C      		lsl r14
5530
 5545 346c FF1C      		rol r15
5531
 5546 346e 001F      		rol r16
5532
 5547 3470 111F      		rol r17
5533
 5548 3472 EE0C      		lsl r14
5534
 5549 3474 FF1C      		rol r15
5535
 5550 3476 001F      		rol r16
5536
 5551 3478 111F      		rol r17
5537
 5552 347a EE0C      		lsl r14
5538
 5553 347c FF1C      		rol r15
5539
 5554 347e 001F      		rol r16
5540
 5555 3480 111F      		rol r17
5541
 5556 3482 EE0C      		lsl r14
5542
 5557 3484 FF1C      		rol r15
5543
 5558 3486 001F      		rol r16
5544
 5559 3488 111F      		rol r17
5545
 5560 348a EE0C      		lsl r14
5546
 5561 348c FF1C      		rol r15
5547
 5562 348e 001F      		rol r16
5548
 5563 3490 111F      		rol r17
5549
 5564 3492 EE0C      		lsl r14
5550
 5565 3494 FF1C      		rol r15
5551
 5566 3496 001F      		rol r16
5552
 5567 3498 111F      		rol r17
5553
 5568 349a 8B89      		ldd r24,Y+19
5554
 5569 349c 9C89      		ldd r25,Y+20
5555
 5570 349e 9C01      		movw r18,r24
5556
 5571 34a0 4427      		clr r20
5557
 5572 34a2 37FD      		sbrc r19,7
5558
 5573 34a4 4095      		com r20
5559
 5574 34a6 542F      		mov r21,r20
5560
 5575 34a8 6F85      		ldd r22,Y+15
5561
 5576 34aa 7889      		ldd r23,Y+16
5562
 5577 34ac 8989      		ldd r24,Y+17
5563
 5578 34ae 9A89      		ldd r25,Y+18
5564
 5579 34b0 0E94 0000 		call __mulsi3
5565
 5580 34b4 E60E      		add r14,r22
5566
 5581 34b6 F71E      		adc r15,r23
5567
 5582 34b8 081F      		adc r16,r24
5568
 5583 34ba 191F      		adc r17,r25
5569
 5584 34bc C801      		movw r24,r16
5570
 5585 34be B701      		movw r22,r14
5571
 5586 34c0 20E4      		ldi r18,lo8(64)
5572
 5587 34c2 30E0      		ldi r19,hi8(64)
5573
 5588 34c4 40E0      		ldi r20,hlo8(64)
5574
 5589 34c6 50E0      		ldi r21,hhi8(64)
5575
 5590 34c8 0E94 0000 		call __divmodsi4
5576
 5591 34cc AB81      		ldd r26,Y+3
5577
 5592 34ce BC81      		ldd r27,Y+4
5578
 5593 34d0 2A0F      		add r18,r26
5579
 5594 34d2 3B1F      		adc r19,r27
5580
 5595 34d4 B0E4      		ldi r27,hi8(16385)
5581
 5596 34d6 2130      		cpi r18,lo8(16385)
5582
 5597 34d8 3B07      		cpc r19,r27
5583
 5598 34da 04F0      		brlt .L683
5584
 5599 34dc 20E0      		ldi r18,lo8(16384)
5585
 5600 34de 30E4      		ldi r19,hi8(16384)
5586
 5601               	.L683:
5587
 5602 34e0 E0EC      		ldi r30,hi8(-16384)
5588
 5603 34e2 2030      		cpi r18,lo8(-16384)
5589
 5604 34e4 3E07      		cpc r19,r30
5590
 5605 34e6 04F4      		brge .L684
5591
 5606 34e8 20E0      		ldi r18,lo8(-16384)
5592
 5607 34ea 30EC      		ldi r19,hi8(-16384)
5593
 5608               	.L684:
5594
 5609 34ec 6901      		movw r12,r18
5595
 5610 34ee 48AD      		ldd r20,Y+56
5596
 5611 34f0 59AD      		ldd r21,Y+57
5597
 5612 34f2 C41A      		sub r12,r20
5598
 5613 34f4 D50A      		sbc r13,r21
5599
 5614 34f6 3090 0000 		lds r3,GyroIFactor
5600
 5615 34fa 3320      		tst r3
5601
 5616 34fc 01F4      		brne .+2
5602
 5617 34fe 00C0      		rjmp .L685
5603
 5618 3500 441A      		sub r4,r20
5604
 5619 3502 550A      		sbc r5,r21
5605
 5620 3504 C201      		movw r24,r4
5606
 5621 3506 AA27      		clr r26
5607
 5622 3508 97FD      		sbrc r25,7
5608
 5623 350a A095      		com r26
5609
 5624 350c BA2F      		mov r27,r26
5610
 5625 350e E090 0000 		lds r14,IPartNick.2408
5611
 5626 3512 F090 0000 		lds r15,(IPartNick.2408)+1
5612
 5627 3516 0091 0000 		lds r16,(IPartNick.2408)+2
5613
 5628 351a 1091 0000 		lds r17,(IPartNick.2408)+3
5614
 5629 351e E80E      		add r14,r24
5615
 5630 3520 F91E      		adc r15,r25
5616
 5631 3522 0A1F      		adc r16,r26
5617
 5632 3524 1B1F      		adc r17,r27
5618
 5633 3526 E092 0000 		sts IPartNick.2408,r14
5619
 5634 352a F092 0000 		sts (IPartNick.2408)+1,r15
5620
 5635 352e 0093 0000 		sts (IPartNick.2408)+2,r16
5621
 5636 3532 1093 0000 		sts (IPartNick.2408)+3,r17
5622
 5637               	.L686:
5623
 5638 3536 50E0      		ldi r21,lo8(-64000)
5624
 5639 3538 E516      		cp r14,r21
5625
 5640 353a 56E0      		ldi r21,hi8(-64000)
5626
 5641 353c F506      		cpc r15,r21
5627
 5642 353e 5FEF      		ldi r21,hlo8(-64000)
5628
 5643 3540 0507      		cpc r16,r21
5629
 5644 3542 5FEF      		ldi r21,hhi8(-64000)
5630
 5645 3544 1507      		cpc r17,r21
5631
 5646 3546 04F0      		brlt .+2
5632
 5647 3548 00C0      		rjmp .L687
5633
 5648 354a 80E0      		ldi r24,lo8(-64000)
5634
 5649 354c 96E0      		ldi r25,hi8(-64000)
5635
 5650 354e AFEF      		ldi r26,hlo8(-64000)
5636
 5651 3550 BFEF      		ldi r27,hhi8(-64000)
5637
 5652 3552 8093 0000 		sts IPartNick.2408,r24
5638
 5653 3556 9093 0000 		sts (IPartNick.2408)+1,r25
5639
 5654 355a A093 0000 		sts (IPartNick.2408)+2,r26
5640
 5655 355e B093 0000 		sts (IPartNick.2408)+3,r27
5641
 5656 3562 E12C      		mov r14,__zero_reg__
5642
 5657 3564 36E0      		ldi r19,hi8(-64000)
5643
 5658 3566 F32E      		mov r15,r19
5644
 5659 3568 3FEF      		ldi r19,hlo8(-64000)
5645
 5660 356a 032F      		mov r16,r19
5646
 5661 356c 3FEF      		ldi r19,hhi8(-64000)
5647
 5662 356e 132F      		mov r17,r19
5648
 5663               	.L688:
5649
 5664 3570 4090 0000 		lds r4,Ki
5650
 5665 3574 5090 0000 		lds r5,(Ki)+1
5651
 5666 3578 9201      		movw r18,r4
5652
 5667 357a 4427      		clr r20
5653
 5668 357c 37FD      		sbrc r19,7
5654
 5669 357e 4095      		com r20
5655
 5670 3580 542F      		mov r21,r20
5656
 5671 3582 C801      		movw r24,r16
5657
 5672 3584 B701      		movw r22,r14
5658
 5673 3586 0E94 0000 		call __divmodsi4
5659
 5674 358a 4601      		movw r8,r12
5660
 5675 358c 820E      		add r8,r18
5661
 5676 358e 931E      		adc r9,r19
5662
 5677 3590 8091 0000 		lds r24,StickRoll
5663
 5678 3594 9091 0000 		lds r25,(StickRoll)+1
5664
 5679 3598 99AF      		std Y+57,r25
5665
 5680 359a 88AF      		std Y+56,r24
5666
 5681 359c AE8D      		ldd r26,Y+30
5667
 5682 359e BF8D      		ldd r27,Y+31
5668
 5683 35a0 7D01      		movw r14,r26
5669
 5684 35a2 0027      		clr r16
5670
 5685 35a4 F7FC      		sbrc r15,7
5671
 5686 35a6 0095      		com r16
5672
 5687 35a8 102F      		mov r17,r16
5673
 5688 35aa EE0C      		lsl r14
5674
 5689 35ac FF1C      		rol r15
5675
 5690 35ae 001F      		rol r16
5676
 5691 35b0 111F      		rol r17
5677
 5692 35b2 EE0C      		lsl r14
5678
 5693 35b4 FF1C      		rol r15
5679
 5694 35b6 001F      		rol r16
5680
 5695 35b8 111F      		rol r17
5681
 5696 35ba EE0C      		lsl r14
5682
 5697 35bc FF1C      		rol r15
5683
 5698 35be 001F      		rol r16
5684
 5699 35c0 111F      		rol r17
5685
 5700 35c2 EE0C      		lsl r14
5686
 5701 35c4 FF1C      		rol r15
5687
 5702 35c6 001F      		rol r16
5688
 5703 35c8 111F      		rol r17
5689
 5704 35ca EE0C      		lsl r14
5690
 5705 35cc FF1C      		rol r15
5691
 5706 35ce 001F      		rol r16
5692
 5707 35d0 111F      		rol r17
5693
 5708 35d2 EE0C      		lsl r14
5694
 5709 35d4 FF1C      		rol r15
5695
 5710 35d6 001F      		rol r16
5696
 5711 35d8 111F      		rol r17
5697
 5712 35da EE0C      		lsl r14
5698
 5713 35dc FF1C      		rol r15
5699
 5714 35de 001F      		rol r16
5700
 5715 35e0 111F      		rol r17
5701
 5716 35e2 ED85      		ldd r30,Y+13
5702
 5717 35e4 FE85      		ldd r31,Y+14
5703
 5718 35e6 BF01      		movw r22,r30
5704
 5719 35e8 8827      		clr r24
5705
 5720 35ea 77FD      		sbrc r23,7
5706
 5721 35ec 8095      		com r24
5707
 5722 35ee 982F      		mov r25,r24
5708
 5723 35f0 2F85      		ldd r18,Y+15
5709
 5724 35f2 3889      		ldd r19,Y+16
5710
 5725 35f4 4989      		ldd r20,Y+17
5711
 5726 35f6 5A89      		ldd r21,Y+18
5712
 5727 35f8 0E94 0000 		call __mulsi3
5713
 5728 35fc E60E      		add r14,r22
5714
 5729 35fe F71E      		adc r15,r23
5715
 5730 3600 081F      		adc r16,r24
5716
 5731 3602 191F      		adc r17,r25
5717
 5732 3604 C801      		movw r24,r16
5718
 5733 3606 B701      		movw r22,r14
5719
 5734 3608 20E4      		ldi r18,lo8(64)
5720
 5735 360a 30E0      		ldi r19,hi8(64)
5721
 5736 360c 40E0      		ldi r20,hlo8(64)
5722
 5737 360e 50E0      		ldi r21,hhi8(64)
5723
 5738 3610 0E94 0000 		call __divmodsi4
5724
 5739 3614 CD80      		ldd r12,Y+5
5725
 5740 3616 DE80      		ldd r13,Y+6
5726
 5741 3618 2C0D      		add r18,r12
5727
 5742 361a 3D1D      		adc r19,r13
5728
 5743 361c 40E4      		ldi r20,hi8(16385)
5729
 5744 361e 2130      		cpi r18,lo8(16385)
5730
 5745 3620 3407      		cpc r19,r20
5731
 5746 3622 04F0      		brlt .L689
5732
 5747 3624 20E0      		ldi r18,lo8(16384)
5733
 5748 3626 30E4      		ldi r19,hi8(16384)
5734
 5749               	.L689:
5735
 5750 3628 50EC      		ldi r21,hi8(-16384)
5736
 5751 362a 2030      		cpi r18,lo8(-16384)
5737
 5752 362c 3507      		cpc r19,r21
5738
 5753 362e 04F4      		brge .L690
5739
 5754 3630 20E0      		ldi r18,lo8(-16384)
5740
 5755 3632 30EC      		ldi r19,hi8(-16384)
5741
 5756               	.L690:
5742
 5757 3634 6901      		movw r12,r18
5743
 5758 3636 68AD      		ldd r22,Y+56
5744
 5759 3638 79AD      		ldd r23,Y+57
5745
 5760 363a C61A      		sub r12,r22
5746
 5761 363c D70A      		sbc r13,r23
5747
 5762 363e 3320      		tst r3
5748
 5763 3640 01F4      		brne .+2
5749
 5764 3642 00C0      		rjmp .L691
5750
 5765 3644 8F81      		ldd r24,Y+7
5751
 5766 3646 9885      		ldd r25,Y+8
5752
 5767 3648 861B      		sub r24,r22
5753
 5768 364a 970B      		sbc r25,r23
5754
 5769 364c FC01      		movw r30,r24
5755
 5770 364e CF01      		movw r24,r30
5756
 5771 3650 AA27      		clr r26
5757
 5772 3652 97FD      		sbrc r25,7
5758
 5773 3654 A095      		com r26
5759
 5774 3656 BA2F      		mov r27,r26
5760
 5775 3658 E090 0000 		lds r14,IPartRoll.2409
5761
 5776 365c F090 0000 		lds r15,(IPartRoll.2409)+1
5762
 5777 3660 0091 0000 		lds r16,(IPartRoll.2409)+2
5763
 5778 3664 1091 0000 		lds r17,(IPartRoll.2409)+3
5764
 5779 3668 E80E      		add r14,r24
5765
 5780 366a F91E      		adc r15,r25
5766
 5781 366c 0A1F      		adc r16,r26
5767
 5782 366e 1B1F      		adc r17,r27
5768
 5783 3670 E092 0000 		sts IPartRoll.2409,r14
5769
 5784 3674 F092 0000 		sts (IPartRoll.2409)+1,r15
5770
 5785 3678 0093 0000 		sts (IPartRoll.2409)+2,r16
5771
 5786 367c 1093 0000 		sts (IPartRoll.2409)+3,r17
5772
 5787               	.L692:
5773
 5788 3680 F0E0      		ldi r31,lo8(-64000)
5774
 5789 3682 EF16      		cp r14,r31
5775
 5790 3684 F6E0      		ldi r31,hi8(-64000)
5776
 5791 3686 FF06      		cpc r15,r31
5777
 5792 3688 FFEF      		ldi r31,hlo8(-64000)
5778
 5793 368a 0F07      		cpc r16,r31
5779
 5794 368c FFEF      		ldi r31,hhi8(-64000)
5780
 5795 368e 1F07      		cpc r17,r31
5781
 5796 3690 04F0      		brlt .+2
5782
 5797 3692 00C0      		rjmp .L693
5783
 5798 3694 80E0      		ldi r24,lo8(-64000)
5784
 5799 3696 96E0      		ldi r25,hi8(-64000)
5785
 5800 3698 AFEF      		ldi r26,hlo8(-64000)
5786
 5801 369a BFEF      		ldi r27,hhi8(-64000)
5787
 5802 369c 8093 0000 		sts IPartRoll.2409,r24
5788
 5803 36a0 9093 0000 		sts (IPartRoll.2409)+1,r25
5789
 5804 36a4 A093 0000 		sts (IPartRoll.2409)+2,r26
5790
 5805 36a8 B093 0000 		sts (IPartRoll.2409)+3,r27
5791
 5806 36ac E12C      		mov r14,__zero_reg__
5792
 5807 36ae F6E0      		ldi r31,hi8(-64000)
5793
 5808 36b0 FF2E      		mov r15,r31
5794
 5809 36b2 FFEF      		ldi r31,hlo8(-64000)
5795
 5810 36b4 0F2F      		mov r16,r31
5796
 5811 36b6 FFEF      		ldi r31,hhi8(-64000)
5797
 5812 36b8 1F2F      		mov r17,r31
5798
 5813               	.L694:
5799
 5814 36ba 9201      		movw r18,r4
5800
 5815 36bc 4427      		clr r20
5801
 5816 36be 37FD      		sbrc r19,7
5802
 5817 36c0 4095      		com r20
5803
 5818 36c2 542F      		mov r21,r20
5804
 5819 36c4 C801      		movw r24,r16
5805
 5820 36c6 B701      		movw r22,r14
5806
 5821 36c8 0E94 0000 		call __divmodsi4
5807
 5822 36cc 8601      		movw r16,r12
5808
 5823 36ce 020F      		add r16,r18
5809
 5824 36d0 131F      		adc r17,r19
5810
 5825 36d2 9301      		movw r18,r6
5811
 5826 36d4 77FC      		sbrc r7,7
5812
 5827 36d6 00C0      		rjmp .L783
5813
 5828               	.L695:
5814
 5829 36d8 3595      		asr r19
5815
 5830 36da 2795      		ror r18
5816
 5831 36dc 2A0D      		add r18,r10
5817
 5832 36de 3B1D      		adc r19,r11
5818
 5833 36e0 B901      		movw r22,r18
5819
 5834 36e2 8827      		clr r24
5820
 5835 36e4 77FD      		sbrc r23,7
5821
 5836 36e6 8095      		com r24
5822
 5837 36e8 982F      		mov r25,r24
5823
 5838 36ea 2091 0000 		lds r18,FCParam+27
5824
 5839 36ee 30E0      		ldi r19,lo8(0)
5825
 5840 36f0 40E0      		ldi r20,lo8(0)
5826
 5841 36f2 50E0      		ldi r21,hi8(0)
5827
 5842 36f4 0E94 0000 		call __mulsi3
5828
 5843 36f8 20E4      		ldi r18,lo8(64)
5829
 5844 36fa 30E0      		ldi r19,hi8(64)
5830
 5845 36fc 40E0      		ldi r20,hlo8(64)
5831
 5846 36fe 50E0      		ldi r21,hhi8(64)
5832
 5847 3700 0E94 0000 		call __divmodsi4
5833
 5848 3704 B901      		movw r22,r18
5834
 5849 3706 8827      		clr r24
5835
 5850 3708 9927      		clr r25
5836
 5851 370a 821B      		sub r24,r18
5837
 5852 370c 930B      		sbc r25,r19
5838
 5853 370e 8816      		cp r8,r24
5839
 5854 3710 9906      		cpc r9,r25
5840
 5855 3712 04F0      		brlt .+2
5841
 5856 3714 00C0      		rjmp .L696
5842
 5857 3716 9C01      		movw r18,r24
5843
 5858               	.L697:
5844
 5859 3718 0817      		cp r16,r24
5845
 5860 371a 1907      		cpc r17,r25
5846
 5861 371c 04F0      		brlt .L700
5847
 5862 371e CB01      		movw r24,r22
5848
 5863 3720 0617      		cp r16,r22
5849
 5864 3722 1707      		cpc r17,r23
5850
 5865 3724 04F4      		brge .+2
5851
 5866 3726 00C0      		rjmp .L784
5852
 5867               	.L700:
5853
 5868 3728 60E0      		ldi r22,lo8(Mixer+13)
5854
 5869 372a 262E      		mov r2,r22
5855
 5870 372c 60E0      		ldi r22,hi8(Mixer+13)
5856
 5871 372e 362E      		mov r3,r22
5857
 5872 3730 50E0      		ldi r21,lo8(Motor)
5858
 5873 3732 852E      		mov r8,r21
5859
 5874 3734 50E0      		ldi r21,hi8(Motor)
5860
 5875 3736 952E      		mov r9,r21
5861
 5876 3738 40E0      		ldi r20,lo8(MotorValue.2421)
5862
 5877 373a C42E      		mov r12,r20
5863
 5878 373c 40E0      		ldi r20,hi8(MotorValue.2421)
5864
 5879 373e D42E      		mov r13,r20
5865
 5880 3740 A301      		movw r20,r6
5866
 5881 3742 6627      		clr r22
5867
 5882 3744 57FD      		sbrc r21,7
5868
 5883 3746 6095      		com r22
5869
 5884 3748 762F      		mov r23,r22
5870
 5885 374a 4CA7      		std Y+44,r20
5871
 5886 374c 5DA7      		std Y+45,r21
5872
 5887 374e 6EA7      		std Y+46,r22
5873
 5888 3750 7FA7      		std Y+47,r23
5874
 5889 3752 A501      		movw r20,r10
5875
 5890 3754 6627      		clr r22
5876
 5891 3756 57FD      		sbrc r21,7
5877
 5892 3758 6095      		com r22
5878
 5893 375a 762F      		mov r23,r22
5879
 5894 375c 48AB      		std Y+48,r20
5880
 5895 375e 59AB      		std Y+49,r21
5881
 5896 3760 6AAB      		std Y+50,r22
5882
 5897 3762 7BAB      		std Y+51,r23
5883
 5898 3764 A901      		movw r20,r18
5884
 5899 3766 6627      		clr r22
5885
 5900 3768 57FD      		sbrc r21,7
5886
 5901 376a 6095      		com r22
5887
 5902 376c 762F      		mov r23,r22
5888
 5903 376e 4CAB      		std Y+52,r20
5889
 5904 3770 5DAB      		std Y+53,r21
5890
 5905 3772 6EAB      		std Y+54,r22
5891
 5906 3774 7FAB      		std Y+55,r23
5892
 5907 3776 2C01      		movw r4,r24
5893
 5908 3778 6624      		clr r6
5894
 5909 377a 57FC      		sbrc r5,7
5895
 5910 377c 6094      		com r6
5896
 5911 377e 762C      		mov r7,r6
5897
 5912 3780 00C0      		rjmp .L707
5898
 5913               	.L787:
5899
 5914 3782 1396      		adiw r26,3
5900
 5915 3784 2C91      		ld r18,X
5901
 5916 3786 3327      		clr r19
5902
 5917 3788 27FD      		sbrc r18,7
5903
 5918 378a 3095      		com r19
5904
 5919 378c 432F      		mov r20,r19
5905
 5920 378e 532F      		mov r21,r19
5906
 5921 3790 6CA5      		ldd r22,Y+44
5907
 5922 3792 7DA5      		ldd r23,Y+45
5908
 5923 3794 8EA5      		ldd r24,Y+46
5909
 5924 3796 9FA5      		ldd r25,Y+47
5910
 5925 3798 0E94 0000 		call __mulsi3
5911
 5926 379c 20E4      		ldi r18,lo8(64)
5912
 5927 379e 30E0      		ldi r19,hi8(64)
5913
 5928 37a0 40E0      		ldi r20,hlo8(64)
5914
 5929 37a2 50E0      		ldi r21,hhi8(64)
5915
 5930 37a4 0E94 0000 		call __divmodsi4
5916
 5931 37a8 7901      		movw r14,r18
5917
 5932 37aa 8A01      		movw r16,r20
5918
 5933 37ac 2A2D      		mov r18,r10
5919
 5934 37ae 3327      		clr r19
5920
 5935 37b0 27FD      		sbrc r18,7
5921
 5936 37b2 3095      		com r19
5922
 5937 37b4 432F      		mov r20,r19
5923
 5938 37b6 532F      		mov r21,r19
5924
 5939 37b8 68A9      		ldd r22,Y+48
5925
 5940 37ba 79A9      		ldd r23,Y+49
5926
 5941 37bc 8AA9      		ldd r24,Y+50
5927
 5942 37be 9BA9      		ldd r25,Y+51
5928
 5943 37c0 0E94 0000 		call __mulsi3
5929
 5944 37c4 20E4      		ldi r18,lo8(64)
5930
 5945 37c6 30E0      		ldi r19,hi8(64)
5931
 5946 37c8 40E0      		ldi r20,hlo8(64)
5932
 5947 37ca 50E0      		ldi r21,hhi8(64)
5933
 5948 37cc 0E94 0000 		call __divmodsi4
5934
 5949 37d0 E20E      		add r14,r18
5935
 5950 37d2 F31E      		adc r15,r19
5936
 5951 37d4 F101      		movw r30,r2
5937
 5952 37d6 2181      		ldd r18,Z+1
5938
 5953 37d8 3327      		clr r19
5939
 5954 37da 27FD      		sbrc r18,7
5940
 5955 37dc 3095      		com r19
5941
 5956 37de 432F      		mov r20,r19
5942
 5957 37e0 532F      		mov r21,r19
5943
 5958 37e2 6CA9      		ldd r22,Y+52
5944
 5959 37e4 7DA9      		ldd r23,Y+53
5945
 5960 37e6 8EA9      		ldd r24,Y+54
5946
 5961 37e8 9FA9      		ldd r25,Y+55
5947
 5962 37ea 0E94 0000 		call __mulsi3
5948
 5963 37ee 20E4      		ldi r18,lo8(64)
5949
 5964 37f0 30E0      		ldi r19,hi8(64)
5950
 5965 37f2 40E0      		ldi r20,hlo8(64)
5951
 5966 37f4 50E0      		ldi r21,hhi8(64)
5952
 5967 37f6 0E94 0000 		call __divmodsi4
5953
 5968 37fa E20E      		add r14,r18
5954
 5969 37fc F31E      		adc r15,r19
5955
 5970 37fe D101      		movw r26,r2
5956
 5971 3800 1296      		adiw r26,2
5957
 5972 3802 2C91      		ld r18,X
5958
 5973 3804 3327      		clr r19
5959
 5974 3806 27FD      		sbrc r18,7
5960
 5975 3808 3095      		com r19
5961
 5976 380a 432F      		mov r20,r19
5962
 5977 380c 532F      		mov r21,r19
5963
 5978 380e C301      		movw r24,r6
5964
 5979 3810 B201      		movw r22,r4
5965
 5980 3812 0E94 0000 		call __mulsi3
5966
 5981 3816 20E4      		ldi r18,lo8(64)
5967
 5982 3818 30E0      		ldi r19,hi8(64)
5968
 5983 381a 40E0      		ldi r20,hlo8(64)
5969
 5984 381c 50E0      		ldi r21,hhi8(64)
5970
 5985 381e 0E94 0000 		call __divmodsi4
5971
 5986 3822 E20E      		add r14,r18
5972
 5987 3824 F31E      		adc r15,r19
5973
 5988 3826 F601      		movw r30,r12
5974
 5989 3828 6081      		ld r22,Z
5975
 5990 382a 7181      		ldd r23,Z+1
5976
 5991 382c C701      		movw r24,r14
5977
 5992 382e 0E94 0000 		call MotorSmoothing
5978
 5993 3832 D601      		movw r26,r12
5979
 5994 3834 8C93      		st X,r24
5980
 5995 3836 1196      		adiw r26,1
5981
 5996 3838 9C93      		st X,r25
5982
 5997 383a 97FD      		sbrc r25,7
5983
 5998 383c 00C0      		rjmp .L785
5984
 5999               	.L703:
5985
 6000 383e AC01      		movw r20,r24
5986
 6001 3840 5595      		asr r21
5987
 6002 3842 4795      		ror r20
5988
 6003 3844 5595      		asr r21
5989
 6004 3846 4795      		ror r20
5990
 6005 3848 8091 0000 		lds r24,ParamSet+21
5991
 6006 384c 282F      		mov r18,r24
5992
 6007 384e 30E0      		ldi r19,lo8(0)
5993
 6008 3850 4217      		cp r20,r18
5994
 6009 3852 5307      		cpc r21,r19
5995
 6010 3854 04F0      		brlt .L704
5996
 6011 3856 8091 0000 		lds r24,ParamSet+22
5997
 6012 385a 90E0      		ldi r25,lo8(0)
5998
 6013 385c 9C01      		movw r18,r24
5999
 6014 385e 4217      		cp r20,r18
6000
 6015 3860 5307      		cpc r21,r19
6001
 6016 3862 04F4      		brge .L704
6002
 6017 3864 9A01      		movw r18,r20
6003
 6018               	.L704:
6004
 6019 3866 F401      		movw r30,r8
6005
 6020 3868 2083      		st Z,r18
6006
 6021               	.L706:
6007
 6022 386a E4E0      		ldi r30,lo8(4)
6008
 6023 386c F0E0      		ldi r31,hi8(4)
6009
 6024 386e 2E0E      		add r2,r30
6010
 6025 3870 3F1E      		adc r3,r31
6011
 6026 3872 95E0      		ldi r25,lo8(5)
6012
 6027 3874 A92E      		mov r10,r25
6013
 6028 3876 B12C      		mov r11,__zero_reg__
6014
 6029 3878 8A0C      		add r8,r10
6015
 6030 387a 9B1C      		adc r9,r11
6016
 6031 387c 22E0      		ldi r18,lo8(2)
6017
 6032 387e 30E0      		ldi r19,hi8(2)
6018
 6033 3880 C20E      		add r12,r18
6019
 6034 3882 D31E      		adc r13,r19
6020
 6035 3884 30E0      		ldi r19,lo8(Mixer+61)
6021
 6036 3886 2316      		cp r2,r19
6022
 6037 3888 30E0      		ldi r19,hi8(Mixer+61)
6023
 6038 388a 3306      		cpc r3,r19
6024
 6039 388c 01F0      		breq .L786
6025
 6040               	.L707:
6026
 6041 388e D101      		movw r26,r2
6027
 6042 3890 AC90      		ld r10,X
6028
 6043 3892 1A14      		cp __zero_reg__,r10
6029
 6044 3894 04F4      		brge .+2
6030
 6045 3896 00C0      		rjmp .L787
6031
 6046 3898 D401      		movw r26,r8
6032
 6047 389a 1C92      		st X,__zero_reg__
6033
 6048 389c 00C0      		rjmp .L706
6034
 6049               	.L785:
6035
 6050 389e 0396      		adiw r24,3
6036
 6051 38a0 00C0      		rjmp .L703
6037
 6052               	.L786:
6038
 6053               	/* epilogue start */
6039
 6054 38a2 E996      		adiw r28,57
6040
 6055 38a4 0FB6      		in __tmp_reg__,__SREG__
6041
 6056 38a6 F894      		cli
6042
 6057 38a8 DEBF      		out __SP_H__,r29
6043
 6058 38aa 0FBE      		out __SREG__,__tmp_reg__
6044
 6059 38ac CDBF      		out __SP_L__,r28
6045
 6060 38ae CF91      		pop r28
6046
 6061 38b0 DF91      		pop r29
6047
 6062 38b2 1F91      		pop r17
6048
 6063 38b4 0F91      		pop r16
6049
 6064 38b6 FF90      		pop r15
6050
 6065 38b8 EF90      		pop r14
6051
 6066 38ba DF90      		pop r13
6052
 6067 38bc CF90      		pop r12
6053
 6068 38be BF90      		pop r11
6054
 6069 38c0 AF90      		pop r10
6055
 6070 38c2 9F90      		pop r9
6056
 6071 38c4 8F90      		pop r8
6057
 6072 38c6 7F90      		pop r7
6058
 6073 38c8 6F90      		pop r6
6059
 6074 38ca 5F90      		pop r5
6060
 6075 38cc 4F90      		pop r4
6061
 6076 38ce 3F90      		pop r3
6062
 6077 38d0 2F90      		pop r2
6063
 6078 38d2 0895      		ret
6064
 6079               	.L696:
6065
 6080 38d4 8216      		cp r8,r18
6066
 6081 38d6 9306      		cpc r9,r19
6067
 6082 38d8 04F0      		brlt .+2
6068
 6083 38da 00C0      		rjmp .L697
6069
 6084 38dc 9401      		movw r18,r8
6070
 6085 38de 00C0      		rjmp .L697
6071
 6086               	.L693:
6072
 6087 38e0 21E0      		ldi r18,lo8(64001)
6073
 6088 38e2 E216      		cp r14,r18
6074
 6089 38e4 2AEF      		ldi r18,hi8(64001)
6075
 6090 38e6 F206      		cpc r15,r18
6076
 6091 38e8 20E0      		ldi r18,hlo8(64001)
6077
 6092 38ea 0207      		cpc r16,r18
6078
 6093 38ec 20E0      		ldi r18,hhi8(64001)
6079
 6094 38ee 1207      		cpc r17,r18
6080
 6095 38f0 04F4      		brge .+2
6081
 6096 38f2 00C0      		rjmp .L694
6082
 6097 38f4 80E0      		ldi r24,lo8(64000)
6083
 6098 38f6 9AEF      		ldi r25,hi8(64000)
6084
 6099 38f8 A0E0      		ldi r26,hlo8(64000)
6085
 6100 38fa B0E0      		ldi r27,hhi8(64000)
6086
 6101 38fc 8093 0000 		sts IPartRoll.2409,r24
6087
 6102 3900 9093 0000 		sts (IPartRoll.2409)+1,r25
6088
 6103 3904 A093 0000 		sts (IPartRoll.2409)+2,r26
6089
 6104 3908 B093 0000 		sts (IPartRoll.2409)+3,r27
6090
 6105 390c E12C      		mov r14,__zero_reg__
6091
 6106 390e EAEF      		ldi r30,hi8(64000)
6092
 6107 3910 FE2E      		mov r15,r30
6093
 6108 3912 012D      		mov r16,__zero_reg__
6094
 6109 3914 112D      		mov r17,__zero_reg__
6095
 6110 3916 00C0      		rjmp .L694
6096
 6111               	.L691:
6097
 6112 3918 C601      		movw r24,r12
6098
 6113 391a AA27      		clr r26
6099
 6114 391c 97FD      		sbrc r25,7
6100
 6115 391e A095      		com r26
6101
 6116 3920 BA2F      		mov r27,r26
6102
 6117 3922 E090 0000 		lds r14,IPartRoll.2409
6103
 6118 3926 F090 0000 		lds r15,(IPartRoll.2409)+1
6104
 6119 392a 0091 0000 		lds r16,(IPartRoll.2409)+2
6105
 6120 392e 1091 0000 		lds r17,(IPartRoll.2409)+3
6106
 6121 3932 E80E      		add r14,r24
6107
 6122 3934 F91E      		adc r15,r25
6108
 6123 3936 0A1F      		adc r16,r26
6109
 6124 3938 1B1F      		adc r17,r27
6110
 6125 393a E092 0000 		sts IPartRoll.2409,r14
6111
 6126 393e F092 0000 		sts (IPartRoll.2409)+1,r15
6112
 6127 3942 0093 0000 		sts (IPartRoll.2409)+2,r16
6113
 6128 3946 1093 0000 		sts (IPartRoll.2409)+3,r17
6114
 6129 394a 00C0      		rjmp .L692
6115
 6130               	.L687:
6116
 6131 394c 61E0      		ldi r22,lo8(64001)
6117
 6132 394e E616      		cp r14,r22
6118
 6133 3950 6AEF      		ldi r22,hi8(64001)
6119
 6134 3952 F606      		cpc r15,r22
6120
 6135 3954 60E0      		ldi r22,hlo8(64001)
6121
 6136 3956 0607      		cpc r16,r22
6122
 6137 3958 60E0      		ldi r22,hhi8(64001)
6123
 6138 395a 1607      		cpc r17,r22
6124
 6139 395c 04F4      		brge .+2
6125
 6140 395e 00C0      		rjmp .L688
6126
 6141 3960 80E0      		ldi r24,lo8(64000)
6127
 6142 3962 9AEF      		ldi r25,hi8(64000)
6128
 6143 3964 A0E0      		ldi r26,hlo8(64000)
6129
 6144 3966 B0E0      		ldi r27,hhi8(64000)
6130
 6145 3968 8093 0000 		sts IPartNick.2408,r24
6131
 6146 396c 9093 0000 		sts (IPartNick.2408)+1,r25
6132
 6147 3970 A093 0000 		sts (IPartNick.2408)+2,r26
6133
 6148 3974 B093 0000 		sts (IPartNick.2408)+3,r27
6134
 6149 3978 E12C      		mov r14,__zero_reg__
6135
 6150 397a 2AEF      		ldi r18,hi8(64000)
6136
 6151 397c F22E      		mov r15,r18
6137
 6152 397e 012D      		mov r16,__zero_reg__
6138
 6153 3980 112D      		mov r17,__zero_reg__
6139
 6154 3982 00C0      		rjmp .L688
6140
 6155               	.L685:
6141
 6156 3984 C601      		movw r24,r12
6142
 6157 3986 AA27      		clr r26
6143
 6158 3988 97FD      		sbrc r25,7
6144
 6159 398a A095      		com r26
6145
 6160 398c BA2F      		mov r27,r26
6146
 6161 398e E090 0000 		lds r14,IPartNick.2408
6147
 6162 3992 F090 0000 		lds r15,(IPartNick.2408)+1
6148
 6163 3996 0091 0000 		lds r16,(IPartNick.2408)+2
6149
 6164 399a 1091 0000 		lds r17,(IPartNick.2408)+3
6150
 6165 399e E80E      		add r14,r24
6151
 6166 39a0 F91E      		adc r15,r25
6152
 6167 39a2 0A1F      		adc r16,r26
6153
 6168 39a4 1B1F      		adc r17,r27
6154
 6169 39a6 E092 0000 		sts IPartNick.2408,r14
6155
 6170 39aa F092 0000 		sts (IPartNick.2408)+1,r15
6156
 6171 39ae 0093 0000 		sts (IPartNick.2408)+2,r16
6157
 6172 39b2 1093 0000 		sts (IPartNick.2408)+3,r17
6158
 6173 39b6 00C0      		rjmp .L686
6159
 6174               	.L675:
6160
 6175 39b8 C701      		movw r24,r14
6161
 6176 39ba 8135      		cpi r24,81
6162
 6177 39bc 9105      		cpc r25,__zero_reg__
6163
 6178 39be 04F0      		brlt .+2
6164
 6179 39c0 00C0      		rjmp .L788
6165
 6180 39c2 9C01      		movw r18,r24
6166
 6181 39c4 5FEF      		ldi r21,hi8(-80)
6167
 6182 39c6 803B      		cpi r24,lo8(-80)
6168
 6183 39c8 9507      		cpc r25,r21
6169
 6184 39ca 04F0      		brlt .+2
6170
 6185 39cc 00C0      		rjmp .L677
6171
 6186               	.L790:
6172
 6187 39ce 20EB      		ldi r18,lo8(-80)
6173
 6188 39d0 3FEF      		ldi r19,hi8(-80)
6174
 6189 39d2 00C0      		rjmp .L677
6175
 6190               	.L601:
6176
 6191 39d4 6091 0000 		lds r22,GyroIFactor
6177
 6192 39d8 70E0      		ldi r23,lo8(0)
6178
 6193 39da 80E0      		ldi r24,lo8(0)
6179
 6194 39dc 90E0      		ldi r25,hi8(0)
6180
 6195 39de 2091 0000 		lds r18,IntegralGyroRoll
6181
 6196 39e2 3091 0000 		lds r19,(IntegralGyroRoll)+1
6182
 6197 39e6 4091 0000 		lds r20,(IntegralGyroRoll)+2
6183
 6198 39ea 5091 0000 		lds r21,(IntegralGyroRoll)+3
6184
 6199 39ee 0E94 0000 		call __mulsi3
6185
 6200 39f2 28EF      		ldi r18,lo8(11000)
6186
 6201 39f4 3AE2      		ldi r19,hi8(11000)
6187
 6202 39f6 40E0      		ldi r20,hlo8(11000)
6188
 6203 39f8 50E0      		ldi r21,hhi8(11000)
6189
 6204 39fa 0E94 0000 		call __divmodsi4
6190
 6205 39fe 3887      		std Y+8,r19
6191
 6206 3a00 2F83      		std Y+7,r18
6192
 6207 3a02 3E83      		std Y+6,r19
6193
 6208 3a04 2D83      		std Y+5,r18
6194
 6209 3a06 00C0      		rjmp .L602
6195
 6210               	.L599:
6196
 6211 3a08 6091 0000 		lds r22,GyroIFactor
6197
 6212 3a0c 70E0      		ldi r23,lo8(0)
6198
 6213 3a0e 80E0      		ldi r24,lo8(0)
6199
 6214 3a10 90E0      		ldi r25,hi8(0)
6200
 6215 3a12 2091 0000 		lds r18,IntegralGyroNick
6201
 6216 3a16 3091 0000 		lds r19,(IntegralGyroNick)+1
6202
 6217 3a1a 4091 0000 		lds r20,(IntegralGyroNick)+2
6203
 6218 3a1e 5091 0000 		lds r21,(IntegralGyroNick)+3
6204
 6219 3a22 0E94 0000 		call __mulsi3
6205
 6220 3a26 28EF      		ldi r18,lo8(11000)
6206
 6221 3a28 3AE2      		ldi r19,hi8(11000)
6207
 6222 3a2a 40E0      		ldi r20,hlo8(11000)
6208
 6223 3a2c 50E0      		ldi r21,hhi8(11000)
6209
 6224 3a2e 0E94 0000 		call __divmodsi4
6210
 6225 3a32 2901      		movw r4,r18
6211
 6226 3a34 3C83      		std Y+4,r19
6212
 6227 3a36 2B83      		std Y+3,r18
6213
 6228 3a38 00C0      		rjmp .L600
6214
 6229               	.L596:
6215
 6230 3a3a EE8D      		ldd r30,Y+30
6216
 6231 3a3c FF8D      		ldd r31,Y+31
6217
 6232 3a3e E93C      		cpi r30,201
6218
 6233 3a40 F105      		cpc r31,__zero_reg__
6219
 6234 3a42 04F4      		brge .+2
6220
 6235 3a44 00C0      		rjmp .L597
6221
 6236 3a46 88EC      		ldi r24,lo8(200)
6222
 6237 3a48 90E0      		ldi r25,hi8(200)
6223
 6238 3a4a 9093 0000 		sts (TrimRoll)+1,r25
6224
 6239 3a4e 8093 0000 		sts TrimRoll,r24
6225
 6240 3a52 9F8F      		std Y+31,r25
6226
 6241 3a54 8E8F      		std Y+30,r24
6227
 6242 3a56 00C0      		rjmp .L597
6228
 6243               	.L594:
6229
 6244 3a58 69EC      		ldi r22,lo8(201)
6230
 6245 3a5a 8616      		cp r8,r22
6231
 6246 3a5c 9104      		cpc r9,__zero_reg__
6232
 6247 3a5e 04F4      		brge .+2
6233
 6248 3a60 00C0      		rjmp .L595
6234
 6249 3a62 88EC      		ldi r24,lo8(200)
6235
 6250 3a64 90E0      		ldi r25,hi8(200)
6236
 6251 3a66 9093 0000 		sts (TrimNick)+1,r25
6237
 6252 3a6a 8093 0000 		sts TrimNick,r24
6238
 6253 3a6e E8EC      		ldi r30,lo8(200)
6239
 6254 3a70 8E2E      		mov r8,r30
6240
 6255 3a72 912C      		mov r9,__zero_reg__
6241
 6256 3a74 00C0      		rjmp .L595
6242
 6257               	.L573:
6243
 6258 3a76 2155      		subi r18,lo8(50001)
6244
 6259 3a78 334C      		sbci r19,hi8(50001)
6245
 6260 3a7a 4040      		sbci r20,hlo8(50001)
6246
 6261 3a7c 5040      		sbci r21,hhi8(50001)
6247
 6262 3a7e 04F4      		brge .+4
6248
 6263 3a80 0C94 0000 		jmp .L574
6249
 6264 3a84 80E5      		ldi r24,lo8(50000)
6250
 6265 3a86 93EC      		ldi r25,hi8(50000)
6251
 6266 3a88 A0E0      		ldi r26,hlo8(50000)
6252
 6267 3a8a B0E0      		ldi r27,hhi8(50000)
6253
 6268 3a8c 8093 0000 		sts ReadingIntegralGyroYaw,r24
6254
 6269 3a90 9093 0000 		sts (ReadingIntegralGyroYaw)+1,r25
6255
 6270 3a94 A093 0000 		sts (ReadingIntegralGyroYaw)+2,r26
6256
 6271 3a98 B093 0000 		sts (ReadingIntegralGyroYaw)+3,r27
6257
 6272 3a9c 0C94 0000 		jmp .L574
6258
 6273               	.L740:
6259
 6274 3aa0 9B01      		movw r18,r22
6260
 6275 3aa2 77FD      		sbrc r23,7
6261
 6276 3aa4 00C0      		rjmp .L789
6262
 6277               	.L484:
6263
 6278 3aa6 C901      		movw r24,r18
6264
 6279 3aa8 9595      		asr r25
6265
 6280 3aaa 8795      		ror r24
6266
 6281 3aac 9595      		asr r25
6267
 6282 3aae 8795      		ror r24
6268
 6283 3ab0 9093 0000 		sts (MaxStickNick)+1,r25
6269
 6284 3ab4 8093 0000 		sts MaxStickNick,r24
6270
 6285 3ab8 8536      		cpi r24,101
6271
 6286 3aba 9105      		cpc r25,__zero_reg__
6272
 6287 3abc 04F4      		brge .+4
6273
 6288 3abe 0C94 0000 		jmp .L485
6274
 6289 3ac2 84E6      		ldi r24,lo8(100)
6275
 6290 3ac4 90E0      		ldi r25,hi8(100)
6276
 6291 3ac6 9093 0000 		sts (MaxStickNick)+1,r25
6277
 6292 3aca 8093 0000 		sts MaxStickNick,r24
6278
 6293 3ace 0C94 0000 		jmp .L485
6279
 6294               	.L737:
6280
 6295 3ad2 1092 0000 		sts GyroIFactor,__zero_reg__
6281
 6296 3ad6 0C94 0000 		jmp .L480
6282
 6297               	.L778:
6283
 6298 3ada 5C01      		movw r10,r24
6284
 6299 3adc 00C0      		rjmp .L669
6285
 6300               	.L788:
6286
 6301 3ade 80E5      		ldi r24,lo8(80)
6287
 6302 3ae0 90E0      		ldi r25,hi8(80)
6288
 6303 3ae2 9C01      		movw r18,r24
6289
 6304 3ae4 5FEF      		ldi r21,hi8(-80)
6290
 6305 3ae6 803B      		cpi r24,lo8(-80)
6291
 6306 3ae8 9507      		cpc r25,r21
6292
 6307 3aea 04F0      		brlt .+2
6293
 6308 3aec 00C0      		rjmp .L677
6294
 6309 3aee 00C0      		rjmp .L790
6295
 6310               	.L784:
6296
 6311 3af0 C801      		movw r24,r16
6297
 6312 3af2 00C0      		rjmp .L700
6298
 6313               	.L782:
6299
 6314 3af4 3C01      		movw r6,r24
6300
 6315 3af6 00C0      		rjmp .L681
6301
 6316               	.L781:
6302
 6317 3af8 9501      		movw r18,r10
6303
 6318 3afa 3595      		asr r19
6304
 6319 3afc 2795      		ror r18
6305
 6320 3afe E216      		cp r14,r18
6306
 6321 3b00 F306      		cpc r15,r19
6307
 6322 3b02 04F0      		brlt .+2
6308
 6323 3b04 00C0      		rjmp .L677
6309
 6324 3b06 9701      		movw r18,r14
6310
 6325 3b08 00C0      		rjmp .L677
6311
 6326               	.L779:
6312
 6327 3b0a 1A14      		cp __zero_reg__,r10
6313
 6328 3b0c 1B04      		cpc __zero_reg__,r11
6314
 6329 3b0e 04F0      		brlt .+2
6315
 6330 3b10 00C0      		rjmp .L672
6316
 6331 3b12 81E0      		ldi r24,lo8(1)
6317
 6332 3b14 90E0      		ldi r25,hi8(1)
6318
 6333 3b16 9093 0000 		sts (ModelIsFlying)+1,r25
6319
 6334 3b1a 8093 0000 		sts ModelIsFlying,r24
6320
 6335 3b1e 8091 0000 		lds r24,ParamSet+21
6321
 6336 3b22 282F      		mov r18,r24
6322
 6337 3b24 30E0      		ldi r19,lo8(0)
6323
 6338 3b26 00C0      		rjmp .L672
6324
 6339               	.L756:
6325
 6340 3b28 81E0      		ldi r24,lo8(1)
6326
 6341 3b2a 90E0      		ldi r25,hi8(1)
6327
 6342 3b2c 9093 0000 		sts (UpdateCompassCourse.2420)+1,r25
6328
 6343 3b30 8093 0000 		sts UpdateCompassCourse.2420,r24
6329
 6344 3b34 0C94 0000 		jmp .L569
6330
 6345               	.L777:
6331
 6346 3b38 9D8D      		ldd r25,Y+29
6332
 6347 3b3a 9923      		tst r25
6333
 6348 3b3c 01F0      		breq .+2
6334
 6349 3b3e 00C0      		rjmp .L604
6335
 6350 3b40 8091 0000 		lds r24,HoverGas.2445
6336
 6351 3b44 9091 0000 		lds r25,(HoverGas.2445)+1
6337
 6352 3b48 9093 0000 		sts (DebugOut+44)+1,r25
6338
 6353 3b4c 8093 0000 		sts DebugOut+44,r24
6339
 6354 3b50 8091 0000 		lds r24,ReadingVario
6340
 6355 3b54 9091 0000 		lds r25,(ReadingVario)+1
6341
 6356 3b58 9093 0000 		sts (DebugOut+38)+1,r25
6342
 6357 3b5c 8093 0000 		sts DebugOut+38,r24
6343
 6358 3b60 2091 0000 		lds r18,BaroExpandActive.2451
6344
 6359 3b64 3091 0000 		lds r19,(BaroExpandActive.2451)+1
6345
 6360 3b68 2115      		cp r18,__zero_reg__
6346
 6361 3b6a 3105      		cpc r19,__zero_reg__
6347
 6362 3b6c 01F4      		brne .+4
6348
 6363 3b6e 0C94 0000 		jmp .L605
6349
 6364 3b72 8091 0000 		lds r24,ReadingHeight
6350
 6365 3b76 9091 0000 		lds r25,(ReadingHeight)+1
6351
 6366 3b7a A091 0000 		lds r26,(ReadingHeight)+2
6352
 6367 3b7e B091 0000 		lds r27,(ReadingHeight)+3
6353
 6368 3b82 880F      		lsl r24
6354
 6369 3b84 991F      		rol r25
6355
 6370 3b86 AA1F      		rol r26
6356
 6371 3b88 BB1F      		rol r27
6357
 6372 3b8a 880F      		lsl r24
6358
 6373 3b8c 991F      		rol r25
6359
 6374 3b8e AA1F      		rol r26
6360
 6375 3b90 BB1F      		rol r27
6361
 6376 3b92 880F      		lsl r24
6362
 6377 3b94 991F      		rol r25
6363
 6378 3b96 AA1F      		rol r26
6364
 6379 3b98 BB1F      		rol r27
6365
 6380 3b9a 880F      		lsl r24
6366
 6381 3b9c 991F      		rol r25
6367
 6382 3b9e AA1F      		rol r26
6368
 6383 3ba0 BB1F      		rol r27
6369
 6384 3ba2 8093 0000 		sts SumHeight,r24
6370
 6385 3ba6 9093 0000 		sts (SumHeight)+1,r25
6371
 6386 3baa A093 0000 		sts (SumHeight)+2,r26
6372
 6387 3bae B093 0000 		sts (SumHeight)+3,r27
6373
 6388 3bb2 1092 0000 		sts (ReadingVario)+1,__zero_reg__
6374
 6389 3bb6 1092 0000 		sts ReadingVario,__zero_reg__
6375
 6390 3bba 2150      		subi r18,lo8(-(-1))
6376
 6391 3bbc 3040      		sbci r19,hi8(-(-1))
6377
 6392 3bbe 3093 0000 		sts (BaroExpandActive.2451)+1,r19
6378
 6393 3bc2 2093 0000 		sts BaroExpandActive.2451,r18
6379
 6394               	.L606:
6380
 6395 3bc6 8091 0000 		lds r24,ParamSet+8
6381
 6396 3bca 81FF      		sbrs r24,1
6382
 6397 3bcc 00C0      		rjmp .L611
6383
 6398 3bce 8091 0000 		lds r24,FCParam+1
6384
 6399 3bd2 8233      		cpi r24,lo8(50)
6385
 6400 3bd4 00F0      		brlo .+4
6386
 6401 3bd6 0C94 0000 		jmp .L612
6387
 6402 3bda 8091 0000 		lds r24,delay.2449
6388
 6403 3bde 8150      		subi r24,lo8(-(-1))
6389
 6404 3be0 8093 0000 		sts delay.2449,r24
6390
 6405 3be4 8F5F      		subi r24,lo8(-(1))
6391
 6406 3be6 01F0      		breq .+2
6392
 6407 3be8 00C0      		rjmp .L613
6393
 6408 3bea 8091 0000 		lds r24,ReadingHeight
6394
 6409 3bee 9091 0000 		lds r25,(ReadingHeight)+1
6395
 6410 3bf2 A091 0000 		lds r26,(ReadingHeight)+2
6396
 6411 3bf6 B091 0000 		lds r27,(ReadingHeight)+3
6397
 6412 3bfa 8093 0000 		sts SetPointHeight,r24
6398
 6413 3bfe 9093 0000 		sts (SetPointHeight)+1,r25
6399
 6414 3c02 A093 0000 		sts (SetPointHeight)+2,r26
6400
 6415 3c06 B093 0000 		sts (SetPointHeight)+3,r27
6401
 6416 3c0a 1092 0000 		sts HCActive.2443,__zero_reg__
6402
 6417 3c0e 81E0      		ldi r24,lo8(1)
6403
 6418 3c10 8093 0000 		sts delay.2449,r24
6404
 6419 3c14 00C0      		rjmp .L613
6405
 6420               	.L732:
6406
 6421 3c16 0197      		sbiw r24,1
6407
 6422 3c18 9093 0000 		sts (RcLostTimer.2415)+1,r25
6408
 6423 3c1c 8093 0000 		sts RcLostTimer.2415,r24
6409
 6424 3c20 AAE0      		ldi r26,lo8(10)
6410
 6425 3c22 BA2E      		mov r11,r26
6411
 6426 3c24 DB14      		cp r13,r11
6412
 6427 3c26 01F0      		breq .+4
6413
 6428 3c28 0C94 0000 		jmp .L791
6414
 6429               	.L406:
6415
 6430 3c2c 289A      		sbi 37-32,0
6416
 6431 3c2e 0C94 0000 		jmp .L408
6417
 6432               	.L754:
6418
 6433 3c32 8091 0000 		lds r24,LoopingRoll
6419
 6434 3c36 8823      		tst r24
6420
 6435 3c38 01F0      		breq .+4
6421
 6436 3c3a 0C94 0000 		jmp .L530
6422
 6437 3c3e 8091 0000 		lds r24,FunnelCourse
6423
 6438 3c42 8823      		tst r24
6424
 6439 3c44 01F0      		breq .+4
6425
 6440 3c46 0C94 0000 		jmp .L530
6426
 6441 3c4a 8091 0000 		lds r24,ParamSet+57
6427
 6442 3c4e 8823      		tst r24
6428
 6443 3c50 01F4      		brne .+4
6429
 6444 3c52 0C94 0000 		jmp .L530
6430
 6445 3c56 8091 0000 		lds r24,MeanIntegralGyroNick
6431
 6446 3c5a 9091 0000 		lds r25,(MeanIntegralGyroNick)+1
6432
 6447 3c5e A091 0000 		lds r26,(MeanIntegralGyroNick)+2
6433
 6448 3c62 B091 0000 		lds r27,(MeanIntegralGyroNick)+3
6434
 6449 3c66 BC01      		movw r22,r24
6435
 6450 3c68 CD01      		movw r24,r26
6436
 6451 3c6a 20E0      		ldi r18,lo8(256)
6437
 6452 3c6c 31E0      		ldi r19,hi8(256)
6438
 6453 3c6e 40E0      		ldi r20,hlo8(256)
6439
 6454 3c70 50E0      		ldi r21,hhi8(256)
6440
 6455 3c72 0E94 0000 		call __divmodsi4
6441
 6456 3c76 2D8B      		std Y+21,r18
6442
 6457 3c78 3E8B      		std Y+22,r19
6443
 6458 3c7a 4F8B      		std Y+23,r20
6444
 6459 3c7c 588F      		std Y+24,r21
6445
 6460 3c7e 28A3      		std Y+32,r18
6446
 6461 3c80 39A3      		std Y+33,r19
6447
 6462 3c82 4AA3      		std Y+34,r20
6448
 6463 3c84 5BA3      		std Y+35,r21
6449
 6464 3c86 2093 0000 		sts MeanIntegralGyroNick,r18
6450
 6465 3c8a 3093 0000 		sts (MeanIntegralGyroNick)+1,r19
6451
 6466 3c8e 4093 0000 		sts (MeanIntegralGyroNick)+2,r20
6452
 6467 3c92 5093 0000 		sts (MeanIntegralGyroNick)+3,r21
6453
 6468 3c96 2090 0000 		lds r2,MeanIntegralGyroRoll
6454
 6469 3c9a 3090 0000 		lds r3,(MeanIntegralGyroRoll)+1
6455
 6470 3c9e 4090 0000 		lds r4,(MeanIntegralGyroRoll)+2
6456
 6471 3ca2 5090 0000 		lds r5,(MeanIntegralGyroRoll)+3
6457
 6472 3ca6 C201      		movw r24,r4
6458
 6473 3ca8 B101      		movw r22,r2
6459
 6474 3caa 20E0      		ldi r18,lo8(256)
6460
 6475 3cac 31E0      		ldi r19,hi8(256)
6461
 6476 3cae 40E0      		ldi r20,hlo8(256)
6462
 6477 3cb0 50E0      		ldi r21,hhi8(256)
6463
 6478 3cb2 0E94 0000 		call __divmodsi4
6464
 6479 3cb6 298F      		std Y+25,r18
6465
 6480 3cb8 3A8F      		std Y+26,r19
6466
 6481 3cba 4B8F      		std Y+27,r20
6467
 6482 3cbc 5C8F      		std Y+28,r21
6468
 6483 3cbe 1901      		movw r2,r18
6469
 6484 3cc0 2A01      		movw r4,r20
6470
 6485 3cc2 2093 0000 		sts MeanIntegralGyroRoll,r18
6471
 6486 3cc6 3093 0000 		sts (MeanIntegralGyroRoll)+1,r19
6472
 6487 3cca 4093 0000 		sts (MeanIntegralGyroRoll)+2,r20
6473
 6488 3cce 5093 0000 		sts (MeanIntegralGyroRoll)+3,r21
6474
 6489 3cd2 E090 0000 		lds r14,ParamSet+23
6475
 6490 3cd6 FF24      		clr r15
6476
 6491 3cd8 00E0      		ldi r16,lo8(0)
6477
 6492 3cda 10E0      		ldi r17,hi8(0)
6478
 6493 3cdc 2091 0000 		lds r18,MeanAccNick
6479
 6494 3ce0 3091 0000 		lds r19,(MeanAccNick)+1
6480
 6495 3ce4 4091 0000 		lds r20,(MeanAccNick)+2
6481
 6496 3ce8 5091 0000 		lds r21,(MeanAccNick)+3
6482
 6497 3cec C801      		movw r24,r16
6483
 6498 3cee B701      		movw r22,r14
6484
 6499 3cf0 0E94 0000 		call __mulsi3
6485
 6500 3cf4 20E0      		ldi r18,lo8(256)
6486
 6501 3cf6 31E0      		ldi r19,hi8(256)
6487
 6502 3cf8 40E0      		ldi r20,hlo8(256)
6488
 6503 3cfa 50E0      		ldi r21,hhi8(256)
6489
 6504 3cfc 0E94 0000 		call __divmodsi4
6490
 6505 3d00 5901      		movw r10,r18
6491
 6506 3d02 6A01      		movw r12,r20
6492
 6507 3d04 2093 0000 		sts MeanAccNick,r18
6493
 6508 3d08 3093 0000 		sts (MeanAccNick)+1,r19
6494
 6509 3d0c 4093 0000 		sts (MeanAccNick)+2,r20
6495
 6510 3d10 5093 0000 		sts (MeanAccNick)+3,r21
6496
 6511 3d14 2091 0000 		lds r18,MeanAccRoll
6497
 6512 3d18 3091 0000 		lds r19,(MeanAccRoll)+1
6498
 6513 3d1c 4091 0000 		lds r20,(MeanAccRoll)+2
6499
 6514 3d20 5091 0000 		lds r21,(MeanAccRoll)+3
6500
 6515 3d24 C801      		movw r24,r16
6501
 6516 3d26 B701      		movw r22,r14
6502
 6517 3d28 0E94 0000 		call __mulsi3
6503
 6518 3d2c 20E0      		ldi r18,lo8(256)
6504
 6519 3d2e 31E0      		ldi r19,hi8(256)
6505
 6520 3d30 40E0      		ldi r20,hlo8(256)
6506
 6521 3d32 50E0      		ldi r21,hhi8(256)
6507
 6522 3d34 0E94 0000 		call __divmodsi4
6508
 6523 3d38 3901      		movw r6,r18
6509
 6524 3d3a 4A01      		movw r8,r20
6510
 6525 3d3c 2093 0000 		sts MeanAccRoll,r18
6511
 6526 3d40 3093 0000 		sts (MeanAccRoll)+1,r19
6512
 6527 3d44 4093 0000 		sts (MeanAccRoll)+2,r20
6513
 6528 3d48 5093 0000 		sts (MeanAccRoll)+3,r21
6514
 6529 3d4c 6D89      		ldd r22,Y+21
6515
 6530 3d4e 7E89      		ldd r23,Y+22
6516
 6531 3d50 8F89      		ldd r24,Y+23
6517
 6532 3d52 988D      		ldd r25,Y+24
6518
 6533 3d54 6A19      		sub r22,r10
6519
 6534 3d56 7B09      		sbc r23,r11
6520
 6535 3d58 8C09      		sbc r24,r12
6521
 6536 3d5a 9D09      		sbc r25,r13
6522
 6537 3d5c 6093 0000 		sts IntegralGyroNickError.2411,r22
6523
 6538 3d60 7093 0000 		sts (IntegralGyroNickError.2411)+1,r23
6524
 6539 3d64 8093 0000 		sts (IntegralGyroNickError.2411)+2,r24
6525
 6540 3d68 9093 0000 		sts (IntegralGyroNickError.2411)+3,r25
6526
 6541 3d6c E090 0000 		lds r14,ParamSet+56
6527
 6542 3d70 FF24      		clr r15
6528
 6543 3d72 00E0      		ldi r16,lo8(0)
6529
 6544 3d74 10E0      		ldi r17,hi8(0)
6530
 6545 3d76 A801      		movw r20,r16
6531
 6546 3d78 9701      		movw r18,r14
6532
 6547 3d7a 0E94 0000 		call __divmodsi4
6533
 6548 3d7e CA01      		movw r24,r20
6534
 6549 3d80 2093 0000 		sts CorrectionNick.2413,r18
6535
 6550 3d84 3093 0000 		sts CorrectionNick.2413+1,r19
6536
 6551 3d88 4093 0000 		sts CorrectionNick.2413+2,r20
6537
 6552 3d8c 9093 0000 		sts CorrectionNick.2413+3,r25
6538
 6553 3d90 B901      		movw r22,r18
6539
 6554 3d92 20E0      		ldi r18,lo8(256)
6540
 6555 3d94 31E0      		ldi r19,hi8(256)
6541
 6556 3d96 40E0      		ldi r20,hlo8(256)
6542
 6557 3d98 50E0      		ldi r21,hhi8(256)
6543
 6558 3d9a 0E94 0000 		call __divmodsi4
6544
 6559 3d9e 3093 0000 		sts (AttitudeCorrectionNick)+1,r19
6545
 6560 3da2 2093 0000 		sts AttitudeCorrectionNick,r18
6546
 6561 3da6 C201      		movw r24,r4
6547
 6562 3da8 B101      		movw r22,r2
6548
 6563 3daa 6619      		sub r22,r6
6549
 6564 3dac 7709      		sbc r23,r7
6550
 6565 3dae 8809      		sbc r24,r8
6551
 6566 3db0 9909      		sbc r25,r9
6552
 6567 3db2 6093 0000 		sts IntegralGyroRollError.2412,r22
6553
 6568 3db6 7093 0000 		sts (IntegralGyroRollError.2412)+1,r23
6554
 6569 3dba 8093 0000 		sts (IntegralGyroRollError.2412)+2,r24
6555
 6570 3dbe 9093 0000 		sts (IntegralGyroRollError.2412)+3,r25
6556
 6571 3dc2 A801      		movw r20,r16
6557
 6572 3dc4 9701      		movw r18,r14
6558
 6573 3dc6 0E94 0000 		call __divmodsi4
6559
 6574 3dca CA01      		movw r24,r20
6560
 6575 3dcc 2093 0000 		sts CorrectionRoll.2414,r18
6561
 6576 3dd0 3093 0000 		sts CorrectionRoll.2414+1,r19
6562
 6577 3dd4 4093 0000 		sts CorrectionRoll.2414+2,r20
6563
 6578 3dd8 9093 0000 		sts CorrectionRoll.2414+3,r25
6564
 6579 3ddc B901      		movw r22,r18
6565
 6580 3dde 20E0      		ldi r18,lo8(256)
6566
 6581 3de0 31E0      		ldi r19,hi8(256)
6567
 6582 3de2 40E0      		ldi r20,hlo8(256)
6568
 6583 3de4 50E0      		ldi r21,hhi8(256)
6569
 6584 3de6 0E94 0000 		call __divmodsi4
6570
 6585 3dea 3093 0000 		sts (AttitudeCorrectionRoll)+1,r19
6571
 6586 3dee 2093 0000 		sts AttitudeCorrectionRoll,r18
6572
 6587 3df2 8091 0000 		lds r24,MaxStickNick
6573
 6588 3df6 9091 0000 		lds r25,(MaxStickNick)+1
6574
 6589 3dfa 8134      		cpi r24,65
6575
 6590 3dfc 9105      		cpc r25,__zero_reg__
6576
 6591 3dfe 04F4      		brge .L531
6577
 6592 3e00 8091 0000 		lds r24,MaxStickRoll
6578
 6593 3e04 9091 0000 		lds r25,(MaxStickRoll)+1
6579
 6594 3e08 8134      		cpi r24,65
6580
 6595 3e0a 9105      		cpc r25,__zero_reg__
6581
 6596 3e0c 04F4      		brge .L531
6582
 6597 3e0e E091 0000 		lds r30,ParamSet+3
6583
 6598 3e12 F0E0      		ldi r31,lo8(0)
6584
 6599 3e14 EE0F      		lsl r30
6585
 6600 3e16 FF1F      		rol r31
6586
 6601 3e18 E050      		subi r30,lo8(-(PPM_in))
6587
 6602 3e1a F040      		sbci r31,hi8(-(PPM_in))
6588
 6603 3e1c 0190      		ld __tmp_reg__,Z+
6589
 6604 3e1e F081      		ld r31,Z
6590
 6605 3e20 E02D      		mov r30,__tmp_reg__
6591
 6606 3e22 F7FF      		sbrs r31,7
6592
 6607 3e24 00C0      		rjmp .+4
6593
 6608 3e26 0C94 0000 		jmp .L792
6594
 6609               	.L533:
6595
 6610 3e2a 7A97      		sbiw r30,26
6596
 6611 3e2c 04F0      		brlt .L532
6597
 6612               	.L531:
6598
 6613 3e2e 8091 0000 		lds r24,FCParam+31
6599
 6614 3e32 8F3F      		cpi r24,lo8(-1)
6600
 6615 3e34 01F4      		brne .+4
6601
 6616 3e36 0C94 0000 		jmp .L793
6602
 6617               	.L532:
6603
 6618 3e3a 2091 0000 		lds r18,IntegralGyroNick2
6604
 6619 3e3e 3091 0000 		lds r19,(IntegralGyroNick2)+1
6605
 6620 3e42 4091 0000 		lds r20,(IntegralGyroNick2)+2
6606
 6621 3e46 5091 0000 		lds r21,(IntegralGyroNick2)+3
6607
 6622 3e4a A8A4      		ldd r10,Y+40
6608
 6623 3e4c B9A4      		ldd r11,Y+41
6609
 6624 3e4e CAA4      		ldd r12,Y+42
6610
 6625 3e50 DBA4      		ldd r13,Y+43
6611
 6626 3e52 2A19      		sub r18,r10
6612
 6627 3e54 3B09      		sbc r19,r11
6613
 6628 3e56 4C09      		sbc r20,r12
6614
 6629 3e58 5D09      		sbc r21,r13
6615
 6630 3e5a 2093 0000 		sts IntegralGyroNickError.2411,r18
6616
 6631 3e5e 3093 0000 		sts (IntegralGyroNickError.2411)+1,r19
6617
 6632 3e62 4093 0000 		sts (IntegralGyroNickError.2411)+2,r20
6618
 6633 3e66 5093 0000 		sts (IntegralGyroNickError.2411)+3,r21
6619
 6634 3e6a 8091 0000 		lds r24,ReadingIntegralGyroNick2
6620
 6635 3e6e 9091 0000 		lds r25,(ReadingIntegralGyroNick2)+1
6621
 6636 3e72 A091 0000 		lds r26,(ReadingIntegralGyroNick2)+2
6622
 6637 3e76 B091 0000 		lds r27,(ReadingIntegralGyroNick2)+3
6623
 6638 3e7a 821B      		sub r24,r18
6624
 6639 3e7c 930B      		sbc r25,r19
6625
 6640 3e7e A40B      		sbc r26,r20
6626
 6641 3e80 B50B      		sbc r27,r21
6627
 6642 3e82 8093 0000 		sts ReadingIntegralGyroNick2,r24
6628
 6643 3e86 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
6629
 6644 3e8a A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
6630
 6645 3e8e B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
6631
 6646 3e92 2091 0000 		lds r18,IntegralGyroRoll2
6632
 6647 3e96 3091 0000 		lds r19,(IntegralGyroRoll2)+1
6633
 6648 3e9a 4091 0000 		lds r20,(IntegralGyroRoll2)+2
6634
 6649 3e9e 5091 0000 		lds r21,(IntegralGyroRoll2)+3
6635
 6650 3ea2 6CA1      		ldd r22,Y+36
6636
 6651 3ea4 7DA1      		ldd r23,Y+37
6637
 6652 3ea6 8EA1      		ldd r24,Y+38
6638
 6653 3ea8 9FA1      		ldd r25,Y+39
6639
 6654 3eaa 261B      		sub r18,r22
6640
 6655 3eac 370B      		sbc r19,r23
6641
 6656 3eae 480B      		sbc r20,r24
6642
 6657 3eb0 590B      		sbc r21,r25
6643
 6658 3eb2 2093 0000 		sts IntegralGyroRollError.2412,r18
6644
 6659 3eb6 3093 0000 		sts (IntegralGyroRollError.2412)+1,r19
6645
 6660 3eba 4093 0000 		sts (IntegralGyroRollError.2412)+2,r20
6646
 6661 3ebe 5093 0000 		sts (IntegralGyroRollError.2412)+3,r21
6647
 6662 3ec2 8091 0000 		lds r24,ReadingIntegralGyroRoll2
6648
 6663 3ec6 9091 0000 		lds r25,(ReadingIntegralGyroRoll2)+1
6649
 6664 3eca A091 0000 		lds r26,(ReadingIntegralGyroRoll2)+2
6650
 6665 3ece B091 0000 		lds r27,(ReadingIntegralGyroRoll2)+3
6651
 6666 3ed2 821B      		sub r24,r18
6652
 6667 3ed4 930B      		sbc r25,r19
6653
 6668 3ed6 A40B      		sbc r26,r20
6654
 6669 3ed8 B50B      		sbc r27,r21
6655
 6670 3eda 8093 0000 		sts ReadingIntegralGyroRoll2,r24
6656
 6671 3ede 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
6657
 6672 3ee2 A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
6658
 6673 3ee6 B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
6659
 6674 3eea 8091 0000 		lds r24,ParamSet+57
6660
 6675 3eee 8823      		tst r24
6661
 6676 3ef0 01F0      		breq .L536
6662
 6677 3ef2 2091 0000 		lds r18,YawGyroDrift
6663
 6678 3ef6 3091 0000 		lds r19,(YawGyroDrift)+1
6664
 6679 3efa 2138      		cpi r18,129
6665
 6680 3efc 3105      		cpc r19,__zero_reg__
6666
 6681 3efe 04F0      		brlt .L537
6667
 6682 3f00 8091 0000 		lds r24,AdBiasGyroYaw
6668
 6683 3f04 9091 0000 		lds r25,(AdBiasGyroYaw)+1
6669
 6684 3f08 0196      		adiw r24,1
6670
 6685 3f0a 9093 0000 		sts (AdBiasGyroYaw)+1,r25
6671
 6686 3f0e 8093 0000 		sts AdBiasGyroYaw,r24
6672
 6687               	.L537:
6673
 6688 3f12 2058      		subi r18,lo8(-128)
6674
 6689 3f14 3F4F      		sbci r19,hi8(-128)
6675
 6690 3f16 04F4      		brge .L536
6676
 6691 3f18 8091 0000 		lds r24,AdBiasGyroYaw
6677
 6692 3f1c 9091 0000 		lds r25,(AdBiasGyroYaw)+1
6678
 6693 3f20 0197      		sbiw r24,1
6679
 6694 3f22 9093 0000 		sts (AdBiasGyroYaw)+1,r25
6680
 6695 3f26 8093 0000 		sts AdBiasGyroYaw,r24
6681
 6696               	.L536:
6682
 6697 3f2a 1092 0000 		sts (YawGyroDrift)+1,__zero_reg__
6683
 6698 3f2e 1092 0000 		sts YawGyroDrift,__zero_reg__
6684
 6699 3f32 81E0      		ldi r24,lo8(1)
6685
 6700 3f34 90E0      		ldi r25,hi8(1)
6686
 6701 3f36 9093 0000 		sts (cnt.2426)+1,r25
6687
 6702 3f3a 8093 0000 		sts cnt.2426,r24
6688
 6703 3f3e E090 0000 		lds r14,IntegralGyroNickError.2411
6689
 6704 3f42 F090 0000 		lds r15,(IntegralGyroNickError.2411)+1
6690
 6705 3f46 0091 0000 		lds r16,(IntegralGyroNickError.2411)+2
6691
 6706 3f4a 1091 0000 		lds r17,(IntegralGyroNickError.2411)+3
6692
 6707 3f4e 81E0      		ldi r24,lo8(513)
6693
 6708 3f50 E816      		cp r14,r24
6694
 6709 3f52 82E0      		ldi r24,hi8(513)
6695
 6710 3f54 F806      		cpc r15,r24
6696
 6711 3f56 80E0      		ldi r24,hlo8(513)
6697
 6712 3f58 0807      		cpc r16,r24
6698
 6713 3f5a 80E0      		ldi r24,hhi8(513)
6699
 6714 3f5c 1807      		cpc r17,r24
6700
 6715 3f5e 04F0      		brlt .L538
6701
 6716 3f60 84E0      		ldi r24,lo8(4)
6702
 6717 3f62 90E0      		ldi r25,hi8(4)
6703
 6718 3f64 9093 0000 		sts (cnt.2426)+1,r25
6704
 6719 3f68 8093 0000 		sts cnt.2426,r24
6705
 6720               	.L538:
6706
 6721 3f6c 1092 0000 		sts CorrectionNick.2413,__zero_reg__
6707
 6722 3f70 1092 0000 		sts (CorrectionNick.2413)+1,__zero_reg__
6708
 6723 3f74 1092 0000 		sts (CorrectionNick.2413)+2,__zero_reg__
6709
 6724 3f78 1092 0000 		sts (CorrectionNick.2413)+3,__zero_reg__
6710
 6725 3f7c 8091 0000 		lds r24,MeanIntegralGyroNick_old.2431
6711
 6726 3f80 9091 0000 		lds r25,(MeanIntegralGyroNick_old.2431)+1
6712
 6727 3f84 A091 0000 		lds r26,(MeanIntegralGyroNick_old.2431)+2
6713
 6728 3f88 B091 0000 		lds r27,(MeanIntegralGyroNick_old.2431)+3
6714
 6729 3f8c AD88      		ldd r10,Y+21
6715
 6730 3f8e BE88      		ldd r11,Y+22
6716
 6731 3f90 CF88      		ldd r12,Y+23
6717
 6732 3f92 D88C      		ldd r13,Y+24
6718
 6733 3f94 8A19      		sub r24,r10
6719
 6734 3f96 9B09      		sbc r25,r11
6720
 6735 3f98 AC09      		sbc r26,r12
6721
 6736 3f9a BD09      		sbc r27,r13
6722
 6737 3f9c 815E      		subi r24,lo8(-(19999))
6723
 6738 3f9e 914B      		sbci r25,hi8(-(19999))
6724
 6739 3fa0 AF4F      		sbci r26,hlo8(-(19999))
6725
 6740 3fa2 BF4F      		sbci r27,hhi8(-(19999))
6726
 6741 3fa4 8F53      		subi r24,lo8(39999)
6727
 6742 3fa6 9C49      		sbci r25,hi8(39999)
6728
 6743 3fa8 A040      		sbci r26,hlo8(39999)
6729
 6744 3faa B040      		sbci r27,hhi8(39999)
6730
 6745 3fac 00F4      		brsh .+4
6731
 6746 3fae 0C94 0000 		jmp .L539
6732
 6747 3fb2 8091 0000 		lds r24,FCParam+32
6733
 6748 3fb6 8931      		cpi r24,lo8(25)
6734
 6749 3fb8 04F0      		brlt .+4
6735
 6750 3fba 0C94 0000 		jmp .L539
6736
 6751 3fbe 1092 0000 		sts (cnt.2426)+1,__zero_reg__
6737
 6752 3fc2 1092 0000 		sts cnt.2426,__zero_reg__
6738
 6753 3fc6 88EE      		ldi r24,lo8(1000)
6739
 6754 3fc8 93E0      		ldi r25,hi8(1000)
6740
 6755 3fca 9093 0000 		sts (BadCompassHeading)+1,r25
6741
 6756 3fce 8093 0000 		sts BadCompassHeading,r24
6742
 6757 3fd2 AA24      		clr r10
6743
 6758 3fd4 BB24      		clr r11
6744
 6759               	.L548:
6745
 6760 3fd6 8091 0000 		lds r24,ParamSet+57
6746
 6761 3fda 90E0      		ldi r25,lo8(0)
6747
 6762 3fdc 8A15      		cp r24,r10
6748
 6763 3fde 9B05      		cpc r25,r11
6749
 6764 3fe0 04F4      		brge .L549
6750
 6765 3fe2 9093 0000 		sts (cnt.2426)+1,r25
6751
 6766 3fe6 8093 0000 		sts cnt.2426,r24
6752
 6767               	.L549:
6753
 6768 3fea D090 0000 		lds r13,FCParam+32
6754
 6769 3fee DD20      		tst r13
6755
 6770 3ff0 01F0      		breq .L550
6756
 6771 3ff2 2D2D      		mov r18,r13
6757
 6772 3ff4 3327      		clr r19
6758
 6773 3ff6 27FD      		sbrc r18,7
6759
 6774 3ff8 3095      		com r19
6760
 6775 3ffa 8091 0000 		lds r24,cnt.2426
6761
 6776 3ffe 9091 0000 		lds r25,(cnt.2426)+1
6762
 6777 4002 2817      		cp r18,r24
6763
 6778 4004 3907      		cpc r19,r25
6764
 6779 4006 04F4      		brge .L550
6765
 6780 4008 3093 0000 		sts (cnt.2426)+1,r19
6766
 6781 400c 2093 0000 		sts cnt.2426,r18
6767
 6782               	.L550:
6768
 6783 4010 71E8      		ldi r23,lo8(129)
6769
 6784 4012 E716      		cp r14,r23
6770
 6785 4014 F104      		cpc r15,__zero_reg__
6771
 6786 4016 0105      		cpc r16,__zero_reg__
6772
 6787 4018 1105      		cpc r17,__zero_reg__
6773
 6788 401a 04F0      		brlt .L551
6774
 6789 401c 8091 0000 		lds r24,BiasHiResGyroNick
6775
 6790 4020 9091 0000 		lds r25,(BiasHiResGyroNick)+1
6776
 6791 4024 2091 0000 		lds r18,cnt.2426
6777
 6792 4028 3091 0000 		lds r19,(cnt.2426)+1
6778
 6793 402c 820F      		add r24,r18
6779
 6794 402e 931F      		adc r25,r19
6780
 6795 4030 9093 0000 		sts (BiasHiResGyroNick)+1,r25
6781
 6796 4034 8093 0000 		sts BiasHiResGyroNick,r24
6782
 6797               	.L551:
6783
 6798 4038 80E8      		ldi r24,lo8(-128)
6784
 6799 403a E816      		cp r14,r24
6785
 6800 403c 8FEF      		ldi r24,hi8(-128)
6786
 6801 403e F806      		cpc r15,r24
6787
 6802 4040 8FEF      		ldi r24,hlo8(-128)
6788
 6803 4042 0807      		cpc r16,r24
6789
 6804 4044 8FEF      		ldi r24,hhi8(-128)
6790
 6805 4046 1807      		cpc r17,r24
6791
 6806 4048 04F4      		brge .L552
6792
 6807 404a 8091 0000 		lds r24,BiasHiResGyroNick
6793
 6808 404e 9091 0000 		lds r25,(BiasHiResGyroNick)+1
6794
 6809 4052 2091 0000 		lds r18,cnt.2426
6795
 6810 4056 3091 0000 		lds r19,(cnt.2426)+1
6796
 6811 405a 821B      		sub r24,r18
6797
 6812 405c 930B      		sbc r25,r19
6798
 6813 405e 9093 0000 		sts (BiasHiResGyroNick)+1,r25
6799
 6814 4062 8093 0000 		sts BiasHiResGyroNick,r24
6800
 6815               	.L552:
6801
 6816 4066 81E0      		ldi r24,lo8(1)
6802
 6817 4068 90E0      		ldi r25,hi8(1)
6803
 6818 406a 9093 0000 		sts (cnt.2426)+1,r25
6804
 6819 406e 8093 0000 		sts cnt.2426,r24
6805
 6820 4072 E090 0000 		lds r14,IntegralGyroRollError.2412
6806
 6821 4076 F090 0000 		lds r15,(IntegralGyroRollError.2412)+1
6807
 6822 407a 0091 0000 		lds r16,(IntegralGyroRollError.2412)+2
6808
 6823 407e 1091 0000 		lds r17,(IntegralGyroRollError.2412)+3
6809
 6824 4082 91E0      		ldi r25,lo8(513)
6810
 6825 4084 E916      		cp r14,r25
6811
 6826 4086 92E0      		ldi r25,hi8(513)
6812
 6827 4088 F906      		cpc r15,r25
6813
 6828 408a 90E0      		ldi r25,hlo8(513)
6814
 6829 408c 0907      		cpc r16,r25
6815
 6830 408e 90E0      		ldi r25,hhi8(513)
6816
 6831 4090 1907      		cpc r17,r25
6817
 6832 4092 04F0      		brlt .L553
6818
 6833 4094 84E0      		ldi r24,lo8(4)
6819
 6834 4096 90E0      		ldi r25,hi8(4)
6820
 6835 4098 9093 0000 		sts (cnt.2426)+1,r25
6821
 6836 409c 8093 0000 		sts cnt.2426,r24
6822
 6837               	.L553:
6823
 6838 40a0 1092 0000 		sts CorrectionRoll.2414,__zero_reg__
6824
 6839 40a4 1092 0000 		sts (CorrectionRoll.2414)+1,__zero_reg__
6825
 6840 40a8 1092 0000 		sts (CorrectionRoll.2414)+2,__zero_reg__
6826
 6841 40ac 1092 0000 		sts (CorrectionRoll.2414)+3,__zero_reg__
6827
 6842 40b0 8091 0000 		lds r24,MeanIntegralGyroRoll_old.2432
6828
 6843 40b4 9091 0000 		lds r25,(MeanIntegralGyroRoll_old.2432)+1
6829
 6844 40b8 A091 0000 		lds r26,(MeanIntegralGyroRoll_old.2432)+2
6830
 6845 40bc B091 0000 		lds r27,(MeanIntegralGyroRoll_old.2432)+3
6831
 6846 40c0 298D      		ldd r18,Y+25
6832
 6847 40c2 3A8D      		ldd r19,Y+26
6833
 6848 40c4 4B8D      		ldd r20,Y+27
6834
 6849 40c6 5C8D      		ldd r21,Y+28
6835
 6850 40c8 821B      		sub r24,r18
6836
 6851 40ca 930B      		sbc r25,r19
6837
 6852 40cc A40B      		sbc r26,r20
6838
 6853 40ce B50B      		sbc r27,r21
6839
 6854 40d0 815E      		subi r24,lo8(-(19999))
6840
 6855 40d2 914B      		sbci r25,hi8(-(19999))
6841
 6856 40d4 AF4F      		sbci r26,hlo8(-(19999))
6842
 6857 40d6 BF4F      		sbci r27,hhi8(-(19999))
6843
 6858 40d8 8F53      		subi r24,lo8(39999)
6844
 6859 40da 9C49      		sbci r25,hi8(39999)
6845
 6860 40dc A040      		sbci r26,hlo8(39999)
6846
 6861 40de B040      		sbci r27,hhi8(39999)
6847
 6862 40e0 00F4      		brsh .+4
6848
 6863 40e2 0C94 0000 		jmp .L554
6849
 6864 40e6 48E1      		ldi r20,lo8(24)
6850
 6865 40e8 4D15      		cp r20,r13
6851
 6866 40ea 04F4      		brge .+4
6852
 6867 40ec 0C94 0000 		jmp .L554
6853
 6868 40f0 1092 0000 		sts (cnt.2426)+1,__zero_reg__
6854
 6869 40f4 1092 0000 		sts cnt.2426,__zero_reg__
6855
 6870 40f8 88EE      		ldi r24,lo8(1000)
6856
 6871 40fa 93E0      		ldi r25,hi8(1000)
6857
 6872 40fc 9093 0000 		sts (BadCompassHeading)+1,r25
6858
 6873 4100 8093 0000 		sts BadCompassHeading,r24
6859
 6874 4104 AA24      		clr r10
6860
 6875 4106 BB24      		clr r11
6861
 6876               	.L563:
6862
 6877 4108 8091 0000 		lds r24,ParamSet+57
6863
 6878 410c 90E0      		ldi r25,lo8(0)
6864
 6879 410e 8A15      		cp r24,r10
6865
 6880 4110 9B05      		cpc r25,r11
6866
 6881 4112 04F4      		brge .L564
6867
 6882 4114 9093 0000 		sts (cnt.2426)+1,r25
6868
 6883 4118 8093 0000 		sts cnt.2426,r24
6869
 6884               	.L564:
6870
 6885 411c DD20      		tst r13
6871
 6886 411e 01F0      		breq .L565
6872
 6887 4120 2D2D      		mov r18,r13
6873
 6888 4122 3327      		clr r19
6874
 6889 4124 27FD      		sbrc r18,7
6875
 6890 4126 3095      		com r19
6876
 6891 4128 8091 0000 		lds r24,cnt.2426
6877
 6892 412c 9091 0000 		lds r25,(cnt.2426)+1
6878
 6893 4130 2817      		cp r18,r24
6879
 6894 4132 3907      		cpc r19,r25
6880
 6895 4134 04F4      		brge .L565
6881
 6896 4136 3093 0000 		sts (cnt.2426)+1,r19
6882
 6897 413a 2093 0000 		sts cnt.2426,r18
6883
 6898               	.L565:
6884
 6899 413e F1E8      		ldi r31,lo8(129)
6885
 6900 4140 EF16      		cp r14,r31
6886
 6901 4142 F104      		cpc r15,__zero_reg__
6887
 6902 4144 0105      		cpc r16,__zero_reg__
6888
 6903 4146 1105      		cpc r17,__zero_reg__
6889
 6904 4148 04F0      		brlt .L566
6890
 6905 414a 8091 0000 		lds r24,BiasHiResGyroRoll
6891
 6906 414e 9091 0000 		lds r25,(BiasHiResGyroRoll)+1
6892
 6907 4152 2091 0000 		lds r18,cnt.2426
6893
 6908 4156 3091 0000 		lds r19,(cnt.2426)+1
6894
 6909 415a 820F      		add r24,r18
6895
 6910 415c 931F      		adc r25,r19
6896
 6911 415e 9093 0000 		sts (BiasHiResGyroRoll)+1,r25
6897
 6912 4162 8093 0000 		sts BiasHiResGyroRoll,r24
6898
 6913               	.L566:
6899
 6914 4166 20E8      		ldi r18,lo8(-128)
6900
 6915 4168 E216      		cp r14,r18
6901
 6916 416a 2FEF      		ldi r18,hi8(-128)
6902
 6917 416c F206      		cpc r15,r18
6903
 6918 416e 2FEF      		ldi r18,hlo8(-128)
6904
 6919 4170 0207      		cpc r16,r18
6905
 6920 4172 2FEF      		ldi r18,hhi8(-128)
6906
 6921 4174 1207      		cpc r17,r18
6907
 6922 4176 04F0      		brlt .+4
6908
 6923 4178 0C94 0000 		jmp .L567
6909
 6924 417c 8091 0000 		lds r24,BiasHiResGyroRoll
6910
 6925 4180 9091 0000 		lds r25,(BiasHiResGyroRoll)+1
6911
 6926 4184 2091 0000 		lds r18,cnt.2426
6912
 6927 4188 3091 0000 		lds r19,(cnt.2426)+1
6913
 6928 418c 821B      		sub r24,r18
6914
 6929 418e 930B      		sbc r25,r19
6915
 6930 4190 9093 0000 		sts (BiasHiResGyroRoll)+1,r25
6916
 6931 4194 8093 0000 		sts BiasHiResGyroRoll,r24
6917
 6932 4198 0C94 0000 		jmp .L567
6918
 6933               	.L753:
6919
 6934 419c 8091 0000 		lds r24,LoopingRoll
6920
 6935 41a0 8823      		tst r24
6921
 6936 41a2 01F0      		breq .+4
6922
 6937 41a4 0C94 0000 		jmp .L510
6923
 6938               	.L509:
6924
 6939 41a8 8091 0000 		lds r24,AdValueAccZ
6925
 6940 41ac 9091 0000 		lds r25,(AdValueAccZ)+1
6926
 6941 41b0 8150      		subi r24,lo8(513)
6927
 6942 41b2 9240      		sbci r25,hi8(513)
6928
 6943 41b4 04F4      		brge .+2
6929
 6944 41b6 00C0      		rjmp .L794
6930
 6945               	.L511:
6931
 6946 41b8 8091 0000 		lds r24,FCParam+31
6932
 6947 41bc 8F3F      		cpi r24,lo8(-1)
6933
 6948 41be 01F4      		brne .+2
6934
 6949 41c0 00C0      		rjmp .L512
6935
 6950 41c2 E090 0000 		lds r14,ParamSet+23
6936
 6951 41c6 FF24      		clr r15
6937
 6952 41c8 00E0      		ldi r16,lo8(0)
6938
 6953 41ca 10E0      		ldi r17,hi8(0)
6939
 6954 41cc A82E      		mov r10,r24
6940
 6955 41ce BB24      		clr r11
6941
 6956 41d0 A7FC      		sbrc r10,7
6942
 6957 41d2 B094      		com r11
6943
 6958 41d4 CB2C      		mov r12,r11
6944
 6959 41d6 DB2C      		mov r13,r11
6945
 6960 41d8 68A5      		ldd r22,Y+40
6946
 6961 41da 79A5      		ldd r23,Y+41
6947
 6962 41dc 8AA5      		ldd r24,Y+42
6948
 6963 41de 9BA5      		ldd r25,Y+43
6949
 6964 41e0 A801      		movw r20,r16
6950
 6965 41e2 9701      		movw r18,r14
6951
 6966 41e4 0E94 0000 		call __divmodsi4
6952
 6967 41e8 8091 0000 		lds r24,AccNick
6953
 6968 41ec 9091 0000 		lds r25,(AccNick)+1
6954
 6969 41f0 AA27      		clr r26
6955
 6970 41f2 97FD      		sbrc r25,7
6956
 6971 41f4 A095      		com r26
6957
 6972 41f6 BA2F      		mov r27,r26
6958
 6973 41f8 281B      		sub r18,r24
6959
 6974 41fa 390B      		sbc r19,r25
6960
 6975 41fc 4A0B      		sbc r20,r26
6961
 6976 41fe 5B0B      		sbc r21,r27
6962
 6977 4200 CA01      		movw r24,r20
6963
 6978 4202 B901      		movw r22,r18
6964
 6979 4204 A601      		movw r20,r12
6965
 6980 4206 9501      		movw r18,r10
6966
 6981 4208 0E94 0000 		call __mulsi3
6967
 6982 420c 20E0      		ldi r18,lo8(512)
6968
 6983 420e 32E0      		ldi r19,hi8(512)
6969
 6984 4210 40E0      		ldi r20,hlo8(512)
6970
 6985 4212 50E0      		ldi r21,hhi8(512)
6971
 6986 4214 0E94 0000 		call __divmodsi4
6972
 6987 4218 522E      		mov r5,r18
6973
 6988 421a 432E      		mov r4,r19
6974
 6989 421c 342E      		mov r3,r20
6975
 6990 421e 252E      		mov r2,r21
6976
 6991 4220 822F      		mov r24,r18
6977
 6992 4222 932F      		mov r25,r19
6978
 6993 4224 A42F      		mov r26,r20
6979
 6994 4226 B52F      		mov r27,r21
6980
 6995 4228 3C01      		movw r6,r24
6981
 6996 422a 4D01      		movw r8,r26
6982
 6997 422c 6CA1      		ldd r22,Y+36
6983
 6998 422e 7DA1      		ldd r23,Y+37
6984
 6999 4230 8EA1      		ldd r24,Y+38
6985
 7000 4232 9FA1      		ldd r25,Y+39
6986
 7001 4234 A801      		movw r20,r16
6987
 7002 4236 9701      		movw r18,r14
6988
 7003 4238 0E94 0000 		call __divmodsi4
6989
 7004 423c 8091 0000 		lds r24,AccRoll
6990
 7005 4240 9091 0000 		lds r25,(AccRoll)+1
6991
 7006 4244 AA27      		clr r26
6992
 7007 4246 97FD      		sbrc r25,7
6993
 7008 4248 A095      		com r26
6994
 7009 424a BA2F      		mov r27,r26
6995
 7010 424c 281B      		sub r18,r24
6996
 7011 424e 390B      		sbc r19,r25
6997
 7012 4250 4A0B      		sbc r20,r26
6998
 7013 4252 5B0B      		sbc r21,r27
6999
 7014 4254 CA01      		movw r24,r20
7000
 7015 4256 B901      		movw r22,r18
7001
 7016 4258 A601      		movw r20,r12
7002
 7017 425a 9501      		movw r18,r10
7003
 7018 425c 0E94 0000 		call __mulsi3
7004
 7019 4260 20E0      		ldi r18,lo8(512)
7005
 7020 4262 32E0      		ldi r19,hi8(512)
7006
 7021 4264 40E0      		ldi r20,hlo8(512)
7007
 7022 4266 50E0      		ldi r21,hhi8(512)
7008
 7023 4268 0E94 0000 		call __divmodsi4
7009
 7024 426c 122F      		mov r17,r18
7010
 7025 426e 032F      		mov r16,r19
7011
 7026 4270 F42E      		mov r15,r20
7012
 7027 4272 E52E      		mov r14,r21
7013
 7028 4274 822F      		mov r24,r18
7014
 7029 4276 932F      		mov r25,r19
7015
 7030 4278 A42F      		mov r26,r20
7016
 7031 427a B52F      		mov r27,r21
7017
 7032 427c 5C01      		movw r10,r24
7018
 7033 427e 6D01      		movw r12,r26
7019
 7034 4280 8091 0000 		lds r24,MaxStickNick
7020
 7035 4284 9091 0000 		lds r25,(MaxStickNick)+1
7021
 7036 4288 8134      		cpi r24,65
7022
 7037 428a 9105      		cpc r25,__zero_reg__
7023
 7038 428c 04F0      		brlt .+2
7024
 7039 428e 00C0      		rjmp .L513
7025
 7040 4290 8091 0000 		lds r24,MaxStickRoll
7026
 7041 4294 9091 0000 		lds r25,(MaxStickRoll)+1
7027
 7042 4298 8134      		cpi r24,65
7028
 7043 429a 9105      		cpc r25,__zero_reg__
7029
 7044 429c 04F0      		brlt .+2
7030
 7045 429e 00C0      		rjmp .L513
7031
 7046               	.L514:
7032
 7047 42a0 E091 0000 		lds r30,ParamSet+3
7033
 7048 42a4 F0E0      		ldi r31,lo8(0)
7034
 7049 42a6 EE0F      		lsl r30
7035
 7050 42a8 FF1F      		rol r31
7036
 7051 42aa E050      		subi r30,lo8(-(PPM_in))
7037
 7052 42ac F040      		sbci r31,hi8(-(PPM_in))
7038
 7053 42ae 0190      		ld __tmp_reg__,Z+
7039
 7054 42b0 F081      		ld r31,Z
7040
 7055 42b2 E02D      		mov r30,__tmp_reg__
7041
 7056 42b4 F7FF      		sbrs r31,7
7042
 7057 42b6 00C0      		rjmp .+4
7043
 7058 42b8 0C94 0000 		jmp .L795
7044
 7059               	.L516:
7045
 7060 42bc 7A97      		sbiw r30,26
7046
 7061 42be 04F0      		brlt .L515
7047
 7062 42c0 C401      		movw r24,r8
7048
 7063 42c2 B301      		movw r22,r6
7049
 7064 42c4 23E0      		ldi r18,lo8(3)
7050
 7065 42c6 30E0      		ldi r19,hi8(3)
7051
 7066 42c8 40E0      		ldi r20,hlo8(3)
7052
 7067 42ca 50E0      		ldi r21,hhi8(3)
7053
 7068 42cc 0E94 0000 		call __divmodsi4
7054
 7069 42d0 C901      		movw r24,r18
7055
 7070 42d2 DA01      		movw r26,r20
7056
 7071 42d4 3C01      		movw r6,r24
7057
 7072 42d6 4D01      		movw r8,r26
7058
 7073 42d8 C601      		movw r24,r12
7059
 7074 42da B501      		movw r22,r10
7060
 7075 42dc 23E0      		ldi r18,lo8(3)
7061
 7076 42de 30E0      		ldi r19,hi8(3)
7062
 7077 42e0 40E0      		ldi r20,hlo8(3)
7063
 7078 42e2 50E0      		ldi r21,hhi8(3)
7064
 7079 42e4 0E94 0000 		call __divmodsi4
7065
 7080 42e8 C901      		movw r24,r18
7066
 7081 42ea DA01      		movw r26,r20
7067
 7082 42ec 5C01      		movw r10,r24
7068
 7083 42ee 6D01      		movw r12,r26
7069
 7084               	.L515:
7070
 7085 42f0 8091 0000 		lds r24,FCParam+33
7071
 7086 42f4 9927      		clr r25
7072
 7087 42f6 87FD      		sbrc r24,7
7073
 7088 42f8 9095      		com r25
7074
 7089 42fa A92F      		mov r26,r25
7075
 7090 42fc B92F      		mov r27,r25
7076
 7091 42fe 2227      		clr r18
7077
 7092 4300 3327      		clr r19
7078
 7093 4302 A901      		movw r20,r18
7079
 7094 4304 281B      		sub r18,r24
7080
 7095 4306 390B      		sbc r19,r25
7081
 7096 4308 4A0B      		sbc r20,r26
7082
 7097 430a 5B0B      		sbc r21,r27
7083
 7098 430c 6216      		cp r6,r18
7084
 7099 430e 7306      		cpc r7,r19
7085
 7100 4310 8406      		cpc r8,r20
7086
 7101 4312 9506      		cpc r9,r21
7087
 7102 4314 04F0      		brlt .+2
7088
 7103 4316 00C0      		rjmp .L517
7089
 7104 4318 7901      		movw r14,r18
7090
 7105 431a 8A01      		movw r16,r20
7091
 7106               	.L518:
7092
 7107 431c A216      		cp r10,r18
7093
 7108 431e B306      		cpc r11,r19
7094
 7109 4320 C406      		cpc r12,r20
7095
 7110 4322 D506      		cpc r13,r21
7096
 7111 4324 04F0      		brlt .+2
7097
 7112 4326 00C0      		rjmp .L520
7098
 7113 4328 5901      		movw r10,r18
7099
 7114 432a 6A01      		movw r12,r20
7100
 7115               	.L521:
7101
 7116 432c 8091 0000 		lds r24,ReadingIntegralGyroNick
7102
 7117 4330 9091 0000 		lds r25,(ReadingIntegralGyroNick)+1
7103
 7118 4334 A091 0000 		lds r26,(ReadingIntegralGyroNick)+2
7104
 7119 4338 B091 0000 		lds r27,(ReadingIntegralGyroNick)+3
7105
 7120 433c 8E19      		sub r24,r14
7106
 7121 433e 9F09      		sbc r25,r15
7107
 7122 4340 A00B      		sbc r26,r16
7108
 7123 4342 B10B      		sbc r27,r17
7109
 7124 4344 8093 0000 		sts ReadingIntegralGyroNick,r24
7110
 7125 4348 9093 0000 		sts (ReadingIntegralGyroNick)+1,r25
7111
 7126 434c A093 0000 		sts (ReadingIntegralGyroNick)+2,r26
7112
 7127 4350 B093 0000 		sts (ReadingIntegralGyroNick)+3,r27
7113
 7128 4354 8091 0000 		lds r24,ReadingIntegralGyroRoll
7114
 7129 4358 9091 0000 		lds r25,(ReadingIntegralGyroRoll)+1
7115
 7130 435c A091 0000 		lds r26,(ReadingIntegralGyroRoll)+2
7116
 7131 4360 B091 0000 		lds r27,(ReadingIntegralGyroRoll)+3
7117
 7132 4364 8A19      		sub r24,r10
7118
 7133 4366 9B09      		sbc r25,r11
7119
 7134 4368 AC09      		sbc r26,r12
7120
 7135 436a BD09      		sbc r27,r13
7121
 7136 436c 8093 0000 		sts ReadingIntegralGyroRoll,r24
7122
 7137 4370 9093 0000 		sts (ReadingIntegralGyroRoll)+1,r25
7123
 7138 4374 A093 0000 		sts (ReadingIntegralGyroRoll)+2,r26
7124
 7139 4378 B093 0000 		sts (ReadingIntegralGyroRoll)+3,r27
7125
 7140 437c 0C94 0000 		jmp .L510
7126
 7141               	.L486:
7127
 7142 4380 4150      		subi r20,lo8(-(-1))
7128
 7143 4382 5040      		sbci r21,hi8(-(-1))
7129
 7144 4384 5093 0000 		sts (MaxStickRoll)+1,r21
7130
 7145 4388 4093 0000 		sts MaxStickRoll,r20
7131
 7146 438c 0C94 0000 		jmp .L490
7132
 7147               	.L468:
7133
 7148 4390 1092 0000 		sts delay_startmotors.2417,__zero_reg__
7134
 7149               	.L469:
7135
 7150 4394 E091 0000 		lds r30,ParamSet+3
7136
 7151 4398 F0E0      		ldi r31,lo8(0)
7137
 7152 439a EE0F      		lsl r30
7138
 7153 439c FF1F      		rol r31
7139
 7154 439e E050      		subi r30,lo8(-(PPM_in))
7140
 7155 43a0 F040      		sbci r31,hi8(-(PPM_in))
7141
 7156 43a2 8081      		ld r24,Z
7142
 7157 43a4 9181      		ldd r25,Z+1
7143
 7158 43a6 8C34      		cpi r24,76
7144
 7159 43a8 9105      		cpc r25,__zero_reg__
7145
 7160 43aa 04F4      		brge .+2
7146
 7161 43ac 00C0      		rjmp .L470
7147
 7162 43ae 8091 0000 		lds r24,delay_stopmotors.2418
7148
 7163 43b2 8F5F      		subi r24,lo8(-(1))
7149
 7164 43b4 8093 0000 		sts delay_stopmotors.2418,r24
7150
 7165 43b8 893C      		cpi r24,lo8(-55)
7151
 7166 43ba 00F4      		brsh .+4
7152
 7167 43bc 0C94 0000 		jmp .L410
7153
 7168 43c0 88EC      		ldi r24,lo8(-56)
7154
 7169 43c2 8093 0000 		sts delay_stopmotors.2418,r24
7155
 7170 43c6 1092 0000 		sts (ModelIsFlying)+1,__zero_reg__
7156
 7171 43ca 1092 0000 		sts ModelIsFlying,__zero_reg__
7157
 7172               	.L727:
7158
 7173 43ce 8091 0000 		lds r24,MKFlags
7159
 7174 43d2 8E7F      		andi r24,lo8(-2)
7160
 7175 43d4 8093 0000 		sts MKFlags,r24
7161
 7176 43d8 0C94 0000 		jmp .L410
7162
 7177               	.L582:
7163
 7178 43dc 6B01      		movw r12,r22
7164
 7179 43de CC0C      		lsl r12
7165
 7180 43e0 DD1C      		rol r13
7166
 7181 43e2 CC0C      		lsl r12
7167
 7182 43e4 DD1C      		rol r13
7168
 7183 43e6 CC0C      		lsl r12
7169
 7184 43e8 DD1C      		rol r13
7170
 7185 43ea 0C94 0000 		jmp .L584
7171
 7186               	.L755:
7172
 7187 43ee 8827      		clr r24
7173
 7188 43f0 9927      		clr r25
7174
 7189 43f2 8E19      		sub r24,r14
7175
 7190 43f4 9F09      		sbc r25,r15
7176
 7191 43f6 0C94 0000 		jmp .L570
7177
 7192               	.L783:
7178
 7193 43fa 2227      		clr r18
7179
 7194 43fc 3327      		clr r19
7180
 7195 43fe 2619      		sub r18,r6
7181
 7196 4400 3709      		sbc r19,r7
7182
 7197 4402 00C0      		rjmp .L695
7183
 7198               	.L758:
7184
 7199 4404 6D5F      		subi r22,lo8(-(3))
7185
 7200 4406 7F4F      		sbci r23,hi8(-(3))
7186
 7201 4408 0C94 0000 		jmp .L572
7187
 7202               	.L757:
7188
 7203 440c EE27      		clr r30
7189
 7204 440e FF27      		clr r31
7190
 7205 4410 EE19      		sub r30,r14
7191
 7206 4412 FF09      		sbc r31,r15
7192
 7207 4414 0C94 0000 		jmp .L571
7193
 7208               	.L736:
7194
 7209 4418 1092 0000 		sts (StickGas)+1,__zero_reg__
7195
 7210 441c 1092 0000 		sts StickGas,__zero_reg__
7196
 7211 4420 8091 0000 		lds r24,ParamSet+8
7197
 7212 4424 82FD      		sbrc r24,2
7198
 7213 4426 00C0      		rjmp .+4
7199
 7214 4428 0C94 0000 		jmp .L480
7200
 7215 442c 00C0      		rjmp .L737
7201
 7216               	.L735:
7202
 7217 442e 2250      		subi r18,lo8(-(-2))
7203
 7218 4430 3040      		sbci r19,hi8(-(-2))
7204
 7219 4432 3093 0000 		sts (StickYaw)+1,r19
7205
 7220 4436 2093 0000 		sts StickYaw,r18
7206
 7221 443a 0C94 0000 		jmp .L475
7207
 7222               	.L744:
7208
 7223 443e 84E6      		ldi r24,lo8(100)
7209
 7224 4440 90E0      		ldi r25,hi8(100)
7210
 7225 4442 9093 0000 		sts (MaxStickRoll)+1,r25
7211
 7226 4446 8093 0000 		sts MaxStickRoll,r24
7212
 7227 444a 0C94 0000 		jmp .L490
7213
 7228               	.L503:
7214
 7229 444e 1092 0000 		sts LoopingNick,__zero_reg__
7215
 7230 4452 0C94 0000 		jmp .L472
7216
 7231               	.L500:
7217
 7232 4456 1092 0000 		sts LoopingRoll,__zero_reg__
7218
 7233 445a 0C94 0000 		jmp .L501
7219
 7234               	.L751:
7220
 7235 445e 81E0      		ldi r24,lo8(1)
7221
 7236 4460 8093 0000 		sts LoopingDown,r24
7222
 7237 4464 0C94 0000 		jmp .L498
7223
 7238               	.L749:
7224
 7239 4468 81E0      		ldi r24,lo8(1)
7225
 7240 446a 8093 0000 		sts LoopingTop,r24
7226
 7241 446e 8091 0000 		lds r24,ParamSet+49
7227
 7242 4472 482F      		mov r20,r24
7228
 7243 4474 50E0      		ldi r21,lo8(0)
7229
 7244 4476 61E0      		ldi r22,lo8(1)
7230
 7245 4478 0C94 0000 		jmp .L496
7231
 7246               	.L747:
7232
 7247 447c 81E0      		ldi r24,lo8(1)
7233
 7248 447e 8093 0000 		sts LoopingRight,r24
7234
 7249 4482 8091 0000 		lds r24,ParamSet+49
7235
 7250 4486 482F      		mov r20,r24
7236
 7251 4488 50E0      		ldi r21,lo8(0)
7237
 7252 448a 0C94 0000 		jmp .L494
7238
 7253               	.L745:
7239
 7254 448e 81E0      		ldi r24,lo8(1)
7240
 7255 4490 8093 0000 		sts LoopingLeft,r24
7241
 7256 4494 71E0      		ldi r23,lo8(1)
7242
 7257 4496 0C94 0000 		jmp .L492
7243
 7258               	.L588:
7244
 7259 449a 0894      		sec
7245
 7260 449c 4108      		sbc r4,__zero_reg__
7246
 7261 449e 5108      		sbc r5,__zero_reg__
7247
 7262 44a0 5092 0000 		sts (BadCompassHeading)+1,r5
7248
 7263 44a4 4092 0000 		sts BadCompassHeading,r4
7249
 7264 44a8 0C94 0000 		jmp .L575
7250
 7265               	.L794:
7251
 7266 44ac 8091 0000 		lds r24,MKFlags
7252
 7267 44b0 80FD      		sbrc r24,0
7253
 7268 44b2 00C0      		rjmp .+4
7254
 7269 44b4 0C94 0000 		jmp .L510
7255
 7270 44b8 00C0      		rjmp .L511
7256
 7271               	.L748:
7257
 7272 44ba 1092 0000 		sts LoopingRight,__zero_reg__
7258
 7273 44be 8091 0000 		lds r24,ParamSet+49
7259
 7274 44c2 482F      		mov r20,r24
7260
 7275 44c4 50E0      		ldi r21,lo8(0)
7261
 7276 44c6 0C94 0000 		jmp .L494
7262
 7277               	.L746:
7263
 7278 44ca 1092 0000 		sts LoopingLeft,__zero_reg__
7264
 7279 44ce 8091 0000 		lds r24,ParamSet+1
7265
 7280 44d2 A82F      		mov r26,r24
7266
 7281 44d4 B0E0      		ldi r27,lo8(0)
7267
 7282 44d6 8091 0000 		lds r24,ParamSet+49
7268
 7283 44da 482F      		mov r20,r24
7269
 7284 44dc 50E0      		ldi r21,lo8(0)
7270
 7285 44de AA0F      		lsl r26
7271
 7286 44e0 BB1F      		rol r27
7272
 7287 44e2 70E0      		ldi r23,lo8(0)
7273
 7288 44e4 0C94 0000 		jmp .L492
7274
 7289               	.L752:
7275
 7290 44e8 1092 0000 		sts LoopingDown,__zero_reg__
7276
 7291 44ec 0C94 0000 		jmp .L498
7277
 7292               	.L750:
7278
 7293 44f0 1092 0000 		sts LoopingTop,__zero_reg__
7279
 7294 44f4 8091 0000 		lds r24,ParamSet
7280
 7295 44f8 A82F      		mov r26,r24
7281
 7296 44fa B0E0      		ldi r27,lo8(0)
7282
 7297 44fc 8091 0000 		lds r24,ParamSet+49
7283
 7298 4500 482F      		mov r20,r24
7284
 7299 4502 50E0      		ldi r21,lo8(0)
7285
 7300 4504 AA0F      		lsl r26
7286
 7301 4506 BB1F      		rol r27
7287
 7302 4508 60E0      		ldi r22,lo8(0)
7288
 7303 450a 0C94 0000 		jmp .L496
7289
 7304               	.L412:
7290
 7305 450e 8091 0000 		lds r24,MKFlags
7291
 7306 4512 8260      		ori r24,lo8(2)
7292
 7307 4514 8093 0000 		sts MKFlags,r24
7293
 7308 4518 0C94 0000 		jmp .L413
7294
 7309               	.L780:
7295
 7310 451c 0196      		adiw r24,1
7296
 7311 451e 0C94 0000 		jmp .L676
7297
 7312               	.L764:
7298
 7313 4522 8091 0000 		lds r24,YawGyroDrift
7299
 7314 4526 9091 0000 		lds r25,(YawGyroDrift)+1
7300
 7315 452a 860F      		add r24,r22
7301
 7316 452c 971F      		adc r25,r23
7302
 7317 452e 9093 0000 		sts (YawGyroDrift)+1,r25
7303
 7318 4532 8093 0000 		sts YawGyroDrift,r24
7304
 7319 4536 8091 0000 		lds r24,UpdateCompassCourse.2420
7305
 7320 453a 9091 0000 		lds r25,(UpdateCompassCourse.2420)+1
7306
 7321 453e 892B      		or r24,r25
7307
 7322 4540 01F4      		brne .+4
7308
 7323 4542 0C94 0000 		jmp .L585
7309
 7324 4546 E090 0000 		lds r14,ParamSet+23
7310
 7325 454a 8AE2      		ldi r24,lo8(42)
7311
 7326 454c E89E      		mul r14,r24
7312
 7327 454e 7001      		movw r14,r0
7313
 7328 4550 1124      		clr r1
7314
 7329 4552 0027      		clr r16
7315
 7330 4554 F7FC      		sbrc r15,7
7316
 7331 4556 0095      		com r16
7317
 7332 4558 102F      		mov r17,r16
7318
 7333 455a 9501      		movw r18,r10
7319
 7334 455c 4427      		clr r20
7320
 7335 455e 37FD      		sbrc r19,7
7321
 7336 4560 4095      		com r20
7322
 7337 4562 542F      		mov r21,r20
7323
 7338 4564 C801      		movw r24,r16
7324
 7339 4566 B701      		movw r22,r14
7325
 7340 4568 0E94 0000 		call __mulsi3
7326
 7341 456c 6093 0000 		sts YawGyroHeading,r22
7327
 7342 4570 7093 0000 		sts (YawGyroHeading)+1,r23
7328
 7343 4574 8093 0000 		sts (YawGyroHeading)+2,r24
7329
 7344 4578 9093 0000 		sts (YawGyroHeading)+3,r25
7330
 7345 457c A801      		movw r20,r16
7331
 7346 457e 9701      		movw r18,r14
7332
 7347 4580 0E94 0000 		call __divmodsi4
7333
 7348 4584 3093 0000 		sts (CompassCourse)+1,r19
7334
 7349 4588 2093 0000 		sts CompassCourse,r18
7335
 7350 458c 1092 0000 		sts (UpdateCompassCourse.2420)+1,__zero_reg__
7336
 7351 4590 1092 0000 		sts UpdateCompassCourse.2420,__zero_reg__
7337
 7352 4594 0C94 0000 		jmp .L585
7338
 7353               	.L513:
7339
 7354 4598 652D      		mov r22,r5
7340
 7355 459a 742D      		mov r23,r4
7341
 7356 459c 832D      		mov r24,r3
7342
 7357 459e 922D      		mov r25,r2
7343
 7358 45a0 22E0      		ldi r18,lo8(2)
7344
 7359 45a2 30E0      		ldi r19,hi8(2)
7345
 7360 45a4 40E0      		ldi r20,hlo8(2)
7346
 7361 45a6 50E0      		ldi r21,hhi8(2)
7347
 7362 45a8 0E94 0000 		call __divmodsi4
7348
 7363 45ac C901      		movw r24,r18
7349
 7364 45ae DA01      		movw r26,r20
7350
 7365 45b0 3C01      		movw r6,r24
7351
 7366 45b2 4D01      		movw r8,r26
7352
 7367 45b4 612F      		mov r22,r17
7353
 7368 45b6 702F      		mov r23,r16
7354
 7369 45b8 8F2D      		mov r24,r15
7355
 7370 45ba 9E2D      		mov r25,r14
7356
 7371 45bc 22E0      		ldi r18,lo8(2)
7357
 7372 45be 30E0      		ldi r19,hi8(2)
7358
 7373 45c0 40E0      		ldi r20,hlo8(2)
7359
 7374 45c2 50E0      		ldi r21,hhi8(2)
7360
 7375 45c4 0E94 0000 		call __divmodsi4
7361
 7376 45c8 C901      		movw r24,r18
7362
 7377 45ca DA01      		movw r26,r20
7363
 7378 45cc 5C01      		movw r10,r24
7364
 7379 45ce 6D01      		movw r12,r26
7365
 7380 45d0 00C0      		rjmp .L514
7366
 7381               	.L451:
7367
 7382 45d2 8091 0000 		lds r24,ParamSet+1
7368
 7383 45d6 A82F      		mov r26,r24
7369
 7384 45d8 B0E0      		ldi r27,lo8(0)
7370
 7385 45da AA0F      		lsl r26
7371
 7386 45dc BB1F      		rol r27
7372
 7387 45de FD01      		movw r30,r26
7373
 7388 45e0 E050      		subi r30,lo8(-(PPM_in))
7374
 7389 45e2 F040      		sbci r31,hi8(-(PPM_in))
7375
 7390 45e4 0190      		ld __tmp_reg__,Z+
7376
 7391 45e6 F081      		ld r31,Z
7377
 7392 45e8 E02D      		mov r30,__tmp_reg__
7378
 7393 45ea F7FF      		sbrs r31,7
7379
 7394 45ec 00C0      		rjmp .+4
7380
 7395 45ee 0C94 0000 		jmp .L796
7381
 7396               	.L454:
7382
 7397 45f2 E734      		cpi r30,71
7383
 7398 45f4 F105      		cpc r31,__zero_reg__
7384
 7399 45f6 04F0      		brlt .+4
7385
 7400 45f8 0C94 0000 		jmp .L452
7386
 7401 45fc 8091 0000 		lds r24,ParamSet+8
7387
 7402 4600 8872      		andi r24,lo8(40)
7388
 7403 4602 01F4      		brne .+4
7389
 7404 4604 0C94 0000 		jmp .L461
7390
 7405 4608 A050      		subi r26,lo8(-(PPM_in))
7391
 7406 460a B040      		sbci r27,hi8(-(PPM_in))
7392
 7407 460c 8D91      		ld r24,X+
7393
 7408 460e 9C91      		ld r25,X
7394
 7409 4610 97FF      		sbrs r25,7
7395
 7410 4612 00C0      		rjmp .+4
7396
 7411 4614 0C94 0000 		jmp .L797
7397
 7412               	.L463:
7398
 7413 4618 4E97      		sbiw r24,30
7399
 7414 461a 04F0      		brlt .+4
7400
 7415 461c 0C94 0000 		jmp .L461
7401
 7416 4620 4050      		subi r20,lo8(-(PPM_in))
7402
 7417 4622 5040      		sbci r21,hi8(-(PPM_in))
7403
 7418 4624 FA01      		movw r30,r20
7404
 7419 4626 8081      		ld r24,Z
7405
 7420 4628 9181      		ldd r25,Z+1
7406
 7421 462a 8A5B      		subi r24,lo8(-70)
7407
 7422 462c 9F4F      		sbci r25,hi8(-70)
7408
 7423 462e 04F0      		brlt .+4
7409
 7424 4630 0C94 0000 		jmp .L461
7410
 7425 4634 81E0      		ldi r24,lo8(1)
7411
 7426 4636 8093 0000 		sts CompassCalState,r24
7412
 7427 463a 88EE      		ldi r24,lo8(1000)
7413
 7428 463c 93E0      		ldi r25,hi8(1000)
7414
 7429 463e 9093 0000 		sts (BeepTime)+1,r25
7415
 7430 4642 8093 0000 		sts BeepTime,r24
7416
 7431 4646 8091 0000 		lds r24,ParamSet+2
7417
 7432 464a A82F      		mov r26,r24
7418
 7433 464c B0E0      		ldi r27,lo8(0)
7419
 7434 464e AA0F      		lsl r26
7420
 7435 4650 BB1F      		rol r27
7421
 7436               	.L446:
7422
 7437 4652 A050      		subi r26,lo8(-(PPM_in))
7423
 7438 4654 B040      		sbci r27,hi8(-(PPM_in))
7424
 7439 4656 8D91      		ld r24,X+
7425
 7440 4658 9C91      		ld r25,X
7426
 7441 465a 8B5A      		subi r24,lo8(-85)
7427
 7442 465c 9F4F      		sbci r25,hi8(-85)
7428
 7443 465e 04F0      		brlt .+4
7429
 7444 4660 0C94 0000 		jmp .L410
7430
 7445 4664 E091 0000 		lds r30,ParamSet+3
7431
 7446 4668 F0E0      		ldi r31,lo8(0)
7432
 7447 466a EE0F      		lsl r30
7433
 7448 466c FF1F      		rol r31
7434
 7449 466e E050      		subi r30,lo8(-(PPM_in))
7435
 7450 4670 F040      		sbci r31,hi8(-(PPM_in))
7436
 7451 4672 8081      		ld r24,Z
7437
 7452 4674 9181      		ldd r25,Z+1
7438
 7453 4676 855B      		subi r24,lo8(-75)
7439
 7454 4678 9F4F      		sbci r25,hi8(-75)
7440
 7455 467a 04F0      		brlt .+2
7441
 7456 467c 00C0      		rjmp .L468
7442
 7457 467e 8091 0000 		lds r24,delay_startmotors.2417
7443
 7458 4682 8F5F      		subi r24,lo8(-(1))
7444
 7459 4684 8093 0000 		sts delay_startmotors.2417,r24
7445
 7460 4688 893C      		cpi r24,lo8(-55)
7446
 7461 468a 00F4      		brsh .+2
7447
 7462 468c 00C0      		rjmp .L469
7448
 7463 468e 88EC      		ldi r24,lo8(-56)
7449
 7464 4690 8093 0000 		sts delay_startmotors.2417,r24
7450
 7465 4694 81E0      		ldi r24,lo8(1)
7451
 7466 4696 90E0      		ldi r25,hi8(1)
7452
 7467 4698 9093 0000 		sts (ModelIsFlying)+1,r25
7453
 7468 469c 8093 0000 		sts ModelIsFlying,r24
7454
 7469 46a0 8091 0000 		lds r24,MKFlags
7455
 7470 46a4 8960      		ori r24,lo8(9)
7456
 7471 46a6 8093 0000 		sts MKFlags,r24
7457
 7472 46aa 1092 0000 		sts SetPointYaw.2410,__zero_reg__
7458
 7473 46ae 1092 0000 		sts (SetPointYaw.2410)+1,__zero_reg__
7459
 7474 46b2 1092 0000 		sts (SetPointYaw.2410)+2,__zero_reg__
7460
 7475 46b6 1092 0000 		sts (SetPointYaw.2410)+3,__zero_reg__
7461
 7476 46ba 1092 0000 		sts ReadingIntegralGyroYaw,__zero_reg__
7462
 7477 46be 1092 0000 		sts (ReadingIntegralGyroYaw)+1,__zero_reg__
7463
 7478 46c2 1092 0000 		sts (ReadingIntegralGyroYaw)+2,__zero_reg__
7464
 7479 46c6 1092 0000 		sts (ReadingIntegralGyroYaw)+3,__zero_reg__
7465
 7480 46ca 6091 0000 		lds r22,AccNick
7466
 7481 46ce 7091 0000 		lds r23,(AccNick)+1
7467
 7482 46d2 8827      		clr r24
7468
 7483 46d4 77FD      		sbrc r23,7
7469
 7484 46d6 8095      		com r24
7470
 7485 46d8 982F      		mov r25,r24
7471
 7486 46da E090 0000 		lds r14,ParamSet+23
7472
 7487 46de FF24      		clr r15
7473
 7488 46e0 00E0      		ldi r16,lo8(0)
7474
 7489 46e2 10E0      		ldi r17,hi8(0)
7475
 7490 46e4 A801      		movw r20,r16
7476
 7491 46e6 9701      		movw r18,r14
7477
 7492 46e8 0E94 0000 		call __mulsi3
7478
 7493 46ec 6093 0000 		sts ReadingIntegralGyroNick,r22
7479
 7494 46f0 7093 0000 		sts (ReadingIntegralGyroNick)+1,r23
7480
 7495 46f4 8093 0000 		sts (ReadingIntegralGyroNick)+2,r24
7481
 7496 46f8 9093 0000 		sts (ReadingIntegralGyroNick)+3,r25
7482
 7497 46fc 6091 0000 		lds r22,AccRoll
7483
 7498 4700 7091 0000 		lds r23,(AccRoll)+1
7484
 7499 4704 8827      		clr r24
7485
 7500 4706 77FD      		sbrc r23,7
7486
 7501 4708 8095      		com r24
7487
 7502 470a 982F      		mov r25,r24
7488
 7503 470c 0E94 0000 		call __mulsi3
7489
 7504 4710 6093 0000 		sts ReadingIntegralGyroRoll,r22
7490
 7505 4714 7093 0000 		sts (ReadingIntegralGyroRoll)+1,r23
7491
 7506 4718 8093 0000 		sts (ReadingIntegralGyroRoll)+2,r24
7492
 7507 471c 9093 0000 		sts (ReadingIntegralGyroRoll)+3,r25
7493
 7508 4720 8091 0000 		lds r24,IntegralGyroNick
7494
 7509 4724 9091 0000 		lds r25,(IntegralGyroNick)+1
7495
 7510 4728 A091 0000 		lds r26,(IntegralGyroNick)+2
7496
 7511 472c B091 0000 		lds r27,(IntegralGyroNick)+3
7497
 7512 4730 8093 0000 		sts ReadingIntegralGyroNick2,r24
7498
 7513 4734 9093 0000 		sts (ReadingIntegralGyroNick2)+1,r25
7499
 7514 4738 A093 0000 		sts (ReadingIntegralGyroNick2)+2,r26
7500
 7515 473c B093 0000 		sts (ReadingIntegralGyroNick2)+3,r27
7501
 7516 4740 8091 0000 		lds r24,IntegralGyroRoll
7502
 7517 4744 9091 0000 		lds r25,(IntegralGyroRoll)+1
7503
 7518 4748 A091 0000 		lds r26,(IntegralGyroRoll)+2
7504
 7519 474c B091 0000 		lds r27,(IntegralGyroRoll)+3
7505
 7520 4750 8093 0000 		sts ReadingIntegralGyroRoll2,r24
7506
 7521 4754 9093 0000 		sts (ReadingIntegralGyroRoll2)+1,r25
7507
 7522 4758 A093 0000 		sts (ReadingIntegralGyroRoll2)+2,r26
7508
 7523 475c B093 0000 		sts (ReadingIntegralGyroRoll2)+3,r27
7509
 7524 4760 1092 0000 		sts IPartNick.2408,__zero_reg__
7510
 7525 4764 1092 0000 		sts (IPartNick.2408)+1,__zero_reg__
7511
 7526 4768 1092 0000 		sts (IPartNick.2408)+2,__zero_reg__
7512
 7527 476c 1092 0000 		sts (IPartNick.2408)+3,__zero_reg__
7513
 7528 4770 1092 0000 		sts IPartRoll.2409,__zero_reg__
7514
 7529 4774 1092 0000 		sts (IPartRoll.2409)+1,__zero_reg__
7515
 7530 4778 1092 0000 		sts (IPartRoll.2409)+2,__zero_reg__
7516
 7531 477c 1092 0000 		sts (IPartRoll.2409)+3,__zero_reg__
7517
 7532 4780 00C0      		rjmp .L469
7518
 7533               	.L739:
7519
 7534 4782 3095      		com r19
7520
 7535 4784 2195      		neg r18
7521
 7536 4786 3F4F      		sbci r19,lo8(-1)
7522
 7537 4788 0C94 0000 		jmp .L483
7523
 7538               	.L742:
7524
 7539 478c 3095      		com r19
7525
 7540 478e 2195      		neg r18
7526
 7541 4790 3F4F      		sbci r19,lo8(-1)
7527
 7542 4792 0C94 0000 		jmp .L488
7528
 7543               	.L741:
7529
 7544 4796 2D5F      		subi r18,lo8(-(3))
7530
 7545 4798 3F4F      		sbci r19,hi8(-(3))
7531
 7546 479a 0C94 0000 		jmp .L487
7532
 7547               	.L733:
7533
 7548 479e 4D5F      		subi r20,lo8(-(3))
7534
 7549 47a0 5F4F      		sbci r21,hi8(-(3))
7535
 7550 47a2 0C94 0000 		jmp .L473
7536
 7551               	.L734:
7537
 7552 47a6 4D5F      		subi r20,lo8(-(3))
7538
 7553 47a8 5F4F      		sbci r21,hi8(-(3))
7539
 7554 47aa 0C94 0000 		jmp .L474
7540
 7555               	.L738:
7541
 7556 47ae 2D5F      		subi r18,lo8(-(3))
7542
 7557 47b0 3F4F      		sbci r19,hi8(-(3))
7543
 7558 47b2 0C94 0000 		jmp .L482
7544
 7559               	.L762:
7545
 7560 47b6 60E0      		ldi r22,lo8(0)
7546
 7561 47b8 70E0      		ldi r23,hi8(0)
7547
 7562 47ba 0C94 0000 		jmp .L581
7548
 7563               	.L587:
7549
 7564 47be 84EF      		ldi r24,lo8(500)
7550
 7565 47c0 91E0      		ldi r25,hi8(500)
7551
 7566 47c2 9093 0000 		sts (BadCompassHeading)+1,r25
7552
 7567 47c6 8093 0000 		sts BadCompassHeading,r24
7553
 7568 47ca 0C94 0000 		jmp .L575
7554
 7569               	.L428:
7555
 7570 47ce 8091 0000 		lds r24,PPM_in+24
7556
 7571 47d2 9091 0000 		lds r25,(PPM_in+24)+1
7557
 7572 47d6 8259      		subi r24,lo8(-(110))
7558
 7573 47d8 9F4F      		sbci r25,hi8(-(110))
7559
 7574 47da 8217      		cp r24,r18
7560
 7575 47dc 9307      		cpc r25,r19
7561
 7576 47de 04F0      		brlt .+4
7562
 7577 47e0 0C94 0000 		jmp .L429
7563
 7578 47e4 2115      		cp r18,__zero_reg__
7564
 7579 47e6 3105      		cpc r19,__zero_reg__
7565
 7580 47e8 01F4      		brne .+4
7566
 7581 47ea 0C94 0000 		jmp .L429
7567
 7582 47ee C901      		movw r24,r18
7568
 7583 47f0 0197      		sbiw r24,1
7569
 7584 47f2 9093 0000 		sts (Poti8)+1,r25
7570
 7585 47f6 8093 0000 		sts Poti8,r24
7571
 7586 47fa 9C01      		movw r18,r24
7572
 7587 47fc 0C94 0000 		jmp .L429
7573
 7588               	.L426:
7574
 7589 4800 8091 0000 		lds r24,PPM_in+22
7575
 7590 4804 9091 0000 		lds r25,(PPM_in+22)+1
7576
 7591 4808 8259      		subi r24,lo8(-(110))
7577
 7592 480a 9F4F      		sbci r25,hi8(-(110))
7578
 7593 480c 8417      		cp r24,r20
7579
 7594 480e 9507      		cpc r25,r21
7580
 7595 4810 04F0      		brlt .+4
7581
 7596 4812 0C94 0000 		jmp .L427
7582
 7597 4816 4115      		cp r20,__zero_reg__
7583
 7598 4818 5105      		cpc r21,__zero_reg__
7584
 7599 481a 01F4      		brne .+4
7585
 7600 481c 0C94 0000 		jmp .L427
7586
 7601 4820 CA01      		movw r24,r20
7587
 7602 4822 0197      		sbiw r24,1
7588
 7603 4824 9093 0000 		sts (Poti7)+1,r25
7589
 7604 4828 8093 0000 		sts Poti7,r24
7590
 7605 482c AC01      		movw r20,r24
7591
 7606 482e 0C94 0000 		jmp .L427
7592
 7607               	.L424:
7593
 7608 4832 8091 0000 		lds r24,PPM_in+20
7594
 7609 4836 9091 0000 		lds r25,(PPM_in+20)+1
7595
 7610 483a 8259      		subi r24,lo8(-(110))
7596
 7611 483c 9F4F      		sbci r25,hi8(-(110))
7597
 7612 483e 8617      		cp r24,r22
7598
 7613 4840 9707      		cpc r25,r23
7599
 7614 4842 04F0      		brlt .+4
7600
 7615 4844 0C94 0000 		jmp .L425
7601
 7616 4848 6115      		cp r22,__zero_reg__
7602
 7617 484a 7105      		cpc r23,__zero_reg__
7603
 7618 484c 01F4      		brne .+4
7604
 7619 484e 0C94 0000 		jmp .L425
7605
 7620 4852 CB01      		movw r24,r22
7606
 7621 4854 0197      		sbiw r24,1
7607
 7622 4856 9093 0000 		sts (Poti6)+1,r25
7608
 7623 485a 8093 0000 		sts Poti6,r24
7609
 7624 485e BC01      		movw r22,r24
7610
 7625 4860 0C94 0000 		jmp .L425
7611
 7626               	.L422:
7612
 7627 4864 8091 0000 		lds r24,PPM_in+18
7613
 7628 4868 9091 0000 		lds r25,(PPM_in+18)+1
7614
 7629 486c 8259      		subi r24,lo8(-(110))
7615
 7630 486e 9F4F      		sbci r25,hi8(-(110))
7616
 7631 4870 8E17      		cp r24,r30
7617
 7632 4872 9F07      		cpc r25,r31
7618
 7633 4874 04F0      		brlt .+4
7619
 7634 4876 0C94 0000 		jmp .L423
7620
 7635 487a 3097      		sbiw r30,0
7621
 7636 487c 01F4      		brne .+4
7622
 7637 487e 0C94 0000 		jmp .L423
7623
 7638 4882 CF01      		movw r24,r30
7624
 7639 4884 0197      		sbiw r24,1
7625
 7640 4886 9093 0000 		sts (Poti5)+1,r25
7626
 7641 488a 8093 0000 		sts Poti5,r24
7627
 7642 488e FC01      		movw r30,r24
7628
 7643 4890 0C94 0000 		jmp .L423
7629
 7644               	.L420:
7630
 7645 4894 8081      		ld r24,Z
7631
 7646 4896 9181      		ldd r25,Z+1
7632
 7647 4898 8259      		subi r24,lo8(-(110))
7633
 7648 489a 9F4F      		sbci r25,hi8(-(110))
7634
 7649 489c 8A17      		cp r24,r26
7635
 7650 489e 9B07      		cpc r25,r27
7636
 7651 48a0 04F0      		brlt .+4
7637
 7652 48a2 0C94 0000 		jmp .L421
7638
 7653 48a6 1097      		sbiw r26,0
7639
 7654 48a8 01F4      		brne .+4
7640
 7655 48aa 0C94 0000 		jmp .L421
7641
 7656 48ae CD01      		movw r24,r26
7642
 7657 48b0 0197      		sbiw r24,1
7643
 7658 48b2 9093 0000 		sts (Poti4)+1,r25
7644
 7659 48b6 8093 0000 		sts Poti4,r24
7645
 7660 48ba DC01      		movw r26,r24
7646
 7661 48bc 0C94 0000 		jmp .L421
7647
 7662               	.L418:
7648
 7663 48c0 8081      		ld r24,Z
7649
 7664 48c2 9181      		ldd r25,Z+1
7650
 7665 48c4 8259      		subi r24,lo8(-(110))
7651
 7666 48c6 9F4F      		sbci r25,hi8(-(110))
7652
 7667 48c8 8A15      		cp r24,r10
7653
 7668 48ca 9B05      		cpc r25,r11
7654
 7669 48cc 04F0      		brlt .+4
7655
 7670 48ce 0C94 0000 		jmp .L419
7656
 7671 48d2 A114      		cp r10,__zero_reg__
7657
 7672 48d4 B104      		cpc r11,__zero_reg__
7658
 7673 48d6 01F4      		brne .+4
7659
 7674 48d8 0C94 0000 		jmp .L419
7660
 7675 48dc C501      		movw r24,r10
7661
 7676 48de 0197      		sbiw r24,1
7662
 7677 48e0 9093 0000 		sts (Poti3)+1,r25
7663
 7678 48e4 8093 0000 		sts Poti3,r24
7664
 7679 48e8 5C01      		movw r10,r24
7665
 7680 48ea 0C94 0000 		jmp .L419
7666
 7681               	.L416:
7667
 7682 48ee 8081      		ld r24,Z
7668
 7683 48f0 9181      		ldd r25,Z+1
7669
 7684 48f2 8259      		subi r24,lo8(-(110))
7670
 7685 48f4 9F4F      		sbci r25,hi8(-(110))
7671
 7686 48f6 8017      		cp r24,r16
7672
 7687 48f8 9107      		cpc r25,r17
7673
 7688 48fa 04F0      		brlt .+4
7674
 7689 48fc 0C94 0000 		jmp .L417
7675
 7690 4900 0115      		cp r16,__zero_reg__
7676
 7691 4902 1105      		cpc r17,__zero_reg__
7677
 7692 4904 01F4      		brne .+4
7678
 7693 4906 0C94 0000 		jmp .L417
7679
 7694 490a C801      		movw r24,r16
7680
 7695 490c 0197      		sbiw r24,1
7681
 7696 490e 9093 0000 		sts (Poti2)+1,r25
7682
 7697 4912 8093 0000 		sts Poti2,r24
7683
 7698 4916 8C01      		movw r16,r24
7684
 7699 4918 0C94 0000 		jmp .L417
7685
 7700               	.L414:
7686
 7701 491c 8081      		ld r24,Z
7687
 7702 491e 9181      		ldd r25,Z+1
7688
 7703 4920 8259      		subi r24,lo8(-(110))
7689
 7704 4922 9F4F      		sbci r25,hi8(-(110))
7690
 7705 4924 8E15      		cp r24,r14
7691
 7706 4926 9F05      		cpc r25,r15
7692
 7707 4928 04F0      		brlt .+4
7693
 7708 492a 0C94 0000 		jmp .L415
7694
 7709 492e E114      		cp r14,__zero_reg__
7695
 7710 4930 F104      		cpc r15,__zero_reg__
7696
 7711 4932 01F4      		brne .+4
7697
 7712 4934 0C94 0000 		jmp .L415
7698
 7713 4938 C701      		movw r24,r14
7699
 7714 493a 0197      		sbiw r24,1
7700
 7715 493c 9093 0000 		sts (Poti1)+1,r25
7701
 7716 4940 8093 0000 		sts Poti1,r24
7702
 7717 4944 7C01      		movw r14,r24
7703
 7718 4946 0C94 0000 		jmp .L415
7704
 7719               	.L765:
7705
 7720 494a 4F96      		adiw r24,31
7706
 7721 494c 0C94 0000 		jmp .L586
7707
 7722               	.L763:
7708
 7723 4950 8827      		clr r24
7709
 7724 4952 9927      		clr r25
7710
 7725 4954 8219      		sub r24,r2
7711
 7726 4956 9309      		sbc r25,r3
7712
 7727 4958 0C94 0000 		jmp .L583
7713
 7728               	.L761:
7714
 7729 495c 9094      		com r9
7715
 7730 495e 8194      		neg r8
7716
 7731 4960 9108      		sbc r9,__zero_reg__
7717
 7732 4962 9394      		inc r9
7718
 7733 4964 0C94 0000 		jmp .L578
7719
 7734               	.L760:
7720
 7735 4968 1095      		com r17
7721
 7736 496a 0195      		neg r16
7722
 7737 496c 1F4F      		sbci r17,lo8(-1)
7723
 7738 496e 0C94 0000 		jmp .L577
7724
 7739               	.L477:
7725
 7740 4972 1092 0000 		sts (StickYaw)+1,__zero_reg__
7726
 7741 4976 1092 0000 		sts StickYaw,__zero_reg__
7727
 7742 497a 0C94 0000 		jmp .L475
7728
 7743               	.L776:
7729
 7744 497e 1092 0000 		sts (Poti8)+1,__zero_reg__
7730
 7745 4982 1092 0000 		sts Poti8,__zero_reg__
7731
 7746 4986 0C94 0000 		jmp .L445
7732
 7747               	.L775:
7733
 7748 498a 1092 0000 		sts (Poti7)+1,__zero_reg__
7734
 7749 498e 1092 0000 		sts Poti7,__zero_reg__
7735
 7750 4992 0C94 0000 		jmp .L443
7736
 7751               	.L774:
7737
 7752 4996 1092 0000 		sts (Poti6)+1,__zero_reg__
7738
 7753 499a 1092 0000 		sts Poti6,__zero_reg__
7739
 7754 499e 0C94 0000 		jmp .L441
7740
 7755               	.L773:
7741
 7756 49a2 1092 0000 		sts (Poti5)+1,__zero_reg__
7742
 7757 49a6 1092 0000 		sts Poti5,__zero_reg__
7743
 7758 49aa 0C94 0000 		jmp .L439
7744
 7759               	.L772:
7745
 7760 49ae 1092 0000 		sts (Poti4)+1,__zero_reg__
7746
 7761 49b2 1092 0000 		sts Poti4,__zero_reg__
7747
 7762 49b6 0C94 0000 		jmp .L437
7748
 7763               	.L771:
7749
 7764 49ba 1092 0000 		sts (Poti3)+1,__zero_reg__
7750
 7765 49be 1092 0000 		sts Poti3,__zero_reg__
7751
 7766 49c2 0C94 0000 		jmp .L435
7752
 7767               	.L770:
7753
 7768 49c6 1092 0000 		sts (Poti2)+1,__zero_reg__
7754
 7769 49ca 1092 0000 		sts Poti2,__zero_reg__
7755
 7770 49ce 0C94 0000 		jmp .L433
7756
 7771               	.L769:
7757
 7772 49d2 1092 0000 		sts (Poti1)+1,__zero_reg__
7758
 7773 49d6 1092 0000 		sts Poti1,__zero_reg__
7759
 7774 49da 0C94 0000 		jmp .L431
7760
 7775               	.L590:
7761
 7776 49de 4427      		clr r20
7762
 7777 49e0 5527      		clr r21
7763
 7778 49e2 481B      		sub r20,r24
7764
 7779 49e4 590B      		sbc r21,r25
7765
 7780 49e6 B901      		movw r22,r18
7766
 7781 49e8 2417      		cp r18,r20
7767
 7782 49ea 3507      		cpc r19,r21
7768
 7783 49ec 04F0      		brlt .+4
7769
 7784 49ee 0C94 0000 		jmp .L591
7770
 7785 49f2 BA01      		movw r22,r20
7771
 7786 49f4 0C94 0000 		jmp .L591
7772
 7787               	.L743:
7773
 7788 49f8 3095      		com r19
7774
 7789 49fa 2195      		neg r18
7775
 7790 49fc 3F4F      		sbci r19,lo8(-1)
7776
 7791 49fe 0C94 0000 		jmp .L489
7777
 7792               	.L789:
7778
 7793 4a02 3095      		com r19
7779
 7794 4a04 2195      		neg r18
7780
 7795 4a06 3F4F      		sbci r19,lo8(-1)
7781
 7796 4a08 00C0      		rjmp .L484
7782
 7797               	.L520:
7783
 7798 4a0a 8A15      		cp r24,r10
7784
 7799 4a0c 9B05      		cpc r25,r11
7785
 7800 4a0e AC05      		cpc r26,r12
7786
 7801 4a10 BD05      		cpc r27,r13
7787
 7802 4a12 04F0      		brlt .+2
7788
 7803 4a14 00C0      		rjmp .L521
7789
 7804 4a16 5C01      		movw r10,r24
7790
 7805 4a18 6D01      		movw r12,r26
7791
 7806 4a1a 00C0      		rjmp .L521
7792
 7807               	.L517:
7793
 7808 4a1c 8401      		movw r16,r8
7794
 7809 4a1e 7301      		movw r14,r6
7795
 7810 4a20 8615      		cp r24,r6
7796
 7811 4a22 9705      		cpc r25,r7
7797
 7812 4a24 A805      		cpc r26,r8
7798
 7813 4a26 B905      		cpc r27,r9
7799
 7814 4a28 04F0      		brlt .+2
7800
 7815 4a2a 00C0      		rjmp .L518
7801
 7816 4a2c 7C01      		movw r14,r24
7802
 7817 4a2e 8D01      		movw r16,r26
7803
 7818 4a30 00C0      		rjmp .L518
7804
 7819               	.L611:
7805
 7820 4a32 8091 0000 		lds r24,BaroFlags.2450
7806
 7821 4a36 90E0      		ldi r25,lo8(0)
7807
 7822 4a38 8370      		andi r24,lo8(3)
7808
 7823 4a3a 9070      		andi r25,hi8(3)
7809
 7824 4a3c 892B      		or r24,r25
7810
 7825 4a3e 01F4      		brne .+2
7811
 7826 4a40 00C0      		rjmp .L798
7812
 7827               	.L614:
7813
 7828 4a42 81E0      		ldi r24,lo8(1)
7814
 7829 4a44 8093 0000 		sts HCActive.2443,r24
7815
 7830               	.L613:
7816
 7831 4a48 E090 0000 		lds r14,ParamSet+23
7817
 7832 4a4c 8AE2      		ldi r24,lo8(42)
7818
 7833 4a4e E89E      		mul r14,r24
7819
 7834 4a50 7001      		movw r14,r0
7820
 7835 4a52 1124      		clr r1
7821
 7836 4a54 0027      		clr r16
7822
 7837 4a56 F7FC      		sbrc r15,7
7823
 7838 4a58 0095      		com r16
7824
 7839 4a5a 102F      		mov r17,r16
7825
 7840 4a5c 6091 0000 		lds r22,IntegralGyroNick
7826
 7841 4a60 7091 0000 		lds r23,(IntegralGyroNick)+1
7827
 7842 4a64 8091 0000 		lds r24,(IntegralGyroNick)+2
7828
 7843 4a68 9091 0000 		lds r25,(IntegralGyroNick)+3
7829
 7844 4a6c A801      		movw r20,r16
7830
 7845 4a6e 9701      		movw r18,r14
7831
 7846 4a70 0E94 0000 		call __divmodsi4
7832
 7847 4a74 5901      		movw r10,r18
7833
 7848 4a76 6A01      		movw r12,r20
7834
 7849 4a78 6091 0000 		lds r22,IntegralGyroRoll
7835
 7850 4a7c 7091 0000 		lds r23,(IntegralGyroRoll)+1
7836
 7851 4a80 8091 0000 		lds r24,(IntegralGyroRoll)+2
7837
 7852 4a84 9091 0000 		lds r25,(IntegralGyroRoll)+3
7838
 7853 4a88 A801      		movw r20,r16
7839
 7854 4a8a 9701      		movw r18,r14
7840
 7855 4a8c 0E94 0000 		call __divmodsi4
7841
 7856 4a90 C501      		movw r24,r10
7842
 7857 4a92 B901      		movw r22,r18
7843
 7858 4a94 0E94 0000 		call ihypot
7844
 7859 4a98 8D33      		cpi r24,61
7845
 7860 4a9a 9105      		cpc r25,__zero_reg__
7846
 7861 4a9c 04F0      		brlt .L615
7847
 7862 4a9e 8CE3      		ldi r24,lo8(60)
7848
 7863 4aa0 90E0      		ldi r25,hi8(60)
7849
 7864               	.L615:
7850
 7865 4aa2 0E94 0000 		call c_cos_8192
7851
 7866 4aa6 9A83      		std Y+2,r25
7852
 7867 4aa8 8983      		std Y+1,r24
7853
 7868 4aaa 8091 0000 		lds r24,HCActive.2443
7854
 7869 4aae 8823      		tst r24
7855
 7870 4ab0 01F4      		brne .+2
7856
 7871 4ab2 00C0      		rjmp .L616
7857
 7872 4ab4 8091 0000 		lds r24,MKFlags
7858
 7873 4ab8 84FD      		sbrc r24,4
7859
 7874 4aba 00C0      		rjmp .L616
7860
 7875 4abc 8091 0000 		lds r24,ParamSet+88
7861
 7876 4ac0 80FD      		sbrc r24,0
7862
 7877 4ac2 00C0      		rjmp .L617
7863
 7878 4ac4 8091 0000 		lds r24,ParamSet+8
7864
 7879 4ac8 81FF      		sbrs r24,1
7865
 7880 4aca 00C0      		rjmp .L617
7866
 7881 4acc 8091 0000 		lds r24,MKFlags
7867
 7882 4ad0 81FF      		sbrs r24,1
7868
 7883 4ad2 00C0      		rjmp .L620
7869
 7884 4ad4 6091 0000 		lds r22,StickGasHover.2444
7870
 7885 4ad8 7091 0000 		lds r23,(StickGasHover.2444)+1
7871
 7886 4adc 4091 0000 		lds r20,StickGas
7872
 7887 4ae0 5091 0000 		lds r21,(StickGas)+1
7873
 7888 4ae4 CB01      		movw r24,r22
7874
 7889 4ae6 0F96      		adiw r24,15
7875
 7890 4ae8 8417      		cp r24,r20
7876
 7891 4aea 9507      		cpc r25,r21
7877
 7892 4aec 04F0      		brlt .+2
7878
 7893 4aee 00C0      		rjmp .L621
7879
 7894 4af0 8091 0000 		lds r24,BaroFlags.2450
7880
 7895 4af4 80FD      		sbrc r24,0
7881
 7896 4af6 00C0      		rjmp .L621
7882
 7897 4af8 2091 0000 		lds r18,HeightTrimmingFlag.2452
7883
 7898 4afc 21FF      		sbrs r18,1
7884
 7899 4afe 00C0      		rjmp .L622
7885
 7900 4b00 2D7F      		andi r18,lo8(-3)
7886
 7901 4b02 2093 0000 		sts HeightTrimmingFlag.2452,r18
7887
 7902 4b06 8091 0000 		lds r24,ReadingHeight
7888
 7903 4b0a 9091 0000 		lds r25,(ReadingHeight)+1
7889
 7904 4b0e A091 0000 		lds r26,(ReadingHeight)+2
7890
 7905 4b12 B091 0000 		lds r27,(ReadingHeight)+3
7891
 7906 4b16 8093 0000 		sts SetPointHeight,r24
7892
 7907 4b1a 9093 0000 		sts (SetPointHeight)+1,r25
7893
 7908 4b1e A093 0000 		sts (SetPointHeight)+2,r26
7894
 7909 4b22 B093 0000 		sts (SetPointHeight)+3,r27
7895
 7910               	.L622:
7896
 7911 4b26 2160      		ori r18,lo8(1)
7897
 7912 4b28 2093 0000 		sts HeightTrimmingFlag.2452,r18
7898
 7913 4b2c CA01      		movw r24,r20
7899
 7914 4b2e 0F97      		sbiw r24,15
7900
 7915 4b30 861B      		sub r24,r22
7901
 7916 4b32 970B      		sbc r25,r23
7902
 7917 4b34 97FD      		sbrc r25,7
7903
 7918 4b36 00C0      		rjmp .L799
7904
 7919               	.L623:
7905
 7920 4b38 2091 0000 		lds r18,HeightTrimming.2442
7906
 7921 4b3c 3091 0000 		lds r19,(HeightTrimming.2442)+1
7907
 7922 4b40 280F      		add r18,r24
7908
 7923 4b42 391F      		adc r19,r25
7909
 7924 4b44 3093 0000 		sts (HeightTrimming.2442)+1,r19
7910
 7925 4b48 2093 0000 		sts HeightTrimming.2442,r18
7911
 7926               	.L624:
7912
 7927 4b4c C901      		movw r24,r18
7913
 7928 4b4e 37FF      		sbrs r19,7
7914
 7929 4b50 00C0      		rjmp .L629
7915
 7930 4b52 8827      		clr r24
7916
 7931 4b54 9927      		clr r25
7917
 7932 4b56 821B      		sub r24,r18
7918
 7933 4b58 930B      		sbc r25,r19
7919
 7934               	.L629:
7920
 7935 4b5a 8150      		subi r24,lo8(513)
7921
 7936 4b5c 9240      		sbci r25,hi8(513)
7922
 7937 4b5e 04F4      		brge .+2
7923
 7938 4b60 00C0      		rjmp .L628
7924
 7939 4b62 8091 0000 		lds r24,ParamSet+13
7925
 7940 4b66 90E0      		ldi r25,lo8(0)
7926
 7941 4b68 5C01      		movw r10,r24
7927
 7942 4b6a A29E      		mul r10,r18
7928
 7943 4b6c C001      		movw r24,r0
7929
 7944 4b6e A39E      		mul r10,r19
7930
 7945 4b70 900D      		add r25,r0
7931
 7946 4b72 B29E      		mul r11,r18
7932
 7947 4b74 900D      		add r25,r0
7933
 7948 4b76 1124      		clr r1
7934
 7949 4b78 60E0      		ldi r22,lo8(1280)
7935
 7950 4b7a 75E0      		ldi r23,hi8(1280)
7936
 7951 4b7c 0E94 0000 		call __divmodhi4
7937
 7952 4b80 8827      		clr r24
7938
 7953 4b82 77FD      		sbrc r23,7
7939
 7954 4b84 8095      		com r24
7940
 7955 4b86 982F      		mov r25,r24
7941
 7956 4b88 2091 0000 		lds r18,SetPointHeight
7942
 7957 4b8c 3091 0000 		lds r19,(SetPointHeight)+1
7943
 7958 4b90 4091 0000 		lds r20,(SetPointHeight)+2
7944
 7959 4b94 5091 0000 		lds r21,(SetPointHeight)+3
7945
 7960 4b98 260F      		add r18,r22
7946
 7961 4b9a 371F      		adc r19,r23
7947
 7962 4b9c 481F      		adc r20,r24
7948
 7963 4b9e 591F      		adc r21,r25
7949
 7964 4ba0 2093 0000 		sts SetPointHeight,r18
7950
 7965 4ba4 3093 0000 		sts (SetPointHeight)+1,r19
7951
 7966 4ba8 4093 0000 		sts (SetPointHeight)+2,r20
7952
 7967 4bac 5093 0000 		sts (SetPointHeight)+3,r21
7953
 7968 4bb0 1092 0000 		sts (HeightTrimming.2442)+1,__zero_reg__
7954
 7969 4bb4 1092 0000 		sts HeightTrimming.2442,__zero_reg__
7955
 7970 4bb8 8091 0000 		lds r24,ParamSet+88
7956
 7971 4bbc 81FF      		sbrs r24,1
7957
 7972 4bbe 00C0      		rjmp .L630
7958
 7973 4bc0 8BE4      		ldi r24,lo8(75)
7959
 7974 4bc2 90E0      		ldi r25,hi8(75)
7960
 7975 4bc4 9093 0000 		sts (BeepTime)+1,r25
7961
 7976 4bc8 8093 0000 		sts BeepTime,r24
7962
 7977               	.L630:
7963
 7978 4bcc 8091 0000 		lds r24,ParamSet+17
7964
 7979 4bd0 8823      		tst r24
7965
 7980 4bd2 01F4      		brne .L628
7966
 7981 4bd4 8091 0000 		lds r24,HoverGas.2445
7967
 7982 4bd8 9091 0000 		lds r25,HoverGas.2445+1
7968
 7983 4bdc 9C01      		movw r18,r24
7969
 7984 4bde 37FD      		sbrc r19,7
7970
 7985 4be0 00C0      		rjmp .L800
7971
 7986               	.L631:
7972
 7987 4be2 3595      		asr r19
7973
 7988 4be4 2795      		ror r18
7974
 7989 4be6 3595      		asr r19
7975
 7990 4be8 2795      		ror r18
7976
 7991 4bea 3093 0000 		sts (StickGasHover.2444)+1,r19
7977
 7992 4bee 2093 0000 		sts StickGasHover.2444,r18
7978
 7993 4bf2 8091 0000 		lds r24,UBat
7979
 7994 4bf6 9091 0000 		lds r25,(UBat)+1
7980
 7995 4bfa 6091 0000 		lds r22,LowVoltageWarning
7981
 7996 4bfe 6C01      		movw r12,r24
7982
 7997 4c00 2C9D      		mul r18,r12
7983
 7998 4c02 C001      		movw r24,r0
7984
 7999 4c04 2D9D      		mul r18,r13
7985
 8000 4c06 900D      		add r25,r0
7986
 8001 4c08 3C9D      		mul r19,r12
7987
 8002 4c0a 900D      		add r25,r0
7988
 8003 4c0c 1124      		clr r1
7989
 8004 4c0e 70E0      		ldi r23,lo8(0)
7990
 8005 4c10 0E94 0000 		call __divmodhi4
7991
 8006 4c14 7093 0000 		sts (StickGasHover.2444)+1,r23
7992
 8007 4c18 6093 0000 		sts StickGasHover.2444,r22
7993
 8008 4c1c 6634      		cpi r22,70
7994
 8009 4c1e 7105      		cpc r23,__zero_reg__
7995
 8010 4c20 04F0      		brlt .+2
7996
 8011 4c22 00C0      		rjmp .L632
7997
 8012 4c24 86E4      		ldi r24,lo8(70)
7998
 8013 4c26 90E0      		ldi r25,hi8(70)
7999
 8014 4c28 9093 0000 		sts (StickGasHover.2444)+1,r25
8000
 8015 4c2c 8093 0000 		sts StickGasHover.2444,r24
8001
 8016               	.L628:
8002
 8017 4c30 8091 0000 		lds r24,BaroExpandActive.2451
8003
 8018 4c34 9091 0000 		lds r25,(BaroExpandActive.2451)+1
8004
 8019 4c38 892B      		or r24,r25
8005
 8020 4c3a 01F0      		breq .L633
8006
 8021 4c3c 8091 0000 		lds r24,ReadingHeight
8007
 8022 4c40 9091 0000 		lds r25,(ReadingHeight)+1
8008
 8023 4c44 A091 0000 		lds r26,(ReadingHeight)+2
8009
 8024 4c48 B091 0000 		lds r27,(ReadingHeight)+3
8010
 8025 4c4c 8093 0000 		sts SetPointHeight,r24
8011
 8026 4c50 9093 0000 		sts (SetPointHeight)+1,r25
8012
 8027 4c54 A093 0000 		sts (SetPointHeight)+2,r26
8013
 8028 4c58 B093 0000 		sts (SetPointHeight)+3,r27
8014
 8029               	.L633:
8015
 8030 4c5c 0091 0000 		lds r16,HoverGas.2445
8016
 8031 4c60 1091 0000 		lds r17,(HoverGas.2445)+1
8017
 8032               	.L619:
8018
 8033 4c64 8091 0000 		lds r24,ReadingHeight
8019
 8034 4c68 9091 0000 		lds r25,(ReadingHeight)+1
8020
 8035 4c6c A091 0000 		lds r26,(ReadingHeight)+2
8021
 8036 4c70 B091 0000 		lds r27,(ReadingHeight)+3
8022
 8037 4c74 2091 0000 		lds r18,SetPointHeight
8023
 8038 4c78 3091 0000 		lds r19,(SetPointHeight)+1
8024
 8039 4c7c 4091 0000 		lds r20,(SetPointHeight)+2
8025
 8040 4c80 5091 0000 		lds r21,(SetPointHeight)+3
8026
 8041 4c84 2817      		cp r18,r24
8027
 8042 4c86 3907      		cpc r19,r25
8028
 8043 4c88 4A07      		cpc r20,r26
8029
 8044 4c8a 5B07      		cpc r21,r27
8030
 8045 4c8c 04F0      		brlt .L635
8031
 8046 4c8e 8091 0000 		lds r24,ParamSet+88
8032
 8047 4c92 80FD      		sbrc r24,0
8033
 8048 4c94 00C0      		rjmp .L636
8034
 8049               	.L635:
8035
 8050 4c96 8091 0000 		lds r24,BaroExpandActive.2451
8036
 8051 4c9a 9091 0000 		lds r25,(BaroExpandActive.2451)+1
8037
 8052 4c9e 892B      		or r24,r25
8038
 8053 4ca0 01F4      		brne .+2
8039
 8054 4ca2 00C0      		rjmp .L637
8040
 8055 4ca4 0091 0000 		lds r16,HoverGas.2445
8041
 8056 4ca8 1091 0000 		lds r17,(HoverGas.2445)+1
8042
 8057               	.L638:
8043
 8058 4cac 6091 0000 		lds r22,NCGpsZ
8044
 8059 4cb0 7727      		clr r23
8045
 8060 4cb2 67FD      		sbrc r22,7
8046
 8061 4cb4 7095      		com r23
8047
 8062 4cb6 872F      		mov r24,r23
8048
 8063 4cb8 972F      		mov r25,r23
8049
 8064 4cba 2091 0000 		lds r18,ParamSet+16
8050
 8065 4cbe 30E0      		ldi r19,lo8(0)
8051
 8066 4cc0 40E0      		ldi r20,lo8(0)
8052
 8067 4cc2 50E0      		ldi r21,hi8(0)
8053
 8068 4cc4 0E94 0000 		call __mulsi3
8054
 8069 4cc8 20E8      		ldi r18,lo8(128)
8055
 8070 4cca 30E0      		ldi r19,hi8(128)
8056
 8071 4ccc 40E0      		ldi r20,hlo8(128)
8057
 8072 4cce 50E0      		ldi r21,hhi8(128)
8058
 8073 4cd0 0E94 0000 		call __divmodsi4
8059
 8074 4cd4 A901      		movw r20,r18
8060
 8075 4cd6 C901      		movw r24,r18
8061
 8076 4cd8 2FEF      		ldi r18,hi8(-127)
8062
 8077 4cda 4138      		cpi r20,lo8(-127)
8063
 8078 4cdc 5207      		cpc r21,r18
8064
 8079 4cde 04F4      		brge .L652
8065
 8080 4ce0 81E8      		ldi r24,lo8(-127)
8066
 8081 4ce2 9FEF      		ldi r25,hi8(-127)
8067
 8082               	.L652:
8068
 8083 4ce4 081B      		sub r16,r24
8069
 8084 4ce6 190B      		sbc r17,r25
8070
 8085 4ce8 B801      		movw r22,r16
8071
 8086 4cea 8827      		clr r24
8072
 8087 4cec 77FD      		sbrc r23,7
8073
 8088 4cee 8095      		com r24
8074
 8089 4cf0 982F      		mov r25,r24
8075
 8090 4cf2 EDE0      		ldi r30,13
8076
 8091 4cf4 660F      	1:	lsl r22
8077
 8092 4cf6 771F      		rol r23
8078
 8093 4cf8 881F      		rol r24
8079
 8094 4cfa 991F      		rol r25
8080
 8095 4cfc EA95      		dec r30
8081
 8096 4cfe 01F4      		brne 1b
8082
 8097 4d00 A981      		ldd r26,Y+1
8083
 8098 4d02 BA81      		ldd r27,Y+2
8084
 8099 4d04 9D01      		movw r18,r26
8085
 8100 4d06 4427      		clr r20
8086
 8101 4d08 37FD      		sbrc r19,7
8087
 8102 4d0a 4095      		com r20
8088
 8103 4d0c 542F      		mov r21,r20
8089
 8104 4d0e 0E94 0000 		call __divmodsi4
8090
 8105 4d12 8091 0000 		lds r24,FilterHCGas.2441
8091
 8106 4d16 9091 0000 		lds r25,(FilterHCGas.2441)+1
8092
 8107 4d1a AC01      		movw r20,r24
8093
 8108 4d1c 440F      		lsl r20
8094
 8109 4d1e 551F      		rol r21
8095
 8110 4d20 480F      		add r20,r24
8096
 8111 4d22 591F      		adc r21,r25
8097
 8112 4d24 420F      		add r20,r18
8098
 8113 4d26 531F      		adc r21,r19
8099
 8114 4d28 57FD      		sbrc r21,7
8100
 8115 4d2a 00C0      		rjmp .L801
8101
 8116               	.L653:
8102
 8117 4d2c 5595      		asr r21
8103
 8118 4d2e 4795      		ror r20
8104
 8119 4d30 5595      		asr r21
8105
 8120 4d32 4795      		ror r20
8106
 8121 4d34 5093 0000 		sts (FilterHCGas.2441)+1,r21
8107
 8122 4d38 4093 0000 		sts FilterHCGas.2441,r20
8108
 8123 4d3c 8091 0000 		lds r24,ParamSet+9
8109
 8124 4d40 282F      		mov r18,r24
8110
 8125 4d42 30E0      		ldi r19,lo8(0)
8111
 8126 4d44 220F      		lsl r18
8112
 8127 4d46 331F      		rol r19
8113
 8128 4d48 220F      		lsl r18
8114
 8129 4d4a 331F      		rol r19
8115
 8130 4d4c 4217      		cp r20,r18
8116
 8131 4d4e 5307      		cpc r21,r19
8117
 8132 4d50 04F0      		brlt .L731
8118
 8133 4d52 8091 0000 		lds r24,ParamSet+22
8119
 8134 4d56 282F      		mov r18,r24
8120
 8135 4d58 30E0      		ldi r19,lo8(0)
8121
 8136 4d5a 2451      		subi r18,lo8(-(-20))
8122
 8137 4d5c 3040      		sbci r19,hi8(-(-20))
8123
 8138 4d5e 220F      		lsl r18
8124
 8139 4d60 331F      		rol r19
8125
 8140 4d62 220F      		lsl r18
8126
 8141 4d64 331F      		rol r19
8127
 8142 4d66 2417      		cp r18,r20
8128
 8143 4d68 3507      		cpc r19,r21
8129
 8144 4d6a 04F4      		brge .L655
8130
 8145               	.L731:
8131
 8146 4d6c 3093 0000 		sts (FilterHCGas.2441)+1,r19
8132
 8147 4d70 2093 0000 		sts FilterHCGas.2441,r18
8133
 8148               	.L655:
8134
 8149 4d74 8091 0000 		lds r24,ParamSet+88
8135
 8150 4d78 80FD      		sbrc r24,0
8136
 8151 4d7a 00C0      		rjmp .L725
8137
 8152 4d7c 8091 0000 		lds r24,FilterHCGas.2441
8138
 8153 4d80 9091 0000 		lds r25,(FilterHCGas.2441)+1
8139
 8154               	.L656:
8140
 8155 4d84 99AF      		std Y+57,r25
8141
 8156 4d86 88AF      		std Y+56,r24
8142
 8157 4d88 00C0      		rjmp .L636
8143
 8158               	.L616:
8144
 8159 4d8a 8091 0000 		lds r24,ParamSet+17
8145
 8160 4d8e 8823      		tst r24
8146
 8161 4d90 01F0      		breq .+2
8147
 8162 4d92 00C0      		rjmp .L802
8148
 8163 4d94 8091 0000 		lds r24,HoverGas.2445
8149
 8164 4d98 9091 0000 		lds r25,HoverGas.2445+1
8150
 8165 4d9c 9C01      		movw r18,r24
8151
 8166 4d9e 37FD      		sbrc r19,7
8152
 8167 4da0 00C0      		rjmp .L803
8153
 8168               	.L659:
8154
 8169 4da2 3595      		asr r19
8155
 8170 4da4 2795      		ror r18
8156
 8171 4da6 3595      		asr r19
8157
 8172 4da8 2795      		ror r18
8158
 8173 4daa 3093 0000 		sts (StickGasHover.2444)+1,r19
8159
 8174 4dae 2093 0000 		sts StickGasHover.2444,r18
8160
 8175 4db2 8091 0000 		lds r24,UBat
8161
 8176 4db6 9091 0000 		lds r25,(UBat)+1
8162
 8177 4dba 6091 0000 		lds r22,LowVoltageWarning
8163
 8178 4dbe 5C01      		movw r10,r24
8164
 8179 4dc0 2A9D      		mul r18,r10
8165
 8180 4dc2 C001      		movw r24,r0
8166
 8181 4dc4 2B9D      		mul r18,r11
8167
 8182 4dc6 900D      		add r25,r0
8168
 8183 4dc8 3A9D      		mul r19,r10
8169
 8184 4dca 900D      		add r25,r0
8170
 8185 4dcc 1124      		clr r1
8171
 8186 4dce 70E0      		ldi r23,lo8(0)
8172
 8187 4dd0 0E94 0000 		call __divmodhi4
8173
 8188 4dd4 7093 0000 		sts (StickGasHover.2444)+1,r23
8174
 8189 4dd8 6093 0000 		sts StickGasHover.2444,r22
8175
 8190               	.L658:
8176
 8191 4ddc 8091 0000 		lds r24,StickGasHover.2444
8177
 8192 4de0 9091 0000 		lds r25,(StickGasHover.2444)+1
8178
 8193 4de4 8634      		cpi r24,70
8179
 8194 4de6 9105      		cpc r25,__zero_reg__
8180
 8195 4de8 04F0      		brlt .+2
8181
 8196 4dea 00C0      		rjmp .L660
8182
 8197 4dec 86E4      		ldi r24,lo8(70)
8183
 8198 4dee 90E0      		ldi r25,hi8(70)
8184
 8199 4df0 9093 0000 		sts (StickGasHover.2444)+1,r25
8185
 8200 4df4 8093 0000 		sts StickGasHover.2444,r24
8186
 8201               	.L661:
8187
 8202 4df8 C8AC      		ldd r12,Y+56
8188
 8203 4dfa D9AC      		ldd r13,Y+57
8189
 8204 4dfc D092 0000 		sts (FilterHCGas.2441)+1,r13
8190
 8205 4e00 C092 0000 		sts FilterHCGas.2441,r12
8191
 8206               	.L636:
8192
 8207 4e04 8091 0000 		lds r24,MKFlags
8193
 8208 4e08 81FD      		sbrc r24,1
8194
 8209 4e0a 00C0      		rjmp .+4
8195
 8210 4e0c 0C94 0000 		jmp .L604
8196
 8211 4e10 8091 0000 		lds r24,MKFlags
8197
 8212 4e14 84FF      		sbrs r24,4
8198
 8213 4e16 00C0      		rjmp .+4
8199
 8214 4e18 0C94 0000 		jmp .L604
8200
 8215 4e1c 8091 0000 		lds r24,HoverGasFilter.2448
8201
 8216 4e20 9091 0000 		lds r25,(HoverGasFilter.2448)+1
8202
 8217 4e24 A091 0000 		lds r26,(HoverGasFilter.2448)+2
8203
 8218 4e28 B091 0000 		lds r27,(HoverGasFilter.2448)+3
8204
 8219 4e2c 0097      		sbiw r24,0
8205
 8220 4e2e A105      		cpc r26,__zero_reg__
8206
 8221 4e30 B105      		cpc r27,__zero_reg__
8207
 8222 4e32 01F4      		brne .L662
8208
 8223 4e34 28AD      		ldd r18,Y+56
8209
 8224 4e36 39AD      		ldd r19,Y+57
8210
 8225 4e38 C901      		movw r24,r18
8211
 8226 4e3a AA27      		clr r26
8212
 8227 4e3c 97FD      		sbrc r25,7
8213
 8228 4e3e A095      		com r26
8214
 8229 4e40 BA2F      		mov r27,r26
8215
 8230 4e42 1CE0      		ldi r17,12
8216
 8231 4e44 880F      	1:	lsl r24
8217
 8232 4e46 991F      		rol r25
8218
 8233 4e48 AA1F      		rol r26
8219
 8234 4e4a BB1F      		rol r27
8220
 8235 4e4c 1A95      		dec r17
8221
 8236 4e4e 01F4      		brne 1b
8222
 8237 4e50 8093 0000 		sts HoverGasFilter.2448,r24
8223
 8238 4e54 9093 0000 		sts (HoverGasFilter.2448)+1,r25
8224
 8239 4e58 A093 0000 		sts (HoverGasFilter.2448)+2,r26
8225
 8240 4e5c B093 0000 		sts (HoverGasFilter.2448)+3,r27
8226
 8241               	.L662:
8227
 8242 4e60 8091 0000 		lds r24,ReadingVario
8228
 8243 4e64 9091 0000 		lds r25,(ReadingVario)+1
8229
 8244 4e68 97FD      		sbrc r25,7
8230
 8245 4e6a 00C0      		rjmp .L804
8231
 8246               	.L663:
8232
 8247 4e6c 8436      		cpi r24,100
8233
 8248 4e6e 9105      		cpc r25,__zero_reg__
8234
 8249 4e70 04F0      		brlt .+4
8235
 8250 4e72 0C94 0000 		jmp .L604
8236
 8251 4e76 48AD      		ldd r20,Y+56
8237
 8252 4e78 59AD      		ldd r21,Y+57
8238
 8253 4e7a BA01      		movw r22,r20
8239
 8254 4e7c 8827      		clr r24
8240
 8255 4e7e 77FD      		sbrc r23,7
8241
 8256 4e80 8095      		com r24
8242
 8257 4e82 982F      		mov r25,r24
8243
 8258 4e84 A981      		ldd r26,Y+1
8244
 8259 4e86 BA81      		ldd r27,Y+2
8245
 8260 4e88 9D01      		movw r18,r26
8246
 8261 4e8a 4427      		clr r20
8247
 8262 4e8c 37FD      		sbrc r19,7
8248
 8263 4e8e 4095      		com r20
8249
 8264 4e90 542F      		mov r21,r20
8250
 8265 4e92 0E94 0000 		call __mulsi3
8251
 8266 4e96 20E0      		ldi r18,lo8(8192)
8252
 8267 4e98 30E2      		ldi r19,hi8(8192)
8253
 8268 4e9a 40E0      		ldi r20,hlo8(8192)
8254
 8269 4e9c 50E0      		ldi r21,hhi8(8192)
8255
 8270 4e9e 0E94 0000 		call __divmodsi4
8256
 8271 4ea2 7901      		movw r14,r18
8257
 8272 4ea4 8A01      		movw r16,r20
8258
 8273 4ea6 8091 0000 		lds r24,ModelIsFlying
8259
 8274 4eaa 9091 0000 		lds r25,(ModelIsFlying)+1
8260
 8275 4eae B7E0      		ldi r27,hi8(2000)
8261
 8276 4eb0 803D      		cpi r24,lo8(2000)
8262
 8277 4eb2 9B07      		cpc r25,r27
8263
 8278 4eb4 00F0      		brlo .+2
8264
 8279 4eb6 00C0      		rjmp .L664
8265
 8280 4eb8 2091 0000 		lds r18,HoverGasFilter.2448
8266
 8281 4ebc 3091 0000 		lds r19,(HoverGasFilter.2448)+1
8267
 8282 4ec0 4091 0000 		lds r20,(HoverGasFilter.2448)+2
8268
 8283 4ec4 5091 0000 		lds r21,(HoverGasFilter.2448)+3
8269
 8284 4ec8 DA01      		movw r26,r20
8270
 8285 4eca C901      		movw r24,r18
8271
 8286 4ecc F9E0      		ldi r31,9
8272
 8287 4ece B695      	1:	lsr r27
8273
 8288 4ed0 A795      		ror r26
8274
 8289 4ed2 9795      		ror r25
8275
 8290 4ed4 8795      		ror r24
8276
 8291 4ed6 FA95      		dec r31
8277
 8292 4ed8 01F4      		brne 1b
8278
 8293 4eda 5901      		movw r10,r18
8279
 8294 4edc 6A01      		movw r12,r20
8280
 8295 4ede A81A      		sub r10,r24
8281
 8296 4ee0 B90A      		sbc r11,r25
8282
 8297 4ee2 CA0A      		sbc r12,r26
8283
 8298 4ee4 DB0A      		sbc r13,r27
8284
 8299 4ee6 A801      		movw r20,r16
8285
 8300 4ee8 9701      		movw r18,r14
8286
 8301 4eea 220F      		lsl r18
8287
 8302 4eec 331F      		rol r19
8288
 8303 4eee 441F      		rol r20
8289
 8304 4ef0 551F      		rol r21
8290
 8305 4ef2 220F      		lsl r18
8291
 8306 4ef4 331F      		rol r19
8292
 8307 4ef6 441F      		rol r20
8293
 8308 4ef8 551F      		rol r21
8294
 8309 4efa 220F      		lsl r18
8295
 8310 4efc 331F      		rol r19
8296
 8311 4efe 441F      		rol r20
8297
 8312 4f00 551F      		rol r21
8298
 8313 4f02 2A0D      		add r18,r10
8299
 8314 4f04 3B1D      		adc r19,r11
8300
 8315 4f06 4C1D      		adc r20,r12
8301
 8316 4f08 5D1D      		adc r21,r13
8302
 8317 4f0a 2093 0000 		sts HoverGasFilter.2448,r18
8303
 8318 4f0e 3093 0000 		sts (HoverGasFilter.2448)+1,r19
8304
 8319 4f12 4093 0000 		sts (HoverGasFilter.2448)+2,r20
8305
 8320 4f16 5093 0000 		sts (HoverGasFilter.2448)+3,r21
8306
 8321               	.L665:
8307
 8322 4f1a DA01      		movw r26,r20
8308
 8323 4f1c C901      		movw r24,r18
8309
 8324 4f1e 2CE0      		ldi r18,12
8310
 8325 4f20 B695      	1:	lsr r27
8311
 8326 4f22 A795      		ror r26
8312
 8327 4f24 9795      		ror r25
8313
 8328 4f26 8795      		ror r24
8314
 8329 4f28 2A95      		dec r18
8315
 8330 4f2a 01F4      		brne 1b
8316
 8331 4f2c FC01      		movw r30,r24
8317
 8332 4f2e 9093 0000 		sts (HoverGas.2445)+1,r25
8318
 8333 4f32 8093 0000 		sts HoverGas.2445,r24
8319
 8334 4f36 6091 0000 		lds r22,ParamSet+15
8320
 8335 4f3a 6623      		tst r22
8321
 8336 4f3c 01F4      		brne .+2
8322
 8337 4f3e 00C0      		rjmp .L668
8323
 8338 4f40 70E0      		ldi r23,lo8(0)
8324
 8339 4f42 0E94 0000 		call __divmodhi4
8325
 8340 4f46 CB01      		movw r24,r22
8326
 8341 4f48 9F01      		movw r18,r30
8327
 8342 4f4a 261B      		sub r18,r22
8328
 8343 4f4c 370B      		sbc r19,r23
8329
 8344 4f4e 3093 0000 		sts (HoverGasMin.2446)+1,r19
8330
 8345 4f52 2093 0000 		sts HoverGasMin.2446,r18
8331
 8346 4f56 8E0F      		add r24,r30
8332
 8347 4f58 9F1F      		adc r25,r31
8333
 8348 4f5a 9093 0000 		sts (HoverGasMax.2447)+1,r25
8334
 8349 4f5e 8093 0000 		sts HoverGasMax.2447,r24
8335
 8350 4f62 0C94 0000 		jmp .L604
8336
 8351               	.L605:
8337
 8352 4f66 8091 0000 		lds r24,AdAirPressure
8338
 8353 4f6a 9091 0000 		lds r25,(AdAirPressure)+1
8339
 8354 4f6e 8C59      		subi r24,lo8(924)
8340
 8355 4f70 9340      		sbci r25,hi8(924)
8341
 8356 4f72 04F0      		brlt .+2
8342
 8357 4f74 00C0      		rjmp .L805
8343
 8358 4f76 8091 0000 		lds r24,AdAirPressure
8344
 8359 4f7a 9091 0000 		lds r25,(AdAirPressure)+1
8345
 8360 4f7e 8436      		cpi r24,100
8346
 8361 4f80 9105      		cpc r25,__zero_reg__
8347
 8362 4f82 04F0      		brlt .+2
8348
 8363 4f84 00C0      		rjmp .L609
8349
 8364 4f86 87B5      		in r24,71-32
8350
 8365 4f88 8B30      		cpi r24,lo8(11)
8351
 8366 4f8a 00F4      		brsh .+2
8352
 8367 4f8c 00C0      		rjmp .L610
8353
 8368 4f8e 8091 0000 		lds r24,ExpandBaro
8354
 8369 4f92 8F5F      		subi r24,lo8(-(1))
8355
 8370               	.L729:
8356
 8371 4f94 8093 0000 		sts ExpandBaro,r24
8357
 8372 4f98 26EF      		ldi r18,lo8(-10)
8358
 8373 4f9a 8202      		muls r24,r18
8359
 8374 4f9c C001      		movw r24,r0
8360
 8375 4f9e 1124      		clr r1
8361
 8376 4fa0 2091 0000 		lds r18,PressureSensorOffset
8362
 8377 4fa4 280F      		add r18,r24
8363
 8378 4fa6 27BD      		out 71-32,r18
8364
 8379 4fa8 8CE2      		ldi r24,lo8(300)
8365
 8380 4faa 91E0      		ldi r25,hi8(300)
8366
 8381 4fac 9093 0000 		sts (BeepTime)+1,r25
8367
 8382 4fb0 8093 0000 		sts BeepTime,r24
8368
 8383 4fb4 8EE5      		ldi r24,lo8(350)
8369
 8384 4fb6 91E0      		ldi r25,hi8(350)
8370
 8385 4fb8 9093 0000 		sts (BaroExpandActive.2451)+1,r25
8371
 8386 4fbc 8093 0000 		sts BaroExpandActive.2451,r24
8372
 8387 4fc0 0C94 0000 		jmp .L606
8373
 8388               	.L470:
8374
 8389 4fc4 1092 0000 		sts delay_stopmotors.2418,__zero_reg__
8375
 8390 4fc8 0C94 0000 		jmp .L410
8376
 8391               	.L512:
8377
 8392 4fcc E090 0000 		lds r14,ParamSet+23
8378
 8393 4fd0 FF24      		clr r15
8379
 8394 4fd2 00E0      		ldi r16,lo8(0)
8380
 8395 4fd4 10E0      		ldi r17,hi8(0)
8381
 8396 4fd6 68A5      		ldd r22,Y+40
8382
 8397 4fd8 79A5      		ldd r23,Y+41
8383
 8398 4fda 8AA5      		ldd r24,Y+42
8384
 8399 4fdc 9BA5      		ldd r25,Y+43
8385
 8400 4fde A801      		movw r20,r16
8386
 8401 4fe0 9701      		movw r18,r14
8387
 8402 4fe2 0E94 0000 		call __divmodsi4
8388
 8403 4fe6 8091 0000 		lds r24,AccNick
8389
 8404 4fea 9091 0000 		lds r25,(AccNick)+1
8390
 8405 4fee AA27      		clr r26
8391
 8406 4ff0 97FD      		sbrc r25,7
8392
 8407 4ff2 A095      		com r26
8393
 8408 4ff4 BA2F      		mov r27,r26
8394
 8409 4ff6 281B      		sub r18,r24
8395
 8410 4ff8 390B      		sbc r19,r25
8396
 8411 4ffa 4A0B      		sbc r20,r26
8397
 8412 4ffc 5B0B      		sbc r21,r27
8398
 8413 4ffe CA01      		movw r24,r20
8399
 8414 5000 B901      		movw r22,r18
8400
 8415 5002 20E1      		ldi r18,lo8(16)
8401
 8416 5004 30E0      		ldi r19,hi8(16)
8402
 8417 5006 40E0      		ldi r20,hlo8(16)
8403
 8418 5008 50E0      		ldi r21,hhi8(16)
8404
 8419 500a 0E94 0000 		call __divmodsi4
8405
 8420 500e 522E      		mov r5,r18
8406
 8421 5010 432E      		mov r4,r19
8407
 8422 5012 342E      		mov r3,r20
8408
 8423 5014 252E      		mov r2,r21
8409
 8424 5016 4201      		movw r8,r4
8410
 8425 5018 3101      		movw r6,r2
8411
 8426 501a 6CA1      		ldd r22,Y+36
8412
 8427 501c 7DA1      		ldd r23,Y+37
8413
 8428 501e 8EA1      		ldd r24,Y+38
8414
 8429 5020 9FA1      		ldd r25,Y+39
8415
 8430 5022 A801      		movw r20,r16
8416
 8431 5024 9701      		movw r18,r14
8417
 8432 5026 0E94 0000 		call __divmodsi4
8418
 8433 502a 8091 0000 		lds r24,AccRoll
8419
 8434 502e 9091 0000 		lds r25,(AccRoll)+1
8420
 8435 5032 AA27      		clr r26
8421
 8436 5034 97FD      		sbrc r25,7
8422
 8437 5036 A095      		com r26
8423
 8438 5038 BA2F      		mov r27,r26
8424
 8439 503a 281B      		sub r18,r24
8425
 8440 503c 390B      		sbc r19,r25
8426
 8441 503e 4A0B      		sbc r20,r26
8427
 8442 5040 5B0B      		sbc r21,r27
8428
 8443 5042 CA01      		movw r24,r20
8429
 8444 5044 B901      		movw r22,r18
8430
 8445 5046 20E1      		ldi r18,lo8(16)
8431
 8446 5048 30E0      		ldi r19,hi8(16)
8432
 8447 504a 40E0      		ldi r20,hlo8(16)
8433
 8448 504c 50E0      		ldi r21,hhi8(16)
8434
 8449 504e 0E94 0000 		call __divmodsi4
8435
 8450 5052 122F      		mov r17,r18
8436
 8451 5054 032F      		mov r16,r19
8437
 8452 5056 F42E      		mov r15,r20
8438
 8453 5058 E52E      		mov r14,r21
8439
 8454 505a 822F      		mov r24,r18
8440
 8455 505c 932F      		mov r25,r19
8441
 8456 505e A42F      		mov r26,r20
8442
 8457 5060 B52F      		mov r27,r21
8443
 8458 5062 5C01      		movw r10,r24
8444
 8459 5064 6D01      		movw r12,r26
8445
 8460 5066 8091 0000 		lds r24,MaxStickNick
8446
 8461 506a 9091 0000 		lds r25,(MaxStickNick)+1
8447
 8462 506e 8134      		cpi r24,65
8448
 8463 5070 9105      		cpc r25,__zero_reg__
8449
 8464 5072 04F4      		brge .+2
8450
 8465 5074 00C0      		rjmp .L806
8451
 8466               	.L522:
8452
 8467 5076 652D      		mov r22,r5
8453
 8468 5078 742D      		mov r23,r4
8454
 8469 507a 832D      		mov r24,r3
8455
 8470 507c 922D      		mov r25,r2
8456
 8471 507e 23E0      		ldi r18,lo8(3)
8457
 8472 5080 30E0      		ldi r19,hi8(3)
8458
 8473 5082 40E0      		ldi r20,hlo8(3)
8459
 8474 5084 50E0      		ldi r21,hhi8(3)
8460
 8475 5086 0E94 0000 		call __divmodsi4
8461
 8476 508a 922E      		mov r9,r18
8462
 8477 508c 832E      		mov r8,r19
8463
 8478 508e 742E      		mov r7,r20
8464
 8479 5090 652E      		mov r6,r21
8465
 8480 5092 612F      		mov r22,r17
8466
 8481 5094 702F      		mov r23,r16
8467
 8482 5096 8F2D      		mov r24,r15
8468
 8483 5098 9E2D      		mov r25,r14
8469
 8484 509a 23E0      		ldi r18,lo8(3)
8470
 8485 509c 30E0      		ldi r19,hi8(3)
8471
 8486 509e 40E0      		ldi r20,hlo8(3)
8472
 8487 50a0 50E0      		ldi r21,hhi8(3)
8473
 8488 50a2 0E94 0000 		call __divmodsi4
8474
 8489 50a6 C901      		movw r24,r18
8475
 8490 50a8 DA01      		movw r26,r20
8476
 8491 50aa 5C01      		movw r10,r24
8477
 8492 50ac 6D01      		movw r12,r26
8478
 8493               	.L523:
8479
 8494 50ae E091 0000 		lds r30,ParamSet+3
8480
 8495 50b2 F0E0      		ldi r31,lo8(0)
8481
 8496 50b4 EE0F      		lsl r30
8482
 8497 50b6 FF1F      		rol r31
8483
 8498 50b8 E050      		subi r30,lo8(-(PPM_in))
8484
 8499 50ba F040      		sbci r31,hi8(-(PPM_in))
8485
 8500 50bc 0190      		ld __tmp_reg__,Z+
8486
 8501 50be F081      		ld r31,Z
8487
 8502 50c0 E02D      		mov r30,__tmp_reg__
8488
 8503 50c2 F7FD      		sbrc r31,7
8489
 8504 50c4 00C0      		rjmp .L807
8490
 8505               	.L525:
8491
 8506 50c6 7A97      		sbiw r30,26
8492
 8507 50c8 04F0      		brlt .L524
8493
 8508 50ca 692D      		mov r22,r9
8494
 8509 50cc 782D      		mov r23,r8
8495
 8510 50ce 872D      		mov r24,r7
8496
 8511 50d0 962D      		mov r25,r6
8497
 8512 50d2 23E0      		ldi r18,lo8(3)
8498
 8513 50d4 30E0      		ldi r19,hi8(3)
8499
 8514 50d6 40E0      		ldi r20,hlo8(3)
8500
 8515 50d8 50E0      		ldi r21,hhi8(3)
8501
 8516 50da 0E94 0000 		call __divmodsi4
8502
 8517 50de 922E      		mov r9,r18
8503
 8518 50e0 832E      		mov r8,r19
8504
 8519 50e2 742E      		mov r7,r20
8505
 8520 50e4 652E      		mov r6,r21
8506
 8521 50e6 C601      		movw r24,r12
8507
 8522 50e8 B501      		movw r22,r10
8508
 8523 50ea 23E0      		ldi r18,lo8(3)
8509
 8524 50ec 30E0      		ldi r19,hi8(3)
8510
 8525 50ee 40E0      		ldi r20,hlo8(3)
8511
 8526 50f0 50E0      		ldi r21,hhi8(3)
8512
 8527 50f2 0E94 0000 		call __divmodsi4
8513
 8528 50f6 C901      		movw r24,r18
8514
 8529 50f8 DA01      		movw r26,r20
8515
 8530 50fa 5C01      		movw r10,r24
8516
 8531 50fc 6D01      		movw r12,r26
8517
 8532               	.L524:
8518
 8533 50fe 292D      		mov r18,r9
8519
 8534 5100 382D      		mov r19,r8
8520
 8535 5102 472D      		mov r20,r7
8521
 8536 5104 562D      		mov r21,r6
8522
 8537 5106 C901      		movw r24,r18
8523
 8538 5108 DA01      		movw r26,r20
8524
 8539 510a BC01      		movw r22,r24
8525
 8540 510c CD01      		movw r24,r26
8526
 8541 510e 6132      		cpi r22,lo8(33)
8527
 8542 5110 7105      		cpc r23,__zero_reg__
8528
 8543 5112 8105      		cpc r24,__zero_reg__
8529
 8544 5114 9105      		cpc r25,__zero_reg__
8530
 8545 5116 04F0      		brlt .L526
8531
 8546 5118 60E2      		ldi r22,lo8(32)
8532
 8547 511a 70E0      		ldi r23,hi8(32)
8533
 8548 511c 80E0      		ldi r24,hlo8(32)
8534
 8549 511e 90E0      		ldi r25,hhi8(32)
8535
 8550               	.L526:
8536
 8551 5120 7B01      		movw r14,r22
8537
 8552 5122 8C01      		movw r16,r24
8538
 8553 5124 603E      		cpi r22,lo8(-32)
8539
 8554 5126 3FEF      		ldi r19,hi8(-32)
8540
 8555 5128 7307      		cpc r23,r19
8541
 8556 512a 3FEF      		ldi r19,hlo8(-32)
8542
 8557 512c 8307      		cpc r24,r19
8543
 8558 512e 3FEF      		ldi r19,hhi8(-32)
8544
 8559 5130 9307      		cpc r25,r19
8545
 8560 5132 04F4      		brge .L527
8546
 8561 5134 90EE      		ldi r25,lo8(-32)
8547
 8562 5136 E92E      		mov r14,r25
8548
 8563 5138 9FEF      		ldi r25,hi8(-32)
8549
 8564 513a F92E      		mov r15,r25
8550
 8565 513c 9FEF      		ldi r25,hlo8(-32)
8551
 8566 513e 092F      		mov r16,r25
8552
 8567 5140 9FEF      		ldi r25,hhi8(-32)
8553
 8568 5142 192F      		mov r17,r25
8554
 8569               	.L527:
8555
 8570 5144 40EE      		ldi r20,lo8(-32)
8556
 8571 5146 A416      		cp r10,r20
8557
 8572 5148 4FEF      		ldi r20,hi8(-32)
8558
 8573 514a B406      		cpc r11,r20
8559
 8574 514c 4FEF      		ldi r20,hlo8(-32)
8560
 8575 514e C406      		cpc r12,r20
8561
 8576 5150 4FEF      		ldi r20,hhi8(-32)
8562
 8577 5152 D406      		cpc r13,r20
8563
 8578 5154 04F0      		brlt .+2
8564
 8579 5156 00C0      		rjmp .L528
8565
 8580 5158 80EE      		ldi r24,lo8(-32)
8566
 8581 515a A82E      		mov r10,r24
8567
 8582 515c 8FEF      		ldi r24,hi8(-32)
8568
 8583 515e B82E      		mov r11,r24
8569
 8584 5160 8FEF      		ldi r24,hlo8(-32)
8570
 8585 5162 C82E      		mov r12,r24
8571
 8586 5164 8FEF      		ldi r24,hhi8(-32)
8572
 8587 5166 D82E      		mov r13,r24
8573
 8588 5168 00C0      		rjmp .L521
8574
 8589               	.L802:
8575
 8590 516a 8093 0000 		sts StickGasHover.2444,r24
8576
 8591 516e 1092 0000 		sts StickGasHover.2444+1,__zero_reg__
8577
 8592 5172 00C0      		rjmp .L658
8578
 8593               	.L798:
8579
 8594 5174 8091 0000 		lds r24,FCParam+1
8580
 8595 5178 2091 0000 		lds r18,ExternHeightValue
8581
 8596 517c 3091 0000 		lds r19,(ExternHeightValue)+1
8582
 8597 5180 280F      		add r18,r24
8583
 8598 5182 311D      		adc r19,__zero_reg__
8584
 8599 5184 8091 0000 		lds r24,ParamSet+13
8585
 8600 5188 90E0      		ldi r25,lo8(0)
8586
 8601 518a FC01      		movw r30,r24
8587
 8602 518c 2E9F      		mul r18,r30
8588
 8603 518e C001      		movw r24,r0
8589
 8604 5190 2F9F      		mul r18,r31
8590
 8605 5192 900D      		add r25,r0
8591
 8606 5194 3E9F      		mul r19,r30
8592
 8607 5196 900D      		add r25,r0
8593
 8608 5198 1124      		clr r1
8594
 8609 519a AA27      		clr r26
8595
 8610 519c 97FD      		sbrc r25,7
8596
 8611 519e A095      		com r26
8597
 8612 51a0 BA2F      		mov r27,r26
8598
 8613 51a2 8093 0000 		sts SetPointHeight,r24
8599
 8614 51a6 9093 0000 		sts (SetPointHeight)+1,r25
8600
 8615 51aa A093 0000 		sts (SetPointHeight)+2,r26
8601
 8616 51ae B093 0000 		sts (SetPointHeight)+3,r27
8602
 8617 51b2 00C0      		rjmp .L614
8603
 8618               	.L612:
8604
 8619 51b4 81E0      		ldi r24,lo8(1)
8605
 8620 51b6 8093 0000 		sts HCActive.2443,r24
8606
 8621 51ba 88EC      		ldi r24,lo8(-56)
8607
 8622 51bc 8093 0000 		sts delay.2449,r24
8608
 8623 51c0 00C0      		rjmp .L613
8609
 8624               	.L447:
8610
 8625 51c2 8081      		ld r24,Z
8611
 8626 51c4 9181      		ldd r25,Z+1
8612
 8627 51c6 855B      		subi r24,lo8(-75)
8613
 8628 51c8 9F4F      		sbci r25,hi8(-75)
8614
 8629 51ca 04F0      		brlt .+2
8615
 8630 51cc 00C0      		rjmp .L464
8616
 8631 51ce 8091 0000 		lds r24,delay_neutral.2416
8617
 8632 51d2 8F5F      		subi r24,lo8(-(1))
8618
 8633 51d4 8093 0000 		sts delay_neutral.2416,r24
8619
 8634 51d8 893C      		cpi r24,lo8(-55)
8620
 8635 51da 00F4      		brsh .+2
8621
 8636 51dc 00C0      		rjmp .L446
8622
 8637 51de 1092 0000 		sts delay_neutral.2416,__zero_reg__
8623
 8638 51e2 3BE0      		ldi r19,lo8(11)
8624
 8639 51e4 3D15      		cp r19,r13
8625
 8640 51e6 00F4      		brsh .+2
8626
 8641 51e8 00C0      		rjmp .L466
8627
 8642 51ea 2998      		cbi 37-32,1
8628
 8643               	.L467:
8629
 8644 51ec 1092 0000 		sts (ModelIsFlying)+1,__zero_reg__
8630
 8645 51f0 1092 0000 		sts ModelIsFlying,__zero_reg__
8631
 8646 51f4 0E94 0000 		call Servo_Off
8632
 8647 51f8 81E0      		ldi r24,lo8(1)
8633
 8648               	.L726:
8634
 8649 51fa 0E94 0000 		call SetNeutral
8635
 8650 51fe 0E94 0000 		call Servo_On
8636
 8651 5202 0E94 0000 		call GetActiveParamSet
8637
 8652 5206 68E7      		ldi r22,lo8(120)
8638
 8653 5208 70E0      		ldi r23,hi8(120)
8639
 8654 520a 0E94 0000 		call Beep
8640
 8655 520e 8091 0000 		lds r24,ParamSet+2
8641
 8656 5212 A82F      		mov r26,r24
8642
 8657 5214 B0E0      		ldi r27,lo8(0)
8643
 8658 5216 AA0F      		lsl r26
8644
 8659 5218 BB1F      		rol r27
8645
 8660 521a 00C0      		rjmp .L446
8646
 8661               	.L805:
8647
 8662 521c 87B5      		in r24,71-32
8648
 8663 521e 853F      		cpi r24,lo8(-11)
8649
 8664 5220 00F0      		brlo .+2
8650
 8665 5222 00C0      		rjmp .L608
8651
 8666 5224 8091 0000 		lds r24,ExpandBaro
8652
 8667 5228 8150      		subi r24,lo8(-(-1))
8653
 8668 522a 00C0      		rjmp .L729
8654
 8669               	.L806:
8655
 8670 522c 8091 0000 		lds r24,MaxStickRoll
8656
 8671 5230 9091 0000 		lds r25,(MaxStickRoll)+1
8657
 8672 5234 8134      		cpi r24,65
8658
 8673 5236 9105      		cpc r25,__zero_reg__
8659
 8674 5238 04F4      		brge .+2
8660
 8675 523a 00C0      		rjmp .L523
8661
 8676 523c 00C0      		rjmp .L522
8662
 8677               	.L660:
8663
 8678 523e 8739      		cpi r24,151
8664
 8679 5240 9105      		cpc r25,__zero_reg__
8665
 8680 5242 04F4      		brge .+2
8666
 8681 5244 00C0      		rjmp .L661
8667
 8682 5246 86E9      		ldi r24,lo8(150)
8668
 8683 5248 90E0      		ldi r25,hi8(150)
8669
 8684 524a 9093 0000 		sts (StickGasHover.2444)+1,r25
8670
 8685 524e 8093 0000 		sts StickGasHover.2444,r24
8671
 8686 5252 00C0      		rjmp .L661
8672
 8687               	.L768:
8673
 8688 5254 81E0      		ldi r24,lo8(1)
8674
 8689 5256 90E0      		ldi r25,hi8(1)
8675
 8690 5258 9093 0000 		sts (UpdateCompassCourse.2420)+1,r25
8676
 8691 525c 8093 0000 		sts UpdateCompassCourse.2420,r24
8677
 8692 5260 1092 0000 		sts ReadingIntegralGyroYaw,__zero_reg__
8678
 8693 5264 1092 0000 		sts (ReadingIntegralGyroYaw)+1,__zero_reg__
8679
 8694 5268 1092 0000 		sts (ReadingIntegralGyroYaw)+2,__zero_reg__
8680
 8695 526c 1092 0000 		sts (ReadingIntegralGyroYaw)+3,__zero_reg__
8681
 8696 5270 1092 0000 		sts SetPointYaw.2410,__zero_reg__
8682
 8697 5274 1092 0000 		sts (SetPointYaw.2410)+1,__zero_reg__
8683
 8698 5278 1092 0000 		sts (SetPointYaw.2410)+2,__zero_reg__
8684
 8699 527c 1092 0000 		sts (SetPointYaw.2410)+3,__zero_reg__
8685
 8700 5280 0C94 0000 		jmp .L413
8686
 8701               	.L766:
8687
 8702 5284 27E0      		ldi r18,lo8(7)
8688
 8703 5286 30E0      		ldi r19,hi8(7)
8689
 8704 5288 420E      		add r4,r18
8690
 8705 528a 531E      		adc r5,r19
8691
 8706 528c 0C94 0000 		jmp .L589
8692
 8707               	.L528:
8693
 8708 5290 51E2      		ldi r21,lo8(33)
8694
 8709 5292 A516      		cp r10,r21
8695
 8710 5294 B104      		cpc r11,__zero_reg__
8696
 8711 5296 C104      		cpc r12,__zero_reg__
8697
 8712 5298 D104      		cpc r13,__zero_reg__
8698
 8713 529a 04F4      		brge .+2
8699
 8714 529c 00C0      		rjmp .L521
8700
 8715 529e B0E2      		ldi r27,lo8(32)
8701
 8716 52a0 AB2E      		mov r10,r27
8702
 8717 52a2 B12C      		mov r11,__zero_reg__
8703
 8718 52a4 C12C      		mov r12,__zero_reg__
8704
 8719 52a6 D12C      		mov r13,__zero_reg__
8705
 8720 52a8 00C0      		rjmp .L521
8706
 8721               	.L795:
8707
 8722 52aa F095      		com r31
8708
 8723 52ac E195      		neg r30
8709
 8724 52ae FF4F      		sbci r31,lo8(-1)
8710
 8725 52b0 0C94 0000 		jmp .L516
8711
 8726               	.L554:
8712
 8727 52b4 51E0      		ldi r21,lo8(4097)
8713
 8728 52b6 E516      		cp r14,r21
8714
 8729 52b8 50E1      		ldi r21,hi8(4097)
8715
 8730 52ba F506      		cpc r15,r21
8716
 8731 52bc 50E0      		ldi r21,hlo8(4097)
8717
 8732 52be 0507      		cpc r16,r21
8718
 8733 52c0 50E0      		ldi r21,hhi8(4097)
8719
 8734 52c2 1507      		cpc r17,r21
8720
 8735 52c4 04F4      		brge .+2
8721
 8736 52c6 00C0      		rjmp .L556
8722
 8737 52c8 8091 0000 		lds r24,last_r_p.2429
8723
 8738 52cc 8823      		tst r24
8724
 8739 52ce 01F4      		brne .+2
8725
 8740 52d0 00C0      		rjmp .L557
8726
 8741 52d2 D801      		movw r26,r16
8727
 8742 52d4 C701      		movw r24,r14
8728
 8743 52d6 39E0      		ldi r19,9
8729
 8744 52d8 B595      	1:	asr r27
8730
 8745 52da A795      		ror r26
8731
 8746 52dc 9795      		ror r25
8732
 8747 52de 8795      		ror r24
8733
 8748 52e0 3A95      		dec r19
8734
 8749 52e2 01F4      		brne 1b
8735
 8750 52e4 A090 0000 		lds r10,cnt.2426
8736
 8751 52e8 B090 0000 		lds r11,(cnt.2426)+1
8737
 8752 52ec A80E      		add r10,r24
8738
 8753 52ee B91E      		adc r11,r25
8739
 8754 52f0 B092 0000 		sts (cnt.2426)+1,r11
8740
 8755 52f4 A092 0000 		sts cnt.2426,r10
8741
 8756 52f8 C801      		movw r24,r16
8742
 8757 52fa B701      		movw r22,r14
8743
 8758 52fc 28E0      		ldi r18,lo8(8)
8744
 8759 52fe 30E0      		ldi r19,hi8(8)
8745
 8760 5300 40E0      		ldi r20,hlo8(8)
8746
 8761 5302 50E0      		ldi r21,hhi8(8)
8747
 8762 5304 0E94 0000 		call __divmodsi4
8748
 8763 5308 C901      		movw r24,r18
8749
 8764 530a DA01      		movw r26,r20
8750
 8765 530c 8093 0000 		sts CorrectionRoll.2414,r24
8751
 8766 5310 9093 0000 		sts (CorrectionRoll.2414)+1,r25
8752
 8767 5314 A093 0000 		sts (CorrectionRoll.2414)+2,r26
8753
 8768 5318 B093 0000 		sts (CorrectionRoll.2414)+3,r27
8754
 8769 531c 8958      		subi r24,lo8(5001)
8755
 8770 531e 9341      		sbci r25,hi8(5001)
8756
 8771 5320 A040      		sbci r26,hlo8(5001)
8757
 8772 5322 B040      		sbci r27,hhi8(5001)
8758
 8773 5324 04F0      		brlt .L558
8759
 8774 5326 88E8      		ldi r24,lo8(5000)
8760
 8775 5328 93E1      		ldi r25,hi8(5000)
8761
 8776 532a A0E0      		ldi r26,hlo8(5000)
8762
 8777 532c B0E0      		ldi r27,hhi8(5000)
8763
 8778 532e 8093 0000 		sts CorrectionRoll.2414,r24
8764
 8779 5332 9093 0000 		sts (CorrectionRoll.2414)+1,r25
8765
 8780 5336 A093 0000 		sts (CorrectionRoll.2414)+2,r26
8766
 8781 533a B093 0000 		sts (CorrectionRoll.2414)+3,r27
8767
 8782               	.L558:
8768
 8783 533e 6091 0000 		lds r22,CorrectionRoll.2414
8769
 8784 5342 7091 0000 		lds r23,(CorrectionRoll.2414)+1
8770
 8785 5346 8091 0000 		lds r24,(CorrectionRoll.2414)+2
8771
 8786 534a 9091 0000 		lds r25,(CorrectionRoll.2414)+3
8772
 8787 534e 20E0      		ldi r18,lo8(256)
8773
 8788 5350 31E0      		ldi r19,hi8(256)
8774
 8789 5352 40E0      		ldi r20,hlo8(256)
8775
 8790 5354 50E0      		ldi r21,hhi8(256)
8776
 8791 5356 0E94 0000 		call __divmodsi4
8777
 8792 535a 8091 0000 		lds r24,AttitudeCorrectionRoll
8778
 8793 535e 9091 0000 		lds r25,(AttitudeCorrectionRoll)+1
8779
 8794 5362 820F      		add r24,r18
8780
 8795 5364 931F      		adc r25,r19
8781
 8796 5366 9093 0000 		sts (AttitudeCorrectionRoll)+1,r25
8782
 8797 536a 8093 0000 		sts AttitudeCorrectionRoll,r24
8783
 8798               	.L559:
8784
 8799 536e 70E0      		ldi r23,lo8(-4096)
8785
 8800 5370 E716      		cp r14,r23
8786
 8801 5372 70EF      		ldi r23,hi8(-4096)
8787
 8802 5374 F706      		cpc r15,r23
8788
 8803 5376 7FEF      		ldi r23,hlo8(-4096)
8789
 8804 5378 0707      		cpc r16,r23
8790
 8805 537a 7FEF      		ldi r23,hhi8(-4096)
8791
 8806 537c 1707      		cpc r17,r23
8792
 8807 537e 04F0      		brlt .+2
8793
 8808 5380 00C0      		rjmp .L560
8794
 8809 5382 8091 0000 		lds r24,last_r_n.2430
8795
 8810 5386 8823      		tst r24
8796
 8811 5388 01F4      		brne .+2
8797
 8812 538a 00C0      		rjmp .L561
8798
 8813 538c 8827      		clr r24
8799
 8814 538e 9927      		clr r25
8800
 8815 5390 DC01      		movw r26,r24
8801
 8816 5392 8E19      		sub r24,r14
8802
 8817 5394 9F09      		sbc r25,r15
8803
 8818 5396 A00B      		sbc r26,r16
8804
 8819 5398 B10B      		sbc r27,r17
8805
 8820 539a 29E0      		ldi r18,9
8806
 8821 539c B595      	1:	asr r27
8807
 8822 539e A795      		ror r26
8808
 8823 53a0 9795      		ror r25
8809
 8824 53a2 8795      		ror r24
8810
 8825 53a4 2A95      		dec r18
8811
 8826 53a6 01F4      		brne 1b
8812
 8827 53a8 A80E      		add r10,r24
8813
 8828 53aa B91E      		adc r11,r25
8814
 8829 53ac B092 0000 		sts (cnt.2426)+1,r11
8815
 8830 53b0 A092 0000 		sts cnt.2426,r10
8816
 8831 53b4 C801      		movw r24,r16
8817
 8832 53b6 B701      		movw r22,r14
8818
 8833 53b8 28E0      		ldi r18,lo8(8)
8819
 8834 53ba 30E0      		ldi r19,hi8(8)
8820
 8835 53bc 40E0      		ldi r20,hlo8(8)
8821
 8836 53be 50E0      		ldi r21,hhi8(8)
8822
 8837 53c0 0E94 0000 		call __divmodsi4
8823
 8838 53c4 C901      		movw r24,r18
8824
 8839 53c6 DA01      		movw r26,r20
8825
 8840 53c8 8093 0000 		sts CorrectionRoll.2414,r24
8826
 8841 53cc 9093 0000 		sts (CorrectionRoll.2414)+1,r25
8827
 8842 53d0 A093 0000 		sts (CorrectionRoll.2414)+2,r26
8828
 8843 53d4 B093 0000 		sts (CorrectionRoll.2414)+3,r27
8829
 8844 53d8 8857      		subi r24,lo8(-5000)
8830
 8845 53da 9C4E      		sbci r25,hi8(-5000)
8831
 8846 53dc AF4F      		sbci r26,hlo8(-5000)
8832
 8847 53de BF4F      		sbci r27,hhi8(-5000)
8833
 8848 53e0 04F4      		brge .L562
8834
 8849 53e2 88E7      		ldi r24,lo8(-5000)
8835
 8850 53e4 9CEE      		ldi r25,hi8(-5000)
8836
 8851 53e6 AFEF      		ldi r26,hlo8(-5000)
8837
 8852 53e8 BFEF      		ldi r27,hhi8(-5000)
8838
 8853 53ea 8093 0000 		sts CorrectionRoll.2414,r24
8839
 8854 53ee 9093 0000 		sts (CorrectionRoll.2414)+1,r25
8840
 8855 53f2 A093 0000 		sts (CorrectionRoll.2414)+2,r26
8841
 8856 53f6 B093 0000 		sts (CorrectionRoll.2414)+3,r27
8842
 8857               	.L562:
8843
 8858 53fa 6091 0000 		lds r22,CorrectionRoll.2414
8844
 8859 53fe 7091 0000 		lds r23,(CorrectionRoll.2414)+1
8845
 8860 5402 8091 0000 		lds r24,(CorrectionRoll.2414)+2
8846
 8861 5406 9091 0000 		lds r25,(CorrectionRoll.2414)+3
8847
 8862 540a 20E0      		ldi r18,lo8(256)
8848
 8863 540c 31E0      		ldi r19,hi8(256)
8849
 8864 540e 40E0      		ldi r20,hlo8(256)
8850
 8865 5410 50E0      		ldi r21,hhi8(256)
8851
 8866 5412 0E94 0000 		call __divmodsi4
8852
 8867 5416 8091 0000 		lds r24,AttitudeCorrectionRoll
8853
 8868 541a 9091 0000 		lds r25,(AttitudeCorrectionRoll)+1
8854
 8869 541e 820F      		add r24,r18
8855
 8870 5420 931F      		adc r25,r19
8856
 8871 5422 9093 0000 		sts (AttitudeCorrectionRoll)+1,r25
8857
 8872 5426 8093 0000 		sts AttitudeCorrectionRoll,r24
8858
 8873 542a 0C94 0000 		jmp .L563
8859
 8874               	.L539:
8860
 8875 542e 31E0      		ldi r19,lo8(4097)
8861
 8876 5430 E316      		cp r14,r19
8862
 8877 5432 30E1      		ldi r19,hi8(4097)
8863
 8878 5434 F306      		cpc r15,r19
8864
 8879 5436 30E0      		ldi r19,hlo8(4097)
8865
 8880 5438 0307      		cpc r16,r19
8866
 8881 543a 30E0      		ldi r19,hhi8(4097)
8867
 8882 543c 1307      		cpc r17,r19
8868
 8883 543e 04F4      		brge .+2
8869
 8884 5440 00C0      		rjmp .L541
8870
 8885 5442 8091 0000 		lds r24,last_n_p.2427
8871
 8886 5446 8823      		tst r24
8872
 8887 5448 01F4      		brne .+2
8873
 8888 544a 00C0      		rjmp .L542
8874
 8889 544c D801      		movw r26,r16
8875
 8890 544e C701      		movw r24,r14
8876
 8891 5450 59E0      		ldi r21,9
8877
 8892 5452 B595      	1:	asr r27
8878
 8893 5454 A795      		ror r26
8879
 8894 5456 9795      		ror r25
8880
 8895 5458 8795      		ror r24
8881
 8896 545a 5A95      		dec r21
8882
 8897 545c 01F4      		brne 1b
8883
 8898 545e A090 0000 		lds r10,cnt.2426
8884
 8899 5462 B090 0000 		lds r11,(cnt.2426)+1
8885
 8900 5466 A80E      		add r10,r24
8886
 8901 5468 B91E      		adc r11,r25
8887
 8902 546a B092 0000 		sts (cnt.2426)+1,r11
8888
 8903 546e A092 0000 		sts cnt.2426,r10
8889
 8904 5472 C801      		movw r24,r16
8890
 8905 5474 B701      		movw r22,r14
8891
 8906 5476 28E0      		ldi r18,lo8(8)
8892
 8907 5478 30E0      		ldi r19,hi8(8)
8893
 8908 547a 40E0      		ldi r20,hlo8(8)
8894
 8909 547c 50E0      		ldi r21,hhi8(8)
8895
 8910 547e 0E94 0000 		call __divmodsi4
8896
 8911 5482 C901      		movw r24,r18
8897
 8912 5484 DA01      		movw r26,r20
8898
 8913 5486 8093 0000 		sts CorrectionNick.2413,r24
8899
 8914 548a 9093 0000 		sts (CorrectionNick.2413)+1,r25
8900
 8915 548e A093 0000 		sts (CorrectionNick.2413)+2,r26
8901
 8916 5492 B093 0000 		sts (CorrectionNick.2413)+3,r27
8902
 8917 5496 8958      		subi r24,lo8(5001)
8903
 8918 5498 9341      		sbci r25,hi8(5001)
8904
 8919 549a A040      		sbci r26,hlo8(5001)
8905
 8920 549c B040      		sbci r27,hhi8(5001)
8906
 8921 549e 04F0      		brlt .L543
8907
 8922 54a0 88E8      		ldi r24,lo8(5000)
8908
 8923 54a2 93E1      		ldi r25,hi8(5000)
8909
 8924 54a4 A0E0      		ldi r26,hlo8(5000)
8910
 8925 54a6 B0E0      		ldi r27,hhi8(5000)
8911
 8926 54a8 8093 0000 		sts CorrectionNick.2413,r24
8912
 8927 54ac 9093 0000 		sts (CorrectionNick.2413)+1,r25
8913
 8928 54b0 A093 0000 		sts (CorrectionNick.2413)+2,r26
8914
 8929 54b4 B093 0000 		sts (CorrectionNick.2413)+3,r27
8915
 8930               	.L543:
8916
 8931 54b8 6091 0000 		lds r22,CorrectionNick.2413
8917
 8932 54bc 7091 0000 		lds r23,(CorrectionNick.2413)+1
8918
 8933 54c0 8091 0000 		lds r24,(CorrectionNick.2413)+2
8919
 8934 54c4 9091 0000 		lds r25,(CorrectionNick.2413)+3
8920
 8935 54c8 20E0      		ldi r18,lo8(256)
8921
 8936 54ca 31E0      		ldi r19,hi8(256)
8922
 8937 54cc 40E0      		ldi r20,hlo8(256)
8923
 8938 54ce 50E0      		ldi r21,hhi8(256)
8924
 8939 54d0 0E94 0000 		call __divmodsi4
8925
 8940 54d4 8091 0000 		lds r24,AttitudeCorrectionNick
8926
 8941 54d8 9091 0000 		lds r25,(AttitudeCorrectionNick)+1
8927
 8942 54dc 820F      		add r24,r18
8928
 8943 54de 931F      		adc r25,r19
8929
 8944 54e0 9093 0000 		sts (AttitudeCorrectionNick)+1,r25
8930
 8945 54e4 8093 0000 		sts AttitudeCorrectionNick,r24
8931
 8946               	.L544:
8932
 8947 54e8 50E0      		ldi r21,lo8(-4096)
8933
 8948 54ea E516      		cp r14,r21
8934
 8949 54ec 50EF      		ldi r21,hi8(-4096)
8935
 8950 54ee F506      		cpc r15,r21
8936
 8951 54f0 5FEF      		ldi r21,hlo8(-4096)
8937
 8952 54f2 0507      		cpc r16,r21
8938
 8953 54f4 5FEF      		ldi r21,hhi8(-4096)
8939
 8954 54f6 1507      		cpc r17,r21
8940
 8955 54f8 04F0      		brlt .+2
8941
 8956 54fa 00C0      		rjmp .L545
8942
 8957 54fc 8091 0000 		lds r24,last_n_n.2428
8943
 8958 5500 8823      		tst r24
8944
 8959 5502 01F4      		brne .+2
8945
 8960 5504 00C0      		rjmp .L546
8946
 8961 5506 8827      		clr r24
8947
 8962 5508 9927      		clr r25
8948
 8963 550a DC01      		movw r26,r24
8949
 8964 550c 8E19      		sub r24,r14
8950
 8965 550e 9F09      		sbc r25,r15
8951
 8966 5510 A00B      		sbc r26,r16
8952
 8967 5512 B10B      		sbc r27,r17
8953
 8968 5514 49E0      		ldi r20,9
8954
 8969 5516 B595      	1:	asr r27
8955
 8970 5518 A795      		ror r26
8956
 8971 551a 9795      		ror r25
8957
 8972 551c 8795      		ror r24
8958
 8973 551e 4A95      		dec r20
8959
 8974 5520 01F4      		brne 1b
8960
 8975 5522 A80E      		add r10,r24
8961
 8976 5524 B91E      		adc r11,r25
8962
 8977 5526 B092 0000 		sts (cnt.2426)+1,r11
8963
 8978 552a A092 0000 		sts cnt.2426,r10
8964
 8979 552e C801      		movw r24,r16
8965
 8980 5530 B701      		movw r22,r14
8966
 8981 5532 28E0      		ldi r18,lo8(8)
8967
 8982 5534 30E0      		ldi r19,hi8(8)
8968
 8983 5536 40E0      		ldi r20,hlo8(8)
8969
 8984 5538 50E0      		ldi r21,hhi8(8)
8970
 8985 553a 0E94 0000 		call __divmodsi4
8971
 8986 553e C901      		movw r24,r18
8972
 8987 5540 DA01      		movw r26,r20
8973
 8988 5542 8093 0000 		sts CorrectionNick.2413,r24
8974
 8989 5546 9093 0000 		sts (CorrectionNick.2413)+1,r25
8975
 8990 554a A093 0000 		sts (CorrectionNick.2413)+2,r26
8976
 8991 554e B093 0000 		sts (CorrectionNick.2413)+3,r27
8977
 8992 5552 8857      		subi r24,lo8(-5000)
8978
 8993 5554 9C4E      		sbci r25,hi8(-5000)
8979
 8994 5556 AF4F      		sbci r26,hlo8(-5000)
8980
 8995 5558 BF4F      		sbci r27,hhi8(-5000)
8981
 8996 555a 04F4      		brge .L547
8982
 8997 555c 88E7      		ldi r24,lo8(-5000)
8983
 8998 555e 9CEE      		ldi r25,hi8(-5000)
8984
 8999 5560 AFEF      		ldi r26,hlo8(-5000)
8985
 9000 5562 BFEF      		ldi r27,hhi8(-5000)
8986
 9001 5564 8093 0000 		sts CorrectionNick.2413,r24
8987
 9002 5568 9093 0000 		sts (CorrectionNick.2413)+1,r25
8988
 9003 556c A093 0000 		sts (CorrectionNick.2413)+2,r26
8989
 9004 5570 B093 0000 		sts (CorrectionNick.2413)+3,r27
8990
 9005               	.L547:
8991
 9006 5574 6091 0000 		lds r22,CorrectionNick.2413
8992
 9007 5578 7091 0000 		lds r23,(CorrectionNick.2413)+1
8993
 9008 557c 8091 0000 		lds r24,(CorrectionNick.2413)+2
8994
 9009 5580 9091 0000 		lds r25,(CorrectionNick.2413)+3
8995
 9010 5584 20E0      		ldi r18,lo8(256)
8996
 9011 5586 31E0      		ldi r19,hi8(256)
8997
 9012 5588 40E0      		ldi r20,hlo8(256)
8998
 9013 558a 50E0      		ldi r21,hhi8(256)
8999
 9014 558c 0E94 0000 		call __divmodsi4
9000
 9015 5590 8091 0000 		lds r24,AttitudeCorrectionNick
9001
 9016 5594 9091 0000 		lds r25,(AttitudeCorrectionNick)+1
9002
 9017 5598 820F      		add r24,r18
9003
 9018 559a 931F      		adc r25,r19
9004
 9019 559c 9093 0000 		sts (AttitudeCorrectionNick)+1,r25
9005
 9020 55a0 8093 0000 		sts AttitudeCorrectionNick,r24
9006
 9021 55a4 0C94 0000 		jmp .L548
9007
 9022               	.L617:
9008
 9023 55a8 1092 0000 		sts (HeightTrimming.2442)+1,__zero_reg__
9009
 9024 55ac 1092 0000 		sts HeightTrimming.2442,__zero_reg__
9010
 9025 55b0 08AD      		ldd r16,Y+56
9011
 9026 55b2 19AD      		ldd r17,Y+57
9012
 9027 55b4 00C0      		rjmp .L619
9013
 9028               	.L608:
9014
 9029 55b6 8091 0000 		lds r24,BaroFlags.2450
9015
 9030 55ba 8260      		ori r24,lo8(2)
9016
 9031 55bc 8093 0000 		sts BaroFlags.2450,r24
9017
 9032 55c0 0C94 0000 		jmp .L606
9018
 9033               	.L609:
9019
 9034 55c4 8091 0000 		lds r24,BaroFlags.2450
9020
 9035 55c8 8C7F      		andi r24,lo8(-4)
9021
 9036 55ca 8093 0000 		sts BaroFlags.2450,r24
9022
 9037 55ce 0C94 0000 		jmp .L606
9023
 9038               	.L464:
9024
 9039 55d2 1092 0000 		sts delay_neutral.2416,__zero_reg__
9025
 9040 55d6 00C0      		rjmp .L446
9026
 9041               	.L637:
9027
 9042 55d8 8091 0000 		lds r24,ReadingHeight
9028
 9043 55dc 9091 0000 		lds r25,(ReadingHeight)+1
9029
 9044 55e0 A091 0000 		lds r26,(ReadingHeight)+2
9030
 9045 55e4 B091 0000 		lds r27,(ReadingHeight)+3
9031
 9046 55e8 5C01      		movw r10,r24
9032
 9047 55ea A21A      		sub r10,r18
9033
 9048 55ec B30A      		sbc r11,r19
9034
 9049 55ee F090 0000 		lds r15,FCParam+2
9035
 9050 55f2 8091 0000 		lds r24,ReadingVario
9036
 9051 55f6 9091 0000 		lds r25,(ReadingVario)+1
9037
 9052 55fa 97FD      		sbrc r25,7
9038
 9053 55fc 00C0      		rjmp .L808
9039
 9054               	.L639:
9040
 9055 55fe 9595      		asr r25
9041
 9056 5600 8795      		ror r24
9042
 9057 5602 9595      		asr r25
9043
 9058 5604 8795      		ror r24
9044
 9059 5606 9595      		asr r25
9045
 9060 5608 8795      		ror r24
9046
 9061 560a 8930      		cpi r24,9
9047
 9062 560c 9105      		cpc r25,__zero_reg__
9048
 9063 560e 04F0      		brlt .+2
9049
 9064 5610 00C0      		rjmp .L809
9050
 9065 5612 1816      		cp __zero_reg__,r24
9051
 9066 5614 1906      		cpc __zero_reg__,r25
9052
 9067 5616 04F4      		brge .+2
9053
 9068 5618 00C0      		rjmp .L641
9054
 9069 561a 889F      		mul r24,r24
9055
 9070 561c A001      		movw r20,r0
9056
 9071 561e 899F      		mul r24,r25
9057
 9072 5620 500D      		add r21,r0
9058
 9073 5622 989F      		mul r25,r24
9059
 9074 5624 500D      		add r21,r0
9060
 9075 5626 1124      		clr r1
9061
 9076 5628 57FF      		sbrs r21,7
9062
 9077 562a 00C0      		rjmp .L644
9063
 9078 562c 4D5F      		subi r20,lo8(-(3))
9064
 9079 562e 5F4F      		sbci r21,hi8(-(3))
9065
 9080               	.L644:
9066
 9081 5630 5595      		asr r21
9067
 9082 5632 4795      		ror r20
9068
 9083 5634 5595      		asr r21
9069
 9084 5636 4795      		ror r20
9070
 9085 5638 2091 0000 		lds r18,ReadingVario
9071
 9086 563c 3091 0000 		lds r19,(ReadingVario)+1
9072
 9087 5640 241B      		sub r18,r20
9073
 9088 5642 350B      		sbc r19,r21
9074
 9089               	.L643:
9075
 9090 5644 B901      		movw r22,r18
9076
 9091 5646 8827      		clr r24
9077
 9092 5648 77FD      		sbrc r23,7
9078
 9093 564a 8095      		com r24
9079
 9094 564c 982F      		mov r25,r24
9080
 9095 564e 2091 0000 		lds r18,FCParam
9081
 9096 5652 30E0      		ldi r19,lo8(0)
9082
 9097 5654 40E0      		ldi r20,lo8(0)
9083
 9098 5656 50E0      		ldi r21,hi8(0)
9084
 9099 5658 0E94 0000 		call __mulsi3
9085
 9100 565c 20E8      		ldi r18,lo8(128)
9086
 9101 565e 30E0      		ldi r19,hi8(128)
9087
 9102 5660 40E0      		ldi r20,hlo8(128)
9088
 9103 5662 50E0      		ldi r21,hhi8(128)
9089
 9104 5664 0E94 0000 		call __divmodsi4
9090
 9105 5668 A901      		movw r20,r18
9091
 9106 566a 2F3F      		cpi r18,255
9092
 9107 566c 3105      		cpc r19,__zero_reg__
9093
 9108 566e 01F0      		breq .L645
9094
 9109 5670 04F0      		brlt .L645
9095
 9110 5672 4FEF      		ldi r20,lo8(255)
9096
 9111 5674 50E0      		ldi r21,hi8(255)
9097
 9112               	.L645:
9098
 9113 5676 8F2D      		mov r24,r15
9099
 9114 5678 90E0      		ldi r25,lo8(0)
9100
 9115 567a A89E      		mul r10,r24
9101
 9116 567c 9001      		movw r18,r0
9102
 9117 567e A99E      		mul r10,r25
9103
 9118 5680 300D      		add r19,r0
9104
 9119 5682 B89E      		mul r11,r24
9105
 9120 5684 300D      		add r19,r0
9106
 9121 5686 1124      		clr r1
9107
 9122 5688 37FD      		sbrc r19,7
9108
 9123 568a 00C0      		rjmp .L810
9109
 9124               	.L646:
9110
 9125 568c 3595      		asr r19
9111
 9126 568e 2795      		ror r18
9112
 9127 5690 3595      		asr r19
9113
 9128 5692 2795      		ror r18
9114
 9129 5694 3595      		asr r19
9115
 9130 5696 2795      		ror r18
9116
 9131 5698 3595      		asr r19
9117
 9132 569a 2795      		ror r18
9118
 9133 569c B801      		movw r22,r16
9119
 9134 569e 621B      		sub r22,r18
9120
 9135 56a0 730B      		sbc r23,r19
9121
 9136 56a2 9B01      		movw r18,r22
9122
 9137 56a4 CA01      		movw r24,r20
9123
 9138 56a6 7FEF      		ldi r23,hi8(-127)
9124
 9139 56a8 4138      		cpi r20,lo8(-127)
9125
 9140 56aa 5707      		cpc r21,r23
9126
 9141 56ac 04F4      		brge .L647
9127
 9142 56ae 81E8      		ldi r24,lo8(-127)
9128
 9143 56b0 9FEF      		ldi r25,hi8(-127)
9129
 9144               	.L647:
9130
 9145 56b2 6901      		movw r12,r18
9131
 9146 56b4 C81A      		sub r12,r24
9132
 9147 56b6 D90A      		sbc r13,r25
9133
 9148 56b8 6091 0000 		lds r22,ReadingIntegralTop
9134
 9149 56bc 7091 0000 		lds r23,(ReadingIntegralTop)+1
9135
 9150 56c0 8091 0000 		lds r24,(ReadingIntegralTop)+2
9136
 9151 56c4 9091 0000 		lds r25,(ReadingIntegralTop)+3
9137
 9152 56c8 E090 0000 		lds r14,FCParam+3
9138
 9153 56cc FF24      		clr r15
9139
 9154 56ce 00E0      		ldi r16,lo8(0)
9140
 9155 56d0 10E0      		ldi r17,hi8(0)
9141
 9156 56d2 20E8      		ldi r18,lo8(128)
9142
 9157 56d4 30E0      		ldi r19,hi8(128)
9143
 9158 56d6 40E0      		ldi r20,hlo8(128)
9144
 9159 56d8 50E0      		ldi r21,hhi8(128)
9145
 9160 56da 0E94 0000 		call __divmodsi4
9146
 9161 56de C801      		movw r24,r16
9147
 9162 56e0 B701      		movw r22,r14
9148
 9163 56e2 0E94 0000 		call __mulsi3
9149
 9164 56e6 20E2      		ldi r18,lo8(32)
9150
 9165 56e8 30E0      		ldi r19,hi8(32)
9151
 9166 56ea 40E0      		ldi r20,hlo8(32)
9152
 9167 56ec 50E0      		ldi r21,hhi8(32)
9153
 9168 56ee 0E94 0000 		call __divmodsi4
9154
 9169 56f2 C901      		movw r24,r18
9155
 9170 56f4 2F3F      		cpi r18,255
9156
 9171 56f6 3105      		cpc r19,__zero_reg__
9157
 9172 56f8 01F0      		breq .L648
9158
 9173 56fa 04F0      		brlt .L648
9159
 9174 56fc 8FEF      		ldi r24,lo8(255)
9160
 9175 56fe 90E0      		ldi r25,hi8(255)
9161
 9176               	.L648:
9162
 9177 5700 AFEF      		ldi r26,hi8(-127)
9163
 9178 5702 8138      		cpi r24,lo8(-127)
9164
 9179 5704 9A07      		cpc r25,r26
9165
 9180 5706 04F4      		brge .L649
9166
 9181 5708 81E8      		ldi r24,lo8(-127)
9167
 9182 570a 9FEF      		ldi r25,hi8(-127)
9168
 9183               	.L649:
9169
 9184 570c 9601      		movw r18,r12
9170
 9185 570e 281B      		sub r18,r24
9171
 9186 5710 390B      		sbc r19,r25
9172
 9187 5712 E5E9      		ldi r30,lo8(149)
9173
 9188 5714 F0E0      		ldi r31,hi8(149)
9174
 9189 5716 AE0E      		add r10,r30
9175
 9190 5718 BF1E      		adc r11,r31
9176
 9191 571a FBE2      		ldi r31,lo8(299)
9177
 9192 571c AF16      		cp r10,r31
9178
 9193 571e F1E0      		ldi r31,hi8(299)
9179
 9194 5720 BF06      		cpc r11,r31
9180
 9195 5722 00F0      		brlo .+2
9181
 9196 5724 00C0      		rjmp .L650
9182
 9197 5726 8091 0000 		lds r24,HeightTrimming.2442
9183
 9198 572a 9091 0000 		lds r25,(HeightTrimming.2442)+1
9184
 9199 572e 892B      		or r24,r25
9185
 9200 5730 01F0      		breq .+2
9186
 9201 5732 00C0      		rjmp .L650
9187
 9202 5734 8091 0000 		lds r24,HoverGas.2445
9188
 9203 5738 9091 0000 		lds r25,(HoverGas.2445)+1
9189
 9204 573c 1816      		cp __zero_reg__,r24
9190
 9205 573e 1906      		cpc __zero_reg__,r25
9191
 9206 5740 04F0      		brlt .+2
9192
 9207 5742 00C0      		rjmp .L650
9193
 9208 5744 0091 0000 		lds r16,HoverGasMin.2446
9194
 9209 5748 1091 0000 		lds r17,(HoverGasMin.2446)+1
9195
 9210 574c 2017      		cp r18,r16
9196
 9211 574e 3107      		cpc r19,r17
9197
 9212 5750 04F4      		brge .+2
9198
 9213 5752 00C0      		rjmp .L638
9199
 9214 5754 8091 0000 		lds r24,HoverGasMax.2447
9200
 9215 5758 9091 0000 		lds r25,(HoverGasMax.2447)+1
9201
 9216 575c 8901      		movw r16,r18
9202
 9217 575e 8217      		cp r24,r18
9203
 9218 5760 9307      		cpc r25,r19
9204
 9219 5762 04F0      		brlt .+2
9205
 9220 5764 00C0      		rjmp .L638
9206
 9221 5766 8C01      		movw r16,r24
9207
 9222 5768 00C0      		rjmp .L638
9208
 9223               	.L725:
9209
 9224 576a 8091 0000 		lds r24,FilterHCGas.2441
9210
 9225 576e 9091 0000 		lds r25,(FilterHCGas.2441)+1
9211
 9226 5772 E8AD      		ldd r30,Y+56
9212
 9227 5774 F9AD      		ldd r31,Y+57
9213
 9228 5776 E817      		cp r30,r24
9214
 9229 5778 F907      		cpc r31,r25
9215
 9230 577a 04F0      		brlt .+2
9216
 9231 577c 00C0      		rjmp .L656
9217
 9232 577e F093 0000 		sts (FilterHCGas.2441)+1,r31
9218
 9233 5782 E093 0000 		sts FilterHCGas.2441,r30
9219
 9234 5786 CF01      		movw r24,r30
9220
 9235 5788 00C0      		rjmp .L656
9221
 9236               	.L545:
9222
 9237 578a 1092 0000 		sts last_n_n.2428,__zero_reg__
9223
 9238 578e 0C94 0000 		jmp .L548
9224
 9239               	.L541:
9225
 9240 5792 1092 0000 		sts last_n_p.2427,__zero_reg__
9226
 9241 5796 A090 0000 		lds r10,cnt.2426
9227
 9242 579a B090 0000 		lds r11,(cnt.2426)+1
9228
 9243 579e 00C0      		rjmp .L544
9229
 9244               	.L556:
9230
 9245 57a0 1092 0000 		sts last_r_p.2429,__zero_reg__
9231
 9246 57a4 A090 0000 		lds r10,cnt.2426
9232
 9247 57a8 B090 0000 		lds r11,(cnt.2426)+1
9233
 9248 57ac 00C0      		rjmp .L559
9234
 9249               	.L560:
9235
 9250 57ae 1092 0000 		sts last_r_n.2430,__zero_reg__
9236
 9251 57b2 0C94 0000 		jmp .L563
9237
 9252               	.L807:
9238
 9253 57b6 F095      		com r31
9239
 9254 57b8 E195      		neg r30
9240
 9255 57ba FF4F      		sbci r31,lo8(-1)
9241
 9256 57bc 00C0      		rjmp .L525
9242
 9257               	.L803:
9243
 9258 57be 2D5F      		subi r18,lo8(-(3))
9244
 9259 57c0 3F4F      		sbci r19,hi8(-(3))
9245
 9260 57c2 00C0      		rjmp .L659
9246
 9261               	.L793:
9247
 9262 57c4 8091 0000 		lds r24,AttitudeCorrectionNick
9248
 9263 57c8 9091 0000 		lds r25,AttitudeCorrectionNick+1
9249
 9264 57cc 9C01      		movw r18,r24
9250
 9265 57ce 37FD      		sbrc r19,7
9251
 9266 57d0 00C0      		rjmp .L811
9252
 9267               	.L534:
9253
 9268 57d2 3595      		asr r19
9254
 9269 57d4 2795      		ror r18
9255
 9270 57d6 3093 0000 		sts (AttitudeCorrectionNick)+1,r19
9256
 9271 57da 2093 0000 		sts AttitudeCorrectionNick,r18
9257
 9272 57de 8091 0000 		lds r24,AttitudeCorrectionRoll
9258
 9273 57e2 9091 0000 		lds r25,AttitudeCorrectionRoll+1
9259
 9274 57e6 9C01      		movw r18,r24
9260
 9275 57e8 37FD      		sbrc r19,7
9261
 9276 57ea 00C0      		rjmp .L812
9262
 9277               	.L535:
9263
 9278 57ec 3595      		asr r19
9264
 9279 57ee 2795      		ror r18
9265
 9280 57f0 3093 0000 		sts (AttitudeCorrectionRoll)+1,r19
9266
 9281 57f4 2093 0000 		sts AttitudeCorrectionRoll,r18
9267
 9282 57f8 0C94 0000 		jmp .L532
9268
 9283               	.L650:
9269
 9284 57fc 8901      		movw r16,r18
9270
 9285 57fe 00C0      		rjmp .L638
9271
 9286               	.L449:
9272
 9287 5800 299A      		sbi 37-32,1
9273
 9288 5802 0C94 0000 		jmp .L450
9274
 9289               	.L668:
9275
 9290 5806 1092 0000 		sts (HoverGasMin.2446)+1,__zero_reg__
9276
 9291 580a 1092 0000 		sts HoverGasMin.2446,__zero_reg__
9277
 9292 580e 8FEF      		ldi r24,lo8(1023)
9278
 9293 5810 93E0      		ldi r25,hi8(1023)
9279
 9294 5812 9093 0000 		sts (HoverGasMax.2447)+1,r25
9280
 9295 5816 8093 0000 		sts HoverGasMax.2447,r24
9281
 9296 581a 0C94 0000 		jmp .L604
9282
 9297               	.L456:
9283
 9298 581e 21E0      		ldi r18,lo8(1)
9284
 9299 5820 0C94 0000 		jmp .L457
9285
 9300               	.L664:
9286
 9301 5824 2FE0      		ldi r18,hi8(4000)
9287
 9302 5826 803A      		cpi r24,lo8(4000)
9288
 9303 5828 9207      		cpc r25,r18
9289
 9304 582a 00F0      		brlo .+2
9290
 9305 582c 00C0      		rjmp .L666
9291
 9306 582e 2091 0000 		lds r18,HoverGasFilter.2448
9292
 9307 5832 3091 0000 		lds r19,(HoverGasFilter.2448)+1
9293
 9308 5836 4091 0000 		lds r20,(HoverGasFilter.2448)+2
9294
 9309 583a 5091 0000 		lds r21,(HoverGasFilter.2448)+3
9295
 9310 583e DA01      		movw r26,r20
9296
 9311 5840 C901      		movw r24,r18
9297
 9312 5842 6AE0      		ldi r22,10
9298
 9313 5844 B695      	1:	lsr r27
9299
 9314 5846 A795      		ror r26
9300
 9315 5848 9795      		ror r25
9301
 9316 584a 8795      		ror r24
9302
 9317 584c 6A95      		dec r22
9303
 9318 584e 01F4      		brne 1b
9304
 9319 5850 5901      		movw r10,r18
9305
 9320 5852 6A01      		movw r12,r20
9306
 9321 5854 A81A      		sub r10,r24
9307
 9322 5856 B90A      		sbc r11,r25
9308
 9323 5858 CA0A      		sbc r12,r26
9309
 9324 585a DB0A      		sbc r13,r27
9310
 9325 585c A801      		movw r20,r16
9311
 9326 585e 9701      		movw r18,r14
9312
 9327 5860 220F      		lsl r18
9313
 9328 5862 331F      		rol r19
9314
 9329 5864 441F      		rol r20
9315
 9330 5866 551F      		rol r21
9316
 9331 5868 220F      		lsl r18
9317
 9332 586a 331F      		rol r19
9318
 9333 586c 441F      		rol r20
9319
 9334 586e 551F      		rol r21
9320
 9335 5870 2A0D      		add r18,r10
9321
 9336 5872 3B1D      		adc r19,r11
9322
 9337 5874 4C1D      		adc r20,r12
9323
 9338 5876 5D1D      		adc r21,r13
9324
 9339 5878 2093 0000 		sts HoverGasFilter.2448,r18
9325
 9340 587c 3093 0000 		sts (HoverGasFilter.2448)+1,r19
9326
 9341 5880 4093 0000 		sts (HoverGasFilter.2448)+2,r20
9327
 9342 5884 5093 0000 		sts (HoverGasFilter.2448)+3,r21
9328
 9343 5888 00C0      		rjmp .L665
9329
 9344               	.L809:
9330
 9345 588a 88E0      		ldi r24,lo8(8)
9331
 9346 588c 90E0      		ldi r25,hi8(8)
9332
 9347               	.L641:
9333
 9348 588e 889F      		mul r24,r24
9334
 9349 5890 9001      		movw r18,r0
9335
 9350 5892 899F      		mul r24,r25
9336
 9351 5894 300D      		add r19,r0
9337
 9352 5896 989F      		mul r25,r24
9338
 9353 5898 300D      		add r19,r0
9339
 9354 589a 1124      		clr r1
9340
 9355 589c 3595      		asr r19
9341
 9356 589e 2795      		ror r18
9342
 9357 58a0 3595      		asr r19
9343
 9358 58a2 2795      		ror r18
9344
 9359 58a4 8091 0000 		lds r24,ReadingVario
9345
 9360 58a8 9091 0000 		lds r25,(ReadingVario)+1
9346
 9361 58ac 280F      		add r18,r24
9347
 9362 58ae 391F      		adc r19,r25
9348
 9363 58b0 00C0      		rjmp .L643
9349
 9364               	.L610:
9350
 9365 58b2 8091 0000 		lds r24,BaroFlags.2450
9351
 9366 58b6 8160      		ori r24,lo8(1)
9352
 9367 58b8 8093 0000 		sts BaroFlags.2450,r24
9353
 9368 58bc 0C94 0000 		jmp .L606
9354
 9369               	.L542:
9355
 9370 58c0 81E0      		ldi r24,lo8(1)
9356
 9371 58c2 8093 0000 		sts last_n_p.2427,r24
9357
 9372 58c6 A090 0000 		lds r10,cnt.2426
9358
 9373 58ca B090 0000 		lds r11,(cnt.2426)+1
9359
 9374 58ce 00C0      		rjmp .L544
9360
 9375               	.L561:
9361
 9376 58d0 81E0      		ldi r24,lo8(1)
9362
 9377 58d2 8093 0000 		sts last_r_n.2430,r24
9363
 9378 58d6 0C94 0000 		jmp .L563
9364
 9379               	.L804:
9365
 9380 58da 9095      		com r25
9366
 9381 58dc 8195      		neg r24
9367
 9382 58de 9F4F      		sbci r25,lo8(-1)
9368
 9383 58e0 00C0      		rjmp .L663
9369
 9384               	.L557:
9370
 9385 58e2 81E0      		ldi r24,lo8(1)
9371
 9386 58e4 8093 0000 		sts last_r_p.2429,r24
9372
 9387 58e8 A090 0000 		lds r10,cnt.2426
9373
 9388 58ec B090 0000 		lds r11,(cnt.2426)+1
9374
 9389 58f0 00C0      		rjmp .L559
9375
 9390               	.L546:
9376
 9391 58f2 81E0      		ldi r24,lo8(1)
9377
 9392 58f4 8093 0000 		sts last_n_n.2428,r24
9378
 9393 58f8 0C94 0000 		jmp .L548
9379
 9394               	.L801:
9380
 9395 58fc 4D5F      		subi r20,lo8(-(3))
9381
 9396 58fe 5F4F      		sbci r21,hi8(-(3))
9382
 9397 5900 00C0      		rjmp .L653
9383
 9398               	.L466:
9384
 9399 5902 299A      		sbi 37-32,1
9385
 9400 5904 00C0      		rjmp .L467
9386
 9401               	.L620:
9387
 9402 5906 8091 0000 		lds r24,ReadingHeight
9388
 9403 590a 9091 0000 		lds r25,(ReadingHeight)+1
9389
 9404 590e A091 0000 		lds r26,(ReadingHeight)+2
9390
 9405 5912 B091 0000 		lds r27,(ReadingHeight)+3
9391
 9406 5916 8059      		subi r24,lo8(-(-400))
9392
 9407 5918 9140      		sbci r25,hi8(-(-400))
9393
 9408 591a A040      		sbci r26,hlo8(-(-400))
9394
 9409 591c B040      		sbci r27,hhi8(-(-400))
9395
 9410 591e 8093 0000 		sts SetPointHeight,r24
9396
 9411 5922 9093 0000 		sts (SetPointHeight)+1,r25
9397
 9412 5926 A093 0000 		sts (SetPointHeight)+2,r26
9398
 9413 592a B093 0000 		sts (SetPointHeight)+3,r27
9399
 9414 592e 8091 0000 		lds r24,ParamSet+17
9400
 9415 5932 8823      		tst r24
9401
 9416 5934 01F4      		brne .+2
9402
 9417 5936 00C0      		rjmp .L634
9403
 9418 5938 8093 0000 		sts StickGasHover.2444,r24
9404
 9419 593c 1092 0000 		sts StickGasHover.2444+1,__zero_reg__
9405
 9420 5940 00C0      		rjmp .L633
9406
 9421               	.L666:
9407
 9422 5942 8054      		subi r24,lo8(8000)
9408
 9423 5944 9F41      		sbci r25,hi8(8000)
9409
 9424 5946 00F0      		brlo .+2
9410
 9425 5948 00C0      		rjmp .L667
9411
 9426 594a 2091 0000 		lds r18,HoverGasFilter.2448
9412
 9427 594e 3091 0000 		lds r19,(HoverGasFilter.2448)+1
9413
 9428 5952 4091 0000 		lds r20,(HoverGasFilter.2448)+2
9414
 9429 5956 5091 0000 		lds r21,(HoverGasFilter.2448)+3
9415
 9430 595a DA01      		movw r26,r20
9416
 9431 595c C901      		movw r24,r18
9417
 9432 595e EBE0      		ldi r30,11
9418
 9433 5960 B695      	1:	lsr r27
9419
 9434 5962 A795      		ror r26
9420
 9435 5964 9795      		ror r25
9421
 9436 5966 8795      		ror r24
9422
 9437 5968 EA95      		dec r30
9423
 9438 596a 01F4      		brne 1b
9424
 9439 596c 5901      		movw r10,r18
9425
 9440 596e 6A01      		movw r12,r20
9426
 9441 5970 A81A      		sub r10,r24
9427
 9442 5972 B90A      		sbc r11,r25
9428
 9443 5974 CA0A      		sbc r12,r26
9429
 9444 5976 DB0A      		sbc r13,r27
9430
 9445 5978 A801      		movw r20,r16
9431
 9446 597a 9701      		movw r18,r14
9432
 9447 597c 220F      		lsl r18
9433
 9448 597e 331F      		rol r19
9434
 9449 5980 441F      		rol r20
9435
 9450 5982 551F      		rol r21
9436
 9451 5984 2A0D      		add r18,r10
9437
 9452 5986 3B1D      		adc r19,r11
9438
 9453 5988 4C1D      		adc r20,r12
9439
 9454 598a 5D1D      		adc r21,r13
9440
 9455 598c 2093 0000 		sts HoverGasFilter.2448,r18
9441
 9456 5990 3093 0000 		sts (HoverGasFilter.2448)+1,r19
9442
 9457 5994 4093 0000 		sts (HoverGasFilter.2448)+2,r20
9443
 9458 5998 5093 0000 		sts (HoverGasFilter.2448)+3,r21
9444
 9459 599c 00C0      		rjmp .L665
9445
 9460               	.L621:
9446
 9461 599e CB01      		movw r24,r22
9447
 9462 59a0 0F97      		sbiw r24,15
9448
 9463 59a2 4817      		cp r20,r24
9449
 9464 59a4 5907      		cpc r21,r25
9450
 9465 59a6 04F4      		brge .L625
9451
 9466 59a8 8091 0000 		lds r24,BaroFlags.2450
9452
 9467 59ac 81FD      		sbrc r24,1
9453
 9468 59ae 00C0      		rjmp .L625
9454
 9469 59b0 2091 0000 		lds r18,HeightTrimmingFlag.2452
9455
 9470 59b4 20FF      		sbrs r18,0
9456
 9471 59b6 00C0      		rjmp .L626
9457
 9472 59b8 2E7F      		andi r18,lo8(-2)
9458
 9473 59ba 2093 0000 		sts HeightTrimmingFlag.2452,r18
9459
 9474 59be 8091 0000 		lds r24,ReadingHeight
9460
 9475 59c2 9091 0000 		lds r25,(ReadingHeight)+1
9461
 9476 59c6 A091 0000 		lds r26,(ReadingHeight)+2
9462
 9477 59ca B091 0000 		lds r27,(ReadingHeight)+3
9463
 9478 59ce 8093 0000 		sts SetPointHeight,r24
9464
 9479 59d2 9093 0000 		sts (SetPointHeight)+1,r25
9465
 9480 59d6 A093 0000 		sts (SetPointHeight)+2,r26
9466
 9481 59da B093 0000 		sts (SetPointHeight)+3,r27
9467
 9482               	.L626:
9468
 9483 59de 2260      		ori r18,lo8(2)
9469
 9484 59e0 2093 0000 		sts HeightTrimmingFlag.2452,r18
9470
 9485 59e4 CA01      		movw r24,r20
9471
 9486 59e6 0F96      		adiw r24,15
9472
 9487 59e8 861B      		sub r24,r22
9473
 9488 59ea 970B      		sbc r25,r23
9474
 9489 59ec 97FD      		sbrc r25,7
9475
 9490 59ee 00C0      		rjmp .L813
9476
 9491               	.L627:
9477
 9492 59f0 2091 0000 		lds r18,HeightTrimming.2442
9478
 9493 59f4 3091 0000 		lds r19,(HeightTrimming.2442)+1
9479
 9494 59f8 281B      		sub r18,r24
9480
 9495 59fa 390B      		sbc r19,r25
9481
 9496 59fc 3093 0000 		sts (HeightTrimming.2442)+1,r19
9482
 9497 5a00 2093 0000 		sts HeightTrimming.2442,r18
9483
 9498 5a04 00C0      		rjmp .L624
9484
 9499               	.L625:
9485
 9500 5a06 2091 0000 		lds r18,HeightTrimmingFlag.2452
9486
 9501 5a0a 822F      		mov r24,r18
9487
 9502 5a0c 90E0      		ldi r25,lo8(0)
9488
 9503 5a0e 8370      		andi r24,lo8(3)
9489
 9504 5a10 9070      		andi r25,hi8(3)
9490
 9505 5a12 892B      		or r24,r25
9491
 9506 5a14 01F0      		breq .+2
9492
 9507 5a16 00C0      		rjmp .L723
9493
 9508 5a18 2091 0000 		lds r18,HeightTrimming.2442
9494
 9509 5a1c 3091 0000 		lds r19,(HeightTrimming.2442)+1
9495
 9510 5a20 00C0      		rjmp .L624
9496
 9511               	.L667:
9497
 9512 5a22 2091 0000 		lds r18,HoverGasFilter.2448
9498
 9513 5a26 3091 0000 		lds r19,(HoverGasFilter.2448)+1
9499
 9514 5a2a 4091 0000 		lds r20,(HoverGasFilter.2448)+2
9500
 9515 5a2e 5091 0000 		lds r21,(HoverGasFilter.2448)+3
9501
 9516 5a32 DA01      		movw r26,r20
9502
 9517 5a34 C901      		movw r24,r18
9503
 9518 5a36 6CE0      		ldi r22,12
9504
 9519 5a38 B695      	1:	lsr r27
9505
 9520 5a3a A795      		ror r26
9506
 9521 5a3c 9795      		ror r25
9507
 9522 5a3e 8795      		ror r24
9508
 9523 5a40 6A95      		dec r22
9509
 9524 5a42 01F4      		brne 1b
9510
 9525 5a44 281B      		sub r18,r24
9511
 9526 5a46 390B      		sbc r19,r25
9512
 9527 5a48 4A0B      		sbc r20,r26
9513
 9528 5a4a 5B0B      		sbc r21,r27
9514
 9529 5a4c 2E0D      		add r18,r14
9515
 9530 5a4e 3F1D      		adc r19,r15
9516
 9531 5a50 401F      		adc r20,r16
9517
 9532 5a52 511F      		adc r21,r17
9518
 9533 5a54 2093 0000 		sts HoverGasFilter.2448,r18
9519
 9534 5a58 3093 0000 		sts (HoverGasFilter.2448)+1,r19
9520
 9535 5a5c 4093 0000 		sts (HoverGasFilter.2448)+2,r20
9521
 9536 5a60 5093 0000 		sts (HoverGasFilter.2448)+3,r21
9522
 9537 5a64 00C0      		rjmp .L665
9523
 9538               	.L808:
9524
 9539 5a66 0796      		adiw r24,7
9525
 9540 5a68 00C0      		rjmp .L639
9526
 9541               	.L461:
9527
 9542 5a6a 0E94 0000 		call GetActiveParamSet
9528
 9543 5a6e 0E94 0000 		call ParamSet_ReadFromEEProm
9529
 9544 5a72 0E94 0000 		call Servo_Off
9530
 9545 5a76 80E0      		ldi r24,lo8(0)
9531
 9546 5a78 00C0      		rjmp .L726
9532
 9547               	.L810:
9533
 9548 5a7a 215F      		subi r18,lo8(-(15))
9534
 9549 5a7c 3F4F      		sbci r19,hi8(-(15))
9535
 9550 5a7e 00C0      		rjmp .L646
9536
 9551               	.L634:
9537
 9552 5a80 88E7      		ldi r24,lo8(120)
9538
 9553 5a82 90E0      		ldi r25,hi8(120)
9539
 9554 5a84 9093 0000 		sts (StickGasHover.2444)+1,r25
9540
 9555 5a88 8093 0000 		sts StickGasHover.2444,r24
9541
 9556 5a8c 00C0      		rjmp .L633
9542
 9557               	.L796:
9543
 9558 5a8e F095      		com r31
9544
 9559 5a90 E195      		neg r30
9545
 9560 5a92 FF4F      		sbci r31,lo8(-1)
9546
 9561 5a94 0C94 0000 		jmp .L454
9547
 9562               	.L812:
9548
 9563 5a98 2F5F      		subi r18,lo8(-(1))
9549
 9564 5a9a 3F4F      		sbci r19,hi8(-(1))
9550
 9565 5a9c 00C0      		rjmp .L535
9551
 9566               	.L811:
9552
 9567 5a9e 2F5F      		subi r18,lo8(-(1))
9553
 9568 5aa0 3F4F      		sbci r19,hi8(-(1))
9554
 9569 5aa2 00C0      		rjmp .L534
9555
 9570               	.L792:
9556
 9571 5aa4 F095      		com r31
9557
 9572 5aa6 E195      		neg r30
9558
 9573 5aa8 FF4F      		sbci r31,lo8(-1)
9559
 9574 5aaa 0C94 0000 		jmp .L533
9560
 9575               	.L723:
9561
 9576 5aae 2C7F      		andi r18,lo8(-4)
9562
 9577 5ab0 2093 0000 		sts HeightTrimmingFlag.2452,r18
9563
 9578 5ab4 1092 0000 		sts (HeightTrimming.2442)+1,__zero_reg__
9564
 9579 5ab8 1092 0000 		sts HeightTrimming.2442,__zero_reg__
9565
 9580 5abc 8091 0000 		lds r24,ReadingHeight
9566
 9581 5ac0 9091 0000 		lds r25,(ReadingHeight)+1
9567
 9582 5ac4 A091 0000 		lds r26,(ReadingHeight)+2
9568
 9583 5ac8 B091 0000 		lds r27,(ReadingHeight)+3
9569
 9584 5acc 8093 0000 		sts SetPointHeight,r24
9570
 9585 5ad0 9093 0000 		sts (SetPointHeight)+1,r25
9571
 9586 5ad4 A093 0000 		sts (SetPointHeight)+2,r26
9572
 9587 5ad8 B093 0000 		sts (SetPointHeight)+3,r27
9573
 9588 5adc 8091 0000 		lds r24,ParamSet+88
9574
 9589 5ae0 81FF      		sbrs r24,1
9575
 9590 5ae2 00C0      		rjmp .L730
9576
 9591 5ae4 84EF      		ldi r24,lo8(500)
9577
 9592 5ae6 91E0      		ldi r25,hi8(500)
9578
 9593 5ae8 9093 0000 		sts (BeepTime)+1,r25
9579
 9594 5aec 8093 0000 		sts BeepTime,r24
9580
 9595               	.L730:
9581
 9596 5af0 20E0      		ldi r18,lo8(0)
9582
 9597 5af2 30E0      		ldi r19,hi8(0)
9583
 9598 5af4 00C0      		rjmp .L624
9584
 9599               	.L813:
9585
 9600 5af6 9095      		com r25
9586
 9601 5af8 8195      		neg r24
9587
 9602 5afa 9F4F      		sbci r25,lo8(-1)
9588
 9603 5afc 00C0      		rjmp .L627
9589
 9604               	.L799:
9590
 9605 5afe 9095      		com r25
9591
 9606 5b00 8195      		neg r24
9592
 9607 5b02 9F4F      		sbci r25,lo8(-1)
9593
 9608 5b04 00C0      		rjmp .L623
9594
 9609               	.L797:
9595
 9610 5b06 9095      		com r25
9596
 9611 5b08 8195      		neg r24
9597
 9612 5b0a 9F4F      		sbci r25,lo8(-1)
9598
 9613 5b0c 0C94 0000 		jmp .L463
9599
 9614               	.L632:
9600
 9615 5b10 6739      		cpi r22,151
9601
 9616 5b12 7105      		cpc r23,__zero_reg__
9602
 9617 5b14 04F4      		brge .+2
9603
 9618 5b16 00C0      		rjmp .L628
9604
 9619 5b18 86E9      		ldi r24,lo8(150)
9605
 9620 5b1a 90E0      		ldi r25,hi8(150)
9606
 9621 5b1c 9093 0000 		sts (StickGasHover.2444)+1,r25
9607
 9622 5b20 8093 0000 		sts StickGasHover.2444,r24
9608
 9623 5b24 00C0      		rjmp .L628
9609
 9624               	.L800:
9610
 9625 5b26 2D5F      		subi r18,lo8(-(3))
9611
 9626 5b28 3F4F      		sbci r19,hi8(-(3))
9612
 9627 5b2a 00C0      		rjmp .L631
9613
 9629               	.global	BiasHiResGyroNick
9614
 9630               	.global	BiasHiResGyroNick
9615
 9631               		.section .bss
9616
 9634               	BiasHiResGyroNick:
9617
 9635 0000 0000      		.skip 2,0
9618
 9636               	.global	BiasHiResGyroRoll
9619
 9637               	.global	BiasHiResGyroRoll
9620
 9640               	BiasHiResGyroRoll:
9621
 9641 0002 0000      		.skip 2,0
9622
 9642               	.global	AdBiasGyroYaw
9623
 9643               	.global	AdBiasGyroYaw
9624
 9646               	AdBiasGyroYaw:
9625
 9647 0004 0000      		.skip 2,0
9626
 9648               	.global	AdBiasAccNick
9627
 9649               	.global	AdBiasAccNick
9628
 9652               	AdBiasAccNick:
9629
 9653 0006 0000      		.skip 2,0
9630
 9654               	.global	AdBiasAccRoll
9631
 9655               	.global	AdBiasAccRoll
9632
 9658               	AdBiasAccRoll:
9633
 9659 0008 0000      		.skip 2,0
9634
 9660               	.global	AdBiasAccTop
9635
 9661               	.global	AdBiasAccTop
9636
 9664               	AdBiasAccTop:
9637
 9665 000a 0000 0000 		.skip 4,0
9638
 9666               	.global	IntegralGyroNick
9639
 9667               	.global	IntegralGyroNick
9640
 9670               	IntegralGyroNick:
9641
 9671 000e 0000 0000 		.skip 4,0
9642
 9672               	.global	IntegralGyroNick2
9643
 9673               	.global	IntegralGyroNick2
9644
 9676               	IntegralGyroNick2:
9645
 9677 0012 0000 0000 		.skip 4,0
9646
 9678               	.global	IntegralGyroRoll
9647
 9679               	.global	IntegralGyroRoll
9648
 9682               	IntegralGyroRoll:
9649
 9683 0016 0000 0000 		.skip 4,0
9650
 9684               	.global	IntegralGyroRoll2
9651
 9685               	.global	IntegralGyroRoll2
9652
 9688               	IntegralGyroRoll2:
9653
 9689 001a 0000 0000 		.skip 4,0
9654
 9690               	.global	IntegralGyroYaw
9655
 9691               	.global	IntegralGyroYaw
9656
 9694               	IntegralGyroYaw:
9657
 9695 001e 0000 0000 		.skip 4,0
9658
 9696               	.global	ReadingIntegralGyroNick
9659
 9697               	.global	ReadingIntegralGyroNick
9660
 9700               	ReadingIntegralGyroNick:
9661
 9701 0022 0000 0000 		.skip 4,0
9662
 9702               	.global	ReadingIntegralGyroNick2
9663
 9703               	.global	ReadingIntegralGyroNick2
9664
 9706               	ReadingIntegralGyroNick2:
9665
 9707 0026 0000 0000 		.skip 4,0
9666
 9708               	.global	ReadingIntegralGyroRoll
9667
 9709               	.global	ReadingIntegralGyroRoll
9668
 9712               	ReadingIntegralGyroRoll:
9669
 9713 002a 0000 0000 		.skip 4,0
9670
 9714               	.global	ReadingIntegralGyroRoll2
9671
 9715               	.global	ReadingIntegralGyroRoll2
9672
 9718               	ReadingIntegralGyroRoll2:
9673
 9719 002e 0000 0000 		.skip 4,0
9674
 9720               	.global	ReadingIntegralGyroYaw
9675
 9721               	.global	ReadingIntegralGyroYaw
9676
 9724               	ReadingIntegralGyroYaw:
9677
 9725 0032 0000 0000 		.skip 4,0
9678
 9726               	.global	MeanAccNick
9679
 9727               	.global	MeanAccNick
9680
 9730               	MeanAccNick:
9681
 9731 0036 0000 0000 		.skip 4,0
9682
 9732               	.global	MeanAccRoll
9683
 9733               	.global	MeanAccRoll
9684
 9736               	MeanAccRoll:
9685
 9737 003a 0000 0000 		.skip 4,0
9686
 9738               	.global	ReadingIntegralTop
9687
 9739               	.global	ReadingIntegralTop
9688
 9742               	ReadingIntegralTop:
9689
 9743 003e 0000 0000 		.skip 4,0
9690
 9744               	.global	CompassHeading
9691
 9745               		.data
9692
 9748               	CompassHeading:
9693
 9749 0000 FFFF      		.word	-1
9694
 9750               	.global	CompassCourse
9695
 9753               	CompassCourse:
9696
 9754 0002 FFFF      		.word	-1
9697
 9755               	.global	CompassOffCourse
9698
 9756               	.global	CompassOffCourse
9699
 9757               		.section .bss
9700
 9760               	CompassOffCourse:
9701
 9761 0042 0000      		.skip 2,0
9702
 9762               	.global	CompassCalState
9703
 9763               	.global	CompassCalState
9704
 9766               	CompassCalState:
9705
 9767 0044 00        		.skip 1,0
9706
 9768               	.global	FunnelCourse
9707
 9769               	.global	FunnelCourse
9708
 9772               	FunnelCourse:
9709
 9773 0045 00        		.skip 1,0
9710
 9774               	.global	BadCompassHeading
9711
 9775               		.data
9712
 9778               	BadCompassHeading:
9713
 9779 0004 F401      		.word	500
9714
 9780               	.global	NaviAccNick
9715
 9781               	.global	NaviAccNick
9716
 9782               		.section .bss
9717
 9785               	NaviAccNick:
9718
 9786 0046 0000      		.skip 2,0
9719
 9787               	.global	NaviAccRoll
9720
 9788               	.global	NaviAccRoll
9721
 9791               	NaviAccRoll:
9722
 9792 0048 0000      		.skip 2,0
9723
 9793               	.global	NaviCntAcc
9724
 9794               	.global	NaviCntAcc
9725
 9797               	NaviCntAcc:
9726
 9798 004a 0000      		.skip 2,0
9727
 9799               	.global	ModelIsFlying
9728
 9800               	.global	ModelIsFlying
9729
 9803               	ModelIsFlying:
9730
 9804 004c 0000      		.skip 2,0
9731
 9805               	.global	MKFlags
9732
 9806               	.global	MKFlags
9733
 9809               	MKFlags:
9734
 9810 004e 00        		.skip 1,0
9735
 9811               	.global	TurnOver180Nick
9736
 9812               		.data
9737
 9815               	TurnOver180Nick:
9738
 9816 0006 90        		.byte	-112
9739
 9817 0007 D0        		.byte	-48
9740
 9818 0008 03        		.byte	3
9741
 9819 0009 00        		.byte	0
9742
 9820               	.global	TurnOver180Roll
9743
 9823               	TurnOver180Roll:
9744
 9824 000a 90        		.byte	-112
9745
 9825 000b D0        		.byte	-48
9746
 9826 000c 03        		.byte	3
9747
 9827 000d 00        		.byte	0
9748
 9828               	.global	Ki
9749
 9831               	Ki:
9750
 9832 000e 3801      		.word	312
9751
 9833               	.global	Poti1
9752
 9834               	.global	Poti1
9753
 9835               		.section .bss
9754
 9838               	Poti1:
9755
 9839 004f 0000      		.skip 2,0
9756
 9840               	.global	Poti2
9757
 9841               	.global	Poti2
9758
 9844               	Poti2:
9759
 9845 0051 0000      		.skip 2,0
9760
 9846               	.global	Poti3
9761
 9847               	.global	Poti3
9762
 9850               	Poti3:
9763
 9851 0053 0000      		.skip 2,0
9764
 9852               	.global	Poti4
9765
 9853               	.global	Poti4
9766
 9856               	Poti4:
9767
 9857 0055 0000      		.skip 2,0
9768
 9858               	.global	Poti5
9769
 9859               	.global	Poti5
9770
 9862               	Poti5:
9771
 9863 0057 0000      		.skip 2,0
9772
 9864               	.global	Poti6
9773
 9865               	.global	Poti6
9774
 9868               	Poti6:
9775
 9869 0059 0000      		.skip 2,0
9776
 9870               	.global	Poti7
9777
 9871               	.global	Poti7
9778
 9874               	Poti7:
9779
 9875 005b 0000      		.skip 2,0
9780
 9876               	.global	Poti8
9781
 9877               	.global	Poti8
9782
 9880               	Poti8:
9783
 9881 005d 0000      		.skip 2,0
9784
 9882               	.global	RequiredMotors
9785
 9883               	.global	RequiredMotors
9786
 9886               	RequiredMotors:
9787
 9887 005f 00        		.skip 1,0
9788
 9888               	.global	StickNick
9789
 9889               	.global	StickNick
9790
 9892               	StickNick:
9791
 9893 0060 0000      		.skip 2,0
9792
 9894               	.global	StickRoll
9793
 9895               	.global	StickRoll
9794
 9898               	StickRoll:
9795
 9899 0062 0000      		.skip 2,0
9796
 9900               	.global	StickYaw
9797
 9901               	.global	StickYaw
9798
 9904               	StickYaw:
9799
 9905 0064 0000      		.skip 2,0
9800
 9906               	.global	StickGas
9801
 9907               	.global	StickGas
9802
 9910               	StickGas:
9803
 9911 0066 0000      		.skip 2,0
9804
 9912               	.global	GPSStickNick
9805
 9913               	.global	GPSStickNick
9806
 9916               	GPSStickNick:
9807
 9917 0068 0000      		.skip 2,0
9808
 9918               	.global	GPSStickRoll
9809
 9919               	.global	GPSStickRoll
9810
 9922               	GPSStickRoll:
9811
 9923 006a 0000      		.skip 2,0
9812
 9924               	.global	MaxStickNick
9813
 9925               	.global	MaxStickNick
9814
 9928               	MaxStickNick:
9815
 9929 006c 0000      		.skip 2,0
9816
 9930               	.global	MaxStickRoll
9817
 9931               	.global	MaxStickRoll
9818
 9934               	MaxStickRoll:
9819
 9935 006e 0000      		.skip 2,0
9820
 9936               	.global	ExternStickNick
9821
 9937               	.global	ExternStickNick
9822
 9940               	ExternStickNick:
9823
 9941 0070 0000      		.skip 2,0
9824
 9942               	.global	ExternStickRoll
9825
 9943               	.global	ExternStickRoll
9826
 9946               	ExternStickRoll:
9827
 9947 0072 0000      		.skip 2,0
9828
 9948               	.global	ExternStickYaw
9829
 9949               	.global	ExternStickYaw
9830
 9952               	ExternStickYaw:
9831
 9953 0074 0000      		.skip 2,0
9832
 9954               	.global	ExternHeightValue
9833
 9955               		.data
9834
 9958               	ExternHeightValue:
9835
 9959 0010 ECFF      		.word	-20
9836
 9960               	.global	SetPointHeight
9837
 9961               	.global	SetPointHeight
9838
 9962               		.section .bss
9839
 9965               	SetPointHeight:
9840
 9966 0076 0000 0000 		.skip 4,0
9841
 9967               	.global	AttitudeCorrectionRoll
9842
 9968               	.global	AttitudeCorrectionRoll
9843
 9971               	AttitudeCorrectionRoll:
9844
 9972 007a 0000      		.skip 2,0
9845
 9973               	.global	AttitudeCorrectionNick
9846
 9974               	.global	AttitudeCorrectionNick
9847
 9977               	AttitudeCorrectionNick:
9848
 9978 007c 0000      		.skip 2,0
9849
 9979               	.global	LoopingNick
9850
 9980               	.global	LoopingNick
9851
 9983               	LoopingNick:
9852
 9984 007e 00        		.skip 1,0
9853
 9985               	.global	LoopingRoll
9854
 9986               	.global	LoopingRoll
9855
 9989               	LoopingRoll:
9856
 9990 007f 00        		.skip 1,0
9857
 9991               	.global	LoopingLeft
9858
 9992               	.global	LoopingLeft
9859
 9995               	LoopingLeft:
9860
 9996 0080 00        		.skip 1,0
9861
 9997               	.global	LoopingRight
9862
 9998               	.global	LoopingRight
9863
 10001               	LoopingRight:
9864
 10002 0081 00        		.skip 1,0
9865
 10003               	.global	LoopingDown
9866
 10004               	.global	LoopingDown
9867
 10007               	LoopingDown:
9868
 10008 0082 00        		.skip 1,0
9869
 10009               	.global	LoopingTop
9870
 10010               	.global	LoopingTop
9871
 10013               	LoopingTop:
9872
 10014 0083 00        		.skip 1,0
9873
 10015               	.global	FCParam
9874
 10016               		.data
9875
 10019               	FCParam:
9876
 10020 0012 30        		.byte	48
9877
 10021 0013 FB        		.byte	-5
9878
 10022 0014 10        		.byte	16
9879
 10023 0015 3A        		.byte	58
9880
 10024 0016 40        		.byte	64
9881
 10025 0017 40        		.byte	64
9882
 10026 0018 08        		.byte	8
9883
 10027 0019 96        		.byte	-106
9884
 10028 001a 96        		.byte	-106
9885
 10029 001b 96        		.byte	-106
9886
 10030 001c 96        		.byte	-106
9887
 10031 001d 02        		.byte	2
9888
 10032 001e 0A        		.byte	10
9889
 10033 001f 00        		.byte	0
9890
 10034 0020 00        		.byte	0
9891
 10035 0021 00        		.byte	0
9892
 10036 0022 00        		.byte	0
9893
 10037 0023 00        		.byte	0
9894
 10038 0024 00        		.byte	0
9895
 10039 0025 00        		.byte	0
9896
 10040 0026 00        		.byte	0
9897
 10041 0027 64        		.byte	100
9898
 10042 0028 64        		.byte	100
9899
 10043 0029 46        		.byte	70
9900
 10044 002a 5A        		.byte	90
9901
 10045 002b 41        		.byte	65
9902
 10046 002c 40        		.byte	64
9903
 10047 002d 64        		.byte	100
9904
 10048 002e 00        		.byte	0
9905
 10049 002f 00        		.byte	0
9906
 10050 0030 00        		.byte	0
9907
 10051 0031 0000 00   		.skip 3,0
9908
 10052               		.lcomm HeightTrimmingFlag.2452,1
9909
 10053               		.lcomm BaroExpandActive.2451,2
9910
 10054               		.lcomm BaroFlags.2450,1
9911
 10057               	delay.2449:
9912
 10058 0034 64        		.byte	100
9913
 10059               		.lcomm HoverGasFilter.2448,4
9914
 10062               	HoverGasMax.2447:
9915
 10063 0035 FF03      		.word	1023
9916
 10064               		.lcomm HoverGasMin.2446,2
9917
 10065               		.lcomm HoverGas.2445,2
9918
 10068               	StickGasHover.2444:
9919
 10069 0037 7800      		.word	120
9920
 10070               		.lcomm HCActive.2443,1
9921
 10071               		.lcomm HeightTrimming.2442,2
9922
 10072               		.lcomm FilterHCGas.2441,2
9923
 10073               		.lcomm MeanIntegralGyroRoll_old.2432,4
9924
 10074               		.lcomm MeanIntegralGyroNick_old.2431,4
9925
 10075               		.lcomm last_r_n.2430,1
9926
 10076               		.lcomm last_r_p.2429,1
9927
 10077               		.lcomm last_n_n.2428,1
9928
 10078               		.lcomm last_n_p.2427,1
9929
 10079               		.lcomm cnt.2426,2
9930
 10080               		.lcomm stick_roll.2425,2
9931
 10081               		.lcomm stick_nick.2424,2
9932
 10082               		.lcomm MotorValue.2421,24
9933
 10083               		.lcomm UpdateCompassCourse.2420,2
9934
 10084               		.lcomm TimerDebugOut.2419,1
9935
 10085               		.lcomm delay_stopmotors.2418,1
9936
 10086               		.lcomm delay_startmotors.2417,1
9937
 10087               		.lcomm delay_neutral.2416,1
9938
 10088               		.lcomm RcLostTimer.2415,2
9939
 10089               		.lcomm CorrectionRoll.2414,4
9940
 10090               		.lcomm CorrectionNick.2413,4
9941
 10091               		.lcomm IntegralGyroRollError.2412,4
9942
 10092               		.lcomm IntegralGyroNickError.2411,4
9943
 10093               		.lcomm SetPointYaw.2410,4
9944
 10094               		.lcomm IPartRoll.2409,4
9945
 10095               		.lcomm IPartNick.2408,4
9946
 10098               	stick.2382:
9947
 10099 0039 01        		.byte	1
9948
 10100               		.lcomm Last_GyroNick.2093,2
9949
 10101               		.lcomm Last_GyroRoll.2092,2
9950
 10102               		.section	.progmem.data,"a",@progbits
9951
 10105               	__c.1965:
9952
 10106 0000 0A0D 4143 		.string	"\n\rACC not calibrated!\r\n"
9953
 10106      4320 6E6F
9954
 10106      7420 6361
9955
 10106      6C69 6272
9956
 10106      6174 6564
9957
 10107               		.comm GyroNick,2,1
9958
 10108               		.comm GyroRoll,2,1
9959
 10109               		.comm GyroYaw,2,1
9960
 10110               		.comm YawGyroHeading,4,1
9961
 10111               		.comm AccNick,2,1
9962
 10112               		.comm AccRoll,2,1
9963
 10113               		.comm AccTop,2,1
9964
 10114               		.comm TrimNick,2,1
9965
 10115               		.comm TrimRoll,2,1
9966
 10116               		.comm MeanIntegralGyroNick,4,1
9967
 10117               		.comm MeanIntegralGyroRoll,4,1
9968
 10118               		.comm YawGyroDrift,2,1
9969
 10119               		.comm GyroPFactor,1,1
9970
 10120               		.comm GyroIFactor,1,1
9971
 10121               		.comm GyroYawPFactor,1,1
9972
 10122               		.comm GyroYawIFactor,1,1
9973
 10123               	.global __do_copy_data
9974
 10124               	.global __do_clear_bss
9975
DEFINED SYMBOLS
9976
                            *ABS*:00000000 fc.c
9977
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2      *ABS*:0000003f __SREG__
9978
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3      *ABS*:0000003e __SP_H__
9979
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:4      *ABS*:0000003d __SP_L__
9980
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:5      *ABS*:00000034 __CCP__
9981
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:6      *ABS*:00000000 __tmp_reg__
9982
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:7      *ABS*:00000001 __zero_reg__
9983
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:11     .text:00000000 MotorSmoothing
9984
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:43     .text:0000002c Mean
9985
                            *COM*:00000002 GyroNick
9986
                            *COM*:00000002 GyroRoll
9987
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9646   .bss:00000004 AdBiasGyroYaw
9988
                            *COM*:00000002 GyroYaw
9989
                            *COM*:00000002 AccNick
9990
                            *COM*:00000002 AccRoll
9991
                            *COM*:00000002 AccTop
9992
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9730   .bss:00000036 MeanAccNick
9993
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9736   .bss:0000003a MeanAccRoll
9994
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9785   .bss:00000046 NaviAccNick
9995
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9791   .bss:00000048 NaviAccRoll
9996
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9797   .bss:0000004a NaviCntAcc
9997
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9700   .bss:00000022 ReadingIntegralGyroNick
9998
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9712   .bss:0000002a ReadingIntegralGyroRoll
9999
                            *COM*:00000004 YawGyroHeading
10000
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9724   .bss:00000032 ReadingIntegralGyroYaw
10001
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9983   .bss:0000007e LoopingNick
10002
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9989   .bss:0000007f LoopingRoll
10003
                            *COM*:00000002 TrimNick
10004
                            *COM*:00000002 TrimRoll
10005
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9718   .bss:0000002e ReadingIntegralGyroRoll2
10006
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9971   .bss:0000007a AttitudeCorrectionRoll
10007
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9823   .data:0000000a TurnOver180Roll
10008
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9706   .bss:00000026 ReadingIntegralGyroNick2
10009
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9977   .bss:0000007c AttitudeCorrectionNick
10010
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9815   .data:00000006 TurnOver180Nick
10011
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9694   .bss:0000001e IntegralGyroYaw
10012
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9670   .bss:0000000e IntegralGyroNick
10013
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9682   .bss:00000016 IntegralGyroRoll
10014
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9676   .bss:00000012 IntegralGyroNick2
10015
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9688   .bss:0000001a IntegralGyroRoll2
10016
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10019  .data:00000012 FCParam
10017
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10095  .bss:000000e3 Last_GyroNick.2093
10018
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10100  .bss:000000e5 Last_GyroRoll.2092
10019
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9772   .bss:00000045 FunnelCourse
10020
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:1559   .text:00000e60 ParameterMapping
10021
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9856   .bss:00000055 Poti4
10022
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9831   .data:0000000e Ki
10023
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9838   .bss:0000004f Poti1
10024
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9844   .bss:00000051 Poti2
10025
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9850   .bss:00000053 Poti3
10026
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2620   .text:000017da SendMotorData
10027
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9809   .bss:0000004e MKFlags
10028
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2681   .text:0000185c SetNeutral
10029
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9652   .bss:00000006 AdBiasAccNick
10030
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9658   .bss:00000008 AdBiasAccRoll
10031
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9664   .bss:0000000a AdBiasAccTop
10032
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9634   .bss:00000000 BiasHiResGyroNick
10033
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9640   .bss:00000002 BiasHiResGyroRoll
10034
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9742   .bss:0000003e ReadingIntegralTop
10035
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9748   .data:00000000 CompassHeading
10036
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9753   .data:00000002 CompassCourse
10037
                            *COM*:00000002 YawGyroDrift
10038
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9958   .data:00000010 ExternHeightValue
10039
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9916   .bss:00000068 GPSStickNick
10040
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9922   .bss:0000006a GPSStickRoll
10041
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10105  .progmem.data:00000000 __c.1965
10042
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3317   .text:00001eca Beep
10043
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3353   .text:00001f08 SetCompassCalState
10044
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10098  .data:00000039 stick.2382
10045
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9766   .bss:00000044 CompassCalState
10046
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3402   .text:00001f6a MotorControl
10047
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10087  .bss:000000c5 RcLostTimer.2415
10048
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9803   .bss:0000004c ModelIsFlying
10049
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10080  .bss:000000a5 stick_nick.2424
10050
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9892   .bss:00000060 StickNick
10051
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10079  .bss:000000a3 stick_roll.2425
10052
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9898   .bss:00000062 StickRoll
10053
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9904   .bss:00000064 StickYaw
10054
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9910   .bss:00000066 StickGas
10055
                            *COM*:00000001 GyroPFactor
10056
                            *COM*:00000001 GyroIFactor
10057
                            *COM*:00000001 GyroYawPFactor
10058
                            *COM*:00000001 GyroYawIFactor
10059
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9928   .bss:0000006c MaxStickNick
10060
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9934   .bss:0000006e MaxStickRoll
10061
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9995   .bss:00000080 LoopingLeft
10062
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10001  .bss:00000081 LoopingRight
10063
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10013  .bss:00000083 LoopingTop
10064
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10007  .bss:00000082 LoopingDown
10065
                            *COM*:00000004 MeanIntegralGyroNick
10066
                            *COM*:00000004 MeanIntegralGyroRoll
10067
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10073  .bss:00000099 MeanIntegralGyroNick_old.2431
10068
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10072  .bss:00000095 MeanIntegralGyroRoll_old.2432
10069
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9778   .data:00000004 BadCompassHeading
10070
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10092  .bss:000000d7 SetPointYaw.2410
10071
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10094  .bss:000000df IPartNick.2408
10072
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10093  .bss:000000db IPartRoll.2409
10073
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9862   .bss:00000057 Poti5
10074
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9868   .bss:00000059 Poti6
10075
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9874   .bss:0000005b Poti7
10076
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9880   .bss:0000005d Poti8
10077
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10086  .bss:000000c4 delay_neutral.2416
10078
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10083  .bss:000000c1 TimerDebugOut.2419
10079
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10081  .bss:000000a7 MotorValue.2421
10080
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10082  .bss:000000bf UpdateCompassCourse.2420
10081
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10064  .bss:0000008e HoverGas.2445
10082
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10052  .bss:00000085 BaroExpandActive.2451
10083
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10057  .data:00000034 delay.2449
10084
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9965   .bss:00000076 SetPointHeight
10085
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10065  .bss:00000090 HCActive.2443
10086
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10091  .bss:000000d3 IntegralGyroNickError.2411
10087
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10089  .bss:000000cb CorrectionNick.2413
10088
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10090  .bss:000000cf IntegralGyroRollError.2412
10089
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10088  .bss:000000c7 CorrectionRoll.2414
10090
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10078  .bss:000000a1 cnt.2426
10091
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10085  .bss:000000c3 delay_startmotors.2417
10092
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10084  .bss:000000c2 delay_stopmotors.2418
10093
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10053  .bss:00000087 BaroFlags.2450
10094
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10068  .data:00000037 StickGasHover.2444
10095
                             .bss:00000084 HeightTrimmingFlag.2452
10096
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10070  .bss:00000091 HeightTrimming.2442
10097
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10071  .bss:00000093 FilterHCGas.2441
10098
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10054  .bss:00000088 HoverGasFilter.2448
10099
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10059  .bss:0000008c HoverGasMin.2446
10100
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10062  .data:00000035 HoverGasMax.2447
10101
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10075  .bss:0000009e last_r_p.2429
10102
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10074  .bss:0000009d last_r_n.2430
10103
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10077  .bss:000000a0 last_n_p.2427
10104
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10076  .bss:0000009f last_n_n.2428
10105
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9760   .bss:00000042 CompassOffCourse
10106
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9886   .bss:0000005f RequiredMotors
10107
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9940   .bss:00000070 ExternStickNick
10108
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9946   .bss:00000072 ExternStickRoll
10109
C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9952   .bss:00000074 ExternStickYaw
10110
 
10111
UNDEFINED SYMBOLS
10112
HiResGyroNick
10113
FilterHiResGyroNick
10114
AdValueGyroNick
10115
BoardRelease
10116
HiResGyroRoll
10117
FilterHiResGyroRoll
10118
AdValueGyroRoll
10119
AdValueGyroYaw
10120
AdValueAccNick
10121
__mulsi3
10122
__divmodsi4
10123
AdValueAccRoll
10124
AdValueAccTop
10125
ADReady
10126
ParamSet
10127
RC_Quality
10128
__divmodhi4
10129
MotorTest_Active
10130
Motor
10131
MotorTest
10132
DebugOut
10133
I2C_Start
10134
ExpandBaro
10135
Delay_ms_Mess
10136
AdAirPressure
10137
AdValueAccZ
10138
__floatsisf
10139
SetParamWord
10140
__fixunssfsi
10141
AirPressure
10142
StartAirPressure
10143
ReadingVario
10144
BeepTime
10145
PPM_in
10146
SearchAirPressureOffset
10147
GetParamWord
10148
_printf_P
10149
__gtsf2
10150
SearchDacGyroOffset
10151
Delay_ms
10152
PPM_diff
10153
NewPpmData
10154
ExternControl
10155
MeasurementCounter
10156
SetActiveParamSet
10157
GetActiveParamSet
10158
ParamSet_ReadFromEEProm
10159
Servo_Off
10160
ReadingHeight
10161
UBat
10162
ServoNickValue
10163
LowVoltageWarning
10164
__udivmodhi4
10165
MissingMotor
10166
Mixer
10167
SumHeight
10168
ihypot
10169
c_cos_8192
10170
NCGpsZ
10171
PressureSensorOffset
10172
Servo_On
10173
__do_copy_data
10174
__do_clear_bss