1 .file "fc.c" 2 __SREG__ = 0x3f 3 __SP_H__ = 0x3e 4 __SP_L__ = 0x3d 5 __CCP__ = 0x34 6 __tmp_reg__ = 0 7 __zero_reg__ = 1 8 .text 9 .global MotorSmoothing 11 MotorSmoothing: 12 /* prologue: function */ 13 /* frame size = 0 */ 14 0000 9B01 movw r18,r22 15 0002 6817 cp r22,r24 16 0004 7907 cpc r23,r25 17 0006 04F0 brlt .L7 18 0008 BC01 movw r22,r24 19 000a 660F lsl r22 20 000c 771F rol r23 21 000e 621B sub r22,r18 22 0010 730B sbc r23,r19 23 0012 CB01 movw r24,r22 24 /* epilogue start */ 25 0014 0895 ret 26 .L7: 27 0016 680F add r22,r24 28 0018 791F adc r23,r25 29 001a 77FD sbrc r23,7 30 001c 00C0 rjmp .L8 31 .L3: 32 001e 7595 asr r23 33 0020 6795 ror r22 34 0022 CB01 movw r24,r22 35 0024 0895 ret 36 .L8: 37 0026 6F5F subi r22,lo8(-(1)) 38 0028 7F4F sbci r23,hi8(-(1)) 39 002a 00C0 rjmp .L3 41 .global Mean 43 Mean: 44 002c 2F92 push r2 45 002e 3F92 push r3 46 0030 4F92 push r4 47 0032 5F92 push r5 48 0034 6F92 push r6 49 0036 7F92 push r7 50 0038 8F92 push r8 51 003a 9F92 push r9 52 003c AF92 push r10 53 003e BF92 push r11 54 0040 CF92 push r12 55 0042 DF92 push r13 56 0044 EF92 push r14 57 0046 FF92 push r15 58 0048 0F93 push r16 59 004a 1F93 push r17 60 004c DF93 push r29 61 004e CF93 push r28 62 0050 CDB7 in r28,__SP_L__ 63 0052 DEB7 in r29,__SP_H__ 64 0054 AD97 sbiw r28,45 65 0056 0FB6 in __tmp_reg__,__SREG__ 66 0058 F894 cli 67 005a DEBF out __SP_H__,r29 68 005c 0FBE out __SREG__,__tmp_reg__ 69 005e CDBF out __SP_L__,r28 70 /* prologue: function */ 71 /* frame size = 45 */ 72 0060 8091 0000 lds r24,HiResGyroNick 73 0064 9091 0000 lds r25,(HiResGyroNick)+1 74 0068 97FD sbrc r25,7 75 006a 00C0 rjmp .L67 76 .L10: 77 006c 9595 asr r25 78 006e 8795 ror r24 79 0070 9595 asr r25 80 0072 8795 ror r24 81 0074 9595 asr r25 82 0076 8795 ror r24 83 0078 9093 0000 sts (GyroNick)+1,r25 84 007c 8093 0000 sts GyroNick,r24 85 0080 2091 0000 lds r18,FilterHiResGyroNick 86 0084 3091 0000 lds r19,(FilterHiResGyroNick)+1 87 0088 37FD sbrc r19,7 88 008a 00C0 rjmp .L68 89 .L11: 90 008c C901 movw r24,r18 91 008e 9595 asr r25 92 0090 8795 ror r24 93 0092 9595 asr r25 94 0094 8795 ror r24 95 0096 9595 asr r25 96 0098 8795 ror r24 97 009a 9D8F std Y+29,r25 98 009c 8C8F std Y+28,r24 99 009e 8091 0000 lds r24,AdValueGyroNick 100 00a2 9091 0000 lds r25,(AdValueGyroNick)+1 101 00a6 0F97 sbiw r24,15 102 00a8 04F4 brge .L12 103 00aa 88E1 ldi r24,lo8(-1000) 104 00ac 9CEF ldi r25,hi8(-1000) 105 00ae 9093 0000 sts (GyroNick)+1,r25 106 00b2 8093 0000 sts GyroNick,r24 107 .L12: 108 00b6 8091 0000 lds r24,AdValueGyroNick 109 00ba 9091 0000 lds r25,(AdValueGyroNick)+1 110 00be 0797 sbiw r24,7 111 00c0 04F4 brge .L13 112 00c2 80E3 ldi r24,lo8(-2000) 113 00c4 98EF ldi r25,hi8(-2000) 114 00c6 9093 0000 sts (GyroNick)+1,r25 115 00ca 8093 0000 sts GyroNick,r24 116 .L13: 117 00ce 4091 0000 lds r20,BoardRelease 118 00d2 4A30 cpi r20,lo8(10) 119 00d4 01F4 brne .+2 120 00d6 00C0 rjmp .L69 121 00d8 8091 0000 lds r24,AdValueGyroNick 122 00dc 9091 0000 lds r25,(AdValueGyroNick)+1 123 00e0 815D subi r24,lo8(2001) 124 00e2 9740 sbci r25,hi8(2001) 125 00e4 04F0 brlt .L17 126 00e6 88EE ldi r24,lo8(1000) 127 00e8 93E0 ldi r25,hi8(1000) 128 00ea 9093 0000 sts (GyroNick)+1,r25 129 00ee 8093 0000 sts GyroNick,r24 130 .L17: 131 00f2 8091 0000 lds r24,AdValueGyroNick 132 00f6 9091 0000 lds r25,(AdValueGyroNick)+1 133 00fa 805E subi r24,lo8(2016) 134 00fc 9740 sbci r25,hi8(2016) 135 00fe 04F0 brlt .+2 136 0100 00C0 rjmp .L65 137 .L16: 138 0102 8091 0000 lds r24,HiResGyroRoll 139 0106 9091 0000 lds r25,(HiResGyroRoll)+1 140 010a 97FD sbrc r25,7 141 010c 00C0 rjmp .L70 142 .L18: 143 010e 9595 asr r25 144 0110 8795 ror r24 145 0112 9595 asr r25 146 0114 8795 ror r24 147 0116 9595 asr r25 148 0118 8795 ror r24 149 011a 9093 0000 sts (GyroRoll)+1,r25 150 011e 8093 0000 sts GyroRoll,r24 151 0122 2091 0000 lds r18,FilterHiResGyroRoll 152 0126 3091 0000 lds r19,(FilterHiResGyroRoll)+1 153 012a 37FD sbrc r19,7 154 012c 00C0 rjmp .L71 155 .L19: 156 012e C901 movw r24,r18 157 0130 9595 asr r25 158 0132 8795 ror r24 159 0134 9595 asr r25 160 0136 8795 ror r24 161 0138 9595 asr r25 162 013a 8795 ror r24 163 013c 9B8F std Y+27,r25 164 013e 8A8F std Y+26,r24 165 0140 8091 0000 lds r24,AdValueGyroRoll 166 0144 9091 0000 lds r25,(AdValueGyroRoll)+1 167 0148 0F97 sbiw r24,15 168 014a 04F4 brge .L20 169 014c 88E1 ldi r24,lo8(-1000) 170 014e 9CEF ldi r25,hi8(-1000) 171 0150 9093 0000 sts (GyroRoll)+1,r25 172 0154 8093 0000 sts GyroRoll,r24 173 .L20: 174 0158 8091 0000 lds r24,AdValueGyroRoll 175 015c 9091 0000 lds r25,(AdValueGyroRoll)+1 176 0160 0797 sbiw r24,7 177 0162 04F4 brge .L21 178 0164 80E3 ldi r24,lo8(-2000) 179 0166 98EF ldi r25,hi8(-2000) 180 0168 9093 0000 sts (GyroRoll)+1,r25 181 016c 8093 0000 sts GyroRoll,r24 182 .L21: 183 0170 4A30 cpi r20,lo8(10) 184 0172 01F4 brne .+2 185 0174 00C0 rjmp .L72 186 0176 8091 0000 lds r24,AdValueGyroRoll 187 017a 9091 0000 lds r25,(AdValueGyroRoll)+1 188 017e 815D subi r24,lo8(2001) 189 0180 9740 sbci r25,hi8(2001) 190 0182 04F0 brlt .L25 191 0184 88EE ldi r24,lo8(1000) 192 0186 93E0 ldi r25,hi8(1000) 193 0188 9093 0000 sts (GyroRoll)+1,r25 194 018c 8093 0000 sts GyroRoll,r24 195 .L25: 196 0190 8091 0000 lds r24,AdValueGyroRoll 197 0194 9091 0000 lds r25,(AdValueGyroRoll)+1 198 0198 805E subi r24,lo8(2016) 199 019a 9740 sbci r25,hi8(2016) 200 019c 04F0 brlt .+2 201 019e 00C0 rjmp .L66 202 .L24: 203 01a0 8091 0000 lds r24,AdValueGyroYaw 204 01a4 9091 0000 lds r25,(AdValueGyroYaw)+1 205 01a8 2091 0000 lds r18,AdBiasGyroYaw 206 01ac 3091 0000 lds r19,(AdBiasGyroYaw)+1 207 01b0 281B sub r18,r24 208 01b2 390B sbc r19,r25 209 01b4 3DA7 std Y+45,r19 210 01b6 2CA7 std Y+44,r18 211 01b8 3093 0000 sts (GyroYaw)+1,r19 212 01bc 2093 0000 sts GyroYaw,r18 213 01c0 6091 0000 lds r22,AdValueAccNick 214 01c4 7091 0000 lds r23,(AdValueAccNick)+1 215 01c8 8827 clr r24 216 01ca 77FD sbrc r23,7 217 01cc 8095 com r24 218 01ce 982F mov r25,r24 219 01d0 26E0 ldi r18,lo8(6) 220 01d2 30E0 ldi r19,hi8(6) 221 01d4 40E0 ldi r20,hlo8(6) 222 01d6 50E0 ldi r21,hhi8(6) 223 01d8 0E94 0000 call __mulsi3 224 01dc 7B01 movw r14,r22 225 01de 8C01 movw r16,r24 226 01e0 2091 0000 lds r18,AccNick 227 01e4 3091 0000 lds r19,(AccNick)+1 228 01e8 4427 clr r20 229 01ea 37FD sbrc r19,7 230 01ec 4095 com r20 231 01ee 542F mov r21,r20 232 01f0 CA01 movw r24,r20 233 01f2 B901 movw r22,r18 234 01f4 660F lsl r22 235 01f6 771F rol r23 236 01f8 881F rol r24 237 01fa 991F rol r25 238 01fc 620F add r22,r18 239 01fe 731F adc r23,r19 240 0200 841F adc r24,r20 241 0202 951F adc r25,r21 242 0204 6E0D add r22,r14 243 0206 7F1D adc r23,r15 244 0208 801F adc r24,r16 245 020a 911F adc r25,r17 246 020c 24E0 ldi r18,lo8(4) 247 020e 30E0 ldi r19,hi8(4) 248 0210 40E0 ldi r20,hlo8(4) 249 0212 50E0 ldi r21,hhi8(4) 250 0214 0E94 0000 call __divmodsi4 251 0218 3093 0000 sts (AccNick)+1,r19 252 021c 2093 0000 sts AccNick,r18 253 0220 6091 0000 lds r22,AdValueAccRoll 254 0224 7091 0000 lds r23,(AdValueAccRoll)+1 255 0228 8827 clr r24 256 022a 77FD sbrc r23,7 257 022c 8095 com r24 258 022e 982F mov r25,r24 259 0230 26E0 ldi r18,lo8(6) 260 0232 30E0 ldi r19,hi8(6) 261 0234 40E0 ldi r20,hlo8(6) 262 0236 50E0 ldi r21,hhi8(6) 263 0238 0E94 0000 call __mulsi3 264 023c 7B01 movw r14,r22 265 023e 8C01 movw r16,r24 266 0240 2091 0000 lds r18,AccRoll 267 0244 3091 0000 lds r19,(AccRoll)+1 268 0248 4427 clr r20 269 024a 37FD sbrc r19,7 270 024c 4095 com r20 271 024e 542F mov r21,r20 272 0250 CA01 movw r24,r20 273 0252 B901 movw r22,r18 274 0254 660F lsl r22 275 0256 771F rol r23 276 0258 881F rol r24 277 025a 991F rol r25 278 025c 620F add r22,r18 279 025e 731F adc r23,r19 280 0260 841F adc r24,r20 281 0262 951F adc r25,r21 282 0264 6E0D add r22,r14 283 0266 7F1D adc r23,r15 284 0268 801F adc r24,r16 285 026a 911F adc r25,r17 286 026c 24E0 ldi r18,lo8(4) 287 026e 30E0 ldi r19,hi8(4) 288 0270 40E0 ldi r20,hlo8(4) 289 0272 50E0 ldi r21,hhi8(4) 290 0274 0E94 0000 call __divmodsi4 291 0278 3093 0000 sts (AccRoll)+1,r19 292 027c 2093 0000 sts AccRoll,r18 293 0280 E090 0000 lds r14,AdValueAccTop 294 0284 F090 0000 lds r15,(AdValueAccTop)+1 295 0288 2091 0000 lds r18,AccTop 296 028c 3091 0000 lds r19,(AccTop)+1 297 0290 4427 clr r20 298 0292 37FD sbrc r19,7 299 0294 4095 com r20 300 0296 542F mov r21,r20 301 0298 CA01 movw r24,r20 302 029a B901 movw r22,r18 303 029c 660F lsl r22 304 029e 771F rol r23 305 02a0 881F rol r24 306 02a2 991F rol r25 307 02a4 620F add r22,r18 308 02a6 731F adc r23,r19 309 02a8 841F adc r24,r20 310 02aa 951F adc r25,r21 311 02ac 0027 clr r16 312 02ae F7FC sbrc r15,7 313 02b0 0095 com r16 314 02b2 102F mov r17,r16 315 02b4 6E0D add r22,r14 316 02b6 7F1D adc r23,r15 317 02b8 801F adc r24,r16 318 02ba 911F adc r25,r17 319 02bc 24E0 ldi r18,lo8(4) 320 02be 30E0 ldi r19,hi8(4) 321 02c0 40E0 ldi r20,hlo8(4) 322 02c2 50E0 ldi r21,hhi8(4) 323 02c4 0E94 0000 call __divmodsi4 324 02c8 3093 0000 sts (AccTop)+1,r19 325 02cc 2093 0000 sts AccTop,r18 326 02d0 2091 0000 lds r18,AdValueAccNick 327 02d4 3091 0000 lds r19,(AdValueAccNick)+1 328 02d8 C901 movw r24,r18 329 02da 880F lsl r24 330 02dc 991F rol r25 331 02de 820F add r24,r18 332 02e0 931F adc r25,r19 333 02e2 880F lsl r24 334 02e4 991F rol r25 335 02e6 AA27 clr r26 336 02e8 97FD sbrc r25,7 337 02ea A095 com r26 338 02ec BA2F mov r27,r26 339 02ee 2091 0000 lds r18,MeanAccNick 340 02f2 3091 0000 lds r19,(MeanAccNick)+1 341 02f6 4091 0000 lds r20,(MeanAccNick)+2 342 02fa 5091 0000 lds r21,(MeanAccNick)+3 343 02fe 280F add r18,r24 344 0300 391F adc r19,r25 345 0302 4A1F adc r20,r26 346 0304 5B1F adc r21,r27 347 0306 2093 0000 sts MeanAccNick,r18 348 030a 3093 0000 sts (MeanAccNick)+1,r19 349 030e 4093 0000 sts (MeanAccNick)+2,r20 350 0312 5093 0000 sts (MeanAccNick)+3,r21 351 0316 2091 0000 lds r18,AdValueAccRoll 352 031a 3091 0000 lds r19,(AdValueAccRoll)+1 353 031e C901 movw r24,r18 354 0320 880F lsl r24 355 0322 991F rol r25 356 0324 820F add r24,r18 357 0326 931F adc r25,r19 358 0328 880F lsl r24 359 032a 991F rol r25 360 032c AA27 clr r26 361 032e 97FD sbrc r25,7 362 0330 A095 com r26 363 0332 BA2F mov r27,r26 364 0334 2091 0000 lds r18,MeanAccRoll 365 0338 3091 0000 lds r19,(MeanAccRoll)+1 366 033c 4091 0000 lds r20,(MeanAccRoll)+2 367 0340 5091 0000 lds r21,(MeanAccRoll)+3 368 0344 280F add r18,r24 369 0346 391F adc r19,r25 370 0348 4A1F adc r20,r26 371 034a 5B1F adc r21,r27 372 034c 2093 0000 sts MeanAccRoll,r18 373 0350 3093 0000 sts (MeanAccRoll)+1,r19 374 0354 4093 0000 sts (MeanAccRoll)+2,r20 375 0358 5093 0000 sts (MeanAccRoll)+3,r21 376 035c 2091 0000 lds r18,AdValueAccNick 377 0360 3091 0000 lds r19,(AdValueAccNick)+1 378 0364 8091 0000 lds r24,NaviAccNick 379 0368 9091 0000 lds r25,(NaviAccNick)+1 380 036c 820F add r24,r18 381 036e 931F adc r25,r19 382 0370 9093 0000 sts (NaviAccNick)+1,r25 383 0374 8093 0000 sts NaviAccNick,r24 384 0378 2091 0000 lds r18,AdValueAccRoll 385 037c 3091 0000 lds r19,(AdValueAccRoll)+1 386 0380 8091 0000 lds r24,NaviAccRoll 387 0384 9091 0000 lds r25,(NaviAccRoll)+1 388 0388 820F add r24,r18 389 038a 931F adc r25,r19 390 038c 9093 0000 sts (NaviAccRoll)+1,r25 391 0390 8093 0000 sts NaviAccRoll,r24 392 0394 8091 0000 lds r24,NaviCntAcc 393 0398 9091 0000 lds r25,(NaviCntAcc)+1 394 039c 0196 adiw r24,1 395 039e 9093 0000 sts (NaviCntAcc)+1,r25 396 03a2 8093 0000 sts NaviCntAcc,r24 397 03a6 8091 7A00 lds r24,122 398 03aa 886C ori r24,lo8(-56) 399 03ac 8093 7A00 sts 122,r24 400 03b0 1092 0000 sts ADReady,__zero_reg__ 401 03b4 8091 0000 lds r24,ReadingIntegralGyroNick 402 03b8 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 403 03bc A091 0000 lds r26,(ReadingIntegralGyroNick)+2 404 03c0 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 405 03c4 8E8B std Y+22,r24 406 03c6 9F8B std Y+23,r25 407 03c8 A88F std Y+24,r26 408 03ca B98F std Y+25,r27 409 03cc 8091 0000 lds r24,ReadingIntegralGyroRoll 410 03d0 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 411 03d4 A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 412 03d8 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 413 03dc 8A8B std Y+18,r24 414 03de 9B8B std Y+19,r25 415 03e0 AC8B std Y+20,r26 416 03e2 BD8B std Y+21,r27 417 03e4 ACA5 ldd r26,Y+44 418 03e6 BDA5 ldd r27,Y+45 419 03e8 3D01 movw r6,r26 420 03ea 8824 clr r8 421 03ec 77FC sbrc r7,7 422 03ee 8094 com r8 423 03f0 982C mov r9,r8 424 03f2 E090 0000 lds r14,YawGyroHeading 425 03f6 F090 0000 lds r15,(YawGyroHeading)+1 426 03fa 0091 0000 lds r16,(YawGyroHeading)+2 427 03fe 1091 0000 lds r17,(YawGyroHeading)+3 428 0402 E60C add r14,r6 429 0404 F71C adc r15,r7 430 0406 081D adc r16,r8 431 0408 191D adc r17,r9 432 040a ECA2 std Y+36,r14 433 040c FDA2 std Y+37,r15 434 040e 0EA3 std Y+38,r16 435 0410 1FA3 std Y+39,r17 436 0412 E092 0000 sts YawGyroHeading,r14 437 0416 F092 0000 sts (YawGyroHeading)+1,r15 438 041a 0093 0000 sts (YawGyroHeading)+2,r16 439 041e 1093 0000 sts (YawGyroHeading)+3,r17 440 0422 2090 0000 lds r2,ReadingIntegralGyroYaw 441 0426 3090 0000 lds r3,(ReadingIntegralGyroYaw)+1 442 042a 4090 0000 lds r4,(ReadingIntegralGyroYaw)+2 443 042e 5090 0000 lds r5,(ReadingIntegralGyroYaw)+3 444 0432 260C add r2,r6 445 0434 371C adc r3,r7 446 0436 481C adc r4,r8 447 0438 591C adc r5,r9 448 043a 28A2 std Y+32,r2 449 043c 39A2 std Y+33,r3 450 043e 4AA2 std Y+34,r4 451 0440 5BA2 std Y+35,r5 452 0442 2092 0000 sts ReadingIntegralGyroYaw,r2 453 0446 3092 0000 sts (ReadingIntegralGyroYaw)+1,r3 454 044a 4092 0000 sts (ReadingIntegralGyroYaw)+2,r4 455 044e 5092 0000 sts (ReadingIntegralGyroYaw)+3,r5 456 0452 B091 0000 lds r27,LoopingNick 457 0456 BF8F std Y+31,r27 458 0458 BB23 tst r27 459 045a 01F4 brne .L26 460 045c 8091 0000 lds r24,LoopingRoll 461 0460 8823 tst r24 462 0462 01F4 brne .+2 463 0464 00C0 rjmp .L27 464 .L26: 465 0466 E091 0000 lds r30,ParamSet+8 466 046a E98B std Y+17,r30 467 .L28: 468 046c 1092 0000 sts (TrimNick)+1,__zero_reg__ 469 0470 1092 0000 sts TrimNick,__zero_reg__ 470 0474 1092 0000 sts (TrimRoll)+1,__zero_reg__ 471 0478 1092 0000 sts TrimRoll,__zero_reg__ 472 047c 1D86 std Y+13,__zero_reg__ 473 047e 1E86 std Y+14,__zero_reg__ 474 0480 1F86 std Y+15,__zero_reg__ 475 0482 188A std Y+16,__zero_reg__ 476 0484 1986 std Y+9,__zero_reg__ 477 0486 1A86 std Y+10,__zero_reg__ 478 0488 1B86 std Y+11,__zero_reg__ 479 048a 1C86 std Y+12,__zero_reg__ 480 .L40: 481 048c 8091 0000 lds r24,ParamSet+23 482 0490 A82E mov r10,r24 483 0492 BB24 clr r11 484 0494 CC24 clr r12 485 0496 DD24 clr r13 486 0498 C601 movw r24,r12 487 049a B501 movw r22,r10 488 049c 20E1 ldi r18,lo8(15120) 489 049e 3BE3 ldi r19,hi8(15120) 490 04a0 40E0 ldi r20,hlo8(15120) 491 04a2 50E0 ldi r21,hhi8(15120) 492 04a4 0E94 0000 call __mulsi3 493 04a8 3B01 movw r6,r22 494 04aa 4C01 movw r8,r24 495 04ac E616 cp r14,r22 496 04ae F706 cpc r15,r23 497 04b0 0807 cpc r16,r24 498 04b2 1907 cpc r17,r25 499 04b4 04F0 brlt .L41 500 04b6 C601 movw r24,r12 501 04b8 B501 movw r22,r10 502 04ba 20EF ldi r18,lo8(-15120) 503 04bc 34EC ldi r19,hi8(-15120) 504 04be 4FEF ldi r20,hlo8(-15120) 505 04c0 5FEF ldi r21,hhi8(-15120) 506 04c2 0E94 0000 call __mulsi3 507 04c6 6E0D add r22,r14 508 04c8 7F1D adc r23,r15 509 04ca 801F adc r24,r16 510 04cc 911F adc r25,r17 511 04ce 6093 0000 sts YawGyroHeading,r22 512 04d2 7093 0000 sts (YawGyroHeading)+1,r23 513 04d6 8093 0000 sts (YawGyroHeading)+2,r24 514 04da 9093 0000 sts (YawGyroHeading)+3,r25 515 04de 7B01 movw r14,r22 516 04e0 8C01 movw r16,r24 517 .L41: 518 04e2 17FD sbrc r17,7 519 04e4 00C0 rjmp .L73 520 .L42: 521 04e6 6090 0000 lds r6,TrimRoll 522 04ea 7090 0000 lds r7,(TrimRoll)+1 523 04ee 2A8D ldd r18,Y+26 524 04f0 3B8D ldd r19,Y+27 525 04f2 260D add r18,r6 526 04f4 371D adc r19,r7 527 04f6 7901 movw r14,r18 528 04f8 0027 clr r16 529 04fa F7FC sbrc r15,7 530 04fc 0095 com r16 531 04fe 102F mov r17,r16 532 0500 8091 0000 lds r24,ReadingIntegralGyroRoll2 533 0504 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 534 0508 A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 535 050c B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 536 0510 8E0D add r24,r14 537 0512 9F1D adc r25,r15 538 0514 A01F adc r26,r16 539 0516 B11F adc r27,r17 540 0518 8093 0000 sts ReadingIntegralGyroRoll2,r24 541 051c 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 542 0520 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 543 0524 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 544 0528 8091 0000 lds r24,AttitudeCorrectionRoll 545 052c 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 546 0530 281B sub r18,r24 547 0532 390B sbc r19,r25 548 0534 4427 clr r20 549 0536 37FD sbrc r19,7 550 0538 4095 com r20 551 053a 542F mov r21,r20 552 053c 8A89 ldd r24,Y+18 553 053e 9B89 ldd r25,Y+19 554 0540 AC89 ldd r26,Y+20 555 0542 BD89 ldd r27,Y+21 556 0544 280F add r18,r24 557 0546 391F adc r19,r25 558 0548 4A1F adc r20,r26 559 054a 5B1F adc r21,r27 560 054c 2093 0000 sts ReadingIntegralGyroRoll,r18 561 0550 3093 0000 sts (ReadingIntegralGyroRoll)+1,r19 562 0554 4093 0000 sts (ReadingIntegralGyroRoll)+2,r20 563 0558 5093 0000 sts (ReadingIntegralGyroRoll)+3,r21 564 055c E090 0000 lds r14,TurnOver180Roll 565 0560 F090 0000 lds r15,(TurnOver180Roll)+1 566 0564 0091 0000 lds r16,(TurnOver180Roll)+2 567 0568 1091 0000 lds r17,(TurnOver180Roll)+3 568 056c E216 cp r14,r18 569 056e F306 cpc r15,r19 570 0570 0407 cpc r16,r20 571 0572 1507 cpc r17,r21 572 0574 04F4 brge .L43 573 0576 80E1 ldi r24,lo8(10000) 574 0578 97E2 ldi r25,hi8(10000) 575 057a A0E0 ldi r26,hlo8(10000) 576 057c B0E0 ldi r27,hhi8(10000) 577 057e 8E19 sub r24,r14 578 0580 9F09 sbc r25,r15 579 0582 A00B sbc r26,r16 580 0584 B10B sbc r27,r17 581 0586 8093 0000 sts ReadingIntegralGyroRoll,r24 582 058a 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 583 058e A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 584 0592 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 585 0596 8093 0000 sts ReadingIntegralGyroRoll2,r24 586 059a 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 587 059e A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 588 05a2 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 589 .L43: 590 05a6 8827 clr r24 591 05a8 9927 clr r25 592 05aa DC01 movw r26,r24 593 05ac 8E19 sub r24,r14 594 05ae 9F09 sbc r25,r15 595 05b0 A00B sbc r26,r16 596 05b2 B10B sbc r27,r17 597 05b4 A090 0000 lds r10,ReadingIntegralGyroRoll 598 05b8 B090 0000 lds r11,(ReadingIntegralGyroRoll)+1 599 05bc C090 0000 lds r12,(ReadingIntegralGyroRoll)+2 600 05c0 D090 0000 lds r13,(ReadingIntegralGyroRoll)+3 601 05c4 A816 cp r10,r24 602 05c6 B906 cpc r11,r25 603 05c8 CA06 cpc r12,r26 604 05ca DB06 cpc r13,r27 605 05cc 04F4 brge .L44 606 05ce D801 movw r26,r16 607 05d0 C701 movw r24,r14 608 05d2 8051 subi r24,lo8(-(-10000)) 609 05d4 9742 sbci r25,hi8(-(-10000)) 610 05d6 A040 sbci r26,hlo8(-(-10000)) 611 05d8 B040 sbci r27,hhi8(-(-10000)) 612 05da 8093 0000 sts ReadingIntegralGyroRoll,r24 613 05de 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 614 05e2 A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 615 05e6 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 616 05ea 8093 0000 sts ReadingIntegralGyroRoll2,r24 617 05ee 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 618 05f2 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 619 05f6 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 620 05fa 5C01 movw r10,r24 621 05fc 6D01 movw r12,r26 622 .L44: 623 05fe 8090 0000 lds r8,TrimNick 624 0602 9090 0000 lds r9,(TrimNick)+1 625 0606 2C8D ldd r18,Y+28 626 0608 3D8D ldd r19,Y+29 627 060a 280D add r18,r8 628 060c 391D adc r19,r9 629 060e 7901 movw r14,r18 630 0610 0027 clr r16 631 0612 F7FC sbrc r15,7 632 0614 0095 com r16 633 0616 102F mov r17,r16 634 0618 8091 0000 lds r24,ReadingIntegralGyroNick2 635 061c 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 636 0620 A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 637 0624 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 638 0628 8E0D add r24,r14 639 062a 9F1D adc r25,r15 640 062c A01F adc r26,r16 641 062e B11F adc r27,r17 642 0630 8093 0000 sts ReadingIntegralGyroNick2,r24 643 0634 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 644 0638 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 645 063c B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 646 0640 8091 0000 lds r24,AttitudeCorrectionNick 647 0644 9091 0000 lds r25,(AttitudeCorrectionNick)+1 648 0648 281B sub r18,r24 649 064a 390B sbc r19,r25 650 064c 4427 clr r20 651 064e 37FD sbrc r19,7 652 0650 4095 com r20 653 0652 542F mov r21,r20 654 0654 8E89 ldd r24,Y+22 655 0656 9F89 ldd r25,Y+23 656 0658 A88D ldd r26,Y+24 657 065a B98D ldd r27,Y+25 658 065c 280F add r18,r24 659 065e 391F adc r19,r25 660 0660 4A1F adc r20,r26 661 0662 5B1F adc r21,r27 662 0664 2093 0000 sts ReadingIntegralGyroNick,r18 663 0668 3093 0000 sts (ReadingIntegralGyroNick)+1,r19 664 066c 4093 0000 sts (ReadingIntegralGyroNick)+2,r20 665 0670 5093 0000 sts (ReadingIntegralGyroNick)+3,r21 666 0674 E090 0000 lds r14,TurnOver180Nick 667 0678 F090 0000 lds r15,(TurnOver180Nick)+1 668 067c 0091 0000 lds r16,(TurnOver180Nick)+2 669 0680 1091 0000 lds r17,(TurnOver180Nick)+3 670 0684 E216 cp r14,r18 671 0686 F306 cpc r15,r19 672 0688 0407 cpc r16,r20 673 068a 1507 cpc r17,r21 674 068c 04F4 brge .L45 675 068e 88EA ldi r24,lo8(25000) 676 0690 91E6 ldi r25,hi8(25000) 677 0692 A0E0 ldi r26,hlo8(25000) 678 0694 B0E0 ldi r27,hhi8(25000) 679 0696 8E19 sub r24,r14 680 0698 9F09 sbc r25,r15 681 069a A00B sbc r26,r16 682 069c B10B sbc r27,r17 683 069e 8093 0000 sts ReadingIntegralGyroNick,r24 684 06a2 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 685 06a6 A093 0000 sts (ReadingIntegralGyroNick)+2,r26 686 06aa B093 0000 sts (ReadingIntegralGyroNick)+3,r27 687 06ae 8093 0000 sts ReadingIntegralGyroNick2,r24 688 06b2 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 689 06b6 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 690 06ba B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 691 .L45: 692 06be 8827 clr r24 693 06c0 9927 clr r25 694 06c2 DC01 movw r26,r24 695 06c4 8E19 sub r24,r14 696 06c6 9F09 sbc r25,r15 697 06c8 A00B sbc r26,r16 698 06ca B10B sbc r27,r17 699 06cc 2091 0000 lds r18,ReadingIntegralGyroNick 700 06d0 3091 0000 lds r19,(ReadingIntegralGyroNick)+1 701 06d4 4091 0000 lds r20,(ReadingIntegralGyroNick)+2 702 06d8 5091 0000 lds r21,(ReadingIntegralGyroNick)+3 703 06dc 2817 cp r18,r24 704 06de 3907 cpc r19,r25 705 06e0 4A07 cpc r20,r26 706 06e2 5B07 cpc r21,r27 707 06e4 04F4 brge .L46 708 06e6 D801 movw r26,r16 709 06e8 C701 movw r24,r14 710 06ea 885A subi r24,lo8(-(-25000)) 711 06ec 9146 sbci r25,hi8(-(-25000)) 712 06ee A040 sbci r26,hlo8(-(-25000)) 713 06f0 B040 sbci r27,hhi8(-(-25000)) 714 06f2 8093 0000 sts ReadingIntegralGyroNick,r24 715 06f6 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 716 06fa A093 0000 sts (ReadingIntegralGyroNick)+2,r26 717 06fe B093 0000 sts (ReadingIntegralGyroNick)+3,r27 718 0702 8093 0000 sts ReadingIntegralGyroNick2,r24 719 0706 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 720 070a A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 721 070e B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 722 0712 9C01 movw r18,r24 723 0714 AD01 movw r20,r26 724 .L46: 725 0716 2092 0000 sts IntegralGyroYaw,r2 726 071a 3092 0000 sts (IntegralGyroYaw)+1,r3 727 071e 4092 0000 sts (IntegralGyroYaw)+2,r4 728 0722 5092 0000 sts (IntegralGyroYaw)+3,r5 729 0726 2093 0000 sts IntegralGyroNick,r18 730 072a 3093 0000 sts (IntegralGyroNick)+1,r19 731 072e 4093 0000 sts (IntegralGyroNick)+2,r20 732 0732 5093 0000 sts (IntegralGyroNick)+3,r21 733 0736 A092 0000 sts IntegralGyroRoll,r10 734 073a B092 0000 sts (IntegralGyroRoll)+1,r11 735 073e C092 0000 sts (IntegralGyroRoll)+2,r12 736 0742 D092 0000 sts (IntegralGyroRoll)+3,r13 737 0746 8091 0000 lds r24,ReadingIntegralGyroNick2 738 074a 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 739 074e A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 740 0752 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 741 0756 8093 0000 sts IntegralGyroNick2,r24 742 075a 9093 0000 sts (IntegralGyroNick2)+1,r25 743 075e A093 0000 sts (IntegralGyroNick2)+2,r26 744 0762 B093 0000 sts (IntegralGyroNick2)+3,r27 745 0766 8091 0000 lds r24,ReadingIntegralGyroRoll2 746 076a 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 747 076e A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 748 0772 B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 749 0776 8093 0000 sts IntegralGyroRoll2,r24 750 077a 9093 0000 sts (IntegralGyroRoll2)+1,r25 751 077e A093 0000 sts (IntegralGyroRoll2)+2,r26 752 0782 B093 0000 sts (IntegralGyroRoll2)+3,r27 753 0786 6091 0000 lds r22,FCParam+6 754 078a 6623 tst r22 755 078c 01F4 brne .+2 756 078e 00C0 rjmp .L47 757 0790 4091 0000 lds r20,HiResGyroNick 758 0794 5091 0000 lds r21,(HiResGyroNick)+1 759 0798 8091 0000 lds r24,Last_GyroNick.2093 760 079c 9091 0000 lds r25,(Last_GyroNick.2093)+1 761 07a0 481B sub r20,r24 762 07a2 590B sbc r21,r25 763 07a4 2091 0000 lds r18,HiResGyroNick 764 07a8 3091 0000 lds r19,(HiResGyroNick)+1 765 07ac 280F add r18,r24 766 07ae 391F adc r19,r25 767 07b0 37FD sbrc r19,7 768 07b2 00C0 rjmp .L74 769 .L48: 770 07b4 3595 asr r19 771 07b6 2795 ror r18 772 07b8 3093 0000 sts (Last_GyroNick.2093)+1,r19 773 07bc 2093 0000 sts Last_GyroNick.2093,r18 774 07c0 CA01 movw r24,r20 775 07c2 4138 cpi r20,129 776 07c4 5105 cpc r21,__zero_reg__ 777 07c6 04F0 brlt .+2 778 07c8 00C0 rjmp .L75 779 07ca E62F mov r30,r22 780 07cc F0E0 ldi r31,lo8(0) 781 07ce AFEF ldi r26,hi8(-128) 782 07d0 8038 cpi r24,lo8(-128) 783 07d2 9A07 cpc r25,r26 784 07d4 04F4 brge .+2 785 07d6 00C0 rjmp .L76 786 .L50: 787 07d8 8E9F mul r24,r30 788 07da B001 movw r22,r0 789 07dc 8F9F mul r24,r31 790 07de 700D add r23,r0 791 07e0 9E9F mul r25,r30 792 07e2 700D add r23,r0 793 07e4 1124 clr r1 794 07e6 9B01 movw r18,r22 795 07e8 77FD sbrc r23,7 796 07ea 00C0 rjmp .L77 797 .L51: 798 07ec 3595 asr r19 799 07ee 2795 ror r18 800 07f0 3595 asr r19 801 07f2 2795 ror r18 802 07f4 3595 asr r19 803 07f6 2795 ror r18 804 07f8 3595 asr r19 805 07fa 2795 ror r18 806 07fc 8091 0000 lds r24,GyroNick 807 0800 9091 0000 lds r25,(GyroNick)+1 808 0804 820F add r24,r18 809 0806 931F adc r25,r19 810 0808 9093 0000 sts (GyroNick)+1,r25 811 080c 8093 0000 sts GyroNick,r24 812 0810 4091 0000 lds r20,HiResGyroRoll 813 0814 5091 0000 lds r21,(HiResGyroRoll)+1 814 0818 8091 0000 lds r24,Last_GyroRoll.2092 815 081c 9091 0000 lds r25,(Last_GyroRoll.2092)+1 816 0820 481B sub r20,r24 817 0822 590B sbc r21,r25 818 0824 2091 0000 lds r18,HiResGyroRoll 819 0828 3091 0000 lds r19,(HiResGyroRoll)+1 820 082c 280F add r18,r24 821 082e 391F adc r19,r25 822 0830 37FD sbrc r19,7 823 0832 00C0 rjmp .L78 824 .L52: 825 0834 3595 asr r19 826 0836 2795 ror r18 827 0838 3093 0000 sts (Last_GyroRoll.2092)+1,r19 828 083c 2093 0000 sts Last_GyroRoll.2092,r18 829 0840 CA01 movw r24,r20 830 0842 4138 cpi r20,129 831 0844 5105 cpc r21,__zero_reg__ 832 0846 04F0 brlt .+2 833 0848 00C0 rjmp .L79 834 084a BFEF ldi r27,hi8(-128) 835 084c 8038 cpi r24,lo8(-128) 836 084e 9B07 cpc r25,r27 837 0850 04F4 brge .+2 838 0852 00C0 rjmp .L80 839 .L54: 840 0854 8E9F mul r24,r30 841 0856 A001 movw r20,r0 842 0858 8F9F mul r24,r31 843 085a 500D add r21,r0 844 085c 9E9F mul r25,r30 845 085e 500D add r21,r0 846 0860 1124 clr r1 847 0862 9A01 movw r18,r20 848 0864 57FD sbrc r21,7 849 0866 00C0 rjmp .L81 850 .L55: 851 0868 3595 asr r19 852 086a 2795 ror r18 853 086c 3595 asr r19 854 086e 2795 ror r18 855 0870 3595 asr r19 856 0872 2795 ror r18 857 0874 3595 asr r19 858 0876 2795 ror r18 859 0878 8091 0000 lds r24,GyroRoll 860 087c 9091 0000 lds r25,(GyroRoll)+1 861 0880 820F add r24,r18 862 0882 931F adc r25,r19 863 0884 9093 0000 sts (GyroRoll)+1,r25 864 0888 8093 0000 sts GyroRoll,r24 865 088c 8091 0000 lds r24,HiResGyroNick 866 0890 9091 0000 lds r25,(HiResGyroNick)+1 867 0894 680F add r22,r24 868 0896 791F adc r23,r25 869 0898 7093 0000 sts (HiResGyroNick)+1,r23 870 089c 6093 0000 sts HiResGyroNick,r22 871 08a0 8091 0000 lds r24,HiResGyroRoll 872 08a4 9091 0000 lds r25,(HiResGyroRoll)+1 873 08a8 480F add r20,r24 874 08aa 591F adc r21,r25 875 08ac 5093 0000 sts (HiResGyroRoll)+1,r21 876 08b0 4093 0000 sts HiResGyroRoll,r20 877 .L47: 878 08b4 EC8D ldd r30,Y+28 879 08b6 FD8D ldd r31,Y+29 880 08b8 1E16 cp __zero_reg__,r30 881 08ba 1F06 cpc __zero_reg__,r31 882 08bc 04F0 brlt .+2 883 08be 00C0 rjmp .L56 884 08c0 6D85 ldd r22,Y+13 885 08c2 7E85 ldd r23,Y+14 886 08c4 8F85 ldd r24,Y+15 887 08c6 9889 ldd r25,Y+16 888 08c8 20E4 ldi r18,lo8(64) 889 08ca 30E0 ldi r19,hi8(64) 890 08cc 40E0 ldi r20,hlo8(64) 891 08ce 50E0 ldi r21,hhi8(64) 892 08d0 0E94 0000 call __divmodsi4 893 08d4 820E add r8,r18 894 08d6 931E adc r9,r19 895 08d8 9092 0000 sts (TrimNick)+1,r9 896 08dc 8092 0000 sts TrimNick,r8 897 .L57: 898 08e0 2A8D ldd r18,Y+26 899 08e2 3B8D ldd r19,Y+27 900 08e4 1216 cp __zero_reg__,r18 901 08e6 1306 cpc __zero_reg__,r19 902 08e8 04F0 brlt .+2 903 08ea 00C0 rjmp .L58 904 08ec 6985 ldd r22,Y+9 905 08ee 7A85 ldd r23,Y+10 906 08f0 8B85 ldd r24,Y+11 907 08f2 9C85 ldd r25,Y+12 908 08f4 20E4 ldi r18,lo8(64) 909 08f6 30E0 ldi r19,hi8(64) 910 08f8 40E0 ldi r20,hlo8(64) 911 08fa 50E0 ldi r21,hhi8(64) 912 08fc 0E94 0000 call __divmodsi4 913 0900 620E add r6,r18 914 0902 731E adc r7,r19 915 0904 7092 0000 sts (TrimRoll)+1,r7 916 0908 6092 0000 sts TrimRoll,r6 917 .L59: 918 090c 3989 ldd r19,Y+17 919 090e 37FD sbrc r19,7 920 0910 00C0 rjmp .L82 921 .L64: 922 /* epilogue start */ 923 0912 AD96 adiw r28,45 924 0914 0FB6 in __tmp_reg__,__SREG__ 925 0916 F894 cli 926 0918 DEBF out __SP_H__,r29 927 091a 0FBE out __SREG__,__tmp_reg__ 928 091c CDBF out __SP_L__,r28 929 091e CF91 pop r28 930 0920 DF91 pop r29 931 0922 1F91 pop r17 932 0924 0F91 pop r16 933 0926 FF90 pop r15 934 0928 EF90 pop r14 935 092a DF90 pop r13 936 092c CF90 pop r12 937 092e BF90 pop r11 938 0930 AF90 pop r10 939 0932 9F90 pop r9 940 0934 8F90 pop r8 941 0936 7F90 pop r7 942 0938 6F90 pop r6 943 093a 5F90 pop r5 944 093c 4F90 pop r4 945 093e 3F90 pop r3 946 0940 2F90 pop r2 947 0942 0895 ret 948 .L72: 949 0944 8091 0000 lds r24,AdValueGyroRoll 950 0948 9091 0000 lds r25,(AdValueGyroRoll)+1 951 094c 835F subi r24,lo8(1011) 952 094e 9340 sbci r25,hi8(1011) 953 0950 04F0 brlt .L23 954 0952 88EE ldi r24,lo8(1000) 955 0954 93E0 ldi r25,hi8(1000) 956 0956 9093 0000 sts (GyroRoll)+1,r25 957 095a 8093 0000 sts GyroRoll,r24 958 .L23: 959 095e 8091 0000 lds r24,AdValueGyroRoll 960 0962 9091 0000 lds r25,(AdValueGyroRoll)+1 961 0966 8A5F subi r24,lo8(1018) 962 0968 9340 sbci r25,hi8(1018) 963 096a 04F4 brge .+2 964 096c 00C0 rjmp .L24 965 .L66: 966 096e 80ED ldi r24,lo8(2000) 967 0970 97E0 ldi r25,hi8(2000) 968 0972 9093 0000 sts (GyroRoll)+1,r25 969 0976 8093 0000 sts GyroRoll,r24 970 097a 00C0 rjmp .L24 971 .L69: 972 097c 8091 0000 lds r24,AdValueGyroNick 973 0980 9091 0000 lds r25,(AdValueGyroNick)+1 974 0984 835F subi r24,lo8(1011) 975 0986 9340 sbci r25,hi8(1011) 976 0988 04F0 brlt .L15 977 098a 88EE ldi r24,lo8(1000) 978 098c 93E0 ldi r25,hi8(1000) 979 098e 9093 0000 sts (GyroNick)+1,r25 980 0992 8093 0000 sts GyroNick,r24 981 .L15: 982 0996 8091 0000 lds r24,AdValueGyroNick 983 099a 9091 0000 lds r25,(AdValueGyroNick)+1 984 099e 8A5F subi r24,lo8(1018) 985 09a0 9340 sbci r25,hi8(1018) 986 09a2 04F4 brge .+2 987 09a4 00C0 rjmp .L16 988 .L65: 989 09a6 80ED ldi r24,lo8(2000) 990 09a8 97E0 ldi r25,hi8(2000) 991 09aa 9093 0000 sts (GyroNick)+1,r25 992 09ae 8093 0000 sts GyroNick,r24 993 09b2 8091 0000 lds r24,HiResGyroRoll 994 09b6 9091 0000 lds r25,(HiResGyroRoll)+1 995 09ba 97FF sbrs r25,7 996 09bc 00C0 rjmp .L18 997 .L70: 998 09be 0796 adiw r24,7 999 09c0 00C0 rjmp .L18 1000 .L80: 1001 09c2 80E8 ldi r24,lo8(-128) 1002 09c4 9FEF ldi r25,hi8(-128) 1003 09c6 00C0 rjmp .L54 1004 .L79: 1005 09c8 80E8 ldi r24,lo8(128) 1006 09ca 90E0 ldi r25,hi8(128) 1007 09cc BFEF ldi r27,hi8(-128) 1008 09ce 8038 cpi r24,lo8(-128) 1009 09d0 9B07 cpc r25,r27 1010 09d2 04F0 brlt .+2 1011 09d4 00C0 rjmp .L54 1012 09d6 00C0 rjmp .L80 1013 .L76: 1014 09d8 80E8 ldi r24,lo8(-128) 1015 09da 9FEF ldi r25,hi8(-128) 1016 09dc 00C0 rjmp .L50 1017 .L75: 1018 09de 80E8 ldi r24,lo8(128) 1019 09e0 90E0 ldi r25,hi8(128) 1020 09e2 E62F mov r30,r22 1021 09e4 F0E0 ldi r31,lo8(0) 1022 09e6 AFEF ldi r26,hi8(-128) 1023 09e8 8038 cpi r24,lo8(-128) 1024 09ea 9A07 cpc r25,r26 1025 09ec 04F0 brlt .+2 1026 09ee 00C0 rjmp .L50 1027 09f0 00C0 rjmp .L76 1028 .L27: 1029 09f2 F091 0000 lds r31,ParamSet+8 1030 09f6 F98B std Y+17,r31 1031 09f8 F6FF sbrs r31,6 1032 09fa 00C0 rjmp .L28 1033 09fc 2E89 ldd r18,Y+22 1034 09fe 3F89 ldd r19,Y+23 1035 0a00 488D ldd r20,Y+24 1036 0a02 598D ldd r21,Y+25 1037 0a04 2934 cpi r18,lo8(93001) 1038 0a06 6BE6 ldi r22,hi8(93001) 1039 0a08 3607 cpc r19,r22 1040 0a0a 61E0 ldi r22,hlo8(93001) 1041 0a0c 4607 cpc r20,r22 1042 0a0e 60E0 ldi r22,hhi8(93001) 1043 0a10 5607 cpc r21,r22 1044 0a12 04F0 brlt .L29 1045 0a14 28E4 ldi r18,lo8(93000) 1046 0a16 3BE6 ldi r19,hi8(93000) 1047 0a18 41E0 ldi r20,hlo8(93000) 1048 0a1a 50E0 ldi r21,hhi8(93000) 1049 .L29: 1050 0a1c 2D83 std Y+5,r18 1051 0a1e 3E83 std Y+6,r19 1052 0a20 4F83 std Y+7,r20 1053 0a22 5887 std Y+8,r21 1054 0a24 285B subi r18,lo8(-93000) 1055 0a26 3449 sbci r19,hi8(-93000) 1056 0a28 4E4F sbci r20,hlo8(-93000) 1057 0a2a 5F4F sbci r21,hhi8(-93000) 1058 0a2c 04F4 brge .L30 1059 0a2e 88EB ldi r24,lo8(-93000) 1060 0a30 94E9 ldi r25,hi8(-93000) 1061 0a32 AEEF ldi r26,hlo8(-93000) 1062 0a34 BFEF ldi r27,hhi8(-93000) 1063 0a36 8D83 std Y+5,r24 1064 0a38 9E83 std Y+6,r25 1065 0a3a AF83 std Y+7,r26 1066 0a3c B887 std Y+8,r27 1067 .L30: 1068 0a3e 2A89 ldd r18,Y+18 1069 0a40 3B89 ldd r19,Y+19 1070 0a42 4C89 ldd r20,Y+20 1071 0a44 5D89 ldd r21,Y+21 1072 0a46 2934 cpi r18,lo8(93001) 1073 0a48 9BE6 ldi r25,hi8(93001) 1074 0a4a 3907 cpc r19,r25 1075 0a4c 91E0 ldi r25,hlo8(93001) 1076 0a4e 4907 cpc r20,r25 1077 0a50 90E0 ldi r25,hhi8(93001) 1078 0a52 5907 cpc r21,r25 1079 0a54 04F0 brlt .L31 1080 0a56 28E4 ldi r18,lo8(93000) 1081 0a58 3BE6 ldi r19,hi8(93000) 1082 0a5a 41E0 ldi r20,hlo8(93000) 1083 0a5c 50E0 ldi r21,hhi8(93000) 1084 .L31: 1085 0a5e 2983 std Y+1,r18 1086 0a60 3A83 std Y+2,r19 1087 0a62 4B83 std Y+3,r20 1088 0a64 5C83 std Y+4,r21 1089 0a66 285B subi r18,lo8(-93000) 1090 0a68 3449 sbci r19,hi8(-93000) 1091 0a6a 4E4F sbci r20,hlo8(-93000) 1092 0a6c 5F4F sbci r21,hhi8(-93000) 1093 0a6e 04F4 brge .L32 1094 0a70 88EB ldi r24,lo8(-93000) 1095 0a72 94E9 ldi r25,hi8(-93000) 1096 0a74 AEEF ldi r26,hlo8(-93000) 1097 0a76 BFEF ldi r27,hhi8(-93000) 1098 0a78 8983 std Y+1,r24 1099 0a7a 9A83 std Y+2,r25 1100 0a7c AB83 std Y+3,r26 1101 0a7e BC83 std Y+4,r27 1102 .L32: 1103 0a80 AA8D ldd r26,Y+26 1104 0a82 BB8D ldd r27,Y+27 1105 0a84 9D01 movw r18,r26 1106 0a86 4427 clr r20 1107 0a88 37FD sbrc r19,7 1108 0a8a 4095 com r20 1109 0a8c 542F mov r21,r20 1110 0a8e 6D81 ldd r22,Y+5 1111 0a90 7E81 ldd r23,Y+6 1112 0a92 8F81 ldd r24,Y+7 1113 0a94 9885 ldd r25,Y+8 1114 0a96 0E94 0000 call __mulsi3 1115 0a9a 20E0 ldi r18,lo8(2048) 1116 0a9c 38E0 ldi r19,hi8(2048) 1117 0a9e 40E0 ldi r20,hlo8(2048) 1118 0aa0 50E0 ldi r21,hhi8(2048) 1119 0aa2 0E94 0000 call __divmodsi4 1120 0aa6 CA01 movw r24,r20 1121 0aa8 A090 0000 lds r10,FCParam+25 1122 0aac BB24 clr r11 1123 0aae CC24 clr r12 1124 0ab0 DD24 clr r13 1125 0ab2 B901 movw r22,r18 1126 0ab4 A601 movw r20,r12 1127 0ab6 9501 movw r18,r10 1128 0ab8 0E94 0000 call __mulsi3 1129 0abc 20E0 ldi r18,lo8(4096) 1130 0abe 30E1 ldi r19,hi8(4096) 1131 0ac0 40E0 ldi r20,hlo8(4096) 1132 0ac2 50E0 ldi r21,hhi8(4096) 1133 0ac4 0E94 0000 call __divmodsi4 1134 0ac8 7901 movw r14,r18 1135 0aca 8A01 movw r16,r20 1136 0acc 2AA7 std Y+42,r18 1137 0ace FBA6 std Y+43,r15 1138 0ad0 EC8D ldd r30,Y+28 1139 0ad2 FD8D ldd r31,Y+29 1140 0ad4 9F01 movw r18,r30 1141 0ad6 4427 clr r20 1142 0ad8 37FD sbrc r19,7 1143 0ada 4095 com r20 1144 0adc 542F mov r21,r20 1145 0ade 6981 ldd r22,Y+1 1146 0ae0 7A81 ldd r23,Y+2 1147 0ae2 8B81 ldd r24,Y+3 1148 0ae4 9C81 ldd r25,Y+4 1149 0ae6 0E94 0000 call __mulsi3 1150 0aea 20E0 ldi r18,lo8(2048) 1151 0aec 38E0 ldi r19,hi8(2048) 1152 0aee 40E0 ldi r20,hlo8(2048) 1153 0af0 50E0 ldi r21,hhi8(2048) 1154 0af2 0E94 0000 call __divmodsi4 1155 0af6 CA01 movw r24,r20 1156 0af8 B901 movw r22,r18 1157 0afa A601 movw r20,r12 1158 0afc 9501 movw r18,r10 1159 0afe 0E94 0000 call __mulsi3 1160 0b02 20E0 ldi r18,lo8(4096) 1161 0b04 30E1 ldi r19,hi8(4096) 1162 0b06 40E0 ldi r20,hlo8(4096) 1163 0b08 50E0 ldi r21,hhi8(4096) 1164 0b0a 0E94 0000 call __divmodsi4 1165 0b0e C901 movw r24,r18 1166 0b10 DA01 movw r26,r20 1167 0b12 28A7 std Y+40,r18 1168 0b14 99A7 std Y+41,r25 1169 0b16 5C01 movw r10,r24 1170 0b18 6D01 movw r12,r26 1171 0b1a AE18 sub r10,r14 1172 0b1c BF08 sbc r11,r15 1173 0b1e C00A sbc r12,r16 1174 0b20 D10A sbc r13,r17 1175 0b22 8CA1 ldd r24,Y+36 1176 0b24 9DA1 ldd r25,Y+37 1177 0b26 AEA1 ldd r26,Y+38 1178 0b28 BFA1 ldd r27,Y+39 1179 0b2a 8A0D add r24,r10 1180 0b2c 9B1D adc r25,r11 1181 0b2e AC1D adc r26,r12 1182 0b30 BD1D adc r27,r13 1183 0b32 8093 0000 sts YawGyroHeading,r24 1184 0b36 9093 0000 sts (YawGyroHeading)+1,r25 1185 0b3a A093 0000 sts (YawGyroHeading)+2,r26 1186 0b3e B093 0000 sts (YawGyroHeading)+3,r27 1187 0b42 9091 0000 lds r25,FCParam+26 1188 0b46 9E8F std Y+30,r25 1189 0b48 9923 tst r25 1190 0b4a 01F4 brne .L33 1191 0b4c C601 movw r24,r12 1192 0b4e B501 movw r22,r10 1193 0b50 22E0 ldi r18,lo8(2) 1194 0b52 30E0 ldi r19,hi8(2) 1195 0b54 40E0 ldi r20,hlo8(2) 1196 0b56 50E0 ldi r21,hhi8(2) 1197 0b58 0E94 0000 call __divmodsi4 1198 0b5c 88A1 ldd r24,Y+32 1199 0b5e 99A1 ldd r25,Y+33 1200 0b60 AAA1 ldd r26,Y+34 1201 0b62 BBA1 ldd r27,Y+35 1202 0b64 821B sub r24,r18 1203 0b66 930B sbc r25,r19 1204 0b68 A40B sbc r26,r20 1205 0b6a B50B sbc r27,r21 1206 0b6c 8093 0000 sts ReadingIntegralGyroYaw,r24 1207 0b70 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 1208 0b74 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 1209 0b78 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 1210 0b7c 1C01 movw r2,r24 1211 0b7e 2D01 movw r4,r26 1212 .L33: 1213 0b80 A60C add r10,r6 1214 0b82 B71C adc r11,r7 1215 0b84 C81C adc r12,r8 1216 0b86 D91C adc r13,r9 1217 0b88 C601 movw r24,r12 1218 0b8a B501 movw r22,r10 1219 0b8c 2D81 ldd r18,Y+5 1220 0b8e 3E81 ldd r19,Y+6 1221 0b90 4F81 ldd r20,Y+7 1222 0b92 5885 ldd r21,Y+8 1223 0b94 0E94 0000 call __mulsi3 1224 0b98 20E0 ldi r18,lo8(2048) 1225 0b9a 38E0 ldi r19,hi8(2048) 1226 0b9c 40E0 ldi r20,hlo8(2048) 1227 0b9e 50E0 ldi r21,hhi8(2048) 1228 0ba0 0E94 0000 call __divmodsi4 1229 0ba4 CA01 movw r24,r20 1230 0ba6 E090 0000 lds r14,FCParam+24 1231 0baa FF24 clr r15 1232 0bac 00E0 ldi r16,lo8(0) 1233 0bae 10E0 ldi r17,hi8(0) 1234 0bb0 B901 movw r22,r18 1235 0bb2 A801 movw r20,r16 1236 0bb4 9701 movw r18,r14 1237 0bb6 0E94 0000 call __mulsi3 1238 0bba 20E0 ldi r18,lo8(4096) 1239 0bbc 30E1 ldi r19,hi8(4096) 1240 0bbe 40E0 ldi r20,hlo8(4096) 1241 0bc0 50E0 ldi r21,hhi8(4096) 1242 0bc2 0E94 0000 call __divmodsi4 1243 0bc6 3901 movw r6,r18 1244 0bc8 4A01 movw r8,r20 1245 0bca C601 movw r24,r12 1246 0bcc B501 movw r22,r10 1247 0bce 2981 ldd r18,Y+1 1248 0bd0 3A81 ldd r19,Y+2 1249 0bd2 4B81 ldd r20,Y+3 1250 0bd4 5C81 ldd r21,Y+4 1251 0bd6 0E94 0000 call __mulsi3 1252 0bda 20E0 ldi r18,lo8(2048) 1253 0bdc 38E0 ldi r19,hi8(2048) 1254 0bde 40E0 ldi r20,hlo8(2048) 1255 0be0 50E0 ldi r21,hhi8(2048) 1256 0be2 0E94 0000 call __divmodsi4 1257 0be6 CA01 movw r24,r20 1258 0be8 B901 movw r22,r18 1259 0bea A801 movw r20,r16 1260 0bec 9701 movw r18,r14 1261 0bee 0E94 0000 call __mulsi3 1262 0bf2 20E0 ldi r18,lo8(4096) 1263 0bf4 30E1 ldi r19,hi8(4096) 1264 0bf6 40E0 ldi r20,hlo8(4096) 1265 0bf8 50E0 ldi r21,hhi8(4096) 1266 0bfa 0E94 0000 call __divmodsi4 1267 0bfe 7901 movw r14,r18 1268 0c00 8A01 movw r16,r20 1269 0c02 ACA5 ldd r26,Y+44 1270 0c04 BDA5 ldd r27,Y+45 1271 0c06 A134 cpi r26,65 1272 0c08 B105 cpc r27,__zero_reg__ 1273 0c0a 04F0 brlt .L34 1274 0c0c D401 movw r26,r8 1275 0c0e C301 movw r24,r6 1276 0c10 97FC sbrc r9,7 1277 0c12 00C0 rjmp .L83 1278 .L36: 1279 0c14 8138 cpi r24,lo8(129) 1280 0c16 9105 cpc r25,__zero_reg__ 1281 0c18 A105 cpc r26,__zero_reg__ 1282 0c1a B105 cpc r27,__zero_reg__ 1283 0c1c 04F0 brlt .+2 1284 0c1e 00C0 rjmp .L35 1285 0c20 D801 movw r26,r16 1286 0c22 C701 movw r24,r14 1287 0c24 17FD sbrc r17,7 1288 0c26 00C0 rjmp .L84 1289 .L37: 1290 0c28 8138 cpi r24,lo8(129) 1291 0c2a 9105 cpc r25,__zero_reg__ 1292 0c2c A105 cpc r26,__zero_reg__ 1293 0c2e B105 cpc r27,__zero_reg__ 1294 0c30 04F0 brlt .+2 1295 0c32 00C0 rjmp .L35 1296 .L34: 1297 0c34 C401 movw r24,r8 1298 0c36 B301 movw r22,r6 1299 0c38 24E6 ldi r18,lo8(100) 1300 0c3a 30E0 ldi r19,hi8(100) 1301 0c3c 40E0 ldi r20,hlo8(100) 1302 0c3e 50E0 ldi r21,hhi8(100) 1303 0c40 0E94 0000 call __divmodsi4 1304 0c44 2E19 sub r18,r14 1305 0c46 3F09 sbc r19,r15 1306 0c48 3093 0000 sts (TrimNick)+1,r19 1307 0c4c 2093 0000 sts TrimNick,r18 1308 0c50 C801 movw r24,r16 1309 0c52 B701 movw r22,r14 1310 0c54 2CE9 ldi r18,lo8(-100) 1311 0c56 3FEF ldi r19,hi8(-100) 1312 0c58 4FEF ldi r20,hlo8(-100) 1313 0c5a 5FEF ldi r21,hhi8(-100) 1314 0c5c 0E94 0000 call __divmodsi4 1315 0c60 260D add r18,r6 1316 0c62 371D adc r19,r7 1317 0c64 3093 0000 sts (TrimRoll)+1,r19 1318 0c68 2093 0000 sts TrimRoll,r18 1319 0c6c BE8D ldd r27,Y+30 1320 0c6e EB2E mov r14,r27 1321 0c70 FF24 clr r15 1322 0c72 00E0 ldi r16,lo8(0) 1323 0c74 10E0 ldi r17,hi8(0) 1324 0c76 28A5 ldd r18,Y+40 1325 0c78 39A5 ldd r19,Y+41 1326 0c7a C901 movw r24,r18 1327 0c7c 9C01 movw r18,r24 1328 0c7e 97FD sbrc r25,7 1329 0c80 00C0 rjmp .L85 1330 .L38: 1331 0c82 B901 movw r22,r18 1332 0c84 8827 clr r24 1333 0c86 77FD sbrc r23,7 1334 0c88 8095 com r24 1335 0c8a 982F mov r25,r24 1336 0c8c A801 movw r20,r16 1337 0c8e 9701 movw r18,r14 1338 0c90 0E94 0000 call __mulsi3 1339 0c94 6D87 std Y+13,r22 1340 0c96 7E87 std Y+14,r23 1341 0c98 8F87 std Y+15,r24 1342 0c9a 988B std Y+16,r25 1343 0c9c 2AA5 ldd r18,Y+42 1344 0c9e 3BA5 ldd r19,Y+43 1345 0ca0 C901 movw r24,r18 1346 0ca2 9C01 movw r18,r24 1347 0ca4 97FD sbrc r25,7 1348 0ca6 00C0 rjmp .L86 1349 .L39: 1350 0ca8 B901 movw r22,r18 1351 0caa 8827 clr r24 1352 0cac 77FD sbrc r23,7 1353 0cae 8095 com r24 1354 0cb0 982F mov r25,r24 1355 0cb2 A801 movw r20,r16 1356 0cb4 9701 movw r18,r14 1357 0cb6 0E94 0000 call __mulsi3 1358 0cba 6987 std Y+9,r22 1359 0cbc 7A87 std Y+10,r23 1360 0cbe 8B87 std Y+11,r24 1361 0cc0 9C87 std Y+12,r25 1362 0cc2 E090 0000 lds r14,YawGyroHeading 1363 0cc6 F090 0000 lds r15,(YawGyroHeading)+1 1364 0cca 0091 0000 lds r16,(YawGyroHeading)+2 1365 0cce 1091 0000 lds r17,(YawGyroHeading)+3 1366 0cd2 00C0 rjmp .L40 1367 .L82: 1368 0cd4 6F8D ldd r22,Y+31 1369 0cd6 6623 tst r22 1370 0cd8 01F0 breq .+2 1371 0cda 00C0 rjmp .L64 1372 0cdc 8091 0000 lds r24,LoopingRoll 1373 0ce0 8823 tst r24 1374 0ce2 01F0 breq .+2 1375 0ce4 00C0 rjmp .L64 1376 0ce6 8C8D ldd r24,Y+28 1377 0ce8 9D8D ldd r25,Y+29 1378 0cea 8150 subi r24,lo8(257) 1379 0cec 9140 sbci r25,hi8(257) 1380 0cee 04F0 brlt .+2 1381 0cf0 00C0 rjmp .L87 1382 0cf2 2C8D ldd r18,Y+28 1383 0cf4 3D8D ldd r19,Y+29 1384 0cf6 6FEF ldi r22,hi8(-256) 1385 0cf8 2030 cpi r18,lo8(-256) 1386 0cfa 3607 cpc r19,r22 1387 0cfc 04F4 brge .L62 1388 0cfe 8091 0000 lds r24,GyroNick 1389 0d02 9091 0000 lds r25,(GyroNick)+1 1390 0d06 8050 subi r24,lo8(-(256)) 1391 0d08 9F4F sbci r25,hi8(-(256)) 1392 0d0a 820F add r24,r18 1393 0d0c 931F adc r25,r19 1394 0d0e 9093 0000 sts (GyroNick)+1,r25 1395 0d12 8093 0000 sts GyroNick,r24 1396 .L62: 1397 0d16 8A8D ldd r24,Y+26 1398 0d18 9B8D ldd r25,Y+27 1399 0d1a 8150 subi r24,lo8(257) 1400 0d1c 9140 sbci r25,hi8(257) 1401 0d1e 04F0 brlt .+2 1402 0d20 00C0 rjmp .L88 1403 0d22 2A8D ldd r18,Y+26 1404 0d24 3B8D ldd r19,Y+27 1405 0d26 6FEF ldi r22,hi8(-256) 1406 0d28 2030 cpi r18,lo8(-256) 1407 0d2a 3607 cpc r19,r22 1408 0d2c 04F0 brlt .+2 1409 0d2e 00C0 rjmp .L64 1410 0d30 8091 0000 lds r24,GyroRoll 1411 0d34 9091 0000 lds r25,(GyroRoll)+1 1412 0d38 8050 subi r24,lo8(-(256)) 1413 0d3a 9F4F sbci r25,hi8(-(256)) 1414 0d3c 820F add r24,r18 1415 0d3e 931F adc r25,r19 1416 0d40 9093 0000 sts (GyroRoll)+1,r25 1417 0d44 8093 0000 sts GyroRoll,r24 1418 0d48 00C0 rjmp .L64 1419 .L73: 1420 0d4a E60C add r14,r6 1421 0d4c F71C adc r15,r7 1422 0d4e 081D adc r16,r8 1423 0d50 191D adc r17,r9 1424 0d52 E092 0000 sts YawGyroHeading,r14 1425 0d56 F092 0000 sts (YawGyroHeading)+1,r15 1426 0d5a 0093 0000 sts (YawGyroHeading)+2,r16 1427 0d5e 1093 0000 sts (YawGyroHeading)+3,r17 1428 0d62 00C0 rjmp .L42 1429 .L58: 1430 0d64 6985 ldd r22,Y+9 1431 0d66 7A85 ldd r23,Y+10 1432 0d68 8B85 ldd r24,Y+11 1433 0d6a 9C85 ldd r25,Y+12 1434 0d6c 20E4 ldi r18,lo8(64) 1435 0d6e 30E0 ldi r19,hi8(64) 1436 0d70 40E0 ldi r20,hlo8(64) 1437 0d72 50E0 ldi r21,hhi8(64) 1438 0d74 0E94 0000 call __divmodsi4 1439 0d78 5095 com r21 1440 0d7a 4095 com r20 1441 0d7c 3095 com r19 1442 0d7e 2195 neg r18 1443 0d80 3F4F sbci r19,lo8(-1) 1444 0d82 4F4F sbci r20,lo8(-1) 1445 0d84 5F4F sbci r21,lo8(-1) 1446 0d86 620E add r6,r18 1447 0d88 731E adc r7,r19 1448 0d8a 7092 0000 sts (TrimRoll)+1,r7 1449 0d8e 6092 0000 sts TrimRoll,r6 1450 0d92 00C0 rjmp .L59 1451 .L56: 1452 0d94 6D85 ldd r22,Y+13 1453 0d96 7E85 ldd r23,Y+14 1454 0d98 8F85 ldd r24,Y+15 1455 0d9a 9889 ldd r25,Y+16 1456 0d9c 20E4 ldi r18,lo8(64) 1457 0d9e 30E0 ldi r19,hi8(64) 1458 0da0 40E0 ldi r20,hlo8(64) 1459 0da2 50E0 ldi r21,hhi8(64) 1460 0da4 0E94 0000 call __divmodsi4 1461 0da8 5095 com r21 1462 0daa 4095 com r20 1463 0dac 3095 com r19 1464 0dae 2195 neg r18 1465 0db0 3F4F sbci r19,lo8(-1) 1466 0db2 4F4F sbci r20,lo8(-1) 1467 0db4 5F4F sbci r21,lo8(-1) 1468 0db6 820E add r8,r18 1469 0db8 931E adc r9,r19 1470 0dba 9092 0000 sts (TrimNick)+1,r9 1471 0dbe 8092 0000 sts TrimNick,r8 1472 0dc2 00C0 rjmp .L57 1473 .L67: 1474 0dc4 0796 adiw r24,7 1475 0dc6 00C0 rjmp .L10 1476 .L71: 1477 0dc8 295F subi r18,lo8(-(7)) 1478 0dca 3F4F sbci r19,hi8(-(7)) 1479 0dcc 00C0 rjmp .L19 1480 .L68: 1481 0dce 295F subi r18,lo8(-(7)) 1482 0dd0 3F4F sbci r19,hi8(-(7)) 1483 0dd2 00C0 rjmp .L11 1484 .L81: 1485 0dd4 215F subi r18,lo8(-(15)) 1486 0dd6 3F4F sbci r19,hi8(-(15)) 1487 0dd8 00C0 rjmp .L55 1488 .L74: 1489 0dda 2F5F subi r18,lo8(-(1)) 1490 0ddc 3F4F sbci r19,hi8(-(1)) 1491 0dde 00C0 rjmp .L48 1492 .L77: 1493 0de0 215F subi r18,lo8(-(15)) 1494 0de2 3F4F sbci r19,hi8(-(15)) 1495 0de4 00C0 rjmp .L51 1496 .L78: 1497 0de6 2F5F subi r18,lo8(-(1)) 1498 0de8 3F4F sbci r19,hi8(-(1)) 1499 0dea 00C0 rjmp .L52 1500 .L87: 1501 0dec 8091 0000 lds r24,GyroNick 1502 0df0 9091 0000 lds r25,(GyroNick)+1 1503 0df4 8050 subi r24,lo8(-(-256)) 1504 0df6 9140 sbci r25,hi8(-(-256)) 1505 0df8 EC8D ldd r30,Y+28 1506 0dfa FD8D ldd r31,Y+29 1507 0dfc 8E0F add r24,r30 1508 0dfe 9F1F adc r25,r31 1509 0e00 9093 0000 sts (GyroNick)+1,r25 1510 0e04 8093 0000 sts GyroNick,r24 1511 0e08 00C0 rjmp .L62 1512 .L35: 1513 0e0a 81E0 ldi r24,lo8(1) 1514 0e0c 8093 0000 sts FunnelCourse,r24 1515 0e10 00C0 rjmp .L34 1516 .L83: 1517 0e12 8827 clr r24 1518 0e14 9927 clr r25 1519 0e16 DC01 movw r26,r24 1520 0e18 8619 sub r24,r6 1521 0e1a 9709 sbc r25,r7 1522 0e1c A809 sbc r26,r8 1523 0e1e B909 sbc r27,r9 1524 0e20 00C0 rjmp .L36 1525 .L88: 1526 0e22 8091 0000 lds r24,GyroRoll 1527 0e26 9091 0000 lds r25,(GyroRoll)+1 1528 0e2a 8050 subi r24,lo8(-(-256)) 1529 0e2c 9140 sbci r25,hi8(-(-256)) 1530 0e2e EA8D ldd r30,Y+26 1531 0e30 FB8D ldd r31,Y+27 1532 0e32 8E0F add r24,r30 1533 0e34 9F1F adc r25,r31 1534 0e36 9093 0000 sts (GyroRoll)+1,r25 1535 0e3a 8093 0000 sts GyroRoll,r24 1536 0e3e 00C0 rjmp .L64 1537 .L86: 1538 0e40 3095 com r19 1539 0e42 2195 neg r18 1540 0e44 3F4F sbci r19,lo8(-1) 1541 0e46 00C0 rjmp .L39 1542 .L85: 1543 0e48 3095 com r19 1544 0e4a 2195 neg r18 1545 0e4c 3F4F sbci r19,lo8(-1) 1546 0e4e 00C0 rjmp .L38 1547 .L84: 1548 0e50 8827 clr r24 1549 0e52 9927 clr r25 1550 0e54 DC01 movw r26,r24 1551 0e56 8E19 sub r24,r14 1552 0e58 9F09 sbc r25,r15 1553 0e5a A00B sbc r26,r16 1554 0e5c B10B sbc r27,r17 1555 0e5e 00C0 rjmp .L37 1557 .global ParameterMapping 1559 ParameterMapping: 1560 /* prologue: function */ 1561 /* frame size = 0 */ 1562 0e60 8091 0000 lds r24,RC_Quality 1563 0e64 813A cpi r24,lo8(-95) 1564 0e66 00F0 brlo .L253 1565 0e68 8091 0000 lds r24,ParamSet+11 1566 0e6c 8B3F cpi r24,lo8(-5) 1567 0e6e 00F4 brsh .L261 1568 0e70 8093 0000 sts FCParam+1,r24 1569 .L93: 1570 0e74 8091 0000 lds r24,ParamSet+10 1571 0e78 8B3F cpi r24,lo8(-5) 1572 0e7a 00F0 brlo .L96 1573 0e7c 8B3F cpi r24,lo8(-5) 1574 0e7e 01F4 brne .+2 1575 0e80 00C0 rjmp .L262 1576 0e82 8C3F cpi r24,lo8(-4) 1577 0e84 01F4 brne .+2 1578 0e86 00C0 rjmp .L263 1579 0e88 8D3F cpi r24,lo8(-3) 1580 0e8a 01F4 brne .+2 1581 0e8c 00C0 rjmp .L264 1582 0e8e 8E3F cpi r24,lo8(-2) 1583 0e90 01F4 brne .L98 1584 0e92 8091 0000 lds r24,Poti4 1585 0e96 8093 0000 sts FCParam,r24 1586 0e9a 00C0 rjmp .L98 1587 .L248: 1588 0e9c 8093 0000 sts FCParam+28,r24 1589 .L250: 1590 0ea0 6091 0000 lds r22,FCParam+12 1591 0ea4 70E0 ldi r23,lo8(0) 1592 0ea6 6F5F subi r22,lo8(-(1)) 1593 0ea8 7F4F sbci r23,hi8(-(1)) 1594 0eaa 8CE3 ldi r24,lo8(10300) 1595 0eac 98E2 ldi r25,hi8(10300) 1596 0eae 0E94 0000 call __divmodhi4 1597 0eb2 7093 0000 sts (Ki)+1,r23 1598 0eb6 6093 0000 sts Ki,r22 1599 .L253: 1600 0eba 0895 ret 1601 .L261: 1602 0ebc 8B3F cpi r24,lo8(-5) 1603 0ebe 01F4 brne .+2 1604 0ec0 00C0 rjmp .L265 1605 0ec2 8C3F cpi r24,lo8(-4) 1606 0ec4 01F4 brne .+2 1607 0ec6 00C0 rjmp .L266 1608 0ec8 8D3F cpi r24,lo8(-3) 1609 0eca 01F4 brne .+2 1610 0ecc 00C0 rjmp .L267 1611 0ece 8E3F cpi r24,lo8(-2) 1612 0ed0 01F4 brne .L93 1613 0ed2 8091 0000 lds r24,Poti4 1614 0ed6 8093 0000 sts FCParam+1,r24 1615 0eda 00C0 rjmp .L93 1616 .L96: 1617 0edc 8093 0000 sts FCParam,r24 1618 .L98: 1619 0ee0 8091 0000 lds r24,FCParam 1620 0ee4 8823 tst r24 1621 0ee6 01F4 brne .L101 1622 0ee8 1092 0000 sts FCParam,__zero_reg__ 1623 .L102: 1624 0eec 8091 0000 lds r24,ParamSet+12 1625 0ef0 8B3F cpi r24,lo8(-5) 1626 0ef2 00F0 brlo .L103 1627 .L271: 1628 0ef4 8B3F cpi r24,lo8(-5) 1629 0ef6 01F4 brne .+2 1630 0ef8 00C0 rjmp .L268 1631 0efa 8C3F cpi r24,lo8(-4) 1632 0efc 01F4 brne .+2 1633 0efe 00C0 rjmp .L269 1634 0f00 8D3F cpi r24,lo8(-3) 1635 0f02 01F4 brne .+2 1636 0f04 00C0 rjmp .L270 1637 0f06 8E3F cpi r24,lo8(-2) 1638 0f08 01F4 brne .L105 1639 0f0a 8091 0000 lds r24,Poti4 1640 0f0e 8093 0000 sts FCParam+2,r24 1641 0f12 00C0 rjmp .L105 1642 .L101: 1643 0f14 8436 cpi r24,lo8(100) 1644 0f16 00F0 brlo .L102 1645 0f18 84E6 ldi r24,lo8(100) 1646 0f1a 8093 0000 sts FCParam,r24 1647 0f1e 8091 0000 lds r24,ParamSet+12 1648 0f22 8B3F cpi r24,lo8(-5) 1649 0f24 00F4 brsh .L271 1650 .L103: 1651 0f26 8093 0000 sts FCParam+2,r24 1652 .L105: 1653 0f2a 8091 0000 lds r24,FCParam+2 1654 0f2e 8823 tst r24 1655 0f30 01F4 brne .L108 1656 0f32 1092 0000 sts FCParam+2,__zero_reg__ 1657 .L109: 1658 0f36 8091 0000 lds r24,ParamSet+14 1659 0f3a 8B3F cpi r24,lo8(-5) 1660 0f3c 00F0 brlo .L110 1661 .L275: 1662 0f3e 8B3F cpi r24,lo8(-5) 1663 0f40 01F4 brne .+2 1664 0f42 00C0 rjmp .L272 1665 0f44 8C3F cpi r24,lo8(-4) 1666 0f46 01F4 brne .+2 1667 0f48 00C0 rjmp .L273 1668 0f4a 8D3F cpi r24,lo8(-3) 1669 0f4c 01F4 brne .+2 1670 0f4e 00C0 rjmp .L274 1671 0f50 8E3F cpi r24,lo8(-2) 1672 0f52 01F4 brne .L112 1673 0f54 8091 0000 lds r24,Poti4 1674 0f58 8093 0000 sts FCParam+3,r24 1675 0f5c 00C0 rjmp .L112 1676 .L108: 1677 0f5e 8436 cpi r24,lo8(100) 1678 0f60 00F0 brlo .L109 1679 0f62 84E6 ldi r24,lo8(100) 1680 0f64 8093 0000 sts FCParam+2,r24 1681 0f68 8091 0000 lds r24,ParamSet+14 1682 0f6c 8B3F cpi r24,lo8(-5) 1683 0f6e 00F4 brsh .L275 1684 .L110: 1685 0f70 8093 0000 sts FCParam+3,r24 1686 .L112: 1687 0f74 8091 0000 lds r24,ParamSet+16 1688 0f78 8B3F cpi r24,lo8(-5) 1689 0f7a 00F0 brlo .L115 1690 0f7c 8B3F cpi r24,lo8(-5) 1691 0f7e 01F4 brne .+2 1692 0f80 00C0 rjmp .L276 1693 0f82 8C3F cpi r24,lo8(-4) 1694 0f84 01F4 brne .+2 1695 0f86 00C0 rjmp .L277 1696 0f88 8D3F cpi r24,lo8(-3) 1697 0f8a 01F4 brne .+2 1698 0f8c 00C0 rjmp .L278 1699 0f8e 8E3F cpi r24,lo8(-2) 1700 0f90 01F4 brne .L117 1701 0f92 8091 0000 lds r24,Poti4 1702 0f96 8093 0000 sts FCParam+4,r24 1703 0f9a 00C0 rjmp .L117 1704 .L115: 1705 0f9c 8093 0000 sts FCParam+4,r24 1706 .L117: 1707 0fa0 8091 0000 lds r24,ParamSet+24 1708 0fa4 8B3F cpi r24,lo8(-5) 1709 0fa6 00F0 brlo .L120 1710 0fa8 8B3F cpi r24,lo8(-5) 1711 0faa 01F4 brne .+2 1712 0fac 00C0 rjmp .L279 1713 0fae 8C3F cpi r24,lo8(-4) 1714 0fb0 01F4 brne .+2 1715 0fb2 00C0 rjmp .L280 1716 0fb4 8D3F cpi r24,lo8(-3) 1717 0fb6 01F4 brne .+2 1718 0fb8 00C0 rjmp .L281 1719 0fba 8E3F cpi r24,lo8(-2) 1720 0fbc 01F4 brne .L122 1721 0fbe 8091 0000 lds r24,Poti4 1722 0fc2 8093 0000 sts FCParam+5,r24 1723 0fc6 00C0 rjmp .L122 1724 .L120: 1725 0fc8 8093 0000 sts FCParam+5,r24 1726 .L122: 1727 0fcc 8091 0000 lds r24,ParamSet+25 1728 0fd0 8B3F cpi r24,lo8(-5) 1729 0fd2 00F0 brlo .L125 1730 0fd4 8B3F cpi r24,lo8(-5) 1731 0fd6 01F4 brne .+2 1732 0fd8 00C0 rjmp .L282 1733 0fda 8C3F cpi r24,lo8(-4) 1734 0fdc 01F4 brne .+2 1735 0fde 00C0 rjmp .L283 1736 0fe0 8D3F cpi r24,lo8(-3) 1737 0fe2 01F4 brne .+2 1738 0fe4 00C0 rjmp .L284 1739 0fe6 8E3F cpi r24,lo8(-2) 1740 0fe8 01F4 brne .L127 1741 0fea 8091 0000 lds r24,Poti4 1742 0fee 8093 0000 sts FCParam+7,r24 1743 0ff2 00C0 rjmp .L127 1744 .L125: 1745 0ff4 8093 0000 sts FCParam+7,r24 1746 .L127: 1747 0ff8 8091 0000 lds r24,FCParam+7 1748 0ffc 8B30 cpi r24,lo8(11) 1749 0ffe 00F4 brsh .L130 1750 1000 8AE0 ldi r24,lo8(10) 1751 1002 8093 0000 sts FCParam+7,r24 1752 .L131: 1753 1006 8091 0000 lds r24,ParamSet+26 1754 100a 8B3F cpi r24,lo8(-5) 1755 100c 00F0 brlo .L132 1756 100e 8B3F cpi r24,lo8(-5) 1757 1010 01F4 brne .+2 1758 1012 00C0 rjmp .L285 1759 1014 8C3F cpi r24,lo8(-4) 1760 1016 01F4 brne .+2 1761 1018 00C0 rjmp .L286 1762 101a 8D3F cpi r24,lo8(-3) 1763 101c 01F4 brne .+2 1764 101e 00C0 rjmp .L287 1765 1020 8E3F cpi r24,lo8(-2) 1766 1022 01F4 brne .L134 1767 1024 8091 0000 lds r24,Poti4 1768 1028 8093 0000 sts FCParam+8,r24 1769 102c 00C0 rjmp .L134 1770 .L130: 1771 102e 8F3F cpi r24,lo8(-1) 1772 1030 01F4 brne .L131 1773 1032 8093 0000 sts FCParam+7,r24 1774 1036 00C0 rjmp .L131 1775 .L132: 1776 1038 8093 0000 sts FCParam+8,r24 1777 .L134: 1778 103c 8091 0000 lds r24,ParamSet+27 1779 1040 8B3F cpi r24,lo8(-5) 1780 1042 00F0 brlo .L137 1781 1044 8B3F cpi r24,lo8(-5) 1782 1046 01F4 brne .+2 1783 1048 00C0 rjmp .L288 1784 104a 8C3F cpi r24,lo8(-4) 1785 104c 01F4 brne .+2 1786 104e 00C0 rjmp .L289 1787 1050 8D3F cpi r24,lo8(-3) 1788 1052 01F4 brne .+2 1789 1054 00C0 rjmp .L290 1790 1056 8E3F cpi r24,lo8(-2) 1791 1058 01F4 brne .L139 1792 105a 8091 0000 lds r24,Poti4 1793 105e 8093 0000 sts FCParam+6,r24 1794 1062 00C0 rjmp .L139 1795 .L137: 1796 1064 8093 0000 sts FCParam+6,r24 1797 .L139: 1798 1068 8091 0000 lds r24,ParamSet+28 1799 106c 8B3F cpi r24,lo8(-5) 1800 106e 00F0 brlo .L142 1801 1070 8B3F cpi r24,lo8(-5) 1802 1072 01F4 brne .+2 1803 1074 00C0 rjmp .L291 1804 1076 8C3F cpi r24,lo8(-4) 1805 1078 01F4 brne .+2 1806 107a 00C0 rjmp .L292 1807 107c 8D3F cpi r24,lo8(-3) 1808 107e 01F4 brne .+2 1809 1080 00C0 rjmp .L293 1810 1082 8E3F cpi r24,lo8(-2) 1811 1084 01F4 brne .L144 1812 1086 8091 0000 lds r24,Poti4 1813 108a 8093 0000 sts FCParam+9,r24 1814 108e 00C0 rjmp .L144 1815 .L142: 1816 1090 8093 0000 sts FCParam+9,r24 1817 .L144: 1818 1094 8091 0000 lds r24,FCParam+9 1819 1098 8B30 cpi r24,lo8(11) 1820 109a 00F4 brsh .L147 1821 109c 8AE0 ldi r24,lo8(10) 1822 109e 8093 0000 sts FCParam+9,r24 1823 .L148: 1824 10a2 8091 0000 lds r24,ParamSet+29 1825 10a6 8B3F cpi r24,lo8(-5) 1826 10a8 00F0 brlo .L149 1827 10aa 8B3F cpi r24,lo8(-5) 1828 10ac 01F4 brne .+2 1829 10ae 00C0 rjmp .L294 1830 10b0 8C3F cpi r24,lo8(-4) 1831 10b2 01F4 brne .+2 1832 10b4 00C0 rjmp .L295 1833 10b6 8D3F cpi r24,lo8(-3) 1834 10b8 01F4 brne .+2 1835 10ba 00C0 rjmp .L296 1836 10bc 8E3F cpi r24,lo8(-2) 1837 10be 01F4 brne .L151 1838 10c0 8091 0000 lds r24,Poti4 1839 10c4 8093 0000 sts FCParam+10,r24 1840 10c8 00C0 rjmp .L151 1841 .L147: 1842 10ca 8F3F cpi r24,lo8(-1) 1843 10cc 01F4 brne .L148 1844 10ce 8093 0000 sts FCParam+9,r24 1845 10d2 00C0 rjmp .L148 1846 .L149: 1847 10d4 8093 0000 sts FCParam+10,r24 1848 .L151: 1849 10d8 8091 0000 lds r24,ParamSet+34 1850 10dc 8B3F cpi r24,lo8(-5) 1851 10de 00F0 brlo .L154 1852 10e0 8B3F cpi r24,lo8(-5) 1853 10e2 01F4 brne .+2 1854 10e4 00C0 rjmp .L297 1855 10e6 8C3F cpi r24,lo8(-4) 1856 10e8 01F4 brne .+2 1857 10ea 00C0 rjmp .L298 1858 10ec 8D3F cpi r24,lo8(-3) 1859 10ee 01F4 brne .+2 1860 10f0 00C0 rjmp .L299 1861 10f2 8E3F cpi r24,lo8(-2) 1862 10f4 01F4 brne .L156 1863 10f6 8091 0000 lds r24,Poti4 1864 10fa 8093 0000 sts FCParam+12,r24 1865 10fe 00C0 rjmp .L156 1866 .L154: 1867 1100 8093 0000 sts FCParam+12,r24 1868 .L156: 1869 1104 8091 0000 lds r24,ParamSet+35 1870 1108 8B3F cpi r24,lo8(-5) 1871 110a 00F0 brlo .L159 1872 110c 8B3F cpi r24,lo8(-5) 1873 110e 01F4 brne .+2 1874 1110 00C0 rjmp .L300 1875 1112 8C3F cpi r24,lo8(-4) 1876 1114 01F4 brne .+2 1877 1116 00C0 rjmp .L301 1878 1118 8D3F cpi r24,lo8(-3) 1879 111a 01F4 brne .+2 1880 111c 00C0 rjmp .L302 1881 111e 8E3F cpi r24,lo8(-2) 1882 1120 01F4 brne .L161 1883 1122 8091 0000 lds r24,Poti4 1884 1126 8093 0000 sts FCParam+13,r24 1885 112a 00C0 rjmp .L161 1886 .L159: 1887 112c 8093 0000 sts FCParam+13,r24 1888 .L161: 1889 1130 8091 0000 lds r24,ParamSet+36 1890 1134 8B3F cpi r24,lo8(-5) 1891 1136 00F0 brlo .L164 1892 1138 8B3F cpi r24,lo8(-5) 1893 113a 01F4 brne .+2 1894 113c 00C0 rjmp .L303 1895 113e 8C3F cpi r24,lo8(-4) 1896 1140 01F4 brne .+2 1897 1142 00C0 rjmp .L304 1898 1144 8D3F cpi r24,lo8(-3) 1899 1146 01F4 brne .+2 1900 1148 00C0 rjmp .L305 1901 114a 8E3F cpi r24,lo8(-2) 1902 114c 01F4 brne .L166 1903 114e 8091 0000 lds r24,Poti4 1904 1152 8093 0000 sts FCParam+14,r24 1905 1156 00C0 rjmp .L166 1906 .L164: 1907 1158 8093 0000 sts FCParam+14,r24 1908 .L166: 1909 115c 8091 0000 lds r24,ParamSet+37 1910 1160 8B3F cpi r24,lo8(-5) 1911 1162 00F0 brlo .L169 1912 1164 8B3F cpi r24,lo8(-5) 1913 1166 01F4 brne .+2 1914 1168 00C0 rjmp .L306 1915 116a 8C3F cpi r24,lo8(-4) 1916 116c 01F4 brne .+2 1917 116e 00C0 rjmp .L307 1918 1170 8D3F cpi r24,lo8(-3) 1919 1172 01F4 brne .+2 1920 1174 00C0 rjmp .L308 1921 1176 8E3F cpi r24,lo8(-2) 1922 1178 01F4 brne .L171 1923 117a 8091 0000 lds r24,Poti4 1924 117e 8093 0000 sts FCParam+15,r24 1925 1182 00C0 rjmp .L171 1926 .L169: 1927 1184 8093 0000 sts FCParam+15,r24 1928 .L171: 1929 1188 8091 0000 lds r24,ParamSet+38 1930 118c 8B3F cpi r24,lo8(-5) 1931 118e 00F0 brlo .L174 1932 1190 8B3F cpi r24,lo8(-5) 1933 1192 01F4 brne .+2 1934 1194 00C0 rjmp .L309 1935 1196 8C3F cpi r24,lo8(-4) 1936 1198 01F4 brne .+2 1937 119a 00C0 rjmp .L310 1938 119c 8D3F cpi r24,lo8(-3) 1939 119e 01F4 brne .+2 1940 11a0 00C0 rjmp .L311 1941 11a2 8E3F cpi r24,lo8(-2) 1942 11a4 01F4 brne .L176 1943 11a6 8091 0000 lds r24,Poti4 1944 11aa 8093 0000 sts FCParam+16,r24 1945 11ae 00C0 rjmp .L176 1946 .L174: 1947 11b0 8093 0000 sts FCParam+16,r24 1948 .L176: 1949 11b4 8091 0000 lds r24,ParamSet+59 1950 11b8 8B3F cpi r24,lo8(-5) 1951 11ba 00F0 brlo .L179 1952 11bc 8B3F cpi r24,lo8(-5) 1953 11be 01F4 brne .+2 1954 11c0 00C0 rjmp .L312 1955 11c2 8C3F cpi r24,lo8(-4) 1956 11c4 01F4 brne .+2 1957 11c6 00C0 rjmp .L313 1958 11c8 8D3F cpi r24,lo8(-3) 1959 11ca 01F4 brne .+2 1960 11cc 00C0 rjmp .L314 1961 11ce 8E3F cpi r24,lo8(-2) 1962 11d0 01F4 brne .L181 1963 11d2 8091 0000 lds r24,Poti4 1964 11d6 8093 0000 sts FCParam+17,r24 1965 11da 00C0 rjmp .L181 1966 .L179: 1967 11dc 8093 0000 sts FCParam+17,r24 1968 .L181: 1969 11e0 8091 0000 lds r24,ParamSet+60 1970 11e4 8B3F cpi r24,lo8(-5) 1971 11e6 00F0 brlo .L184 1972 11e8 8B3F cpi r24,lo8(-5) 1973 11ea 01F4 brne .+2 1974 11ec 00C0 rjmp .L315 1975 11ee 8C3F cpi r24,lo8(-4) 1976 11f0 01F4 brne .+2 1977 11f2 00C0 rjmp .L316 1978 11f4 8D3F cpi r24,lo8(-3) 1979 11f6 01F4 brne .+2 1980 11f8 00C0 rjmp .L317 1981 11fa 8E3F cpi r24,lo8(-2) 1982 11fc 01F4 brne .L186 1983 11fe 8091 0000 lds r24,Poti4 1984 1202 8093 0000 sts FCParam+18,r24 1985 1206 00C0 rjmp .L186 1986 .L184: 1987 1208 8093 0000 sts FCParam+18,r24 1988 .L186: 1989 120c 8091 0000 lds r24,ParamSet+61 1990 1210 8B3F cpi r24,lo8(-5) 1991 1212 00F0 brlo .L189 1992 1214 8B3F cpi r24,lo8(-5) 1993 1216 01F4 brne .+2 1994 1218 00C0 rjmp .L318 1995 121a 8C3F cpi r24,lo8(-4) 1996 121c 01F4 brne .+2 1997 121e 00C0 rjmp .L319 1998 1220 8D3F cpi r24,lo8(-3) 1999 1222 01F4 brne .+2 2000 1224 00C0 rjmp .L320 2001 1226 8E3F cpi r24,lo8(-2) 2002 1228 01F4 brne .L191 2003 122a 8091 0000 lds r24,Poti4 2004 122e 8093 0000 sts FCParam+19,r24 2005 1232 00C0 rjmp .L191 2006 .L189: 2007 1234 8093 0000 sts FCParam+19,r24 2008 .L191: 2009 1238 8091 0000 lds r24,ParamSet+62 2010 123c 8B3F cpi r24,lo8(-5) 2011 123e 00F0 brlo .L194 2012 1240 8B3F cpi r24,lo8(-5) 2013 1242 01F4 brne .+2 2014 1244 00C0 rjmp .L321 2015 1246 8C3F cpi r24,lo8(-4) 2016 1248 01F4 brne .+2 2017 124a 00C0 rjmp .L322 2018 124c 8D3F cpi r24,lo8(-3) 2019 124e 01F4 brne .+2 2020 1250 00C0 rjmp .L323 2021 1252 8E3F cpi r24,lo8(-2) 2022 1254 01F4 brne .L196 2023 1256 8091 0000 lds r24,Poti4 2024 125a 8093 0000 sts FCParam+20,r24 2025 125e 00C0 rjmp .L196 2026 .L194: 2027 1260 8093 0000 sts FCParam+20,r24 2028 .L196: 2029 1264 8091 0000 lds r24,ParamSet+39 2030 1268 8B3F cpi r24,lo8(-5) 2031 126a 00F0 brlo .L199 2032 126c 8B3F cpi r24,lo8(-5) 2033 126e 01F4 brne .+2 2034 1270 00C0 rjmp .L324 2035 1272 8C3F cpi r24,lo8(-4) 2036 1274 01F4 brne .+2 2037 1276 00C0 rjmp .L325 2038 1278 8D3F cpi r24,lo8(-3) 2039 127a 01F4 brne .+2 2040 127c 00C0 rjmp .L326 2041 127e 8E3F cpi r24,lo8(-2) 2042 1280 01F4 brne .L201 2043 1282 8091 0000 lds r24,Poti4 2044 1286 8093 0000 sts FCParam+21,r24 2045 128a 00C0 rjmp .L201 2046 .L199: 2047 128c 8093 0000 sts FCParam+21,r24 2048 .L201: 2049 1290 8091 0000 lds r24,ParamSet+43 2050 1294 8B3F cpi r24,lo8(-5) 2051 1296 00F0 brlo .L204 2052 1298 8B3F cpi r24,lo8(-5) 2053 129a 01F4 brne .+2 2054 129c 00C0 rjmp .L327 2055 129e 8C3F cpi r24,lo8(-4) 2056 12a0 01F4 brne .+2 2057 12a2 00C0 rjmp .L328 2058 12a4 8D3F cpi r24,lo8(-3) 2059 12a6 01F4 brne .+2 2060 12a8 00C0 rjmp .L329 2061 12aa 8E3F cpi r24,lo8(-2) 2062 12ac 01F4 brne .L206 2063 12ae 8091 0000 lds r24,Poti4 2064 12b2 8093 0000 sts FCParam+22,r24 2065 12b6 00C0 rjmp .L206 2066 .L204: 2067 12b8 8093 0000 sts FCParam+22,r24 2068 .L206: 2069 12bc 8091 0000 lds r24,ParamSet+48 2070 12c0 8B3F cpi r24,lo8(-5) 2071 12c2 00F0 brlo .L209 2072 12c4 8B3F cpi r24,lo8(-5) 2073 12c6 01F4 brne .+2 2074 12c8 00C0 rjmp .L330 2075 12ca 8C3F cpi r24,lo8(-4) 2076 12cc 01F4 brne .+2 2077 12ce 00C0 rjmp .L331 2078 12d0 8D3F cpi r24,lo8(-3) 2079 12d2 01F4 brne .+2 2080 12d4 00C0 rjmp .L332 2081 12d6 8E3F cpi r24,lo8(-2) 2082 12d8 01F4 brne .L211 2083 12da 8091 0000 lds r24,Poti4 2084 12de 8093 0000 sts FCParam+23,r24 2085 12e2 00C0 rjmp .L211 2086 .L209: 2087 12e4 8093 0000 sts FCParam+23,r24 2088 .L211: 2089 12e8 8091 0000 lds r24,ParamSet+51 2090 12ec 8B3F cpi r24,lo8(-5) 2091 12ee 00F0 brlo .L214 2092 12f0 8B3F cpi r24,lo8(-5) 2093 12f2 01F4 brne .+2 2094 12f4 00C0 rjmp .L333 2095 12f6 8C3F cpi r24,lo8(-4) 2096 12f8 01F4 brne .+2 2097 12fa 00C0 rjmp .L334 2098 12fc 8D3F cpi r24,lo8(-3) 2099 12fe 01F4 brne .+2 2100 1300 00C0 rjmp .L335 2101 1302 8E3F cpi r24,lo8(-2) 2102 1304 01F4 brne .L216 2103 1306 8091 0000 lds r24,Poti4 2104 130a 8093 0000 sts FCParam+24,r24 2105 130e 00C0 rjmp .L216 2106 .L214: 2107 1310 8093 0000 sts FCParam+24,r24 2108 .L216: 2109 1314 8091 0000 lds r24,ParamSet+52 2110 1318 8B3F cpi r24,lo8(-5) 2111 131a 00F0 brlo .L219 2112 131c 8B3F cpi r24,lo8(-5) 2113 131e 01F4 brne .+2 2114 1320 00C0 rjmp .L336 2115 1322 8C3F cpi r24,lo8(-4) 2116 1324 01F4 brne .+2 2117 1326 00C0 rjmp .L337 2118 1328 8D3F cpi r24,lo8(-3) 2119 132a 01F4 brne .+2 2120 132c 00C0 rjmp .L338 2121 132e 8E3F cpi r24,lo8(-2) 2122 1330 01F4 brne .L221 2123 1332 8091 0000 lds r24,Poti4 2124 1336 8093 0000 sts FCParam+25,r24 2125 133a 00C0 rjmp .L221 2126 .L219: 2127 133c 8093 0000 sts FCParam+25,r24 2128 .L221: 2129 1340 8091 0000 lds r24,ParamSet+53 2130 1344 8B3F cpi r24,lo8(-5) 2131 1346 00F0 brlo .L224 2132 1348 8B3F cpi r24,lo8(-5) 2133 134a 01F4 brne .+2 2134 134c 00C0 rjmp .L339 2135 134e 8C3F cpi r24,lo8(-4) 2136 1350 01F4 brne .+2 2137 1352 00C0 rjmp .L340 2138 1354 8D3F cpi r24,lo8(-3) 2139 1356 01F4 brne .+2 2140 1358 00C0 rjmp .L341 2141 135a 8E3F cpi r24,lo8(-2) 2142 135c 01F4 brne .L226 2143 135e 8091 0000 lds r24,Poti4 2144 1362 8093 0000 sts FCParam+26,r24 2145 1366 00C0 rjmp .L226 2146 .L224: 2147 1368 8093 0000 sts FCParam+26,r24 2148 .L226: 2149 136c 8091 0000 lds r24,ParamSet+58 2150 1370 8B3F cpi r24,lo8(-5) 2151 1372 00F0 brlo .L229 2152 1374 8B3F cpi r24,lo8(-5) 2153 1376 01F4 brne .+2 2154 1378 00C0 rjmp .L342 2155 137a 8C3F cpi r24,lo8(-4) 2156 137c 01F4 brne .+2 2157 137e 00C0 rjmp .L343 2158 1380 8D3F cpi r24,lo8(-3) 2159 1382 01F4 brne .+2 2160 1384 00C0 rjmp .L344 2161 1386 8E3F cpi r24,lo8(-2) 2162 1388 01F4 brne .L231 2163 138a 8091 0000 lds r24,Poti4 2164 138e 8093 0000 sts FCParam+27,r24 2165 1392 00C0 rjmp .L231 2166 .L229: 2167 1394 8093 0000 sts FCParam+27,r24 2168 .L231: 2169 1398 8091 0000 lds r24,ParamSet+64 2170 139c 8B3F cpi r24,lo8(-5) 2171 139e 00F0 brlo .L234 2172 13a0 8B3F cpi r24,lo8(-5) 2173 13a2 01F4 brne .+2 2174 13a4 00C0 rjmp .L345 2175 13a6 8C3F cpi r24,lo8(-4) 2176 13a8 01F4 brne .+2 2177 13aa 00C0 rjmp .L346 2178 13ac 8D3F cpi r24,lo8(-3) 2179 13ae 01F4 brne .+2 2180 13b0 00C0 rjmp .L347 2181 13b2 8E3F cpi r24,lo8(-2) 2182 13b4 01F4 brne .L236 2183 13b6 8091 0000 lds r24,Poti4 2184 13ba 8093 0000 sts FCParam+29,r24 2185 13be 00C0 rjmp .L236 2186 .L234: 2187 13c0 8093 0000 sts FCParam+29,r24 2188 .L236: 2189 13c4 8091 0000 lds r24,FCParam+29 2190 13c8 8230 cpi r24,lo8(2) 2191 13ca 00F4 brsh .+2 2192 13cc 00C0 rjmp .L348 2193 13ce 8F3F cpi r24,lo8(-1) 2194 13d0 01F4 brne .+2 2195 13d2 00C0 rjmp .L349 2196 .L240: 2197 13d4 8091 0000 lds r24,ParamSet+66 2198 13d8 8B3F cpi r24,lo8(-5) 2199 13da 00F0 brlo .L241 2200 13dc 8B3F cpi r24,lo8(-5) 2201 13de 01F4 brne .+2 2202 13e0 00C0 rjmp .L350 2203 13e2 8C3F cpi r24,lo8(-4) 2204 13e4 01F4 brne .+2 2205 13e6 00C0 rjmp .L351 2206 13e8 8D3F cpi r24,lo8(-3) 2207 13ea 01F4 brne .+2 2208 13ec 00C0 rjmp .L352 2209 13ee 8E3F cpi r24,lo8(-2) 2210 13f0 01F4 brne .L243 2211 13f2 8091 0000 lds r24,Poti4 2212 13f6 8093 0000 sts FCParam+30,r24 2213 13fa 00C0 rjmp .L243 2214 .L241: 2215 13fc 8093 0000 sts FCParam+30,r24 2216 .L243: 2217 1400 8091 0000 lds r24,FCParam+30 2218 1404 8230 cpi r24,lo8(2) 2219 1406 00F4 brsh .+2 2220 1408 00C0 rjmp .L353 2221 140a 8F3F cpi r24,lo8(-1) 2222 140c 01F4 brne .+2 2223 140e 00C0 rjmp .L354 2224 .L247: 2225 1410 8091 0000 lds r24,ParamSet+85 2226 1414 8B3F cpi r24,lo8(-5) 2227 1416 00F4 brsh .+2 2228 1418 00C0 rjmp .L248 2229 141a 8B3F cpi r24,lo8(-5) 2230 141c 01F0 breq .L355 2231 141e 8C3F cpi r24,lo8(-4) 2232 1420 01F4 brne .+2 2233 1422 00C0 rjmp .L356 2234 1424 8D3F cpi r24,lo8(-3) 2235 1426 01F4 brne .+2 2236 1428 00C0 rjmp .L357 2237 142a 8E3F cpi r24,lo8(-2) 2238 142c 01F0 breq .+2 2239 142e 00C0 rjmp .L250 2240 1430 8091 0000 lds r24,Poti4 2241 1434 8093 0000 sts FCParam+28,r24 2242 1438 00C0 rjmp .L250 2243 .L355: 2244 143a 8091 0000 lds r24,Poti1 2245 143e 8093 0000 sts FCParam+28,r24 2246 1442 00C0 rjmp .L250 2247 .L276: 2248 1444 8091 0000 lds r24,Poti1 2249 1448 8093 0000 sts FCParam+4,r24 2250 144c 00C0 rjmp .L117 2251 .L282: 2252 144e 8091 0000 lds r24,Poti1 2253 1452 8093 0000 sts FCParam+7,r24 2254 1456 00C0 rjmp .L127 2255 .L279: 2256 1458 8091 0000 lds r24,Poti1 2257 145c 8093 0000 sts FCParam+5,r24 2258 1460 00C0 rjmp .L122 2259 .L265: 2260 1462 8091 0000 lds r24,Poti1 2261 1466 8093 0000 sts FCParam+1,r24 2262 146a 00C0 rjmp .L93 2263 .L262: 2264 146c 8091 0000 lds r24,Poti1 2265 1470 8093 0000 sts FCParam,r24 2266 1474 00C0 rjmp .L98 2267 .L268: 2268 1476 8091 0000 lds r24,Poti1 2269 147a 8093 0000 sts FCParam+2,r24 2270 147e 00C0 rjmp .L105 2271 .L272: 2272 1480 8091 0000 lds r24,Poti1 2273 1484 8093 0000 sts FCParam+3,r24 2274 1488 00C0 rjmp .L112 2275 .L288: 2276 148a 8091 0000 lds r24,Poti1 2277 148e 8093 0000 sts FCParam+6,r24 2278 1492 00C0 rjmp .L139 2279 .L285: 2280 1494 8091 0000 lds r24,Poti1 2281 1498 8093 0000 sts FCParam+8,r24 2282 149c 00C0 rjmp .L134 2283 .L294: 2284 149e 8091 0000 lds r24,Poti1 2285 14a2 8093 0000 sts FCParam+10,r24 2286 14a6 00C0 rjmp .L151 2287 .L291: 2288 14a8 8091 0000 lds r24,Poti1 2289 14ac 8093 0000 sts FCParam+9,r24 2290 14b0 00C0 rjmp .L144 2291 .L303: 2292 14b2 8091 0000 lds r24,Poti1 2293 14b6 8093 0000 sts FCParam+14,r24 2294 14ba 00C0 rjmp .L166 2295 .L300: 2296 14bc 8091 0000 lds r24,Poti1 2297 14c0 8093 0000 sts FCParam+13,r24 2298 14c4 00C0 rjmp .L161 2299 .L297: 2300 14c6 8091 0000 lds r24,Poti1 2301 14ca 8093 0000 sts FCParam+12,r24 2302 14ce 00C0 rjmp .L156 2303 .L350: 2304 14d0 8091 0000 lds r24,Poti1 2305 14d4 8093 0000 sts FCParam+30,r24 2306 14d8 00C0 rjmp .L243 2307 .L345: 2308 14da 8091 0000 lds r24,Poti1 2309 14de 8093 0000 sts FCParam+29,r24 2310 14e2 00C0 rjmp .L236 2311 .L342: 2312 14e4 8091 0000 lds r24,Poti1 2313 14e8 8093 0000 sts FCParam+27,r24 2314 14ec 00C0 rjmp .L231 2315 .L339: 2316 14ee 8091 0000 lds r24,Poti1 2317 14f2 8093 0000 sts FCParam+26,r24 2318 14f6 00C0 rjmp .L226 2319 .L336: 2320 14f8 8091 0000 lds r24,Poti1 2321 14fc 8093 0000 sts FCParam+25,r24 2322 1500 00C0 rjmp .L221 2323 .L333: 2324 1502 8091 0000 lds r24,Poti1 2325 1506 8093 0000 sts FCParam+24,r24 2326 150a 00C0 rjmp .L216 2327 .L330: 2328 150c 8091 0000 lds r24,Poti1 2329 1510 8093 0000 sts FCParam+23,r24 2330 1514 00C0 rjmp .L211 2331 .L327: 2332 1516 8091 0000 lds r24,Poti1 2333 151a 8093 0000 sts FCParam+22,r24 2334 151e 00C0 rjmp .L206 2335 .L324: 2336 1520 8091 0000 lds r24,Poti1 2337 1524 8093 0000 sts FCParam+21,r24 2338 1528 00C0 rjmp .L201 2339 .L321: 2340 152a 8091 0000 lds r24,Poti1 2341 152e 8093 0000 sts FCParam+20,r24 2342 1532 00C0 rjmp .L196 2343 .L318: 2344 1534 8091 0000 lds r24,Poti1 2345 1538 8093 0000 sts FCParam+19,r24 2346 153c 00C0 rjmp .L191 2347 .L315: 2348 153e 8091 0000 lds r24,Poti1 2349 1542 8093 0000 sts FCParam+18,r24 2350 1546 00C0 rjmp .L186 2351 .L312: 2352 1548 8091 0000 lds r24,Poti1 2353 154c 8093 0000 sts FCParam+17,r24 2354 1550 00C0 rjmp .L181 2355 .L309: 2356 1552 8091 0000 lds r24,Poti1 2357 1556 8093 0000 sts FCParam+16,r24 2358 155a 00C0 rjmp .L176 2359 .L306: 2360 155c 8091 0000 lds r24,Poti1 2361 1560 8093 0000 sts FCParam+15,r24 2362 1564 00C0 rjmp .L171 2363 .L348: 2364 1566 81E0 ldi r24,lo8(1) 2365 1568 8093 0000 sts FCParam+29,r24 2366 156c 00C0 rjmp .L240 2367 .L353: 2368 156e 81E0 ldi r24,lo8(1) 2369 1570 8093 0000 sts FCParam+30,r24 2370 1574 00C0 rjmp .L247 2371 .L349: 2372 1576 8093 0000 sts FCParam+29,r24 2373 157a 00C0 rjmp .L240 2374 .L354: 2375 157c 8093 0000 sts FCParam+30,r24 2376 1580 00C0 rjmp .L247 2377 .L319: 2378 1582 8091 0000 lds r24,Poti2 2379 1586 8093 0000 sts FCParam+19,r24 2380 158a 00C0 rjmp .L191 2381 .L322: 2382 158c 8091 0000 lds r24,Poti2 2383 1590 8093 0000 sts FCParam+20,r24 2384 1594 00C0 rjmp .L196 2385 .L310: 2386 1596 8091 0000 lds r24,Poti2 2387 159a 8093 0000 sts FCParam+16,r24 2388 159e 00C0 rjmp .L176 2389 .L313: 2390 15a0 8091 0000 lds r24,Poti2 2391 15a4 8093 0000 sts FCParam+17,r24 2392 15a8 00C0 rjmp .L181 2393 .L328: 2394 15aa 8091 0000 lds r24,Poti2 2395 15ae 8093 0000 sts FCParam+22,r24 2396 15b2 00C0 rjmp .L206 2397 .L325: 2398 15b4 8091 0000 lds r24,Poti2 2399 15b8 8093 0000 sts FCParam+21,r24 2400 15bc 00C0 rjmp .L201 2401 .L337: 2402 15be 8091 0000 lds r24,Poti2 2403 15c2 8093 0000 sts FCParam+25,r24 2404 15c6 00C0 rjmp .L221 2405 .L351: 2406 15c8 8091 0000 lds r24,Poti2 2407 15cc 8093 0000 sts FCParam+30,r24 2408 15d0 00C0 rjmp .L243 2409 .L343: 2410 15d2 8091 0000 lds r24,Poti2 2411 15d6 8093 0000 sts FCParam+27,r24 2412 15da 00C0 rjmp .L231 2413 .L346: 2414 15dc 8091 0000 lds r24,Poti2 2415 15e0 8093 0000 sts FCParam+29,r24 2416 15e4 00C0 rjmp .L236 2417 .L316: 2418 15e6 8091 0000 lds r24,Poti2 2419 15ea 8093 0000 sts FCParam+18,r24 2420 15ee 00C0 rjmp .L186 2421 .L307: 2422 15f0 8091 0000 lds r24,Poti2 2423 15f4 8093 0000 sts FCParam+15,r24 2424 15f8 00C0 rjmp .L171 2425 .L334: 2426 15fa 8091 0000 lds r24,Poti2 2427 15fe 8093 0000 sts FCParam+24,r24 2428 1602 00C0 rjmp .L216 2429 .L340: 2430 1604 8091 0000 lds r24,Poti2 2431 1608 8093 0000 sts FCParam+26,r24 2432 160c 00C0 rjmp .L226 2433 .L286: 2434 160e 8091 0000 lds r24,Poti2 2435 1612 8093 0000 sts FCParam+8,r24 2436 1616 00C0 rjmp .L134 2437 .L273: 2438 1618 8091 0000 lds r24,Poti2 2439 161c 8093 0000 sts FCParam+3,r24 2440 1620 00C0 rjmp .L112 2441 .L331: 2442 1622 8091 0000 lds r24,Poti2 2443 1626 8093 0000 sts FCParam+23,r24 2444 162a 00C0 rjmp .L211 2445 .L356: 2446 162c 8091 0000 lds r24,Poti2 2447 1630 8093 0000 sts FCParam+28,r24 2448 1634 00C0 rjmp .L250 2449 .L283: 2450 1636 8091 0000 lds r24,Poti2 2451 163a 8093 0000 sts FCParam+7,r24 2452 163e 00C0 rjmp .L127 2453 .L280: 2454 1640 8091 0000 lds r24,Poti2 2455 1644 8093 0000 sts FCParam+5,r24 2456 1648 00C0 rjmp .L122 2457 .L269: 2458 164a 8091 0000 lds r24,Poti2 2459 164e 8093 0000 sts FCParam+2,r24 2460 1652 00C0 rjmp .L105 2461 .L266: 2462 1654 8091 0000 lds r24,Poti2 2463 1658 8093 0000 sts FCParam+1,r24 2464 165c 00C0 rjmp .L93 2465 .L304: 2466 165e 8091 0000 lds r24,Poti2 2467 1662 8093 0000 sts FCParam+14,r24 2468 1666 00C0 rjmp .L166 2469 .L301: 2470 1668 8091 0000 lds r24,Poti2 2471 166c 8093 0000 sts FCParam+13,r24 2472 1670 00C0 rjmp .L161 2473 .L277: 2474 1672 8091 0000 lds r24,Poti2 2475 1676 8093 0000 sts FCParam+4,r24 2476 167a 00C0 rjmp .L117 2477 .L263: 2478 167c 8091 0000 lds r24,Poti2 2479 1680 8093 0000 sts FCParam,r24 2480 1684 00C0 rjmp .L98 2481 .L292: 2482 1686 8091 0000 lds r24,Poti2 2483 168a 8093 0000 sts FCParam+9,r24 2484 168e 00C0 rjmp .L144 2485 .L289: 2486 1690 8091 0000 lds r24,Poti2 2487 1694 8093 0000 sts FCParam+6,r24 2488 1698 00C0 rjmp .L139 2489 .L298: 2490 169a 8091 0000 lds r24,Poti2 2491 169e 8093 0000 sts FCParam+12,r24 2492 16a2 00C0 rjmp .L156 2493 .L295: 2494 16a4 8091 0000 lds r24,Poti2 2495 16a8 8093 0000 sts FCParam+10,r24 2496 16ac 00C0 rjmp .L151 2497 .L352: 2498 16ae 8091 0000 lds r24,Poti3 2499 16b2 8093 0000 sts FCParam+30,r24 2500 16b6 00C0 rjmp .L243 2501 .L357: 2502 16b8 8091 0000 lds r24,Poti3 2503 16bc 8093 0000 sts FCParam+28,r24 2504 16c0 00C0 rjmp .L250 2505 .L329: 2506 16c2 8091 0000 lds r24,Poti3 2507 16c6 8093 0000 sts FCParam+22,r24 2508 16ca 00C0 rjmp .L206 2509 .L305: 2510 16cc 8091 0000 lds r24,Poti3 2511 16d0 8093 0000 sts FCParam+14,r24 2512 16d4 00C0 rjmp .L166 2513 .L341: 2514 16d6 8091 0000 lds r24,Poti3 2515 16da 8093 0000 sts FCParam+26,r24 2516 16de 00C0 rjmp .L226 2517 .L284: 2518 16e0 8091 0000 lds r24,Poti3 2519 16e4 8093 0000 sts FCParam+7,r24 2520 16e8 00C0 rjmp .L127 2521 .L323: 2522 16ea 8091 0000 lds r24,Poti3 2523 16ee 8093 0000 sts FCParam+20,r24 2524 16f2 00C0 rjmp .L196 2525 .L299: 2526 16f4 8091 0000 lds r24,Poti3 2527 16f8 8093 0000 sts FCParam+12,r24 2528 16fc 00C0 rjmp .L156 2529 .L335: 2530 16fe 8091 0000 lds r24,Poti3 2531 1702 8093 0000 sts FCParam+24,r24 2532 1706 00C0 rjmp .L216 2533 .L278: 2534 1708 8091 0000 lds r24,Poti3 2535 170c 8093 0000 sts FCParam+4,r24 2536 1710 00C0 rjmp .L117 2537 .L317: 2538 1712 8091 0000 lds r24,Poti3 2539 1716 8093 0000 sts FCParam+18,r24 2540 171a 00C0 rjmp .L186 2541 .L293: 2542 171c 8091 0000 lds r24,Poti3 2543 1720 8093 0000 sts FCParam+9,r24 2544 1724 00C0 rjmp .L144 2545 .L347: 2546 1726 8091 0000 lds r24,Poti3 2547 172a 8093 0000 sts FCParam+29,r24 2548 172e 00C0 rjmp .L236 2549 .L270: 2550 1730 8091 0000 lds r24,Poti3 2551 1734 8093 0000 sts FCParam+2,r24 2552 1738 00C0 rjmp .L105 2553 .L320: 2554 173a 8091 0000 lds r24,Poti3 2555 173e 8093 0000 sts FCParam+19,r24 2556 1742 00C0 rjmp .L191 2557 .L296: 2558 1744 8091 0000 lds r24,Poti3 2559 1748 8093 0000 sts FCParam+10,r24 2560 174c 00C0 rjmp .L151 2561 .L308: 2562 174e 8091 0000 lds r24,Poti3 2563 1752 8093 0000 sts FCParam+15,r24 2564 1756 00C0 rjmp .L171 2565 .L274: 2566 1758 8091 0000 lds r24,Poti3 2567 175c 8093 0000 sts FCParam+3,r24 2568 1760 00C0 rjmp .L112 2569 .L314: 2570 1762 8091 0000 lds r24,Poti3 2571 1766 8093 0000 sts FCParam+17,r24 2572 176a 00C0 rjmp .L181 2573 .L290: 2574 176c 8091 0000 lds r24,Poti3 2575 1770 8093 0000 sts FCParam+6,r24 2576 1774 00C0 rjmp .L139 2577 .L332: 2578 1776 8091 0000 lds r24,Poti3 2579 177a 8093 0000 sts FCParam+23,r24 2580 177e 00C0 rjmp .L211 2581 .L264: 2582 1780 8091 0000 lds r24,Poti3 2583 1784 8093 0000 sts FCParam,r24 2584 1788 00C0 rjmp .L98 2585 .L326: 2586 178a 8091 0000 lds r24,Poti3 2587 178e 8093 0000 sts FCParam+21,r24 2588 1792 00C0 rjmp .L201 2589 .L302: 2590 1794 8091 0000 lds r24,Poti3 2591 1798 8093 0000 sts FCParam+13,r24 2592 179c 00C0 rjmp .L161 2593 .L338: 2594 179e 8091 0000 lds r24,Poti3 2595 17a2 8093 0000 sts FCParam+25,r24 2596 17a6 00C0 rjmp .L221 2597 .L281: 2598 17a8 8091 0000 lds r24,Poti3 2599 17ac 8093 0000 sts FCParam+5,r24 2600 17b0 00C0 rjmp .L122 2601 .L311: 2602 17b2 8091 0000 lds r24,Poti3 2603 17b6 8093 0000 sts FCParam+16,r24 2604 17ba 00C0 rjmp .L176 2605 .L287: 2606 17bc 8091 0000 lds r24,Poti3 2607 17c0 8093 0000 sts FCParam+8,r24 2608 17c4 00C0 rjmp .L134 2609 .L344: 2610 17c6 8091 0000 lds r24,Poti3 2611 17ca 8093 0000 sts FCParam+27,r24 2612 17ce 00C0 rjmp .L231 2613 .L267: 2614 17d0 8091 0000 lds r24,Poti3 2615 17d4 8093 0000 sts FCParam+1,r24 2616 17d8 00C0 rjmp .L93 2618 .global SendMotorData 2620 SendMotorData: 2621 /* prologue: function */ 2622 /* frame size = 0 */ 2623 17da 8091 0000 lds r24,MKFlags 2624 17de 80FD sbrc r24,0 2625 17e0 00C0 rjmp .L359 2626 17e2 8091 0000 lds r24,MKFlags 2627 17e6 857F andi r24,lo8(-11) 2628 17e8 8093 0000 sts MKFlags,r24 2629 17ec 9091 0000 lds r25,MotorTest_Active 2630 17f0 E0E0 ldi r30,lo8(Motor) 2631 17f2 F0E0 ldi r31,hi8(Motor) 2632 17f4 A0E0 ldi r26,lo8(MotorTest) 2633 17f6 B0E0 ldi r27,hi8(MotorTest) 2634 17f8 00C0 rjmp .L362 2635 .L367: 2636 17fa 1082 st Z,__zero_reg__ 2637 17fc 3596 adiw r30,5 2638 17fe 1196 adiw r26,1 2639 1800 80E0 ldi r24,hi8(Motor+60) 2640 1802 E030 cpi r30,lo8(Motor+60) 2641 1804 F807 cpc r31,r24 2642 1806 01F0 breq .L366 2643 .L362: 2644 1808 9923 tst r25 2645 180a 01F0 breq .L367 2646 180c 8C91 ld r24,X 2647 180e 8083 st Z,r24 2648 1810 3596 adiw r30,5 2649 1812 1196 adiw r26,1 2650 1814 80E0 ldi r24,hi8(Motor+60) 2651 1816 E030 cpi r30,lo8(Motor+60) 2652 1818 F807 cpc r31,r24 2653 181a 01F4 brne .L362 2654 .L366: 2655 181c 9923 tst r25 2656 181e 01F0 breq .L359 2657 1820 9150 subi r25,lo8(-(-1)) 2658 1822 9093 0000 sts MotorTest_Active,r25 2659 .L359: 2660 1826 E0E0 ldi r30,lo8(DebugOut+26) 2661 1828 F0E0 ldi r31,hi8(DebugOut+26) 2662 182a 8091 0000 lds r24,Motor 2663 182e 8093 0000 sts DebugOut+26,r24 2664 1832 1092 0000 sts DebugOut+27,__zero_reg__ 2665 1836 8091 0000 lds r24,Motor+5 2666 183a 8093 0000 sts DebugOut+28,r24 2667 183e 1382 std Z+3,__zero_reg__ 2668 1840 8091 0000 lds r24,Motor+15 2669 1844 8093 0000 sts DebugOut+30,r24 2670 1848 1582 std Z+5,__zero_reg__ 2671 184a 8091 0000 lds r24,Motor+10 2672 184e 8093 0000 sts DebugOut+32,r24 2673 1852 1782 std Z+7,__zero_reg__ 2674 1854 80E0 ldi r24,lo8(0) 2675 1856 0E94 0000 call I2C_Start 2676 /* epilogue start */ 2677 185a 0895 ret 2679 .global SetNeutral 2681 SetNeutral: 2682 185c 2F92 push r2 2683 185e 3F92 push r3 2684 1860 4F92 push r4 2685 1862 5F92 push r5 2686 1864 6F92 push r6 2687 1866 7F92 push r7 2688 1868 8F92 push r8 2689 186a 9F92 push r9 2690 186c AF92 push r10 2691 186e BF92 push r11 2692 1870 CF92 push r12 2693 1872 DF92 push r13 2694 1874 EF92 push r14 2695 1876 FF92 push r15 2696 1878 0F93 push r16 2697 187a 1F93 push r17 2698 /* prologue: function */ 2699 /* frame size = 0 */ 2700 187c 582E mov r5,r24 2701 187e 1092 0000 sts (AdBiasAccNick)+1,__zero_reg__ 2702 1882 1092 0000 sts AdBiasAccNick,__zero_reg__ 2703 1886 1092 0000 sts (AdBiasAccRoll)+1,__zero_reg__ 2704 188a 1092 0000 sts AdBiasAccRoll,__zero_reg__ 2705 188e 80E0 ldi r24,lo8(0x0) 2706 1890 90E0 ldi r25,hi8(0x0) 2707 1892 A0E0 ldi r26,hlo8(0x0) 2708 1894 B0E0 ldi r27,hhi8(0x0) 2709 1896 8093 0000 sts AdBiasAccTop,r24 2710 189a 9093 0000 sts (AdBiasAccTop)+1,r25 2711 189e A093 0000 sts (AdBiasAccTop)+2,r26 2712 18a2 B093 0000 sts (AdBiasAccTop)+3,r27 2713 18a6 1092 0000 sts (BiasHiResGyroNick)+1,__zero_reg__ 2714 18aa 1092 0000 sts BiasHiResGyroNick,__zero_reg__ 2715 18ae 1092 0000 sts (BiasHiResGyroRoll)+1,__zero_reg__ 2716 18b2 1092 0000 sts BiasHiResGyroRoll,__zero_reg__ 2717 18b6 1092 0000 sts (AdBiasGyroYaw)+1,__zero_reg__ 2718 18ba 1092 0000 sts AdBiasGyroYaw,__zero_reg__ 2719 18be 1092 0000 sts FCParam+24,__zero_reg__ 2720 18c2 1092 0000 sts FCParam+25,__zero_reg__ 2721 18c6 1092 0000 sts ExpandBaro,__zero_reg__ 2722 18ca 84E6 ldi r24,lo8(100) 2723 18cc 90E0 ldi r25,hi8(100) 2724 18ce 0E94 0000 call Delay_ms_Mess 2725 18d2 8091 0000 lds r24,BoardRelease 2726 18d6 8D30 cpi r24,lo8(13) 2727 18d8 01F4 brne .+2 2728 18da 00C0 rjmp .L384 2729 .L369: 2730 18dc 8091 0000 lds r24,ParamSet+8 2731 18e0 80FF sbrs r24,0 2732 18e2 00C0 rjmp .L370 2733 18e4 8091 0000 lds r24,AdAirPressure 2734 18e8 9091 0000 lds r25,(AdAirPressure)+1 2735 18ec 875B subi r24,lo8(951) 2736 18ee 9340 sbci r25,hi8(951) 2737 18f0 04F0 brlt .+2 2738 18f2 00C0 rjmp .L371 2739 18f4 8091 0000 lds r24,AdAirPressure 2740 18f8 9091 0000 lds r25,(AdAirPressure)+1 2741 18fc 8E5E subi r24,lo8(750) 2742 18fe 9240 sbci r25,hi8(750) 2743 1900 04F4 brge .+2 2744 1902 00C0 rjmp .L371 2745 .L370: 2746 1904 2224 clr r2 2747 1906 EE24 clr r14 2748 1908 FF24 clr r15 2749 190a 8701 movw r16,r14 2750 190c AA24 clr r10 2751 190e BB24 clr r11 2752 1910 6501 movw r12,r10 2753 1912 6624 clr r6 2754 1914 7724 clr r7 2755 1916 4301 movw r8,r6 2756 .L372: 2757 1918 8AE0 ldi r24,lo8(10) 2758 191a 90E0 ldi r25,hi8(10) 2759 191c 0E94 0000 call Delay_ms_Mess 2760 1920 8091 0000 lds r24,AdValueGyroNick 2761 1924 9091 0000 lds r25,(AdValueGyroNick)+1 2762 1928 880F lsl r24 2763 192a 991F rol r25 2764 192c 880F lsl r24 2765 192e 991F rol r25 2766 1930 880F lsl r24 2767 1932 991F rol r25 2768 1934 AA27 clr r26 2769 1936 97FD sbrc r25,7 2770 1938 A095 com r26 2771 193a BA2F mov r27,r26 2772 193c E80E add r14,r24 2773 193e F91E adc r15,r25 2774 1940 0A1F adc r16,r26 2775 1942 1B1F adc r17,r27 2776 1944 8091 0000 lds r24,AdValueGyroRoll 2777 1948 9091 0000 lds r25,(AdValueGyroRoll)+1 2778 194c 880F lsl r24 2779 194e 991F rol r25 2780 1950 880F lsl r24 2781 1952 991F rol r25 2782 1954 880F lsl r24 2783 1956 991F rol r25 2784 1958 AA27 clr r26 2785 195a 97FD sbrc r25,7 2786 195c A095 com r26 2787 195e BA2F mov r27,r26 2788 1960 A80E add r10,r24 2789 1962 B91E adc r11,r25 2790 1964 CA1E adc r12,r26 2791 1966 DB1E adc r13,r27 2792 1968 8091 0000 lds r24,AdValueGyroYaw 2793 196c 9091 0000 lds r25,(AdValueGyroYaw)+1 2794 1970 AA27 clr r26 2795 1972 97FD sbrc r25,7 2796 1974 A095 com r26 2797 1976 BA2F mov r27,r26 2798 1978 680E add r6,r24 2799 197a 791E adc r7,r25 2800 197c 8A1E adc r8,r26 2801 197e 9B1E adc r9,r27 2802 1980 2394 inc r2 2803 1982 B0E2 ldi r27,lo8(32) 2804 1984 2B16 cp r2,r27 2805 1986 01F4 brne .L372 2806 1988 80E1 ldi r24,lo8(16) 2807 198a 90E0 ldi r25,hi8(16) 2808 198c A0E0 ldi r26,hlo8(16) 2809 198e B0E0 ldi r27,hhi8(16) 2810 1990 E80E add r14,r24 2811 1992 F91E adc r15,r25 2812 1994 0A1F adc r16,r26 2813 1996 1B1F adc r17,r27 2814 1998 C801 movw r24,r16 2815 199a B701 movw r22,r14 2816 199c 20E2 ldi r18,lo8(32) 2817 199e 30E0 ldi r19,hi8(32) 2818 19a0 40E0 ldi r20,hlo8(32) 2819 19a2 50E0 ldi r21,hhi8(32) 2820 19a4 0E94 0000 call __divmodsi4 2821 19a8 3093 0000 sts (BiasHiResGyroNick)+1,r19 2822 19ac 2093 0000 sts BiasHiResGyroNick,r18 2823 19b0 80E1 ldi r24,lo8(16) 2824 19b2 90E0 ldi r25,hi8(16) 2825 19b4 A0E0 ldi r26,hlo8(16) 2826 19b6 B0E0 ldi r27,hhi8(16) 2827 19b8 A80E add r10,r24 2828 19ba B91E adc r11,r25 2829 19bc CA1E adc r12,r26 2830 19be DB1E adc r13,r27 2831 19c0 C601 movw r24,r12 2832 19c2 B501 movw r22,r10 2833 19c4 20E2 ldi r18,lo8(32) 2834 19c6 30E0 ldi r19,hi8(32) 2835 19c8 40E0 ldi r20,hlo8(32) 2836 19ca 50E0 ldi r21,hhi8(32) 2837 19cc 0E94 0000 call __divmodsi4 2838 19d0 3093 0000 sts (BiasHiResGyroRoll)+1,r19 2839 19d4 2093 0000 sts BiasHiResGyroRoll,r18 2840 19d8 80E1 ldi r24,lo8(16) 2841 19da 90E0 ldi r25,hi8(16) 2842 19dc A0E0 ldi r26,hlo8(16) 2843 19de B0E0 ldi r27,hhi8(16) 2844 19e0 680E add r6,r24 2845 19e2 791E adc r7,r25 2846 19e4 8A1E adc r8,r26 2847 19e6 9B1E adc r9,r27 2848 19e8 C401 movw r24,r8 2849 19ea B301 movw r22,r6 2850 19ec 20E2 ldi r18,lo8(32) 2851 19ee 30E0 ldi r19,hi8(32) 2852 19f0 40E0 ldi r20,hlo8(32) 2853 19f2 50E0 ldi r21,hhi8(32) 2854 19f4 0E94 0000 call __divmodsi4 2855 19f8 3093 0000 sts (AdBiasGyroYaw)+1,r19 2856 19fc 2093 0000 sts AdBiasGyroYaw,r18 2857 1a00 5520 tst r5 2858 1a02 01F4 brne .+2 2859 1a04 00C0 rjmp .L373 2860 1a06 2224 clr r2 2861 1a08 EE24 clr r14 2862 1a0a FF24 clr r15 2863 1a0c 8701 movw r16,r14 2864 1a0e AA24 clr r10 2865 1a10 BB24 clr r11 2866 1a12 6501 movw r12,r10 2867 1a14 6624 clr r6 2868 1a16 7724 clr r7 2869 1a18 4301 movw r8,r6 2870 .L374: 2871 1a1a 8AE0 ldi r24,lo8(10) 2872 1a1c 90E0 ldi r25,hi8(10) 2873 1a1e 0E94 0000 call Delay_ms_Mess 2874 1a22 8091 0000 lds r24,AdValueAccNick 2875 1a26 9091 0000 lds r25,(AdValueAccNick)+1 2876 1a2a AA27 clr r26 2877 1a2c 97FD sbrc r25,7 2878 1a2e A095 com r26 2879 1a30 BA2F mov r27,r26 2880 1a32 E80E add r14,r24 2881 1a34 F91E adc r15,r25 2882 1a36 0A1F adc r16,r26 2883 1a38 1B1F adc r17,r27 2884 1a3a 8091 0000 lds r24,AdValueAccRoll 2885 1a3e 9091 0000 lds r25,(AdValueAccRoll)+1 2886 1a42 AA27 clr r26 2887 1a44 97FD sbrc r25,7 2888 1a46 A095 com r26 2889 1a48 BA2F mov r27,r26 2890 1a4a A80E add r10,r24 2891 1a4c B91E adc r11,r25 2892 1a4e CA1E adc r12,r26 2893 1a50 DB1E adc r13,r27 2894 1a52 8091 0000 lds r24,AdValueAccZ 2895 1a56 9091 0000 lds r25,(AdValueAccZ)+1 2896 1a5a AA27 clr r26 2897 1a5c 97FD sbrc r25,7 2898 1a5e A095 com r26 2899 1a60 BA2F mov r27,r26 2900 1a62 680E add r6,r24 2901 1a64 791E adc r7,r25 2902 1a66 8A1E adc r8,r26 2903 1a68 9B1E adc r9,r27 2904 1a6a 2394 inc r2 2905 1a6c 9AE0 ldi r25,lo8(10) 2906 1a6e 2916 cp r2,r25 2907 1a70 01F4 brne .L374 2908 1a72 C701 movw r24,r14 2909 1a74 F7FC sbrc r15,7 2910 1a76 00C0 rjmp .L385 2911 .L375: 2912 1a78 0596 adiw r24,5 2913 1a7a 6AE0 ldi r22,lo8(10) 2914 1a7c 70E0 ldi r23,hi8(10) 2915 1a7e 0E94 0000 call __divmodhi4 2916 1a82 162F mov r17,r22 2917 1a84 072F mov r16,r23 2918 1a86 6093 0000 sts AdBiasAccNick,r22 2919 1a8a 7093 0000 sts AdBiasAccNick+1,r23 2920 1a8e C501 movw r24,r10 2921 1a90 B7FC sbrc r11,7 2922 1a92 00C0 rjmp .L386 2923 .L376: 2924 1a94 0596 adiw r24,5 2925 1a96 6AE0 ldi r22,lo8(10) 2926 1a98 70E0 ldi r23,hi8(10) 2927 1a9a 0E94 0000 call __divmodhi4 2928 1a9e 7093 0000 sts (AdBiasAccRoll)+1,r23 2929 1aa2 6093 0000 sts AdBiasAccRoll,r22 2930 1aa6 C301 movw r24,r6 2931 1aa8 77FC sbrc r7,7 2932 1aaa 00C0 rjmp .L387 2933 .L377: 2934 1aac 0596 adiw r24,5 2935 1aae 6AE0 ldi r22,lo8(10) 2936 1ab0 70E0 ldi r23,hi8(10) 2937 1ab2 0E94 0000 call __divmodhi4 2938 1ab6 8827 clr r24 2939 1ab8 77FD sbrc r23,7 2940 1aba 8095 com r24 2941 1abc 982F mov r25,r24 2942 1abe 0E94 0000 call __floatsisf 2943 1ac2 6093 0000 sts AdBiasAccTop,r22 2944 1ac6 7093 0000 sts (AdBiasAccTop)+1,r23 2945 1aca 8093 0000 sts (AdBiasAccTop)+2,r24 2946 1ace 9093 0000 sts (AdBiasAccTop)+3,r25 2947 1ad2 84E0 ldi r24,lo8(4) 2948 1ad4 90E0 ldi r25,hi8(4) 2949 1ad6 612F mov r22,r17 2950 1ad8 702F mov r23,r16 2951 1ada 0E94 0000 call SetParamWord 2952 1ade 6091 0000 lds r22,AdBiasAccRoll 2953 1ae2 7091 0000 lds r23,(AdBiasAccRoll)+1 2954 1ae6 86E0 ldi r24,lo8(6) 2955 1ae8 90E0 ldi r25,hi8(6) 2956 1aea 0E94 0000 call SetParamWord 2957 1aee 6091 0000 lds r22,AdBiasAccTop 2958 1af2 7091 0000 lds r23,(AdBiasAccTop)+1 2959 1af6 8091 0000 lds r24,(AdBiasAccTop)+2 2960 1afa 9091 0000 lds r25,(AdBiasAccTop)+3 2961 1afe 0E94 0000 call __fixunssfsi 2962 1b02 9B01 movw r18,r22 2963 1b04 AC01 movw r20,r24 2964 1b06 88E0 ldi r24,lo8(8) 2965 1b08 90E0 ldi r25,hi8(8) 2966 1b0a B901 movw r22,r18 2967 1b0c 0E94 0000 call SetParamWord 2968 .L378: 2969 1b10 8091 0000 lds r24,AirPressure 2970 1b14 9091 0000 lds r25,(AirPressure)+1 2971 1b18 A091 0000 lds r26,(AirPressure)+2 2972 1b1c B091 0000 lds r27,(AirPressure)+3 2973 1b20 8093 0000 sts StartAirPressure,r24 2974 1b24 9093 0000 sts (StartAirPressure)+1,r25 2975 1b28 A093 0000 sts (StartAirPressure)+2,r26 2976 1b2c B093 0000 sts (StartAirPressure)+3,r27 2977 1b30 84E6 ldi r24,lo8(100) 2978 1b32 90E0 ldi r25,hi8(100) 2979 1b34 0E94 0000 call Delay_ms_Mess 2980 1b38 1092 0000 sts (ReadingVario)+1,__zero_reg__ 2981 1b3c 1092 0000 sts ReadingVario,__zero_reg__ 2982 1b40 6091 0000 lds r22,AdValueAccNick 2983 1b44 7091 0000 lds r23,(AdValueAccNick)+1 2984 1b48 8827 clr r24 2985 1b4a 77FD sbrc r23,7 2986 1b4c 8095 com r24 2987 1b4e 982F mov r25,r24 2988 1b50 26E0 ldi r18,lo8(6) 2989 1b52 30E0 ldi r19,hi8(6) 2990 1b54 40E0 ldi r20,hlo8(6) 2991 1b56 50E0 ldi r21,hhi8(6) 2992 1b58 0E94 0000 call __mulsi3 2993 1b5c 3B01 movw r6,r22 2994 1b5e 4C01 movw r8,r24 2995 1b60 7093 0000 sts (AccNick)+1,r23 2996 1b64 6093 0000 sts AccNick,r22 2997 1b68 6091 0000 lds r22,AdValueAccRoll 2998 1b6c 7091 0000 lds r23,(AdValueAccRoll)+1 2999 1b70 8827 clr r24 3000 1b72 77FD sbrc r23,7 3001 1b74 8095 com r24 3002 1b76 982F mov r25,r24 3003 1b78 0E94 0000 call __mulsi3 3004 1b7c 5B01 movw r10,r22 3005 1b7e 6C01 movw r12,r24 3006 1b80 7093 0000 sts (AccRoll)+1,r23 3007 1b84 6093 0000 sts AccRoll,r22 3008 1b88 8091 0000 lds r24,AdValueAccTop 3009 1b8c 9091 0000 lds r25,(AdValueAccTop)+1 3010 1b90 9093 0000 sts (AccTop)+1,r25 3011 1b94 8093 0000 sts AccTop,r24 3012 1b98 8091 0000 lds r24,AdValueAccTop 3013 1b9c 9091 0000 lds r25,(AdValueAccTop)+1 3014 1ba0 982F mov r25,r24 3015 1ba2 8827 clr r24 3016 1ba4 990F lsl r25 3017 1ba6 990F lsl r25 3018 1ba8 AA27 clr r26 3019 1baa 97FD sbrc r25,7 3020 1bac A095 com r26 3021 1bae BA2F mov r27,r26 3022 1bb0 8093 0000 sts ReadingIntegralTop,r24 3023 1bb4 9093 0000 sts (ReadingIntegralTop)+1,r25 3024 1bb8 A093 0000 sts (ReadingIntegralTop)+2,r26 3025 1bbc B093 0000 sts (ReadingIntegralTop)+3,r27 3026 1bc0 1092 0000 sts (GyroNick)+1,__zero_reg__ 3027 1bc4 1092 0000 sts GyroNick,__zero_reg__ 3028 1bc8 1092 0000 sts (GyroRoll)+1,__zero_reg__ 3029 1bcc 1092 0000 sts GyroRoll,__zero_reg__ 3030 1bd0 1092 0000 sts (GyroYaw)+1,__zero_reg__ 3031 1bd4 1092 0000 sts GyroYaw,__zero_reg__ 3032 1bd8 E090 0000 lds r14,ParamSet+23 3033 1bdc 2E2C mov r2,r14 3034 1bde 3324 clr r3 3035 1be0 4424 clr r4 3036 1be2 5524 clr r5 3037 1be4 8824 clr r8 3038 1be6 77FC sbrc r7,7 3039 1be8 8094 com r8 3040 1bea 982C mov r9,r8 3041 1bec C401 movw r24,r8 3042 1bee B301 movw r22,r6 3043 1bf0 A201 movw r20,r4 3044 1bf2 9101 movw r18,r2 3045 1bf4 0E94 0000 call __mulsi3 3046 1bf8 3B01 movw r6,r22 3047 1bfa 4C01 movw r8,r24 3048 1bfc 6093 0000 sts IntegralGyroNick,r22 3049 1c00 7093 0000 sts (IntegralGyroNick)+1,r23 3050 1c04 8093 0000 sts (IntegralGyroNick)+2,r24 3051 1c08 9093 0000 sts (IntegralGyroNick)+3,r25 3052 1c0c CC24 clr r12 3053 1c0e B7FC sbrc r11,7 3054 1c10 C094 com r12 3055 1c12 DC2C mov r13,r12 3056 1c14 C601 movw r24,r12 3057 1c16 B501 movw r22,r10 3058 1c18 0E94 0000 call __mulsi3 3059 1c1c 6093 0000 sts IntegralGyroRoll,r22 3060 1c20 7093 0000 sts (IntegralGyroRoll)+1,r23 3061 1c24 8093 0000 sts (IntegralGyroRoll)+2,r24 3062 1c28 9093 0000 sts (IntegralGyroRoll)+3,r25 3063 1c2c 6092 0000 sts ReadingIntegralGyroNick2,r6 3064 1c30 7092 0000 sts (ReadingIntegralGyroNick2)+1,r7 3065 1c34 8092 0000 sts (ReadingIntegralGyroNick2)+2,r8 3066 1c38 9092 0000 sts (ReadingIntegralGyroNick2)+3,r9 3067 1c3c 6093 0000 sts ReadingIntegralGyroRoll2,r22 3068 1c40 7093 0000 sts (ReadingIntegralGyroRoll2)+1,r23 3069 1c44 8093 0000 sts (ReadingIntegralGyroRoll2)+2,r24 3070 1c48 9093 0000 sts (ReadingIntegralGyroRoll2)+3,r25 3071 1c4c 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ 3072 1c50 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ 3073 1c54 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ 3074 1c58 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ 3075 1c5c 2091 0000 lds r18,CompassHeading 3076 1c60 3091 0000 lds r19,(CompassHeading)+1 3077 1c64 3093 0000 sts (CompassCourse)+1,r19 3078 1c68 2093 0000 sts CompassCourse,r18 3079 1c6c 8AE2 ldi r24,lo8(42) 3080 1c6e E89E mul r14,r24 3081 1c70 7001 movw r14,r0 3082 1c72 1124 clr r1 3083 1c74 0027 clr r16 3084 1c76 F7FC sbrc r15,7 3085 1c78 0095 com r16 3086 1c7a 102F mov r17,r16 3087 1c7c 4427 clr r20 3088 1c7e 37FD sbrc r19,7 3089 1c80 4095 com r20 3090 1c82 542F mov r21,r20 3091 1c84 C801 movw r24,r16 3092 1c86 B701 movw r22,r14 3093 1c88 0E94 0000 call __mulsi3 3094 1c8c 6093 0000 sts YawGyroHeading,r22 3095 1c90 7093 0000 sts (YawGyroHeading)+1,r23 3096 1c94 8093 0000 sts (YawGyroHeading)+2,r24 3097 1c98 9093 0000 sts (YawGyroHeading)+3,r25 3098 1c9c 1092 0000 sts (YawGyroDrift)+1,__zero_reg__ 3099 1ca0 1092 0000 sts YawGyroDrift,__zero_reg__ 3100 1ca4 82E3 ldi r24,lo8(50) 3101 1ca6 90E0 ldi r25,hi8(50) 3102 1ca8 9093 0000 sts (BeepTime)+1,r25 3103 1cac 8093 0000 sts BeepTime,r24 3104 1cb0 6091 0000 lds r22,ParamSet+54 3105 1cb4 70E0 ldi r23,lo8(0) 3106 1cb6 80E0 ldi r24,lo8(0) 3107 1cb8 90E0 ldi r25,hi8(0) 3108 1cba 24EC ldi r18,lo8(2500) 3109 1cbc 39E0 ldi r19,hi8(2500) 3110 1cbe 40E0 ldi r20,hlo8(2500) 3111 1cc0 50E0 ldi r21,hhi8(2500) 3112 1cc2 0E94 0000 call __mulsi3 3113 1cc6 6856 subi r22,lo8(-(15000)) 3114 1cc8 754C sbci r23,hi8(-(15000)) 3115 1cca 8F4F sbci r24,hlo8(-(15000)) 3116 1ccc 9F4F sbci r25,hhi8(-(15000)) 3117 1cce 6093 0000 sts TurnOver180Nick,r22 3118 1cd2 7093 0000 sts (TurnOver180Nick)+1,r23 3119 1cd6 8093 0000 sts (TurnOver180Nick)+2,r24 3120 1cda 9093 0000 sts (TurnOver180Nick)+3,r25 3121 1cde 6091 0000 lds r22,ParamSet+55 3122 1ce2 70E0 ldi r23,lo8(0) 3123 1ce4 80E0 ldi r24,lo8(0) 3124 1ce6 90E0 ldi r25,hi8(0) 3125 1ce8 0E94 0000 call __mulsi3 3126 1cec 6856 subi r22,lo8(-(15000)) 3127 1cee 754C sbci r23,hi8(-(15000)) 3128 1cf0 8F4F sbci r24,hlo8(-(15000)) 3129 1cf2 9F4F sbci r25,hhi8(-(15000)) 3130 1cf4 6093 0000 sts TurnOver180Roll,r22 3131 1cf8 7093 0000 sts (TurnOver180Roll)+1,r23 3132 1cfc 8093 0000 sts (TurnOver180Roll)+2,r24 3133 1d00 9093 0000 sts (TurnOver180Roll)+3,r25 3134 1d04 1092 0000 sts (ExternHeightValue)+1,__zero_reg__ 3135 1d08 1092 0000 sts ExternHeightValue,__zero_reg__ 3136 1d0c 1092 0000 sts (GPSStickNick)+1,__zero_reg__ 3137 1d10 1092 0000 sts GPSStickNick,__zero_reg__ 3138 1d14 1092 0000 sts (GPSStickRoll)+1,__zero_reg__ 3139 1d18 1092 0000 sts GPSStickRoll,__zero_reg__ 3140 1d1c 8091 0000 lds r24,MKFlags 3141 1d20 8460 ori r24,lo8(4) 3142 1d22 8093 0000 sts MKFlags,r24 3143 1d26 8FEF ldi r24,lo8(-1) 3144 1d28 8093 0000 sts FCParam+31,r24 3145 1d2c 1092 0000 sts FCParam+32,__zero_reg__ 3146 1d30 80E2 ldi r24,lo8(32) 3147 1d32 8093 0000 sts FCParam+33,r24 3148 1d36 E091 0000 lds r30,ParamSet+4 3149 1d3a F0E0 ldi r31,lo8(0) 3150 1d3c EE0F lsl r30 3151 1d3e FF1F rol r31 3152 1d40 E050 subi r30,lo8(-(PPM_in)) 3153 1d42 F040 sbci r31,hi8(-(PPM_in)) 3154 1d44 8081 ld r24,Z 3155 1d46 9181 ldd r25,Z+1 3156 1d48 8259 subi r24,lo8(-(110)) 3157 1d4a 9F4F sbci r25,hi8(-(110)) 3158 1d4c 9093 0000 sts (Poti1)+1,r25 3159 1d50 8093 0000 sts Poti1,r24 3160 1d54 E091 0000 lds r30,ParamSet+5 3161 1d58 F0E0 ldi r31,lo8(0) 3162 1d5a EE0F lsl r30 3163 1d5c FF1F rol r31 3164 1d5e E050 subi r30,lo8(-(PPM_in)) 3165 1d60 F040 sbci r31,hi8(-(PPM_in)) 3166 1d62 8081 ld r24,Z 3167 1d64 9181 ldd r25,Z+1 3168 1d66 8259 subi r24,lo8(-(110)) 3169 1d68 9F4F sbci r25,hi8(-(110)) 3170 1d6a 9093 0000 sts (Poti2)+1,r25 3171 1d6e 8093 0000 sts Poti2,r24 3172 1d72 E091 0000 lds r30,ParamSet+6 3173 1d76 F0E0 ldi r31,lo8(0) 3174 1d78 EE0F lsl r30 3175 1d7a FF1F rol r31 3176 1d7c E050 subi r30,lo8(-(PPM_in)) 3177 1d7e F040 sbci r31,hi8(-(PPM_in)) 3178 1d80 8081 ld r24,Z 3179 1d82 9181 ldd r25,Z+1 3180 1d84 8259 subi r24,lo8(-(110)) 3181 1d86 9F4F sbci r25,hi8(-(110)) 3182 1d88 9093 0000 sts (Poti3)+1,r25 3183 1d8c 8093 0000 sts Poti3,r24 3184 1d90 E091 0000 lds r30,ParamSet+7 3185 1d94 F0E0 ldi r31,lo8(0) 3186 1d96 EE0F lsl r30 3187 1d98 FF1F rol r31 3188 1d9a E050 subi r30,lo8(-(PPM_in)) 3189 1d9c F040 sbci r31,hi8(-(PPM_in)) 3190 1d9e 8081 ld r24,Z 3191 1da0 9181 ldd r25,Z+1 3192 1da2 8259 subi r24,lo8(-(110)) 3193 1da4 9F4F sbci r25,hi8(-(110)) 3194 1da6 9093 0000 sts (Poti4)+1,r25 3195 1daa 8093 0000 sts Poti4,r24 3196 1dae 84E6 ldi r24,lo8(100) 3197 1db0 8093 0000 sts RC_Quality,r24 3198 /* epilogue start */ 3199 1db4 1F91 pop r17 3200 1db6 0F91 pop r16 3201 1db8 FF90 pop r15 3202 1dba EF90 pop r14 3203 1dbc DF90 pop r13 3204 1dbe CF90 pop r12 3205 1dc0 BF90 pop r11 3206 1dc2 AF90 pop r10 3207 1dc4 9F90 pop r9 3208 1dc6 8F90 pop r8 3209 1dc8 7F90 pop r7 3210 1dca 6F90 pop r6 3211 1dcc 5F90 pop r5 3212 1dce 4F90 pop r4 3213 1dd0 3F90 pop r3 3214 1dd2 2F90 pop r2 3215 1dd4 0895 ret 3216 .L371: 3217 1dd6 0E94 0000 call SearchAirPressureOffset 3218 1dda 00C0 rjmp .L370 3219 .L373: 3220 1ddc 84E0 ldi r24,lo8(4) 3221 1dde 90E0 ldi r25,hi8(4) 3222 1de0 0E94 0000 call GetParamWord 3223 1de4 9093 0000 sts (AdBiasAccNick)+1,r25 3224 1de8 8093 0000 sts AdBiasAccNick,r24 3225 1dec 86E0 ldi r24,lo8(6) 3226 1dee 90E0 ldi r25,hi8(6) 3227 1df0 0E94 0000 call GetParamWord 3228 1df4 9093 0000 sts (AdBiasAccRoll)+1,r25 3229 1df8 8093 0000 sts AdBiasAccRoll,r24 3230 1dfc 88E0 ldi r24,lo8(8) 3231 1dfe 90E0 ldi r25,hi8(8) 3232 1e00 0E94 0000 call GetParamWord 3233 1e04 AA27 clr r26 3234 1e06 97FD sbrc r25,7 3235 1e08 A095 com r26 3236 1e0a BA2F mov r27,r26 3237 1e0c BC01 movw r22,r24 3238 1e0e CD01 movw r24,r26 3239 1e10 0E94 0000 call __floatsisf 3240 1e14 6093 0000 sts AdBiasAccTop,r22 3241 1e18 7093 0000 sts (AdBiasAccTop)+1,r23 3242 1e1c 8093 0000 sts (AdBiasAccTop)+2,r24 3243 1e20 9093 0000 sts (AdBiasAccTop)+3,r25 3244 1e24 8091 0000 lds r24,AdBiasAccNick 3245 1e28 9091 0000 lds r25,(AdBiasAccNick)+1 3246 1e2c 8150 subi r24,lo8(2049) 3247 1e2e 9840 sbci r25,hi8(2049) 3248 1e30 04F4 brge .L379 3249 1e32 8091 0000 lds r24,AdBiasAccRoll 3250 1e36 9091 0000 lds r25,(AdBiasAccRoll)+1 3251 1e3a 8150 subi r24,lo8(2049) 3252 1e3c 9840 sbci r25,hi8(2049) 3253 1e3e 04F0 brlt .L388 3254 .L379: 3255 1e40 00D0 rcall . 3256 1e42 0F92 push __tmp_reg__ 3257 1e44 EDB7 in r30,__SP_L__ 3258 1e46 FEB7 in r31,__SP_H__ 3259 1e48 1182 std Z+1,__zero_reg__ 3260 1e4a 80E0 ldi r24,lo8(__c.1965) 3261 1e4c 90E0 ldi r25,hi8(__c.1965) 3262 1e4e 9383 std Z+3,r25 3263 1e50 8283 std Z+2,r24 3264 1e52 0E94 0000 call _printf_P 3265 1e56 80E0 ldi r24,lo8(1024) 3266 1e58 94E0 ldi r25,hi8(1024) 3267 1e5a 9093 0000 sts (AdBiasAccNick)+1,r25 3268 1e5e 8093 0000 sts AdBiasAccNick,r24 3269 1e62 9093 0000 sts (AdBiasAccRoll)+1,r25 3270 1e66 8093 0000 sts AdBiasAccRoll,r24 3271 1e6a 80E0 ldi r24,lo8(0x44354000) 3272 1e6c 90E4 ldi r25,hi8(0x44354000) 3273 1e6e A5E3 ldi r26,hlo8(0x44354000) 3274 1e70 B4E4 ldi r27,hhi8(0x44354000) 3275 1e72 8093 0000 sts AdBiasAccTop,r24 3276 1e76 9093 0000 sts (AdBiasAccTop)+1,r25 3277 1e7a A093 0000 sts (AdBiasAccTop)+2,r26 3278 1e7e B093 0000 sts (AdBiasAccTop)+3,r27 3279 1e82 0F90 pop __tmp_reg__ 3280 1e84 0F90 pop __tmp_reg__ 3281 1e86 0F90 pop __tmp_reg__ 3282 1e88 00C0 rjmp .L378 3283 .L388: 3284 1e8a 6091 0000 lds r22,AdBiasAccTop 3285 1e8e 7091 0000 lds r23,(AdBiasAccTop)+1 3286 1e92 8091 0000 lds r24,(AdBiasAccTop)+2 3287 1e96 9091 0000 lds r25,(AdBiasAccTop)+3 3288 1e9a 20E0 ldi r18,lo8(0x44800000) 3289 1e9c 30E0 ldi r19,hi8(0x44800000) 3290 1e9e 40E8 ldi r20,hlo8(0x44800000) 3291 1ea0 54E4 ldi r21,hhi8(0x44800000) 3292 1ea2 0E94 0000 call __gtsf2 3293 1ea6 1816 cp __zero_reg__,r24 3294 1ea8 04F0 brlt .L379 3295 1eaa 00C0 rjmp .L378 3296 .L384: 3297 1eac 0E94 0000 call SearchDacGyroOffset 3298 1eb0 00C0 rjmp .L369 3299 .L385: 3300 1eb2 9095 com r25 3301 1eb4 8195 neg r24 3302 1eb6 9F4F sbci r25,lo8(-1) 3303 1eb8 00C0 rjmp .L375 3304 .L386: 3305 1eba 9095 com r25 3306 1ebc 8195 neg r24 3307 1ebe 9F4F sbci r25,lo8(-1) 3308 1ec0 00C0 rjmp .L376 3309 .L387: 3310 1ec2 9095 com r25 3311 1ec4 8195 neg r24 3312 1ec6 9F4F sbci r25,lo8(-1) 3313 1ec8 00C0 rjmp .L377 3315 .global Beep 3317 Beep: 3318 1eca EF92 push r14 3319 1ecc FF92 push r15 3320 1ece 1F93 push r17 3321 1ed0 CF93 push r28 3322 1ed2 DF93 push r29 3323 /* prologue: function */ 3324 /* frame size = 0 */ 3325 1ed4 182F mov r17,r24 3326 1ed6 7B01 movw r14,r22 3327 1ed8 8091 0000 lds r24,MKFlags 3328 1edc 80FD sbrc r24,0 3329 1ede 00C0 rjmp .L392 3330 1ee0 1123 tst r17 3331 1ee2 01F0 breq .L392 3332 1ee4 EB01 movw r28,r22 3333 1ee6 CC0F lsl r28 3334 1ee8 DD1F rol r29 3335 .L391: 3336 1eea F092 0000 sts (BeepTime)+1,r15 3337 1eee E092 0000 sts BeepTime,r14 3338 1ef2 CE01 movw r24,r28 3339 1ef4 0E94 0000 call Delay_ms 3340 1ef8 1150 subi r17,lo8(-(-1)) 3341 1efa 01F4 brne .L391 3342 .L392: 3343 /* epilogue start */ 3344 1efc DF91 pop r29 3345 1efe CF91 pop r28 3346 1f00 1F91 pop r17 3347 1f02 FF90 pop r15 3348 1f04 EF90 pop r14 3349 1f06 0895 ret 3351 .global SetCompassCalState 3353 SetCompassCalState: 3354 /* prologue: function */ 3355 /* frame size = 0 */ 3356 1f08 8091 0000 lds r24,ParamSet 3357 1f0c A82F mov r26,r24 3358 1f0e B0E0 ldi r27,lo8(0) 3359 1f10 AA0F lsl r26 3360 1f12 BB1F rol r27 3361 1f14 FD01 movw r30,r26 3362 1f16 E050 subi r30,lo8(-(PPM_in)) 3363 1f18 F040 sbci r31,hi8(-(PPM_in)) 3364 1f1a 8081 ld r24,Z 3365 1f1c 9181 ldd r25,Z+1 3366 1f1e 8D5E subi r24,lo8(-19) 3367 1f20 9F4F sbci r25,hi8(-19) 3368 1f22 04F0 brlt .L395 3369 1f24 1092 0000 sts stick.2382,__zero_reg__ 3370 .L395: 3371 1f28 A050 subi r26,lo8(-(PPM_in)) 3372 1f2a B040 sbci r27,hi8(-(PPM_in)) 3373 1f2c 8D91 ld r24,X+ 3374 1f2e 9C91 ld r25,X 3375 1f30 8A5B subi r24,lo8(-70) 3376 1f32 9F4F sbci r25,hi8(-70) 3377 1f34 04F4 brge .L398 3378 1f36 8091 0000 lds r24,stick.2382 3379 1f3a 8823 tst r24 3380 1f3c 01F4 brne .L398 3381 1f3e 81E0 ldi r24,lo8(1) 3382 1f40 8093 0000 sts stick.2382,r24 3383 1f44 8091 0000 lds r24,CompassCalState 3384 1f48 8F5F subi r24,lo8(-(1)) 3385 1f4a 8093 0000 sts CompassCalState,r24 3386 1f4e 8530 cpi r24,lo8(5) 3387 1f50 00F0 brlo .L399 3388 1f52 88EE ldi r24,lo8(1000) 3389 1f54 93E0 ldi r25,hi8(1000) 3390 1f56 9093 0000 sts (BeepTime)+1,r25 3391 1f5a 8093 0000 sts BeepTime,r24 3392 .L398: 3393 1f5e 0895 ret 3394 .L399: 3395 1f60 66E9 ldi r22,lo8(150) 3396 1f62 70E0 ldi r23,hi8(150) 3397 1f64 0E94 0000 call Beep 3398 1f68 0895 ret 3400 .global MotorControl 3402 MotorControl: 3403 1f6a 2F92 push r2 3404 1f6c 3F92 push r3 3405 1f6e 4F92 push r4 3406 1f70 5F92 push r5 3407 1f72 6F92 push r6 3408 1f74 7F92 push r7 3409 1f76 8F92 push r8 3410 1f78 9F92 push r9 3411 1f7a AF92 push r10 3412 1f7c BF92 push r11 3413 1f7e CF92 push r12 3414 1f80 DF92 push r13 3415 1f82 EF92 push r14 3416 1f84 FF92 push r15 3417 1f86 0F93 push r16 3418 1f88 1F93 push r17 3419 1f8a DF93 push r29 3420 1f8c CF93 push r28 3421 1f8e CDB7 in r28,__SP_L__ 3422 1f90 DEB7 in r29,__SP_H__ 3423 1f92 E997 sbiw r28,57 3424 1f94 0FB6 in __tmp_reg__,__SREG__ 3425 1f96 F894 cli 3426 1f98 DEBF out __SP_H__,r29 3427 1f9a 0FBE out __SREG__,__tmp_reg__ 3428 1f9c CDBF out __SP_L__,r28 3429 /* prologue: function */ 3430 /* frame size = 57 */ 3431 1f9e 0E94 0000 call Mean 3432 1fa2 D090 0000 lds r13,BoardRelease 3433 1fa6 BBE0 ldi r27,lo8(11) 3434 1fa8 AB2E mov r10,r27 3435 1faa AD14 cp r10,r13 3436 1fac 00F4 brsh .+2 3437 1fae 00C0 rjmp .L401 3438 1fb0 299A sbi 37-32,1 3439 1fb2 8091 0000 lds r24,RC_Quality 3440 1fb6 8436 cpi r24,lo8(100) 3441 1fb8 00F0 brlo .+2 3442 1fba 00C0 rjmp .L403 3443 .L767: 3444 1fbc 8091 0000 lds r24,RcLostTimer.2415 3445 1fc0 9091 0000 lds r25,(RcLostTimer.2415)+1 3446 1fc4 0097 sbiw r24,0 3447 1fc6 01F0 breq .+4 3448 1fc8 0C94 0000 jmp .L732 3449 1fcc 8091 0000 lds r24,MKFlags 3450 1fd0 8E7E andi r24,lo8(-18) 3451 1fd2 8093 0000 sts MKFlags,r24 3452 1fd6 AAE0 ldi r26,lo8(10) 3453 1fd8 BA2E mov r11,r26 3454 1fda DB14 cp r13,r11 3455 1fdc 01F4 brne .+4 3456 1fde 0C94 0000 jmp .L406 3457 .L791: 3458 1fe2 F4E1 ldi r31,lo8(20) 3459 1fe4 CF2E mov r12,r31 3460 1fe6 DC14 cp r13,r12 3461 1fe8 01F4 brne .+4 3462 1fea 0C94 0000 jmp .L406 3463 1fee 2898 cbi 37-32,0 3464 .L408: 3465 1ff0 8091 0000 lds r24,ModelIsFlying 3466 1ff4 9091 0000 lds r25,(ModelIsFlying)+1 3467 1ff8 895E subi r24,lo8(1001) 3468 1ffa 9340 sbci r25,hi8(1001) 3469 1ffc 00F4 brsh .+4 3470 1ffe 0C94 0000 jmp .L727 3471 2002 8091 0000 lds r24,MKFlags 3472 2006 8061 ori r24,lo8(16) 3473 2008 8093 0000 sts MKFlags,r24 3474 200c E091 0000 lds r30,ParamSet 3475 2010 2E2F mov r18,r30 3476 2012 30E0 ldi r19,lo8(0) 3477 2014 220F lsl r18 3478 2016 331F rol r19 3479 2018 F901 movw r30,r18 3480 201a E050 subi r30,lo8(-(PPM_diff)) 3481 201c F040 sbci r31,hi8(-(PPM_diff)) 3482 201e 1182 std Z+1,__zero_reg__ 3483 2020 1082 st Z,__zero_reg__ 3484 2022 A091 0000 lds r26,ParamSet+1 3485 2026 B0E0 ldi r27,lo8(0) 3486 2028 AA0F lsl r26 3487 202a BB1F rol r27 3488 202c FD01 movw r30,r26 3489 202e E050 subi r30,lo8(-(PPM_diff)) 3490 2030 F040 sbci r31,hi8(-(PPM_diff)) 3491 2032 1182 std Z+1,__zero_reg__ 3492 2034 1082 st Z,__zero_reg__ 3493 2036 E091 0000 lds r30,ParamSet+3 3494 203a 4E2F mov r20,r30 3495 203c 50E0 ldi r21,lo8(0) 3496 203e 440F lsl r20 3497 2040 551F rol r21 3498 2042 CA01 movw r24,r20 3499 2044 8050 subi r24,lo8(-(PPM_diff)) 3500 2046 9040 sbci r25,hi8(-(PPM_diff)) 3501 2048 FC01 movw r30,r24 3502 204a 1182 std Z+1,__zero_reg__ 3503 204c 1082 st Z,__zero_reg__ 3504 204e 2050 subi r18,lo8(-(PPM_in)) 3505 2050 3040 sbci r19,hi8(-(PPM_in)) 3506 2052 F901 movw r30,r18 3507 2054 1182 std Z+1,__zero_reg__ 3508 2056 1082 st Z,__zero_reg__ 3509 2058 A050 subi r26,lo8(-(PPM_in)) 3510 205a B040 sbci r27,hi8(-(PPM_in)) 3511 205c 1196 adiw r26,1 3512 205e 1C92 st X,__zero_reg__ 3513 2060 1E92 st -X,__zero_reg__ 3514 2062 4050 subi r20,lo8(-(PPM_in)) 3515 2064 5040 sbci r21,hi8(-(PPM_in)) 3516 2066 DA01 movw r26,r20 3517 2068 1196 adiw r26,1 3518 206a 1C92 st X,__zero_reg__ 3519 206c 1E92 st -X,__zero_reg__ 3520 .L410: 3521 206e 8091 0000 lds r24,NewPpmData 3522 2072 8150 subi r24,lo8(-(-1)) 3523 2074 8093 0000 sts NewPpmData,r24 3524 2078 8F5F subi r24,lo8(-(1)) 3525 207a 01F0 breq .L471 3526 207c 8091 0000 lds r24,MKFlags 3527 2080 84FF sbrs r24,4 3528 2082 00C0 rjmp .L472 3529 .L471: 3530 2084 0E94 0000 call ParameterMapping 3531 2088 E091 0000 lds r30,ParamSet 3532 208c F0E0 ldi r31,lo8(0) 3533 208e EE0F lsl r30 3534 2090 FF1F rol r31 3535 2092 E050 subi r30,lo8(-(PPM_in)) 3536 2094 F040 sbci r31,hi8(-(PPM_in)) 3537 2096 2081 ld r18,Z 3538 2098 3181 ldd r19,Z+1 3539 209a 8091 0000 lds r24,ParamSet+18 3540 209e 90E0 ldi r25,lo8(0) 3541 20a0 289F mul r18,r24 3542 20a2 A001 movw r20,r0 3543 20a4 299F mul r18,r25 3544 20a6 500D add r21,r0 3545 20a8 389F mul r19,r24 3546 20aa 500D add r21,r0 3547 20ac 1124 clr r1 3548 20ae 2091 0000 lds r18,stick_nick.2424 3549 20b2 3091 0000 lds r19,(stick_nick.2424)+1 3550 20b6 C901 movw r24,r18 3551 20b8 880F lsl r24 3552 20ba 991F rol r25 3553 20bc 820F add r24,r18 3554 20be 931F adc r25,r19 3555 20c0 480F add r20,r24 3556 20c2 591F adc r21,r25 3557 20c4 57FF sbrs r21,7 3558 20c6 00C0 rjmp .+4 3559 20c8 0C94 0000 jmp .L733 3560 .L473: 3561 20cc 5595 asr r21 3562 20ce 4795 ror r20 3563 20d0 5595 asr r21 3564 20d2 4795 ror r20 3565 20d4 5093 0000 sts (stick_nick.2424)+1,r21 3566 20d8 4093 0000 sts stick_nick.2424,r20 3567 20dc E091 0000 lds r30,ParamSet 3568 20e0 F0E0 ldi r31,lo8(0) 3569 20e2 EE0F lsl r30 3570 20e4 FF1F rol r31 3571 20e6 E050 subi r30,lo8(-(PPM_diff)) 3572 20e8 F040 sbci r31,hi8(-(PPM_diff)) 3573 20ea 2081 ld r18,Z 3574 20ec 3181 ldd r19,Z+1 3575 20ee 8091 0000 lds r24,ParamSet+19 3576 20f2 90E0 ldi r25,lo8(0) 3577 20f4 BC01 movw r22,r24 3578 20f6 269F mul r18,r22 3579 20f8 C001 movw r24,r0 3580 20fa 279F mul r18,r23 3581 20fc 900D add r25,r0 3582 20fe 369F mul r19,r22 3583 2100 900D add r25,r0 3584 2102 1124 clr r1 3585 2104 840F add r24,r20 3586 2106 951F adc r25,r21 3587 2108 9093 0000 sts (stick_nick.2424)+1,r25 3588 210c 8093 0000 sts stick_nick.2424,r24 3589 2110 2091 0000 lds r18,GPSStickNick 3590 2114 3091 0000 lds r19,(GPSStickNick)+1 3591 2118 821B sub r24,r18 3592 211a 930B sbc r25,r19 3593 211c 9093 0000 sts (StickNick)+1,r25 3594 2120 8093 0000 sts StickNick,r24 3595 2124 E091 0000 lds r30,ParamSet+1 3596 2128 F0E0 ldi r31,lo8(0) 3597 212a EE0F lsl r30 3598 212c FF1F rol r31 3599 212e E050 subi r30,lo8(-(PPM_in)) 3600 2130 F040 sbci r31,hi8(-(PPM_in)) 3601 2132 2081 ld r18,Z 3602 2134 3181 ldd r19,Z+1 3603 2136 8091 0000 lds r24,ParamSet+18 3604 213a 90E0 ldi r25,lo8(0) 3605 213c 289F mul r18,r24 3606 213e A001 movw r20,r0 3607 2140 299F mul r18,r25 3608 2142 500D add r21,r0 3609 2144 389F mul r19,r24 3610 2146 500D add r21,r0 3611 2148 1124 clr r1 3612 214a 2091 0000 lds r18,stick_roll.2425 3613 214e 3091 0000 lds r19,(stick_roll.2425)+1 3614 2152 C901 movw r24,r18 3615 2154 880F lsl r24 3616 2156 991F rol r25 3617 2158 820F add r24,r18 3618 215a 931F adc r25,r19 3619 215c 480F add r20,r24 3620 215e 591F adc r21,r25 3621 2160 57FF sbrs r21,7 3622 2162 00C0 rjmp .+4 3623 2164 0C94 0000 jmp .L734 3624 .L474: 3625 2168 5595 asr r21 3626 216a 4795 ror r20 3627 216c 5595 asr r21 3628 216e 4795 ror r20 3629 2170 5093 0000 sts (stick_roll.2425)+1,r21 3630 2174 4093 0000 sts stick_roll.2425,r20 3631 2178 E091 0000 lds r30,ParamSet+1 3632 217c F0E0 ldi r31,lo8(0) 3633 217e EE0F lsl r30 3634 2180 FF1F rol r31 3635 2182 E050 subi r30,lo8(-(PPM_diff)) 3636 2184 F040 sbci r31,hi8(-(PPM_diff)) 3637 2186 2081 ld r18,Z 3638 2188 3181 ldd r19,Z+1 3639 218a 8091 0000 lds r24,ParamSet+19 3640 218e 90E0 ldi r25,lo8(0) 3641 2190 DC01 movw r26,r24 3642 2192 2A9F mul r18,r26 3643 2194 C001 movw r24,r0 3644 2196 2B9F mul r18,r27 3645 2198 900D add r25,r0 3646 219a 3A9F mul r19,r26 3647 219c 900D add r25,r0 3648 219e 1124 clr r1 3649 21a0 840F add r24,r20 3650 21a2 951F adc r25,r21 3651 21a4 9093 0000 sts (stick_roll.2425)+1,r25 3652 21a8 8093 0000 sts stick_roll.2425,r24 3653 21ac 2091 0000 lds r18,GPSStickRoll 3654 21b0 3091 0000 lds r19,(GPSStickRoll)+1 3655 21b4 821B sub r24,r18 3656 21b6 930B sbc r25,r19 3657 21b8 9093 0000 sts (StickRoll)+1,r25 3658 21bc 8093 0000 sts StickRoll,r24 3659 21c0 E091 0000 lds r30,ParamSet+3 3660 21c4 F0E0 ldi r31,lo8(0) 3661 21c6 EE0F lsl r30 3662 21c8 FF1F rol r31 3663 21ca E050 subi r30,lo8(-(PPM_in)) 3664 21cc F040 sbci r31,hi8(-(PPM_in)) 3665 21ce 8081 ld r24,Z 3666 21d0 9181 ldd r25,Z+1 3667 21d2 2227 clr r18 3668 21d4 3327 clr r19 3669 21d6 281B sub r18,r24 3670 21d8 390B sbc r19,r25 3671 21da 3093 0000 sts (StickYaw)+1,r19 3672 21de 2093 0000 sts StickYaw,r18 3673 21e2 8091 0000 lds r24,ParamSet+8 3674 21e6 8872 andi r24,lo8(40) 3675 21e8 01F0 breq .L475 3676 21ea 2330 cpi r18,3 3677 21ec 3105 cpc r19,__zero_reg__ 3678 21ee 04F0 brlt .+4 3679 21f0 0C94 0000 jmp .L735 3680 21f4 BFEF ldi r27,hi8(-2) 3681 21f6 2E3F cpi r18,lo8(-2) 3682 21f8 3B07 cpc r19,r27 3683 21fa 04F0 brlt .+4 3684 21fc 0C94 0000 jmp .L477 3685 2200 2E5F subi r18,lo8(-(2)) 3686 2202 3F4F sbci r19,hi8(-(2)) 3687 2204 3093 0000 sts (StickYaw)+1,r19 3688 2208 2093 0000 sts StickYaw,r18 3689 .L475: 3690 220c E091 0000 lds r30,ParamSet+2 3691 2210 F0E0 ldi r31,lo8(0) 3692 2212 EE0F lsl r30 3693 2214 FF1F rol r31 3694 2216 E050 subi r30,lo8(-(PPM_in)) 3695 2218 F040 sbci r31,hi8(-(PPM_in)) 3696 221a 0190 ld __tmp_reg__,Z+ 3697 221c F081 ld r31,Z 3698 221e E02D mov r30,__tmp_reg__ 3699 2220 E858 subi r30,lo8(-(120)) 3700 2222 FF4F sbci r31,hi8(-(120)) 3701 2224 F093 0000 sts (StickGas)+1,r31 3702 2228 E093 0000 sts StickGas,r30 3703 222c 8091 0000 lds r24,FCParam+7 3704 2230 865F subi r24,lo8(-(10)) 3705 2232 8093 0000 sts GyroPFactor,r24 3706 2236 8091 0000 lds r24,FCParam+8 3707 223a 8093 0000 sts GyroIFactor,r24 3708 223e 8091 0000 lds r24,FCParam+9 3709 2242 865F subi r24,lo8(-(10)) 3710 2244 8093 0000 sts GyroYawPFactor,r24 3711 2248 8091 0000 lds r24,FCParam+10 3712 224c 8093 0000 sts GyroYawIFactor,r24 3713 2250 8091 0000 lds r24,ExternControl+10 3714 2254 80FF sbrs r24,0 3715 2256 00C0 rjmp .L478 3716 2258 8091 0000 lds r24,FCParam+28 3717 225c 8138 cpi r24,lo8(-127) 3718 225e 00F4 brsh .+2 3719 2260 00C0 rjmp .L478 3720 2262 2091 0000 lds r18,ParamSet+18 3721 2266 30E0 ldi r19,lo8(0) 3722 2268 8091 0000 lds r24,ExternControl+3 3723 226c 9927 clr r25 3724 226e 87FD sbrc r24,7 3725 2270 9095 com r25 3726 2272 289F mul r18,r24 3727 2274 A001 movw r20,r0 3728 2276 299F mul r18,r25 3729 2278 500D add r21,r0 3730 227a 389F mul r19,r24 3731 227c 500D add r21,r0 3732 227e 1124 clr r1 3733 2280 8091 0000 lds r24,StickNick 3734 2284 9091 0000 lds r25,(StickNick)+1 3735 2288 840F add r24,r20 3736 228a 951F adc r25,r21 3737 228c 9093 0000 sts (StickNick)+1,r25 3738 2290 8093 0000 sts StickNick,r24 3739 2294 8091 0000 lds r24,ExternControl+4 3740 2298 9927 clr r25 3741 229a 87FD sbrc r24,7 3742 229c 9095 com r25 3743 229e 5901 movw r10,r18 3744 22a0 A89E mul r10,r24 3745 22a2 9001 movw r18,r0 3746 22a4 A99E mul r10,r25 3747 22a6 300D add r19,r0 3748 22a8 B89E mul r11,r24 3749 22aa 300D add r19,r0 3750 22ac 1124 clr r1 3751 22ae 8091 0000 lds r24,StickRoll 3752 22b2 9091 0000 lds r25,(StickRoll)+1 3753 22b6 820F add r24,r18 3754 22b8 931F adc r25,r19 3755 22ba 9093 0000 sts (StickRoll)+1,r25 3756 22be 8093 0000 sts StickRoll,r24 3757 22c2 2091 0000 lds r18,ExternControl+5 3758 22c6 3327 clr r19 3759 22c8 27FD sbrc r18,7 3760 22ca 3095 com r19 3761 22cc 8091 0000 lds r24,StickYaw 3762 22d0 9091 0000 lds r25,(StickYaw)+1 3763 22d4 820F add r24,r18 3764 22d6 931F adc r25,r19 3765 22d8 9093 0000 sts (StickYaw)+1,r25 3766 22dc 8093 0000 sts StickYaw,r24 3767 22e0 2091 0000 lds r18,ParamSet+13 3768 22e4 30E0 ldi r19,lo8(0) 3769 22e6 8091 0000 lds r24,ExternControl+7 3770 22ea 9927 clr r25 3771 22ec 87FD sbrc r24,7 3772 22ee 9095 com r25 3773 22f0 6C01 movw r12,r24 3774 22f2 2C9D mul r18,r12 3775 22f4 C001 movw r24,r0 3776 22f6 2D9D mul r18,r13 3777 22f8 900D add r25,r0 3778 22fa 3C9D mul r19,r12 3779 22fc 900D add r25,r0 3780 22fe 1124 clr r1 3781 2300 9093 0000 sts (ExternHeightValue)+1,r25 3782 2304 8093 0000 sts ExternHeightValue,r24 3783 2308 8091 0000 lds r24,ExternControl+6 3784 230c 90E0 ldi r25,lo8(0) 3785 230e 8E17 cp r24,r30 3786 2310 9F07 cpc r25,r31 3787 2312 04F4 brge .L478 3788 2314 9093 0000 sts (StickGas)+1,r25 3789 2318 8093 0000 sts StickGas,r24 3790 .L478: 3791 231c 8091 0000 lds r24,StickGas 3792 2320 9091 0000 lds r25,(StickGas)+1 3793 2324 97FF sbrs r25,7 3794 2326 00C0 rjmp .+4 3795 2328 0C94 0000 jmp .L736 3796 232c 8091 0000 lds r24,ParamSet+8 3797 2330 82FF sbrs r24,2 3798 2332 00C0 rjmp .+4 3799 2334 0C94 0000 jmp .L737 3800 .L480: 3801 2338 6091 0000 lds r22,StickNick 3802 233c 7091 0000 lds r23,StickNick+1 3803 2340 4091 0000 lds r20,MaxStickNick 3804 2344 5091 0000 lds r21,(MaxStickNick)+1 3805 2348 CB01 movw r24,r22 3806 234a 9C01 movw r18,r24 3807 234c 97FF sbrs r25,7 3808 234e 00C0 rjmp .+4 3809 2350 0C94 0000 jmp .L738 3810 .L482: 3811 2354 3595 asr r19 3812 2356 2795 ror r18 3813 2358 3595 asr r19 3814 235a 2795 ror r18 3815 235c 37FF sbrs r19,7 3816 235e 00C0 rjmp .+4 3817 2360 0C94 0000 jmp .L739 3818 .L483: 3819 2364 4217 cp r20,r18 3820 2366 5307 cpc r21,r19 3821 2368 04F4 brge .+4 3822 236a 0C94 0000 jmp .L740 3823 236e 4150 subi r20,lo8(-(-1)) 3824 2370 5040 sbci r21,hi8(-(-1)) 3825 2372 5093 0000 sts (MaxStickNick)+1,r21 3826 2376 4093 0000 sts MaxStickNick,r20 3827 .L485: 3828 237a 6091 0000 lds r22,StickRoll 3829 237e 7091 0000 lds r23,StickRoll+1 3830 2382 4091 0000 lds r20,MaxStickRoll 3831 2386 5091 0000 lds r21,(MaxStickRoll)+1 3832 238a CB01 movw r24,r22 3833 238c 9C01 movw r18,r24 3834 238e 97FF sbrs r25,7 3835 2390 00C0 rjmp .+4 3836 2392 0C94 0000 jmp .L741 3837 .L487: 3838 2396 3595 asr r19 3839 2398 2795 ror r18 3840 239a 3595 asr r19 3841 239c 2795 ror r18 3842 239e 37FF sbrs r19,7 3843 23a0 00C0 rjmp .+4 3844 23a2 0C94 0000 jmp .L742 3845 .L488: 3846 23a6 4217 cp r20,r18 3847 23a8 5307 cpc r21,r19 3848 23aa 04F0 brlt .+4 3849 23ac 0C94 0000 jmp .L486 3850 23b0 9B01 movw r18,r22 3851 23b2 77FF sbrs r23,7 3852 23b4 00C0 rjmp .+4 3853 23b6 0C94 0000 jmp .L743 3854 .L489: 3855 23ba C901 movw r24,r18 3856 23bc 9595 asr r25 3857 23be 8795 ror r24 3858 23c0 9595 asr r25 3859 23c2 8795 ror r24 3860 23c4 9093 0000 sts (MaxStickRoll)+1,r25 3861 23c8 8093 0000 sts MaxStickRoll,r24 3862 23cc 8536 cpi r24,101 3863 23ce 9105 cpc r25,__zero_reg__ 3864 23d0 04F0 brlt .+4 3865 23d2 0C94 0000 jmp .L744 3866 .L490: 3867 23d6 8091 0000 lds r24,ParamSet+1 3868 23da A82F mov r26,r24 3869 23dc B0E0 ldi r27,lo8(0) 3870 23de AA0F lsl r26 3871 23e0 BB1F rol r27 3872 23e2 FD01 movw r30,r26 3873 23e4 E050 subi r30,lo8(-(PPM_in)) 3874 23e6 F040 sbci r31,hi8(-(PPM_in)) 3875 23e8 2081 ld r18,Z 3876 23ea 3181 ldd r19,Z+1 3877 23ec 8091 0000 lds r24,ParamSet+49 3878 23f0 482F mov r20,r24 3879 23f2 50E0 ldi r21,lo8(0) 3880 23f4 4217 cp r20,r18 3881 23f6 5307 cpc r21,r19 3882 23f8 04F4 brge .L491 3883 23fa 8091 0000 lds r24,ParamSet+86 3884 23fe 82FF sbrs r24,2 3885 2400 00C0 rjmp .+4 3886 2402 0C94 0000 jmp .L745 3887 .L491: 3888 2406 7091 0000 lds r23,LoopingLeft 3889 240a 7723 tst r23 3890 240c 01F0 breq .L492 3891 240e FD01 movw r30,r26 3892 2410 E050 subi r30,lo8(-(PPM_in)) 3893 2412 F040 sbci r31,hi8(-(PPM_in)) 3894 2414 2081 ld r18,Z 3895 2416 3181 ldd r19,Z+1 3896 2418 8091 0000 lds r24,ParamSet+50 3897 241c FA01 movw r30,r20 3898 241e E81B sub r30,r24 3899 2420 F109 sbc r31,__zero_reg__ 3900 2422 2E17 cp r18,r30 3901 2424 3F07 cpc r19,r31 3902 2426 04F4 brge .+4 3903 2428 0C94 0000 jmp .L746 3904 .L492: 3905 242c FD01 movw r30,r26 3906 242e E050 subi r30,lo8(-(PPM_in)) 3907 2430 F040 sbci r31,hi8(-(PPM_in)) 3908 2432 2081 ld r18,Z 3909 2434 3181 ldd r19,Z+1 3910 2436 8827 clr r24 3911 2438 9927 clr r25 3912 243a 841B sub r24,r20 3913 243c 950B sbc r25,r21 3914 243e 2817 cp r18,r24 3915 2440 3907 cpc r19,r25 3916 2442 04F4 brge .L493 3917 2444 8091 0000 lds r24,ParamSet+86 3918 2448 83FF sbrs r24,3 3919 244a 00C0 rjmp .+4 3920 244c 0C94 0000 jmp .L747 3921 .L493: 3922 2450 8091 0000 lds r24,LoopingRight 3923 2454 8823 tst r24 3924 2456 01F0 breq .L494 3925 2458 A050 subi r26,lo8(-(PPM_in)) 3926 245a B040 sbci r27,hi8(-(PPM_in)) 3927 245c 2D91 ld r18,X+ 3928 245e 3C91 ld r19,X 3929 2460 8091 0000 lds r24,ParamSet+50 3930 2464 90E0 ldi r25,lo8(0) 3931 2466 841B sub r24,r20 3932 2468 950B sbc r25,r21 3933 246a 8217 cp r24,r18 3934 246c 9307 cpc r25,r19 3935 246e 04F4 brge .+4 3936 2470 0C94 0000 jmp .L748 3937 .L494: 3938 2474 8091 0000 lds r24,ParamSet 3939 2478 A82F mov r26,r24 3940 247a B0E0 ldi r27,lo8(0) 3941 247c AA0F lsl r26 3942 247e BB1F rol r27 3943 2480 FD01 movw r30,r26 3944 2482 E050 subi r30,lo8(-(PPM_in)) 3945 2484 F040 sbci r31,hi8(-(PPM_in)) 3946 2486 8081 ld r24,Z 3947 2488 9181 ldd r25,Z+1 3948 248a 4817 cp r20,r24 3949 248c 5907 cpc r21,r25 3950 248e 04F4 brge .L495 3951 2490 8091 0000 lds r24,ParamSet+86 3952 2494 80FF sbrs r24,0 3953 2496 00C0 rjmp .+4 3954 2498 0C94 0000 jmp .L749 3955 .L495: 3956 249c 6091 0000 lds r22,LoopingTop 3957 24a0 6623 tst r22 3958 24a2 01F0 breq .L496 3959 24a4 FD01 movw r30,r26 3960 24a6 E050 subi r30,lo8(-(PPM_in)) 3961 24a8 F040 sbci r31,hi8(-(PPM_in)) 3962 24aa 2081 ld r18,Z 3963 24ac 3181 ldd r19,Z+1 3964 24ae 8091 0000 lds r24,ParamSet+50 3965 24b2 5A01 movw r10,r20 3966 24b4 A81A sub r10,r24 3967 24b6 B108 sbc r11,__zero_reg__ 3968 24b8 2A15 cp r18,r10 3969 24ba 3B05 cpc r19,r11 3970 24bc 04F4 brge .+4 3971 24be 0C94 0000 jmp .L750 3972 .L496: 3973 24c2 FD01 movw r30,r26 3974 24c4 E050 subi r30,lo8(-(PPM_in)) 3975 24c6 F040 sbci r31,hi8(-(PPM_in)) 3976 24c8 2081 ld r18,Z 3977 24ca 3181 ldd r19,Z+1 3978 24cc 8827 clr r24 3979 24ce 9927 clr r25 3980 24d0 841B sub r24,r20 3981 24d2 950B sbc r25,r21 3982 24d4 2817 cp r18,r24 3983 24d6 3907 cpc r19,r25 3984 24d8 04F4 brge .L497 3985 24da 8091 0000 lds r24,ParamSet+86 3986 24de 81FF sbrs r24,1 3987 24e0 00C0 rjmp .+4 3988 24e2 0C94 0000 jmp .L751 3989 .L497: 3990 24e6 8091 0000 lds r24,LoopingDown 3991 24ea 8823 tst r24 3992 24ec 01F0 breq .L498 3993 24ee A050 subi r26,lo8(-(PPM_in)) 3994 24f0 B040 sbci r27,hi8(-(PPM_in)) 3995 24f2 2D91 ld r18,X+ 3996 24f4 3C91 ld r19,X 3997 24f6 8091 0000 lds r24,ParamSet+50 3998 24fa 90E0 ldi r25,lo8(0) 3999 24fc 841B sub r24,r20 4000 24fe 950B sbc r25,r21 4001 2500 8217 cp r24,r18 4002 2502 9307 cpc r25,r19 4003 2504 04F4 brge .+4 4004 2506 0C94 0000 jmp .L752 4005 .L498: 4006 250a 7723 tst r23 4007 250c 01F4 brne .L499 4008 250e 8091 0000 lds r24,LoopingRight 4009 2512 8823 tst r24 4010 2514 01F4 brne .+4 4011 2516 0C94 0000 jmp .L500 4012 .L499: 4013 251a 81E0 ldi r24,lo8(1) 4014 251c 8093 0000 sts LoopingRoll,r24 4015 .L501: 4016 2520 6623 tst r22 4017 2522 01F4 brne .L502 4018 2524 8091 0000 lds r24,LoopingDown 4019 2528 8823 tst r24 4020 252a 01F4 brne .+4 4021 252c 0C94 0000 jmp .L503 4022 .L502: 4023 2530 81E0 ldi r24,lo8(1) 4024 2532 8093 0000 sts LoopingNick,r24 4025 2536 1092 0000 sts LoopingRoll,__zero_reg__ 4026 253a 1092 0000 sts LoopingLeft,__zero_reg__ 4027 253e 1092 0000 sts LoopingRight,__zero_reg__ 4028 .L472: 4029 2542 8091 0000 lds r24,LoopingRoll 4030 2546 8823 tst r24 4031 2548 01F0 breq .+2 4032 254a 00C0 rjmp .L504 4033 254c 8091 0000 lds r24,LoopingNick 4034 2550 8823 tst r24 4035 2552 01F0 breq .+2 4036 2554 00C0 rjmp .L504 4037 .L505: 4038 2556 8091 0000 lds r24,MKFlags 4039 255a 84FF sbrs r24,4 4040 255c 00C0 rjmp .L507 4041 255e 1092 0000 sts (StickYaw)+1,__zero_reg__ 4042 2562 1092 0000 sts StickYaw,__zero_reg__ 4043 2566 1092 0000 sts (StickNick)+1,__zero_reg__ 4044 256a 1092 0000 sts StickNick,__zero_reg__ 4045 256e 1092 0000 sts (StickRoll)+1,__zero_reg__ 4046 2572 1092 0000 sts StickRoll,__zero_reg__ 4047 2576 8091 0000 lds r24,ParamSet+31 4048 257a 8093 0000 sts StickGas,r24 4049 257e 1092 0000 sts StickGas+1,__zero_reg__ 4050 2582 9AE5 ldi r25,lo8(90) 4051 2584 9093 0000 sts GyroPFactor,r25 4052 2588 88E7 ldi r24,lo8(120) 4053 258a 8093 0000 sts GyroIFactor,r24 4054 258e 9093 0000 sts GyroYawPFactor,r25 4055 2592 8093 0000 sts GyroYawIFactor,r24 4056 2596 1092 0000 sts LoopingRoll,__zero_reg__ 4057 259a 1092 0000 sts LoopingNick,__zero_reg__ 4058 259e 1092 0000 sts (MaxStickNick)+1,__zero_reg__ 4059 25a2 1092 0000 sts MaxStickNick,__zero_reg__ 4060 25a6 1092 0000 sts (MaxStickRoll)+1,__zero_reg__ 4061 25aa 1092 0000 sts MaxStickRoll,__zero_reg__ 4062 .L507: 4063 25ae 2091 0000 lds r18,MeanIntegralGyroNick 4064 25b2 3091 0000 lds r19,(MeanIntegralGyroNick)+1 4065 25b6 4091 0000 lds r20,(MeanIntegralGyroNick)+2 4066 25ba 5091 0000 lds r21,(MeanIntegralGyroNick)+3 4067 25be 28A3 std Y+32,r18 4068 25c0 39A3 std Y+33,r19 4069 25c2 4AA3 std Y+34,r20 4070 25c4 5BA3 std Y+35,r21 4071 25c6 4091 0000 lds r20,IntegralGyroNick 4072 25ca 5091 0000 lds r21,(IntegralGyroNick)+1 4073 25ce 6091 0000 lds r22,(IntegralGyroNick)+2 4074 25d2 7091 0000 lds r23,(IntegralGyroNick)+3 4075 25d6 48A7 std Y+40,r20 4076 25d8 59A7 std Y+41,r21 4077 25da 6AA7 std Y+42,r22 4078 25dc 7BA7 std Y+43,r23 4079 25de 88A1 ldd r24,Y+32 4080 25e0 99A1 ldd r25,Y+33 4081 25e2 AAA1 ldd r26,Y+34 4082 25e4 BBA1 ldd r27,Y+35 4083 25e6 840F add r24,r20 4084 25e8 951F adc r25,r21 4085 25ea A61F adc r26,r22 4086 25ec B71F adc r27,r23 4087 25ee 8093 0000 sts MeanIntegralGyroNick,r24 4088 25f2 9093 0000 sts (MeanIntegralGyroNick)+1,r25 4089 25f6 A093 0000 sts (MeanIntegralGyroNick)+2,r26 4090 25fa B093 0000 sts (MeanIntegralGyroNick)+3,r27 4091 25fe 2090 0000 lds r2,MeanIntegralGyroRoll 4092 2602 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 4093 2606 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 4094 260a 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 4095 260e A090 0000 lds r10,IntegralGyroRoll 4096 2612 B090 0000 lds r11,(IntegralGyroRoll)+1 4097 2616 C090 0000 lds r12,(IntegralGyroRoll)+2 4098 261a D090 0000 lds r13,(IntegralGyroRoll)+3 4099 261e ACA2 std Y+36,r10 4100 2620 BDA2 std Y+37,r11 4101 2622 CEA2 std Y+38,r12 4102 2624 DFA2 std Y+39,r13 4103 2626 2A0C add r2,r10 4104 2628 3B1C adc r3,r11 4105 262a 4C1C adc r4,r12 4106 262c 5D1C adc r5,r13 4107 262e 2092 0000 sts MeanIntegralGyroRoll,r2 4108 2632 3092 0000 sts (MeanIntegralGyroRoll)+1,r3 4109 2636 4092 0000 sts (MeanIntegralGyroRoll)+2,r4 4110 263a 5092 0000 sts (MeanIntegralGyroRoll)+3,r5 4111 263e B090 0000 lds r11,LoopingNick 4112 2642 BD8E std Y+29,r11 4113 2644 BB20 tst r11 4114 2646 01F4 brne .L508 4115 2648 8091 0000 lds r24,LoopingRoll 4116 264c 8823 tst r24 4117 264e 01F4 brne .+4 4118 2650 0C94 0000 jmp .L509 4119 .L508: 4120 2654 1092 0000 sts (MeasurementCounter)+1,__zero_reg__ 4121 2658 1092 0000 sts MeasurementCounter,__zero_reg__ 4122 265c 1092 0000 sts MeanAccNick,__zero_reg__ 4123 2660 1092 0000 sts (MeanAccNick)+1,__zero_reg__ 4124 2664 1092 0000 sts (MeanAccNick)+2,__zero_reg__ 4125 2668 1092 0000 sts (MeanAccNick)+3,__zero_reg__ 4126 266c 1092 0000 sts MeanAccRoll,__zero_reg__ 4127 2670 1092 0000 sts (MeanAccRoll)+1,__zero_reg__ 4128 2674 1092 0000 sts (MeanAccRoll)+2,__zero_reg__ 4129 2678 1092 0000 sts (MeanAccRoll)+3,__zero_reg__ 4130 267c 1092 0000 sts MeanIntegralGyroNick,__zero_reg__ 4131 2680 1092 0000 sts (MeanIntegralGyroNick)+1,__zero_reg__ 4132 2684 1092 0000 sts (MeanIntegralGyroNick)+2,__zero_reg__ 4133 2688 1092 0000 sts (MeanIntegralGyroNick)+3,__zero_reg__ 4134 268c 1092 0000 sts MeanIntegralGyroRoll,__zero_reg__ 4135 2690 1092 0000 sts (MeanIntegralGyroRoll)+1,__zero_reg__ 4136 2694 1092 0000 sts (MeanIntegralGyroRoll)+2,__zero_reg__ 4137 2698 1092 0000 sts (MeanIntegralGyroRoll)+3,__zero_reg__ 4138 269c 8091 0000 lds r24,ReadingIntegralGyroNick 4139 26a0 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 4140 26a4 A091 0000 lds r26,(ReadingIntegralGyroNick)+2 4141 26a8 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 4142 26ac 8093 0000 sts ReadingIntegralGyroNick2,r24 4143 26b0 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 4144 26b4 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 4145 26b8 B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 4146 26bc 8091 0000 lds r24,ReadingIntegralGyroRoll 4147 26c0 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 4148 26c4 A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 4149 26c8 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 4150 26cc 8093 0000 sts ReadingIntegralGyroRoll2,r24 4151 26d0 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 4152 26d4 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 4153 26d8 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 4154 26dc 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ 4155 26e0 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ 4156 26e4 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ 4157 26e8 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ 4158 26ec CD8C ldd r12,Y+29 4159 26ee CC20 tst r12 4160 26f0 01F4 brne .+4 4161 26f2 0C94 0000 jmp .L753 4162 .L510: 4163 26f6 8091 0000 lds r24,MeasurementCounter 4164 26fa 9091 0000 lds r25,(MeasurementCounter)+1 4165 26fe 8F3F cpi r24,255 4166 2700 9105 cpc r25,__zero_reg__ 4167 2702 01F0 breq .+2 4168 2704 00F4 brsh .+2 4169 2706 00C0 rjmp .L529 4170 2708 6D8D ldd r22,Y+29 4171 270a 6623 tst r22 4172 270c 01F4 brne .+4 4173 270e 0C94 0000 jmp .L754 4174 .L530: 4175 2712 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ 4176 2716 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ 4177 271a 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ 4178 271e 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ 4179 2722 1092 0000 sts FunnelCourse,__zero_reg__ 4180 2726 4091 0000 lds r20,MeanIntegralGyroNick 4181 272a 5091 0000 lds r21,(MeanIntegralGyroNick)+1 4182 272e 6091 0000 lds r22,(MeanIntegralGyroNick)+2 4183 2732 7091 0000 lds r23,(MeanIntegralGyroNick)+3 4184 2736 48A3 std Y+32,r20 4185 2738 59A3 std Y+33,r21 4186 273a 6AA3 std Y+34,r22 4187 273c 7BA3 std Y+35,r23 4188 273e 2090 0000 lds r2,MeanIntegralGyroRoll 4189 2742 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 4190 2746 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 4191 274a 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 4192 .L567: 4193 274e 8091 0000 lds r24,GyroIFactor 4194 2752 8823 tst r24 4195 2754 01F4 brne .L568 4196 2756 1092 0000 sts (AttitudeCorrectionRoll)+1,__zero_reg__ 4197 275a 1092 0000 sts AttitudeCorrectionRoll,__zero_reg__ 4198 275e 1092 0000 sts (AttitudeCorrectionNick)+1,__zero_reg__ 4199 2762 1092 0000 sts AttitudeCorrectionNick,__zero_reg__ 4200 .L568: 4201 2766 68A1 ldd r22,Y+32 4202 2768 79A1 ldd r23,Y+33 4203 276a 8AA1 ldd r24,Y+34 4204 276c 9BA1 ldd r25,Y+35 4205 276e 6093 0000 sts MeanIntegralGyroNick_old.2431,r22 4206 2772 7093 0000 sts (MeanIntegralGyroNick_old.2431)+1,r23 4207 2776 8093 0000 sts (MeanIntegralGyroNick_old.2431)+2,r24 4208 277a 9093 0000 sts (MeanIntegralGyroNick_old.2431)+3,r25 4209 277e 2092 0000 sts MeanIntegralGyroRoll_old.2432,r2 4210 2782 3092 0000 sts (MeanIntegralGyroRoll_old.2432)+1,r3 4211 2786 4092 0000 sts (MeanIntegralGyroRoll_old.2432)+2,r4 4212 278a 5092 0000 sts (MeanIntegralGyroRoll_old.2432)+3,r5 4213 278e 1092 0000 sts MeanAccNick,__zero_reg__ 4214 2792 1092 0000 sts (MeanAccNick)+1,__zero_reg__ 4215 2796 1092 0000 sts (MeanAccNick)+2,__zero_reg__ 4216 279a 1092 0000 sts (MeanAccNick)+3,__zero_reg__ 4217 279e 1092 0000 sts MeanAccRoll,__zero_reg__ 4218 27a2 1092 0000 sts (MeanAccRoll)+1,__zero_reg__ 4219 27a6 1092 0000 sts (MeanAccRoll)+2,__zero_reg__ 4220 27aa 1092 0000 sts (MeanAccRoll)+3,__zero_reg__ 4221 27ae 1092 0000 sts MeanIntegralGyroNick,__zero_reg__ 4222 27b2 1092 0000 sts (MeanIntegralGyroNick)+1,__zero_reg__ 4223 27b6 1092 0000 sts (MeanIntegralGyroNick)+2,__zero_reg__ 4224 27ba 1092 0000 sts (MeanIntegralGyroNick)+3,__zero_reg__ 4225 27be 1092 0000 sts MeanIntegralGyroRoll,__zero_reg__ 4226 27c2 1092 0000 sts (MeanIntegralGyroRoll)+1,__zero_reg__ 4227 27c6 1092 0000 sts (MeanIntegralGyroRoll)+2,__zero_reg__ 4228 27ca 1092 0000 sts (MeanIntegralGyroRoll)+3,__zero_reg__ 4229 27ce 1092 0000 sts (MeasurementCounter)+1,__zero_reg__ 4230 27d2 1092 0000 sts MeasurementCounter,__zero_reg__ 4231 .L529: 4232 27d6 E090 0000 lds r14,StickYaw 4233 27da F090 0000 lds r15,(StickYaw)+1 4234 27de C701 movw r24,r14 4235 27e0 F7FE sbrs r15,7 4236 27e2 00C0 rjmp .+4 4237 27e4 0C94 0000 jmp .L755 4238 .L570: 4239 27e8 4097 sbiw r24,16 4240 27ea 04F0 brlt .L569 4241 27ec 88EE ldi r24,lo8(1000) 4242 27ee 93E0 ldi r25,hi8(1000) 4243 27f0 9093 0000 sts (BadCompassHeading)+1,r25 4244 27f4 8093 0000 sts BadCompassHeading,r24 4245 27f8 8091 0000 lds r24,ParamSet+8 4246 27fc 84FD sbrc r24,4 4247 27fe 00C0 rjmp .+4 4248 2800 0C94 0000 jmp .L756 4249 .L569: 4250 2804 1091 0000 lds r17,ParamSet+20 4251 2808 B701 movw r22,r14 4252 280a 8827 clr r24 4253 280c 77FD sbrc r23,7 4254 280e 8095 com r24 4255 2810 982F mov r25,r24 4256 2812 212F mov r18,r17 4257 2814 30E0 ldi r19,lo8(0) 4258 2816 40E0 ldi r20,lo8(0) 4259 2818 50E0 ldi r21,hi8(0) 4260 281a 0E94 0000 call __mulsi3 4261 281e F701 movw r30,r14 4262 2820 F7FE sbrs r15,7 4263 2822 00C0 rjmp .+4 4264 2824 0C94 0000 jmp .L757 4265 .L571: 4266 2828 9F01 movw r18,r30 4267 282a 4427 clr r20 4268 282c 37FD sbrc r19,7 4269 282e 4095 com r20 4270 2830 542F mov r21,r20 4271 2832 0E94 0000 call __mulsi3 4272 2836 20E0 ldi r18,lo8(512) 4273 2838 32E0 ldi r19,hi8(512) 4274 283a 40E0 ldi r20,hlo8(512) 4275 283c 50E0 ldi r21,hhi8(512) 4276 283e 0E94 0000 call __divmodsi4 4277 2842 812F mov r24,r17 4278 2844 90E0 ldi r25,lo8(0) 4279 2846 8E9D mul r24,r14 4280 2848 B001 movw r22,r0 4281 284a 8F9D mul r24,r15 4282 284c 700D add r23,r0 4283 284e 9E9D mul r25,r14 4284 2850 700D add r23,r0 4285 2852 1124 clr r1 4286 2854 77FF sbrs r23,7 4287 2856 00C0 rjmp .+4 4288 2858 0C94 0000 jmp .L758 4289 .L572: 4290 285c 7595 asr r23 4291 285e 6795 ror r22 4292 2860 7595 asr r23 4293 2862 6795 ror r22 4294 2864 260F add r18,r22 4295 2866 371F adc r19,r23 4296 2868 C901 movw r24,r18 4297 286a AA27 clr r26 4298 286c 97FD sbrc r25,7 4299 286e A095 com r26 4300 2870 BA2F mov r27,r26 4301 2872 8093 0000 sts SetPointYaw.2410,r24 4302 2876 9093 0000 sts (SetPointYaw.2410)+1,r25 4303 287a A093 0000 sts (SetPointYaw.2410)+2,r26 4304 287e B093 0000 sts (SetPointYaw.2410)+3,r27 4305 2882 2091 0000 lds r18,ReadingIntegralGyroYaw 4306 2886 3091 0000 lds r19,(ReadingIntegralGyroYaw)+1 4307 288a 4091 0000 lds r20,(ReadingIntegralGyroYaw)+2 4308 288e 5091 0000 lds r21,(ReadingIntegralGyroYaw)+3 4309 2892 281B sub r18,r24 4310 2894 390B sbc r19,r25 4311 2896 4A0B sbc r20,r26 4312 2898 5B0B sbc r21,r27 4313 289a 2093 0000 sts ReadingIntegralGyroYaw,r18 4314 289e 3093 0000 sts (ReadingIntegralGyroYaw)+1,r19 4315 28a2 4093 0000 sts (ReadingIntegralGyroYaw)+2,r20 4316 28a6 5093 0000 sts (ReadingIntegralGyroYaw)+3,r21 4317 28aa 203B cpi r18,lo8(-50000) 4318 28ac 7CE3 ldi r23,hi8(-50000) 4319 28ae 3707 cpc r19,r23 4320 28b0 7FEF ldi r23,hlo8(-50000) 4321 28b2 4707 cpc r20,r23 4322 28b4 7FEF ldi r23,hhi8(-50000) 4323 28b6 5707 cpc r21,r23 4324 28b8 04F0 brlt .+4 4325 28ba 0C94 0000 jmp .L573 4326 28be 80EB ldi r24,lo8(-50000) 4327 28c0 9CE3 ldi r25,hi8(-50000) 4328 28c2 AFEF ldi r26,hlo8(-50000) 4329 28c4 BFEF ldi r27,hhi8(-50000) 4330 28c6 8093 0000 sts ReadingIntegralGyroYaw,r24 4331 28ca 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 4332 28ce A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 4333 28d2 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 4334 .L574: 4335 28d6 8091 0000 lds r24,ParamSet+8 4336 28da 8872 andi r24,lo8(40) 4337 28dc 01F4 brne .+2 4338 28de 00C0 rjmp .L728 4339 28e0 8091 0000 lds r24,CompassCalState 4340 28e4 8823 tst r24 4341 28e6 01F0 breq .+2 4342 28e8 00C0 rjmp .L759 4343 .L576: 4344 28ea 68A5 ldd r22,Y+40 4345 28ec 79A5 ldd r23,Y+41 4346 28ee 8AA5 ldd r24,Y+42 4347 28f0 9BA5 ldd r25,Y+43 4348 28f2 20E0 ldi r18,lo8(512) 4349 28f4 32E0 ldi r19,hi8(512) 4350 28f6 40E0 ldi r20,hlo8(512) 4351 28f8 50E0 ldi r21,hhi8(512) 4352 28fa 0E94 0000 call __divmodsi4 4353 28fe 8901 movw r16,r18 4354 2900 37FF sbrs r19,7 4355 2902 00C0 rjmp .+4 4356 2904 0C94 0000 jmp .L760 4357 .L577: 4358 2908 6CA1 ldd r22,Y+36 4359 290a 7DA1 ldd r23,Y+37 4360 290c 8EA1 ldd r24,Y+38 4361 290e 9FA1 ldd r25,Y+39 4362 2910 20E0 ldi r18,lo8(512) 4363 2912 32E0 ldi r19,hi8(512) 4364 2914 40E0 ldi r20,hlo8(512) 4365 2916 50E0 ldi r21,hhi8(512) 4366 2918 0E94 0000 call __divmodsi4 4367 291c 4901 movw r8,r18 4368 291e 37FF sbrs r19,7 4369 2920 00C0 rjmp .+4 4370 2922 0C94 0000 jmp .L761 4371 .L578: 4372 2926 8016 cp r8,r16 4373 2928 9106 cpc r9,r17 4374 292a 04F4 brge .L579 4375 292c 4801 movw r8,r16 4376 .L579: 4377 292e A090 0000 lds r10,CompassHeading 4378 2932 B090 0000 lds r11,(CompassHeading)+1 4379 2936 B7FE sbrs r11,7 4380 2938 00C0 rjmp .+4 4381 293a 0C94 0000 jmp .L762 4382 293e ACE1 ldi r26,lo8(540) 4383 2940 B2E0 ldi r27,hi8(540) 4384 2942 AA0E add r10,r26 4385 2944 BB1E adc r11,r27 4386 2946 7501 movw r14,r10 4387 2948 0027 clr r16 4388 294a F7FC sbrc r15,7 4389 294c 0095 com r16 4390 294e 102F mov r17,r16 4391 2950 E4EE ldi r30,lo8(-540) 4392 2952 FDEF ldi r31,hi8(-540) 4393 2954 AE0E add r10,r30 4394 2956 BF1E adc r11,r31 4395 2958 2091 0000 lds r18,ParamSet+23 4396 295c 8AE2 ldi r24,lo8(42) 4397 295e 289F mul r18,r24 4398 2960 9001 movw r18,r0 4399 2962 1124 clr r1 4400 2964 4427 clr r20 4401 2966 37FD sbrc r19,7 4402 2968 4095 com r20 4403 296a 542F mov r21,r20 4404 296c 6091 0000 lds r22,YawGyroHeading 4405 2970 7091 0000 lds r23,(YawGyroHeading)+1 4406 2974 8091 0000 lds r24,(YawGyroHeading)+2 4407 2978 9091 0000 lds r25,(YawGyroHeading)+3 4408 297c 0E94 0000 call __divmodsi4 4409 2980 E21A sub r14,r18 4410 2982 F30A sbc r15,r19 4411 2984 040B sbc r16,r20 4412 2986 150B sbc r17,r21 4413 2988 C801 movw r24,r16 4414 298a B701 movw r22,r14 4415 298c 28E6 ldi r18,lo8(360) 4416 298e 31E0 ldi r19,hi8(360) 4417 2990 40E0 ldi r20,hlo8(360) 4418 2992 50E0 ldi r21,hhi8(360) 4419 2994 0E94 0000 call __divmodsi4 4420 2998 645B subi r22,lo8(-(-180)) 4421 299a 7040 sbci r23,hi8(-(-180)) 4422 .L581: 4423 299c 2090 0000 lds r2,GyroYaw 4424 29a0 3090 0000 lds r3,(GyroYaw)+1 4425 29a4 C101 movw r24,r2 4426 29a6 37FE sbrs r3,7 4427 29a8 00C0 rjmp .+4 4428 29aa 0C94 0000 jmp .L763 4429 .L583: 4430 29ae 8138 cpi r24,129 4431 29b0 9105 cpc r25,__zero_reg__ 4432 29b2 04F4 brge .+4 4433 29b4 0C94 0000 jmp .L582 4434 29b8 60E0 ldi r22,lo8(0) 4435 29ba 70E0 ldi r23,hi8(0) 4436 29bc CC24 clr r12 4437 29be DD24 clr r13 4438 .L584: 4439 29c0 4090 0000 lds r4,BadCompassHeading 4440 29c4 5090 0000 lds r5,(BadCompassHeading)+1 4441 29c8 4114 cp r4,__zero_reg__ 4442 29ca 5104 cpc r5,__zero_reg__ 4443 29cc 01F4 brne .L585 4444 29ce F9E1 ldi r31,lo8(25) 4445 29d0 8F16 cp r8,r31 4446 29d2 9104 cpc r9,__zero_reg__ 4447 29d4 04F4 brge .+4 4448 29d6 0C94 0000 jmp .L764 4449 .L585: 4450 29da B401 movw r22,r8 4451 29dc 7595 asr r23 4452 29de 6795 ror r22 4453 29e0 7595 asr r23 4454 29e2 6795 ror r22 4455 29e4 7595 asr r23 4456 29e6 6795 ror r22 4457 29e8 6F5F subi r22,lo8(-(1)) 4458 29ea 7F4F sbci r23,hi8(-(1)) 4459 29ec C601 movw r24,r12 4460 29ee 0E94 0000 call __divmodhi4 4461 29f2 5B01 movw r10,r22 4462 29f4 CC24 clr r12 4463 29f6 B7FC sbrc r11,7 4464 29f8 C094 com r12 4465 29fa DC2C mov r13,r12 4466 29fc 8091 0000 lds r24,YawGyroHeading 4467 2a00 9091 0000 lds r25,(YawGyroHeading)+1 4468 2a04 A091 0000 lds r26,(YawGyroHeading)+2 4469 2a08 B091 0000 lds r27,(YawGyroHeading)+3 4470 2a0c A80E add r10,r24 4471 2a0e B91E adc r11,r25 4472 2a10 CA1E adc r12,r26 4473 2a12 DB1E adc r13,r27 4474 2a14 A092 0000 sts YawGyroHeading,r10 4475 2a18 B092 0000 sts (YawGyroHeading)+1,r11 4476 2a1c C092 0000 sts (YawGyroHeading)+2,r12 4477 2a20 D092 0000 sts (YawGyroHeading)+3,r13 4478 2a24 8091 0000 lds r24,FCParam+5 4479 2a28 682E mov r6,r24 4480 2a2a 7724 clr r7 4481 2a2c 869C mul r8,r6 4482 2a2e C001 movw r24,r0 4483 2a30 879C mul r8,r7 4484 2a32 900D add r25,r0 4485 2a34 969C mul r9,r6 4486 2a36 900D add r25,r0 4487 2a38 1124 clr r1 4488 2a3a 97FF sbrs r25,7 4489 2a3c 00C0 rjmp .+4 4490 2a3e 0C94 0000 jmp .L765 4491 .L586: 4492 2a42 9595 asr r25 4493 2a44 8795 ror r24 4494 2a46 9595 asr r25 4495 2a48 8795 ror r24 4496 2a4a 9595 asr r25 4497 2a4c 8795 ror r24 4498 2a4e 9595 asr r25 4499 2a50 8795 ror r24 4500 2a52 9595 asr r25 4501 2a54 8795 ror r24 4502 2a56 4301 movw r8,r6 4503 2a58 881A sub r8,r24 4504 2a5a 990A sbc r9,r25 4505 2a5c 97FE sbrs r9,7 4506 2a5e 00C0 rjmp .+4 4507 2a60 0C94 0000 jmp .L587 4508 2a64 4114 cp r4,__zero_reg__ 4509 2a66 5104 cpc r5,__zero_reg__ 4510 2a68 01F0 breq .+4 4511 2a6a 0C94 0000 jmp .L588 4512 2a6e 4090 0000 lds r4,MaxStickRoll 4513 2a72 5090 0000 lds r5,(MaxStickRoll)+1 4514 2a76 8091 0000 lds r24,MaxStickNick 4515 2a7a 9091 0000 lds r25,(MaxStickNick)+1 4516 2a7e 480E add r4,r24 4517 2a80 591E adc r5,r25 4518 2a82 57FE sbrs r5,7 4519 2a84 00C0 rjmp .+4 4520 2a86 0C94 0000 jmp .L766 4521 .L589: 4522 2a8a 5594 asr r5 4523 2a8c 4794 ror r4 4524 2a8e 5594 asr r5 4525 2a90 4794 ror r4 4526 2a92 5594 asr r5 4527 2a94 4794 ror r4 4528 2a96 40E4 ldi r20,lo8(64) 4529 2a98 50E0 ldi r21,hi8(64) 4530 2a9a 440E add r4,r20 4531 2a9c 551E adc r5,r21 4532 2a9e 8091 0000 lds r24,CompassCourse 4533 2aa2 9091 0000 lds r25,(CompassCourse)+1 4534 2aa6 AA27 clr r26 4535 2aa8 97FD sbrc r25,7 4536 2aaa A095 com r26 4537 2aac BA2F mov r27,r26 4538 2aae 7CE1 ldi r23,lo8(540) 4539 2ab0 E72E mov r14,r23 4540 2ab2 72E0 ldi r23,hi8(540) 4541 2ab4 F72E mov r15,r23 4542 2ab6 012D mov r16,__zero_reg__ 4543 2ab8 112D mov r17,__zero_reg__ 4544 2aba E81A sub r14,r24 4545 2abc F90A sbc r15,r25 4546 2abe 0A0B sbc r16,r26 4547 2ac0 1B0B sbc r17,r27 4548 2ac2 2091 0000 lds r18,ParamSet+23 4549 2ac6 8AE2 ldi r24,lo8(42) 4550 2ac8 289F mul r18,r24 4551 2aca 9001 movw r18,r0 4552 2acc 1124 clr r1 4553 2ace 4427 clr r20 4554 2ad0 37FD sbrc r19,7 4555 2ad2 4095 com r20 4556 2ad4 542F mov r21,r20 4557 2ad6 C601 movw r24,r12 4558 2ad8 B501 movw r22,r10 4559 2ada 0E94 0000 call __divmodsi4 4560 2ade E20E add r14,r18 4561 2ae0 F31E adc r15,r19 4562 2ae2 041F adc r16,r20 4563 2ae4 151F adc r17,r21 4564 2ae6 C801 movw r24,r16 4565 2ae8 B701 movw r22,r14 4566 2aea 28E6 ldi r18,lo8(360) 4567 2aec 31E0 ldi r19,hi8(360) 4568 2aee 40E0 ldi r20,hlo8(360) 4569 2af0 50E0 ldi r21,hhi8(360) 4570 2af2 0E94 0000 call __divmodsi4 4571 2af6 645B subi r22,lo8(-(-180)) 4572 2af8 7040 sbci r23,hi8(-(-180)) 4573 2afa 689D mul r22,r8 4574 2afc C001 movw r24,r0 4575 2afe 699D mul r22,r9 4576 2b00 900D add r25,r0 4577 2b02 789D mul r23,r8 4578 2b04 900D add r25,r0 4579 2b06 1124 clr r1 4580 2b08 B201 movw r22,r4 4581 2b0a 0E94 0000 call __divmodhi4 4582 2b0e 9B01 movw r18,r22 4583 2b10 C301 movw r24,r6 4584 2b12 880F lsl r24 4585 2b14 991F rol r25 4586 2b16 860D add r24,r6 4587 2b18 971D adc r25,r7 4588 2b1a 8217 cp r24,r18 4589 2b1c 9307 cpc r25,r19 4590 2b1e 04F0 brlt .+4 4591 2b20 0C94 0000 jmp .L590 4592 2b24 BC01 movw r22,r24 4593 .L591: 4594 2b26 9B01 movw r18,r22 4595 2b28 4427 clr r20 4596 2b2a 37FD sbrc r19,7 4597 2b2c 4095 com r20 4598 2b2e 542F mov r21,r20 4599 2b30 8091 0000 lds r24,ReadingIntegralGyroYaw 4600 2b34 9091 0000 lds r25,(ReadingIntegralGyroYaw)+1 4601 2b38 A091 0000 lds r26,(ReadingIntegralGyroYaw)+2 4602 2b3c B091 0000 lds r27,(ReadingIntegralGyroYaw)+3 4603 2b40 820F add r24,r18 4604 2b42 931F adc r25,r19 4605 2b44 A41F adc r26,r20 4606 2b46 B51F adc r27,r21 4607 2b48 8093 0000 sts ReadingIntegralGyroYaw,r24 4608 2b4c 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 4609 2b50 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 4610 2b54 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 4611 2b58 00C0 rjmp .L575 4612 .L401: 4613 2b5a 2998 cbi 37-32,1 4614 2b5c 8091 0000 lds r24,RC_Quality 4615 2b60 8436 cpi r24,lo8(100) 4616 2b62 00F4 brsh .+2 4617 2b64 00C0 rjmp .L767 4618 .L403: 4619 2b66 8091 0000 lds r24,RC_Quality 4620 2b6a 8D38 cpi r24,lo8(-115) 4621 2b6c 00F4 brsh .+2 4622 2b6e 00C0 rjmp .L410 4623 2b70 8091 0000 lds r24,MKFlags 4624 2b74 8F7E andi r24,lo8(-17) 4625 2b76 8093 0000 sts MKFlags,r24 4626 2b7a 8091 0000 lds r24,ParamSet+32 4627 2b7e 22E3 ldi r18,lo8(50) 4628 2b80 829F mul r24,r18 4629 2b82 C001 movw r24,r0 4630 2b84 1124 clr r1 4631 2b86 9093 0000 sts (RcLostTimer.2415)+1,r25 4632 2b8a 8093 0000 sts RcLostTimer.2415,r24 4633 2b8e 8091 0000 lds r24,StickGas 4634 2b92 9091 0000 lds r25,(StickGas)+1 4635 2b96 8997 sbiw r24,41 4636 2b98 04F0 brlt .L411 4637 2b9a 8091 0000 lds r24,MKFlags 4638 2b9e 80FF sbrs r24,0 4639 2ba0 00C0 rjmp .L411 4640 2ba2 8091 0000 lds r24,ModelIsFlying 4641 2ba6 9091 0000 lds r25,(ModelIsFlying)+1 4642 2baa BFEF ldi r27,hi8(-1) 4643 2bac 8F3F cpi r24,lo8(-1) 4644 2bae 9B07 cpc r25,r27 4645 2bb0 01F4 brne .+4 4646 2bb2 0C94 0000 jmp .L412 4647 2bb6 0196 adiw r24,1 4648 2bb8 9093 0000 sts (ModelIsFlying)+1,r25 4649 2bbc 8093 0000 sts ModelIsFlying,r24 4650 .L411: 4651 2bc0 8091 0000 lds r24,ModelIsFlying 4652 2bc4 9091 0000 lds r25,(ModelIsFlying)+1 4653 2bc8 8F3F cpi r24,255 4654 2bca 9105 cpc r25,__zero_reg__ 4655 2bcc 01F0 breq .+6 4656 2bce 00F0 brlo .+4 4657 2bd0 0C94 0000 jmp .L412 4658 2bd4 1092 0000 sts IPartNick.2408,__zero_reg__ 4659 2bd8 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ 4660 2bdc 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ 4661 2be0 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ 4662 2be4 1092 0000 sts IPartRoll.2409,__zero_reg__ 4663 2be8 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ 4664 2bec 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ 4665 2bf0 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ 4666 2bf4 1092 0000 sts (StickYaw)+1,__zero_reg__ 4667 2bf8 1092 0000 sts StickYaw,__zero_reg__ 4668 2bfc 8A3F cpi r24,250 4669 2bfe 9105 cpc r25,__zero_reg__ 4670 2c00 01F4 brne .+4 4671 2c02 0C94 0000 jmp .L768 4672 .L413: 4673 2c06 8091 0000 lds r24,ParamSet+4 4674 2c0a E82F mov r30,r24 4675 2c0c F0E0 ldi r31,lo8(0) 4676 2c0e EE0F lsl r30 4677 2c10 FF1F rol r31 4678 2c12 E050 subi r30,lo8(-(PPM_in)) 4679 2c14 F040 sbci r31,hi8(-(PPM_in)) 4680 2c16 8081 ld r24,Z 4681 2c18 9181 ldd r25,Z+1 4682 2c1a E090 0000 lds r14,Poti1 4683 2c1e F090 0000 lds r15,(Poti1)+1 4684 2c22 8359 subi r24,lo8(-(109)) 4685 2c24 9F4F sbci r25,hi8(-(109)) 4686 2c26 8E15 cp r24,r14 4687 2c28 9F05 cpc r25,r15 4688 2c2a 04F4 brge .+4 4689 2c2c 0C94 0000 jmp .L414 4690 2c30 C701 movw r24,r14 4691 2c32 0196 adiw r24,1 4692 2c34 9093 0000 sts (Poti1)+1,r25 4693 2c38 8093 0000 sts Poti1,r24 4694 2c3c 7C01 movw r14,r24 4695 .L415: 4696 2c3e 8091 0000 lds r24,ParamSet+5 4697 2c42 E82F mov r30,r24 4698 2c44 F0E0 ldi r31,lo8(0) 4699 2c46 EE0F lsl r30 4700 2c48 FF1F rol r31 4701 2c4a E050 subi r30,lo8(-(PPM_in)) 4702 2c4c F040 sbci r31,hi8(-(PPM_in)) 4703 2c4e 8081 ld r24,Z 4704 2c50 9181 ldd r25,Z+1 4705 2c52 0091 0000 lds r16,Poti2 4706 2c56 1091 0000 lds r17,(Poti2)+1 4707 2c5a 8359 subi r24,lo8(-(109)) 4708 2c5c 9F4F sbci r25,hi8(-(109)) 4709 2c5e 8017 cp r24,r16 4710 2c60 9107 cpc r25,r17 4711 2c62 04F4 brge .+4 4712 2c64 0C94 0000 jmp .L416 4713 2c68 C801 movw r24,r16 4714 2c6a 0196 adiw r24,1 4715 2c6c 9093 0000 sts (Poti2)+1,r25 4716 2c70 8093 0000 sts Poti2,r24 4717 2c74 8C01 movw r16,r24 4718 .L417: 4719 2c76 8091 0000 lds r24,ParamSet+6 4720 2c7a E82F mov r30,r24 4721 2c7c F0E0 ldi r31,lo8(0) 4722 2c7e EE0F lsl r30 4723 2c80 FF1F rol r31 4724 2c82 E050 subi r30,lo8(-(PPM_in)) 4725 2c84 F040 sbci r31,hi8(-(PPM_in)) 4726 2c86 8081 ld r24,Z 4727 2c88 9181 ldd r25,Z+1 4728 2c8a A090 0000 lds r10,Poti3 4729 2c8e B090 0000 lds r11,(Poti3)+1 4730 2c92 8359 subi r24,lo8(-(109)) 4731 2c94 9F4F sbci r25,hi8(-(109)) 4732 2c96 8A15 cp r24,r10 4733 2c98 9B05 cpc r25,r11 4734 2c9a 04F4 brge .+4 4735 2c9c 0C94 0000 jmp .L418 4736 2ca0 C501 movw r24,r10 4737 2ca2 0196 adiw r24,1 4738 2ca4 9093 0000 sts (Poti3)+1,r25 4739 2ca8 8093 0000 sts Poti3,r24 4740 2cac 5C01 movw r10,r24 4741 .L419: 4742 2cae 8091 0000 lds r24,ParamSet+7 4743 2cb2 E82F mov r30,r24 4744 2cb4 F0E0 ldi r31,lo8(0) 4745 2cb6 EE0F lsl r30 4746 2cb8 FF1F rol r31 4747 2cba E050 subi r30,lo8(-(PPM_in)) 4748 2cbc F040 sbci r31,hi8(-(PPM_in)) 4749 2cbe 8081 ld r24,Z 4750 2cc0 9181 ldd r25,Z+1 4751 2cc2 A091 0000 lds r26,Poti4 4752 2cc6 B091 0000 lds r27,(Poti4)+1 4753 2cca 8359 subi r24,lo8(-(109)) 4754 2ccc 9F4F sbci r25,hi8(-(109)) 4755 2cce 8A17 cp r24,r26 4756 2cd0 9B07 cpc r25,r27 4757 2cd2 04F4 brge .+4 4758 2cd4 0C94 0000 jmp .L420 4759 2cd8 CD01 movw r24,r26 4760 2cda 0196 adiw r24,1 4761 2cdc 9093 0000 sts (Poti4)+1,r25 4762 2ce0 8093 0000 sts Poti4,r24 4763 2ce4 DC01 movw r26,r24 4764 .L421: 4765 2ce6 8091 0000 lds r24,PPM_in+18 4766 2cea 9091 0000 lds r25,(PPM_in+18)+1 4767 2cee E091 0000 lds r30,Poti5 4768 2cf2 F091 0000 lds r31,(Poti5)+1 4769 2cf6 8359 subi r24,lo8(-(109)) 4770 2cf8 9F4F sbci r25,hi8(-(109)) 4771 2cfa 8E17 cp r24,r30 4772 2cfc 9F07 cpc r25,r31 4773 2cfe 04F4 brge .+4 4774 2d00 0C94 0000 jmp .L422 4775 2d04 CF01 movw r24,r30 4776 2d06 0196 adiw r24,1 4777 2d08 9093 0000 sts (Poti5)+1,r25 4778 2d0c 8093 0000 sts Poti5,r24 4779 2d10 FC01 movw r30,r24 4780 .L423: 4781 2d12 8091 0000 lds r24,PPM_in+20 4782 2d16 9091 0000 lds r25,(PPM_in+20)+1 4783 2d1a 6091 0000 lds r22,Poti6 4784 2d1e 7091 0000 lds r23,(Poti6)+1 4785 2d22 8359 subi r24,lo8(-(109)) 4786 2d24 9F4F sbci r25,hi8(-(109)) 4787 2d26 8617 cp r24,r22 4788 2d28 9707 cpc r25,r23 4789 2d2a 04F4 brge .+4 4790 2d2c 0C94 0000 jmp .L424 4791 2d30 CB01 movw r24,r22 4792 2d32 0196 adiw r24,1 4793 2d34 9093 0000 sts (Poti6)+1,r25 4794 2d38 8093 0000 sts Poti6,r24 4795 2d3c BC01 movw r22,r24 4796 .L425: 4797 2d3e 8091 0000 lds r24,PPM_in+22 4798 2d42 9091 0000 lds r25,(PPM_in+22)+1 4799 2d46 4091 0000 lds r20,Poti7 4800 2d4a 5091 0000 lds r21,(Poti7)+1 4801 2d4e 8359 subi r24,lo8(-(109)) 4802 2d50 9F4F sbci r25,hi8(-(109)) 4803 2d52 8417 cp r24,r20 4804 2d54 9507 cpc r25,r21 4805 2d56 04F4 brge .+4 4806 2d58 0C94 0000 jmp .L426 4807 2d5c CA01 movw r24,r20 4808 2d5e 0196 adiw r24,1 4809 2d60 9093 0000 sts (Poti7)+1,r25 4810 2d64 8093 0000 sts Poti7,r24 4811 2d68 AC01 movw r20,r24 4812 .L427: 4813 2d6a 8091 0000 lds r24,PPM_in+24 4814 2d6e 9091 0000 lds r25,(PPM_in+24)+1 4815 2d72 2091 0000 lds r18,Poti8 4816 2d76 3091 0000 lds r19,(Poti8)+1 4817 2d7a 8359 subi r24,lo8(-(109)) 4818 2d7c 9F4F sbci r25,hi8(-(109)) 4819 2d7e 8217 cp r24,r18 4820 2d80 9307 cpc r25,r19 4821 2d82 04F4 brge .+4 4822 2d84 0C94 0000 jmp .L428 4823 2d88 C901 movw r24,r18 4824 2d8a 0196 adiw r24,1 4825 2d8c 9093 0000 sts (Poti8)+1,r25 4826 2d90 8093 0000 sts Poti8,r24 4827 2d94 9C01 movw r18,r24 4828 .L429: 4829 2d96 F7FE sbrs r15,7 4830 2d98 00C0 rjmp .+4 4831 2d9a 0C94 0000 jmp .L769 4832 2d9e 8FEF ldi r24,lo8(255) 4833 2da0 E816 cp r14,r24 4834 2da2 F104 cpc r15,__zero_reg__ 4835 2da4 01F0 breq .L431 4836 2da6 04F0 brlt .L431 4837 2da8 8FEF ldi r24,lo8(255) 4838 2daa 90E0 ldi r25,hi8(255) 4839 2dac 9093 0000 sts (Poti1)+1,r25 4840 2db0 8093 0000 sts Poti1,r24 4841 .L431: 4842 2db4 17FF sbrs r17,7 4843 2db6 00C0 rjmp .+4 4844 2db8 0C94 0000 jmp .L770 4845 2dbc 0F3F cpi r16,255 4846 2dbe 1105 cpc r17,__zero_reg__ 4847 2dc0 01F0 breq .L433 4848 2dc2 04F0 brlt .L433 4849 2dc4 8FEF ldi r24,lo8(255) 4850 2dc6 90E0 ldi r25,hi8(255) 4851 2dc8 9093 0000 sts (Poti2)+1,r25 4852 2dcc 8093 0000 sts Poti2,r24 4853 .L433: 4854 2dd0 B7FE sbrs r11,7 4855 2dd2 00C0 rjmp .+4 4856 2dd4 0C94 0000 jmp .L771 4857 2dd8 9FEF ldi r25,lo8(255) 4858 2dda A916 cp r10,r25 4859 2ddc B104 cpc r11,__zero_reg__ 4860 2dde 01F0 breq .L435 4861 2de0 04F0 brlt .L435 4862 2de2 8FEF ldi r24,lo8(255) 4863 2de4 90E0 ldi r25,hi8(255) 4864 2de6 9093 0000 sts (Poti3)+1,r25 4865 2dea 8093 0000 sts Poti3,r24 4866 .L435: 4867 2dee B7FF sbrs r27,7 4868 2df0 00C0 rjmp .+4 4869 2df2 0C94 0000 jmp .L772 4870 2df6 AF3F cpi r26,255 4871 2df8 B105 cpc r27,__zero_reg__ 4872 2dfa 01F0 breq .L437 4873 2dfc 04F0 brlt .L437 4874 2dfe 8FEF ldi r24,lo8(255) 4875 2e00 90E0 ldi r25,hi8(255) 4876 2e02 9093 0000 sts (Poti4)+1,r25 4877 2e06 8093 0000 sts Poti4,r24 4878 .L437: 4879 2e0a F7FF sbrs r31,7 4880 2e0c 00C0 rjmp .+4 4881 2e0e 0C94 0000 jmp .L773 4882 2e12 EF3F cpi r30,255 4883 2e14 F105 cpc r31,__zero_reg__ 4884 2e16 01F0 breq .L439 4885 2e18 04F0 brlt .L439 4886 2e1a 8FEF ldi r24,lo8(255) 4887 2e1c 90E0 ldi r25,hi8(255) 4888 2e1e 9093 0000 sts (Poti5)+1,r25 4889 2e22 8093 0000 sts Poti5,r24 4890 .L439: 4891 2e26 77FF sbrs r23,7 4892 2e28 00C0 rjmp .+4 4893 2e2a 0C94 0000 jmp .L774 4894 2e2e 6F3F cpi r22,255 4895 2e30 7105 cpc r23,__zero_reg__ 4896 2e32 01F0 breq .L441 4897 2e34 04F0 brlt .L441 4898 2e36 8FEF ldi r24,lo8(255) 4899 2e38 90E0 ldi r25,hi8(255) 4900 2e3a 9093 0000 sts (Poti6)+1,r25 4901 2e3e 8093 0000 sts Poti6,r24 4902 .L441: 4903 2e42 57FF sbrs r21,7 4904 2e44 00C0 rjmp .+4 4905 2e46 0C94 0000 jmp .L775 4906 2e4a 4F3F cpi r20,255 4907 2e4c 5105 cpc r21,__zero_reg__ 4908 2e4e 01F0 breq .L443 4909 2e50 04F0 brlt .L443 4910 2e52 8FEF ldi r24,lo8(255) 4911 2e54 90E0 ldi r25,hi8(255) 4912 2e56 9093 0000 sts (Poti7)+1,r25 4913 2e5a 8093 0000 sts Poti7,r24 4914 .L443: 4915 2e5e 37FF sbrs r19,7 4916 2e60 00C0 rjmp .+4 4917 2e62 0C94 0000 jmp .L776 4918 2e66 2F3F cpi r18,255 4919 2e68 3105 cpc r19,__zero_reg__ 4920 2e6a 01F0 breq .L445 4921 2e6c 04F0 brlt .L445 4922 2e6e 8FEF ldi r24,lo8(255) 4923 2e70 90E0 ldi r25,hi8(255) 4924 2e72 9093 0000 sts (Poti8)+1,r25 4925 2e76 8093 0000 sts Poti8,r24 4926 .L445: 4927 2e7a 8091 0000 lds r24,ParamSet+2 4928 2e7e A82F mov r26,r24 4929 2e80 B0E0 ldi r27,lo8(0) 4930 2e82 AA0F lsl r26 4931 2e84 BB1F rol r27 4932 2e86 FD01 movw r30,r26 4933 2e88 E050 subi r30,lo8(-(PPM_in)) 4934 2e8a F040 sbci r31,hi8(-(PPM_in)) 4935 2e8c 8081 ld r24,Z 4936 2e8e 9181 ldd r25,Z+1 4937 2e90 8135 cpi r24,81 4938 2e92 9105 cpc r25,__zero_reg__ 4939 2e94 04F4 brge .+4 4940 2e96 0C94 0000 jmp .L446 4941 2e9a 8091 0000 lds r24,MKFlags 4942 2e9e 80FF sbrs r24,0 4943 2ea0 00C0 rjmp .+4 4944 2ea2 0C94 0000 jmp .L446 4945 2ea6 8091 0000 lds r24,ParamSet+3 4946 2eaa E82F mov r30,r24 4947 2eac F0E0 ldi r31,lo8(0) 4948 2eae EE0F lsl r30 4949 2eb0 FF1F rol r31 4950 2eb2 E050 subi r30,lo8(-(PPM_in)) 4951 2eb4 F040 sbci r31,hi8(-(PPM_in)) 4952 2eb6 8081 ld r24,Z 4953 2eb8 9181 ldd r25,Z+1 4954 2eba 8C34 cpi r24,76 4955 2ebc 9105 cpc r25,__zero_reg__ 4956 2ebe 04F4 brge .+4 4957 2ec0 0C94 0000 jmp .L447 4958 2ec4 8091 0000 lds r24,delay_neutral.2416 4959 2ec8 8F5F subi r24,lo8(-(1)) 4960 2eca 8093 0000 sts delay_neutral.2416,r24 4961 2ece 893C cpi r24,lo8(-55) 4962 2ed0 00F4 brsh .+4 4963 2ed2 0C94 0000 jmp .L446 4964 2ed6 1092 0000 sts delay_neutral.2416,__zero_reg__ 4965 2eda ABE0 ldi r26,lo8(11) 4966 2edc AD15 cp r26,r13 4967 2ede 00F4 brsh .+4 4968 2ee0 0C94 0000 jmp .L449 4969 2ee4 2998 cbi 37-32,1 4970 .L450: 4971 2ee6 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ 4972 2eea 1092 0000 sts ModelIsFlying,__zero_reg__ 4973 2eee 8091 0000 lds r24,ParamSet 4974 2ef2 482F mov r20,r24 4975 2ef4 50E0 ldi r21,lo8(0) 4976 2ef6 440F lsl r20 4977 2ef8 551F rol r21 4978 2efa FA01 movw r30,r20 4979 2efc E050 subi r30,lo8(-(PPM_in)) 4980 2efe F040 sbci r31,hi8(-(PPM_in)) 4981 2f00 8081 ld r24,Z 4982 2f02 9181 ldd r25,Z+1 4983 2f04 8734 cpi r24,71 4984 2f06 9105 cpc r25,__zero_reg__ 4985 2f08 04F4 brge .+4 4986 2f0a 0C94 0000 jmp .L451 4987 2f0e 8091 0000 lds r24,ParamSet+1 4988 2f12 A82F mov r26,r24 4989 2f14 B0E0 ldi r27,lo8(0) 4990 2f16 AA0F lsl r26 4991 2f18 BB1F rol r27 4992 .L452: 4993 2f1a FD01 movw r30,r26 4994 2f1c E050 subi r30,lo8(-(PPM_in)) 4995 2f1e F040 sbci r31,hi8(-(PPM_in)) 4996 2f20 8081 ld r24,Z 4997 2f22 9181 ldd r25,Z+1 4998 2f24 8734 cpi r24,71 4999 2f26 9105 cpc r25,__zero_reg__ 5000 2f28 04F0 brlt .L455 5001 2f2a FA01 movw r30,r20 5002 2f2c E050 subi r30,lo8(-(PPM_in)) 5003 2f2e F040 sbci r31,hi8(-(PPM_in)) 5004 2f30 0190 ld __tmp_reg__,Z+ 5005 2f32 F081 ld r31,Z 5006 2f34 E02D mov r30,__tmp_reg__ 5007 .L455: 5008 2f36 FD01 movw r30,r26 5009 2f38 E050 subi r30,lo8(-(PPM_in)) 5010 2f3a F040 sbci r31,hi8(-(PPM_in)) 5011 2f3c 8081 ld r24,Z 5012 2f3e 9181 ldd r25,Z+1 5013 2f40 8734 cpi r24,71 5014 2f42 9105 cpc r25,__zero_reg__ 5015 2f44 04F4 brge .+4 5016 2f46 0C94 0000 jmp .L456 5017 2f4a FA01 movw r30,r20 5018 2f4c E050 subi r30,lo8(-(PPM_in)) 5019 2f4e F040 sbci r31,hi8(-(PPM_in)) 5020 2f50 8081 ld r24,Z 5021 2f52 9181 ldd r25,Z+1 5022 2f54 8734 cpi r24,71 5023 2f56 9105 cpc r25,__zero_reg__ 5024 2f58 04F4 brge .+4 5025 2f5a 0C94 0000 jmp .L456 5026 2f5e 22E0 ldi r18,lo8(2) 5027 .L457: 5028 2f60 FD01 movw r30,r26 5029 2f62 E050 subi r30,lo8(-(PPM_in)) 5030 2f64 F040 sbci r31,hi8(-(PPM_in)) 5031 2f66 8081 ld r24,Z 5032 2f68 9181 ldd r25,Z+1 5033 2f6a 8634 cpi r24,70 5034 2f6c 9105 cpc r25,__zero_reg__ 5035 2f6e 04F4 brge .L458 5036 2f70 FA01 movw r30,r20 5037 2f72 E050 subi r30,lo8(-(PPM_in)) 5038 2f74 F040 sbci r31,hi8(-(PPM_in)) 5039 2f76 8081 ld r24,Z 5040 2f78 9181 ldd r25,Z+1 5041 2f7a 8734 cpi r24,71 5042 2f7c 9105 cpc r25,__zero_reg__ 5043 2f7e 04F0 brlt .L458 5044 2f80 23E0 ldi r18,lo8(3) 5045 .L458: 5046 2f82 FD01 movw r30,r26 5047 2f84 E050 subi r30,lo8(-(PPM_in)) 5048 2f86 F040 sbci r31,hi8(-(PPM_in)) 5049 2f88 8081 ld r24,Z 5050 2f8a 9181 ldd r25,Z+1 5051 2f8c 8A5B subi r24,lo8(-70) 5052 2f8e 9F4F sbci r25,hi8(-70) 5053 2f90 04F4 brge .L459 5054 2f92 FA01 movw r30,r20 5055 2f94 E050 subi r30,lo8(-(PPM_in)) 5056 2f96 F040 sbci r31,hi8(-(PPM_in)) 5057 2f98 8081 ld r24,Z 5058 2f9a 9181 ldd r25,Z+1 5059 2f9c 8734 cpi r24,71 5060 2f9e 9105 cpc r25,__zero_reg__ 5061 2fa0 04F0 brlt .L459 5062 2fa2 24E0 ldi r18,lo8(4) 5063 .L459: 5064 2fa4 A050 subi r26,lo8(-(PPM_in)) 5065 2fa6 B040 sbci r27,hi8(-(PPM_in)) 5066 2fa8 8D91 ld r24,X+ 5067 2faa 9C91 ld r25,X 5068 2fac 8A5B subi r24,lo8(-70) 5069 2fae 9F4F sbci r25,hi8(-70) 5070 2fb0 04F4 brge .L460 5071 2fb2 4050 subi r20,lo8(-(PPM_in)) 5072 2fb4 5040 sbci r21,hi8(-(PPM_in)) 5073 2fb6 DA01 movw r26,r20 5074 2fb8 8D91 ld r24,X+ 5075 2fba 9C91 ld r25,X 5076 2fbc 8634 cpi r24,70 5077 2fbe 9105 cpc r25,__zero_reg__ 5078 2fc0 04F4 brge .L460 5079 2fc2 25E0 ldi r18,lo8(5) 5080 .L460: 5081 2fc4 822F mov r24,r18 5082 2fc6 0E94 0000 call SetActiveParamSet 5083 2fca 0E94 0000 call GetActiveParamSet 5084 2fce 0E94 0000 call ParamSet_ReadFromEEProm 5085 2fd2 0E94 0000 call Servo_Off 5086 2fd6 80E0 ldi r24,lo8(0) 5087 2fd8 0C94 0000 jmp .L726 5088 .L504: 5089 2fdc 8091 0000 lds r24,ParamSet+48 5090 2fe0 282F mov r18,r24 5091 2fe2 30E0 ldi r19,lo8(0) 5092 2fe4 8091 0000 lds r24,StickGas 5093 2fe8 9091 0000 lds r25,(StickGas)+1 5094 2fec 2817 cp r18,r24 5095 2fee 3907 cpc r19,r25 5096 2ff0 04F4 brge .L506 5097 2ff2 3093 0000 sts (StickGas)+1,r19 5098 2ff6 2093 0000 sts StickGas,r18 5099 .L506: 5100 2ffa 81E0 ldi r24,lo8(1) 5101 2ffc 8093 0000 sts FunnelCourse,r24 5102 3000 00C0 rjmp .L505 5103 .L759: 5104 3002 8091 0000 lds r24,MKFlags 5105 3006 80FD sbrc r24,0 5106 3008 00C0 rjmp .L576 5107 300a 0E94 0000 call SetCompassCalState 5108 300e 9091 0000 lds r25,LoopingNick 5109 3012 9D8F std Y+29,r25 5110 .L728: 5111 3014 2090 0000 lds r2,GyroYaw 5112 3018 3090 0000 lds r3,(GyroYaw)+1 5113 .L575: 5114 301c 8091 0000 lds r24,TimerDebugOut.2419 5115 3020 8150 subi r24,lo8(-(-1)) 5116 3022 8093 0000 sts TimerDebugOut.2419,r24 5117 3026 8F5F subi r24,lo8(-(1)) 5118 3028 01F0 breq .+2 5119 302a 00C0 rjmp .L593 5120 302c 88E1 ldi r24,lo8(24) 5121 302e 8093 0000 sts TimerDebugOut.2419,r24 5122 3032 6091 0000 lds r22,IntegralGyroNick 5123 3036 7091 0000 lds r23,(IntegralGyroNick)+1 5124 303a 8091 0000 lds r24,(IntegralGyroNick)+2 5125 303e 9091 0000 lds r25,(IntegralGyroNick)+3 5126 3042 2AE0 ldi r18,lo8(10) 5127 3044 30E0 ldi r19,hi8(10) 5128 3046 40E0 ldi r20,hlo8(10) 5129 3048 50E0 ldi r21,hhi8(10) 5130 304a 0E94 0000 call __mulsi3 5131 304e E090 0000 lds r14,ParamSet+23 5132 3052 2AE2 ldi r18,lo8(42) 5133 3054 E29E mul r14,r18 5134 3056 7001 movw r14,r0 5135 3058 1124 clr r1 5136 305a 0027 clr r16 5137 305c F7FC sbrc r15,7 5138 305e 0095 com r16 5139 3060 102F mov r17,r16 5140 3062 A801 movw r20,r16 5141 3064 9701 movw r18,r14 5142 3066 0E94 0000 call __divmodsi4 5143 306a 3093 0000 sts (DebugOut+2)+1,r19 5144 306e 2093 0000 sts DebugOut+2,r18 5145 3072 6091 0000 lds r22,IntegralGyroRoll 5146 3076 7091 0000 lds r23,(IntegralGyroRoll)+1 5147 307a 8091 0000 lds r24,(IntegralGyroRoll)+2 5148 307e 9091 0000 lds r25,(IntegralGyroRoll)+3 5149 3082 2AE0 ldi r18,lo8(10) 5150 3084 30E0 ldi r19,hi8(10) 5151 3086 40E0 ldi r20,hlo8(10) 5152 3088 50E0 ldi r21,hhi8(10) 5153 308a 0E94 0000 call __mulsi3 5154 308e A801 movw r20,r16 5155 3090 9701 movw r18,r14 5156 3092 0E94 0000 call __divmodsi4 5157 3096 3093 0000 sts (DebugOut+4)+1,r19 5158 309a 2093 0000 sts DebugOut+4,r18 5159 309e 8091 0000 lds r24,AccNick 5160 30a2 9091 0000 lds r25,AccNick+1 5161 30a6 9C01 movw r18,r24 5162 30a8 220F lsl r18 5163 30aa 331F rol r19 5164 30ac 220F lsl r18 5165 30ae 331F rol r19 5166 30b0 220F lsl r18 5167 30b2 331F rol r19 5168 30b4 880F lsl r24 5169 30b6 991F rol r25 5170 30b8 820F add r24,r18 5171 30ba 931F adc r25,r19 5172 30bc 6AE2 ldi r22,lo8(42) 5173 30be 70E0 ldi r23,hi8(42) 5174 30c0 0E94 0000 call __divmodhi4 5175 30c4 7093 0000 sts (DebugOut+6)+1,r23 5176 30c8 6093 0000 sts DebugOut+6,r22 5177 30cc 8091 0000 lds r24,AccRoll 5178 30d0 9091 0000 lds r25,AccRoll+1 5179 30d4 9C01 movw r18,r24 5180 30d6 220F lsl r18 5181 30d8 331F rol r19 5182 30da 220F lsl r18 5183 30dc 331F rol r19 5184 30de 220F lsl r18 5185 30e0 331F rol r19 5186 30e2 880F lsl r24 5187 30e4 991F rol r25 5188 30e6 820F add r24,r18 5189 30e8 931F adc r25,r19 5190 30ea 6AE2 ldi r22,lo8(42) 5191 30ec 70E0 ldi r23,hi8(42) 5192 30ee 0E94 0000 call __divmodhi4 5193 30f2 7093 0000 sts (DebugOut+8)+1,r23 5194 30f6 6093 0000 sts DebugOut+8,r22 5195 30fa 3092 0000 sts (DebugOut+10)+1,r3 5196 30fe 2092 0000 sts DebugOut+10,r2 5197 3102 6091 0000 lds r22,ReadingHeight 5198 3106 7091 0000 lds r23,(ReadingHeight)+1 5199 310a 8091 0000 lds r24,(ReadingHeight)+2 5200 310e 9091 0000 lds r25,(ReadingHeight)+3 5201 3112 25E0 ldi r18,lo8(5) 5202 3114 30E0 ldi r19,hi8(5) 5203 3116 40E0 ldi r20,hlo8(5) 5204 3118 50E0 ldi r21,hhi8(5) 5205 311a 0E94 0000 call __divmodsi4 5206 311e 3093 0000 sts (DebugOut+12)+1,r19 5207 3122 2093 0000 sts DebugOut+12,r18 5208 3126 6091 0000 lds r22,ReadingIntegralTop 5209 312a 7091 0000 lds r23,(ReadingIntegralTop)+1 5210 312e 8091 0000 lds r24,(ReadingIntegralTop)+2 5211 3132 9091 0000 lds r25,(ReadingIntegralTop)+3 5212 3136 20E0 ldi r18,lo8(512) 5213 3138 32E0 ldi r19,hi8(512) 5214 313a 40E0 ldi r20,hlo8(512) 5215 313c 50E0 ldi r21,hhi8(512) 5216 313e 0E94 0000 call __divmodsi4 5217 3142 3093 0000 sts (DebugOut+14)+1,r19 5218 3146 2093 0000 sts DebugOut+14,r18 5219 314a 8091 0000 lds r24,CompassHeading 5220 314e 9091 0000 lds r25,(CompassHeading)+1 5221 3152 9093 0000 sts (DebugOut+18)+1,r25 5222 3156 8093 0000 sts DebugOut+18,r24 5223 315a 8091 0000 lds r24,UBat 5224 315e 9091 0000 lds r25,(UBat)+1 5225 3162 9093 0000 sts (DebugOut+20)+1,r25 5226 3166 8093 0000 sts DebugOut+20,r24 5227 316a 8091 0000 lds r24,RC_Quality 5228 316e 8093 0000 sts DebugOut+22,r24 5229 3172 1092 0000 sts DebugOut+23,__zero_reg__ 5230 3176 6091 0000 lds r22,YawGyroHeading 5231 317a 7091 0000 lds r23,(YawGyroHeading)+1 5232 317e 8091 0000 lds r24,(YawGyroHeading)+2 5233 3182 9091 0000 lds r25,(YawGyroHeading)+3 5234 3186 A801 movw r20,r16 5235 3188 9701 movw r18,r14 5236 318a 0E94 0000 call __divmodsi4 5237 318e 3093 0000 sts (DebugOut+24)+1,r19 5238 3192 2093 0000 sts DebugOut+24,r18 5239 3196 8091 0000 lds r24,CompassCalState 5240 319a 8093 0000 sts DebugOut+40,r24 5241 319e 1092 0000 sts DebugOut+41,__zero_reg__ 5242 31a2 8091 0000 lds r24,ServoNickValue 5243 31a6 9091 0000 lds r25,(ServoNickValue)+1 5244 31aa 9093 0000 sts (DebugOut+42)+1,r25 5245 31ae 8093 0000 sts DebugOut+42,r24 5246 31b2 8091 0000 lds r24,GPSStickNick 5247 31b6 9091 0000 lds r25,(GPSStickNick)+1 5248 31ba 9093 0000 sts (DebugOut+62)+1,r25 5249 31be 8093 0000 sts DebugOut+62,r24 5250 31c2 8091 0000 lds r24,GPSStickRoll 5251 31c6 9091 0000 lds r25,(GPSStickRoll)+1 5252 31ca 9093 0000 sts (DebugOut+64)+1,r25 5253 31ce 8093 0000 sts DebugOut+64,r24 5254 .L593: 5255 31d2 8090 0000 lds r8,TrimNick 5256 31d6 9090 0000 lds r9,(TrimNick)+1 5257 31da 58E3 ldi r21,lo8(-200) 5258 31dc 8516 cp r8,r21 5259 31de 5FEF ldi r21,hi8(-200) 5260 31e0 9506 cpc r9,r21 5261 31e2 04F0 brlt .+2 5262 31e4 00C0 rjmp .L594 5263 31e6 88E3 ldi r24,lo8(-200) 5264 31e8 9FEF ldi r25,hi8(-200) 5265 31ea 9093 0000 sts (TrimNick)+1,r25 5266 31ee 8093 0000 sts TrimNick,r24 5267 31f2 F8E3 ldi r31,lo8(-200) 5268 31f4 8F2E mov r8,r31 5269 31f6 FFEF ldi r31,hi8(-200) 5270 31f8 9F2E mov r9,r31 5271 .L595: 5272 31fa 8091 0000 lds r24,TrimRoll 5273 31fe 9091 0000 lds r25,(TrimRoll)+1 5274 3202 9F8F std Y+31,r25 5275 3204 8E8F std Y+30,r24 5276 3206 8853 subi r24,lo8(-200) 5277 3208 9F4F sbci r25,hi8(-200) 5278 320a 04F0 brlt .+2 5279 320c 00C0 rjmp .L596 5280 320e 88E3 ldi r24,lo8(-200) 5281 3210 9FEF ldi r25,hi8(-200) 5282 3212 9093 0000 sts (TrimRoll)+1,r25 5283 3216 8093 0000 sts TrimRoll,r24 5284 321a 9F8F std Y+31,r25 5285 321c 8E8F std Y+30,r24 5286 .L597: 5287 321e 8091 0000 lds r24,FunnelCourse 5288 3222 8823 tst r24 5289 3224 01F0 breq .L598 5290 3226 1092 0000 sts IPartNick.2408,__zero_reg__ 5291 322a 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ 5292 322e 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ 5293 3232 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ 5294 3236 1092 0000 sts IPartRoll.2409,__zero_reg__ 5295 323a 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ 5296 323e 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ 5297 3242 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ 5298 .L598: 5299 3246 FD8D ldd r31,Y+29 5300 3248 FF23 tst r31 5301 324a 01F4 brne .+2 5302 324c 00C0 rjmp .L599 5303 324e 4424 clr r4 5304 3250 5524 clr r5 5305 3252 1C82 std Y+4,__zero_reg__ 5306 3254 1B82 std Y+3,__zero_reg__ 5307 .L600: 5308 3256 A090 0000 lds r10,GyroNick 5309 325a B090 0000 lds r11,(GyroNick)+1 5310 325e BC8A std Y+20,r11 5311 3260 AB8A std Y+19,r10 5312 3262 8091 0000 lds r24,GyroPFactor 5313 3266 282F mov r18,r24 5314 3268 30E0 ldi r19,lo8(0) 5315 326a 40E0 ldi r20,lo8(0) 5316 326c 50E0 ldi r21,hi8(0) 5317 326e 2F87 std Y+15,r18 5318 3270 388B std Y+16,r19 5319 3272 498B std Y+17,r20 5320 3274 5A8B std Y+18,r21 5321 3276 1091 0000 lds r17,LoopingRoll 5322 327a 1123 tst r17 5323 327c 01F4 brne .+2 5324 327e 00C0 rjmp .L601 5325 3280 1886 std Y+8,__zero_reg__ 5326 3282 1F82 std Y+7,__zero_reg__ 5327 3284 1E82 std Y+6,__zero_reg__ 5328 3286 1D82 std Y+5,__zero_reg__ 5329 .L602: 5330 3288 4091 0000 lds r20,GyroRoll 5331 328c 5091 0000 lds r21,(GyroRoll)+1 5332 3290 5E87 std Y+14,r21 5333 3292 4D87 std Y+13,r20 5334 3294 6090 0000 lds r6,GyroYawPFactor 5335 3298 7090 0000 lds r7,GyroYawIFactor 5336 329c 6091 0000 lds r22,IntegralGyroYaw 5337 32a0 7091 0000 lds r23,(IntegralGyroYaw)+1 5338 32a4 8091 0000 lds r24,(IntegralGyroYaw)+2 5339 32a8 9091 0000 lds r25,(IntegralGyroYaw)+3 5340 32ac 6987 std Y+9,r22 5341 32ae 7A87 std Y+10,r23 5342 32b0 8B87 std Y+11,r24 5343 32b2 9C87 std Y+12,r25 5344 32b4 4091 0000 lds r20,StickGas 5345 32b8 5091 0000 lds r21,(StickGas)+1 5346 32bc 2091 0000 lds r18,LowVoltageWarning 5347 32c0 8091 0000 lds r24,UBat 5348 32c4 9091 0000 lds r25,(UBat)+1 5349 32c8 30E0 ldi r19,lo8(0) 5350 32ca 2817 cp r18,r24 5351 32cc 3907 cpc r19,r25 5352 32ce 04F4 brge .L603 5353 32d0 6091 0000 lds r22,UBat 5354 32d4 7091 0000 lds r23,(UBat)+1 5355 32d8 429F mul r20,r18 5356 32da C001 movw r24,r0 5357 32dc 439F mul r20,r19 5358 32de 900D add r25,r0 5359 32e0 529F mul r21,r18 5360 32e2 900D add r25,r0 5361 32e4 1124 clr r1 5362 32e6 0E94 0000 call __udivmodhi4 5363 32ea AB01 movw r20,r22 5364 .L603: 5365 32ec CA01 movw r24,r20 5366 32ee 880F lsl r24 5367 32f0 991F rol r25 5368 32f2 880F lsl r24 5369 32f4 991F rol r25 5370 32f6 99AF std Y+57,r25 5371 32f8 88AF std Y+56,r24 5372 32fa 8091 0000 lds r24,ParamSet+8 5373 32fe 80FF sbrs r24,0 5374 3300 00C0 rjmp .L604 5375 3302 1123 tst r17 5376 3304 01F4 brne .+2 5377 3306 00C0 rjmp .L777 5378 .L604: 5379 3308 8091 0000 lds r24,ParamSet+21 5380 330c 282F mov r18,r24 5381 330e 30E0 ldi r19,lo8(0) 5382 3310 265F subi r18,lo8(-(10)) 5383 3312 3F4F sbci r19,hi8(-(10)) 5384 3314 5901 movw r10,r18 5385 3316 AA0C lsl r10 5386 3318 BB1C rol r11 5387 331a AA0C lsl r10 5388 331c BB1C rol r11 5389 331e 2A50 subi r18,lo8(-(-10)) 5390 3320 3040 sbci r19,hi8(-(-10)) 5391 3322 C8AC ldd r12,Y+56 5392 3324 D9AC ldd r13,Y+57 5393 3326 CA14 cp r12,r10 5394 3328 DB04 cpc r13,r11 5395 332a 04F0 brlt .L669 5396 332c 8091 0000 lds r24,ParamSet+22 5397 3330 90E0 ldi r25,lo8(0) 5398 3332 4497 sbiw r24,20 5399 3334 880F lsl r24 5400 3336 991F rol r25 5401 3338 880F lsl r24 5402 333a 991F rol r25 5403 333c 5601 movw r10,r12 5404 333e 8C15 cp r24,r12 5405 3340 9D05 cpc r25,r13 5406 3342 04F4 brge .+2 5407 3344 00C0 rjmp .L778 5408 .L669: 5409 3346 8091 0000 lds r24,MissingMotor 5410 334a 8823 tst r24 5411 334c 01F0 breq .L671 5412 334e 8091 0000 lds r24,ModelIsFlying 5413 3352 9091 0000 lds r25,(ModelIsFlying)+1 5414 3356 0297 sbiw r24,2 5415 3358 C097 sbiw r24,48 5416 335a 00F4 brsh .+2 5417 335c 00C0 rjmp .L779 5418 .L672: 5419 335e 5901 movw r10,r18 5420 3360 AA0C lsl r10 5421 3362 BB1C rol r11 5422 3364 AA0C lsl r10 5423 3366 BB1C rol r11 5424 .L671: 5425 3368 B092 0000 sts (DebugOut+16)+1,r11 5426 336c A092 0000 sts DebugOut+16,r10 5427 3370 220C lsl r2 5428 3372 331C rol r3 5429 3374 B101 movw r22,r2 5430 3376 8827 clr r24 5431 3378 77FD sbrc r23,7 5432 337a 8095 com r24 5433 337c 982F mov r25,r24 5434 337e 262D mov r18,r6 5435 3380 30E0 ldi r19,lo8(0) 5436 3382 40E0 ldi r20,lo8(0) 5437 3384 50E0 ldi r21,hi8(0) 5438 3386 0E94 0000 call __mulsi3 5439 338a 20E4 ldi r18,lo8(64) 5440 338c 30E0 ldi r19,hi8(64) 5441 338e 40E0 ldi r20,hlo8(64) 5442 3390 50E0 ldi r21,hhi8(64) 5443 3392 0E94 0000 call __divmodsi4 5444 3396 7901 movw r14,r18 5445 3398 8A01 movw r16,r20 5446 339a 672D mov r22,r7 5447 339c 70E0 ldi r23,lo8(0) 5448 339e 80E0 ldi r24,lo8(0) 5449 33a0 90E0 ldi r25,hi8(0) 5450 33a2 2985 ldd r18,Y+9 5451 33a4 3A85 ldd r19,Y+10 5452 33a6 4B85 ldd r20,Y+11 5453 33a8 5C85 ldd r21,Y+12 5454 33aa 0E94 0000 call __mulsi3 5455 33ae 20EF ldi r18,lo8(22000) 5456 33b0 35E5 ldi r19,hi8(22000) 5457 33b2 40E0 ldi r20,hlo8(22000) 5458 33b4 50E0 ldi r21,hhi8(22000) 5459 33b6 0E94 0000 call __divmodsi4 5460 33ba C701 movw r24,r14 5461 33bc 820F add r24,r18 5462 33be 931F adc r25,r19 5463 33c0 20E4 ldi r18,hi8(16385) 5464 33c2 8130 cpi r24,lo8(16385) 5465 33c4 9207 cpc r25,r18 5466 33c6 04F0 brlt .L673 5467 33c8 80E0 ldi r24,lo8(16384) 5468 33ca 90E4 ldi r25,hi8(16384) 5469 .L673: 5470 33cc 8C01 movw r16,r24 5471 33ce 30EC ldi r19,hi8(-16384) 5472 33d0 8030 cpi r24,lo8(-16384) 5473 33d2 9307 cpc r25,r19 5474 33d4 04F4 brge .L674 5475 33d6 00E0 ldi r16,lo8(-16384) 5476 33d8 10EC ldi r17,hi8(-16384) 5477 .L674: 5478 33da 6091 0000 lds r22,SetPointYaw.2410 5479 33de 7091 0000 lds r23,(SetPointYaw.2410)+1 5480 33e2 8091 0000 lds r24,(SetPointYaw.2410)+2 5481 33e6 9091 0000 lds r25,(SetPointYaw.2410)+3 5482 33ea 2CEF ldi r18,lo8(-4) 5483 33ec 3FEF ldi r19,hi8(-4) 5484 33ee 4FEF ldi r20,hlo8(-4) 5485 33f0 5FEF ldi r21,hhi8(-4) 5486 33f2 0E94 0000 call __mulsi3 5487 33f6 7801 movw r14,r16 5488 33f8 E60E add r14,r22 5489 33fa F71E adc r15,r23 5490 33fc 41EA ldi r20,lo8(161) 5491 33fe A416 cp r10,r20 5492 3400 B104 cpc r11,__zero_reg__ 5493 3402 04F4 brge .+2 5494 3404 00C0 rjmp .L675 5495 3406 C501 movw r24,r10 5496 3408 B7FE sbrs r11,7 5497 340a 00C0 rjmp .+4 5498 340c 0C94 0000 jmp .L780 5499 .L676: 5500 3410 9595 asr r25 5501 3412 8795 ror r24 5502 3414 2227 clr r18 5503 3416 3327 clr r19 5504 3418 281B sub r18,r24 5505 341a 390B sbc r19,r25 5506 341c E216 cp r14,r18 5507 341e F306 cpc r15,r19 5508 3420 04F0 brlt .+2 5509 3422 00C0 rjmp .L781 5510 .L677: 5511 3424 8091 0000 lds r24,ParamSet+22 5512 3428 90E0 ldi r25,lo8(0) 5513 342a 880F lsl r24 5514 342c 991F rol r25 5515 342e 880F lsl r24 5516 3430 991F rol r25 5517 3432 3501 movw r6,r10 5518 3434 681A sub r6,r24 5519 3436 790A sbc r7,r25 5520 3438 2615 cp r18,r6 5521 343a 3705 cpc r19,r7 5522 343c 04F0 brlt .L681 5523 343e 8A19 sub r24,r10 5524 3440 9B09 sbc r25,r11 5525 3442 3901 movw r6,r18 5526 3444 8217 cp r24,r18 5527 3446 9307 cpc r25,r19 5528 3448 04F4 brge .+2 5529 344a 00C0 rjmp .L782 5530 .L681: 5531 344c 6091 0000 lds r22,StickNick 5532 3450 7091 0000 lds r23,(StickNick)+1 5533 3454 79AF std Y+57,r23 5534 3456 68AF std Y+56,r22 5535 3458 7401 movw r14,r8 5536 345a 0027 clr r16 5537 345c F7FC sbrc r15,7 5538 345e 0095 com r16 5539 3460 102F mov r17,r16 5540 3462 EE0C lsl r14 5541 3464 FF1C rol r15 5542 3466 001F rol r16 5543 3468 111F rol r17 5544 346a EE0C lsl r14 5545 346c FF1C rol r15 5546 346e 001F rol r16 5547 3470 111F rol r17 5548 3472 EE0C lsl r14 5549 3474 FF1C rol r15 5550 3476 001F rol r16 5551 3478 111F rol r17 5552 347a EE0C lsl r14 5553 347c FF1C rol r15 5554 347e 001F rol r16 5555 3480 111F rol r17 5556 3482 EE0C lsl r14 5557 3484 FF1C rol r15 5558 3486 001F rol r16 5559 3488 111F rol r17 5560 348a EE0C lsl r14 5561 348c FF1C rol r15 5562 348e 001F rol r16 5563 3490 111F rol r17 5564 3492 EE0C lsl r14 5565 3494 FF1C rol r15 5566 3496 001F rol r16 5567 3498 111F rol r17 5568 349a 8B89 ldd r24,Y+19 5569 349c 9C89 ldd r25,Y+20 5570 349e 9C01 movw r18,r24 5571 34a0 4427 clr r20 5572 34a2 37FD sbrc r19,7 5573 34a4 4095 com r20 5574 34a6 542F mov r21,r20 5575 34a8 6F85 ldd r22,Y+15 5576 34aa 7889 ldd r23,Y+16 5577 34ac 8989 ldd r24,Y+17 5578 34ae 9A89 ldd r25,Y+18 5579 34b0 0E94 0000 call __mulsi3 5580 34b4 E60E add r14,r22 5581 34b6 F71E adc r15,r23 5582 34b8 081F adc r16,r24 5583 34ba 191F adc r17,r25 5584 34bc C801 movw r24,r16 5585 34be B701 movw r22,r14 5586 34c0 20E4 ldi r18,lo8(64) 5587 34c2 30E0 ldi r19,hi8(64) 5588 34c4 40E0 ldi r20,hlo8(64) 5589 34c6 50E0 ldi r21,hhi8(64) 5590 34c8 0E94 0000 call __divmodsi4 5591 34cc AB81 ldd r26,Y+3 5592 34ce BC81 ldd r27,Y+4 5593 34d0 2A0F add r18,r26 5594 34d2 3B1F adc r19,r27 5595 34d4 B0E4 ldi r27,hi8(16385) 5596 34d6 2130 cpi r18,lo8(16385) 5597 34d8 3B07 cpc r19,r27 5598 34da 04F0 brlt .L683 5599 34dc 20E0 ldi r18,lo8(16384) 5600 34de 30E4 ldi r19,hi8(16384) 5601 .L683: 5602 34e0 E0EC ldi r30,hi8(-16384) 5603 34e2 2030 cpi r18,lo8(-16384) 5604 34e4 3E07 cpc r19,r30 5605 34e6 04F4 brge .L684 5606 34e8 20E0 ldi r18,lo8(-16384) 5607 34ea 30EC ldi r19,hi8(-16384) 5608 .L684: 5609 34ec 6901 movw r12,r18 5610 34ee 48AD ldd r20,Y+56 5611 34f0 59AD ldd r21,Y+57 5612 34f2 C41A sub r12,r20 5613 34f4 D50A sbc r13,r21 5614 34f6 3090 0000 lds r3,GyroIFactor 5615 34fa 3320 tst r3 5616 34fc 01F4 brne .+2 5617 34fe 00C0 rjmp .L685 5618 3500 441A sub r4,r20 5619 3502 550A sbc r5,r21 5620 3504 C201 movw r24,r4 5621 3506 AA27 clr r26 5622 3508 97FD sbrc r25,7 5623 350a A095 com r26 5624 350c BA2F mov r27,r26 5625 350e E090 0000 lds r14,IPartNick.2408 5626 3512 F090 0000 lds r15,(IPartNick.2408)+1 5627 3516 0091 0000 lds r16,(IPartNick.2408)+2 5628 351a 1091 0000 lds r17,(IPartNick.2408)+3 5629 351e E80E add r14,r24 5630 3520 F91E adc r15,r25 5631 3522 0A1F adc r16,r26 5632 3524 1B1F adc r17,r27 5633 3526 E092 0000 sts IPartNick.2408,r14 5634 352a F092 0000 sts (IPartNick.2408)+1,r15 5635 352e 0093 0000 sts (IPartNick.2408)+2,r16 5636 3532 1093 0000 sts (IPartNick.2408)+3,r17 5637 .L686: 5638 3536 50E0 ldi r21,lo8(-64000) 5639 3538 E516 cp r14,r21 5640 353a 56E0 ldi r21,hi8(-64000) 5641 353c F506 cpc r15,r21 5642 353e 5FEF ldi r21,hlo8(-64000) 5643 3540 0507 cpc r16,r21 5644 3542 5FEF ldi r21,hhi8(-64000) 5645 3544 1507 cpc r17,r21 5646 3546 04F0 brlt .+2 5647 3548 00C0 rjmp .L687 5648 354a 80E0 ldi r24,lo8(-64000) 5649 354c 96E0 ldi r25,hi8(-64000) 5650 354e AFEF ldi r26,hlo8(-64000) 5651 3550 BFEF ldi r27,hhi8(-64000) 5652 3552 8093 0000 sts IPartNick.2408,r24 5653 3556 9093 0000 sts (IPartNick.2408)+1,r25 5654 355a A093 0000 sts (IPartNick.2408)+2,r26 5655 355e B093 0000 sts (IPartNick.2408)+3,r27 5656 3562 E12C mov r14,__zero_reg__ 5657 3564 36E0 ldi r19,hi8(-64000) 5658 3566 F32E mov r15,r19 5659 3568 3FEF ldi r19,hlo8(-64000) 5660 356a 032F mov r16,r19 5661 356c 3FEF ldi r19,hhi8(-64000) 5662 356e 132F mov r17,r19 5663 .L688: 5664 3570 4090 0000 lds r4,Ki 5665 3574 5090 0000 lds r5,(Ki)+1 5666 3578 9201 movw r18,r4 5667 357a 4427 clr r20 5668 357c 37FD sbrc r19,7 5669 357e 4095 com r20 5670 3580 542F mov r21,r20 5671 3582 C801 movw r24,r16 5672 3584 B701 movw r22,r14 5673 3586 0E94 0000 call __divmodsi4 5674 358a 4601 movw r8,r12 5675 358c 820E add r8,r18 5676 358e 931E adc r9,r19 5677 3590 8091 0000 lds r24,StickRoll 5678 3594 9091 0000 lds r25,(StickRoll)+1 5679 3598 99AF std Y+57,r25 5680 359a 88AF std Y+56,r24 5681 359c AE8D ldd r26,Y+30 5682 359e BF8D ldd r27,Y+31 5683 35a0 7D01 movw r14,r26 5684 35a2 0027 clr r16 5685 35a4 F7FC sbrc r15,7 5686 35a6 0095 com r16 5687 35a8 102F mov r17,r16 5688 35aa EE0C lsl r14 5689 35ac FF1C rol r15 5690 35ae 001F rol r16 5691 35b0 111F rol r17 5692 35b2 EE0C lsl r14 5693 35b4 FF1C rol r15 5694 35b6 001F rol r16 5695 35b8 111F rol r17 5696 35ba EE0C lsl r14 5697 35bc FF1C rol r15 5698 35be 001F rol r16 5699 35c0 111F rol r17 5700 35c2 EE0C lsl r14 5701 35c4 FF1C rol r15 5702 35c6 001F rol r16 5703 35c8 111F rol r17 5704 35ca EE0C lsl r14 5705 35cc FF1C rol r15 5706 35ce 001F rol r16 5707 35d0 111F rol r17 5708 35d2 EE0C lsl r14 5709 35d4 FF1C rol r15 5710 35d6 001F rol r16 5711 35d8 111F rol r17 5712 35da EE0C lsl r14 5713 35dc FF1C rol r15 5714 35de 001F rol r16 5715 35e0 111F rol r17 5716 35e2 ED85 ldd r30,Y+13 5717 35e4 FE85 ldd r31,Y+14 5718 35e6 BF01 movw r22,r30 5719 35e8 8827 clr r24 5720 35ea 77FD sbrc r23,7 5721 35ec 8095 com r24 5722 35ee 982F mov r25,r24 5723 35f0 2F85 ldd r18,Y+15 5724 35f2 3889 ldd r19,Y+16 5725 35f4 4989 ldd r20,Y+17 5726 35f6 5A89 ldd r21,Y+18 5727 35f8 0E94 0000 call __mulsi3 5728 35fc E60E add r14,r22 5729 35fe F71E adc r15,r23 5730 3600 081F adc r16,r24 5731 3602 191F adc r17,r25 5732 3604 C801 movw r24,r16 5733 3606 B701 movw r22,r14 5734 3608 20E4 ldi r18,lo8(64) 5735 360a 30E0 ldi r19,hi8(64) 5736 360c 40E0 ldi r20,hlo8(64) 5737 360e 50E0 ldi r21,hhi8(64) 5738 3610 0E94 0000 call __divmodsi4 5739 3614 CD80 ldd r12,Y+5 5740 3616 DE80 ldd r13,Y+6 5741 3618 2C0D add r18,r12 5742 361a 3D1D adc r19,r13 5743 361c 40E4 ldi r20,hi8(16385) 5744 361e 2130 cpi r18,lo8(16385) 5745 3620 3407 cpc r19,r20 5746 3622 04F0 brlt .L689 5747 3624 20E0 ldi r18,lo8(16384) 5748 3626 30E4 ldi r19,hi8(16384) 5749 .L689: 5750 3628 50EC ldi r21,hi8(-16384) 5751 362a 2030 cpi r18,lo8(-16384) 5752 362c 3507 cpc r19,r21 5753 362e 04F4 brge .L690 5754 3630 20E0 ldi r18,lo8(-16384) 5755 3632 30EC ldi r19,hi8(-16384) 5756 .L690: 5757 3634 6901 movw r12,r18 5758 3636 68AD ldd r22,Y+56 5759 3638 79AD ldd r23,Y+57 5760 363a C61A sub r12,r22 5761 363c D70A sbc r13,r23 5762 363e 3320 tst r3 5763 3640 01F4 brne .+2 5764 3642 00C0 rjmp .L691 5765 3644 8F81 ldd r24,Y+7 5766 3646 9885 ldd r25,Y+8 5767 3648 861B sub r24,r22 5768 364a 970B sbc r25,r23 5769 364c FC01 movw r30,r24 5770 364e CF01 movw r24,r30 5771 3650 AA27 clr r26 5772 3652 97FD sbrc r25,7 5773 3654 A095 com r26 5774 3656 BA2F mov r27,r26 5775 3658 E090 0000 lds r14,IPartRoll.2409 5776 365c F090 0000 lds r15,(IPartRoll.2409)+1 5777 3660 0091 0000 lds r16,(IPartRoll.2409)+2 5778 3664 1091 0000 lds r17,(IPartRoll.2409)+3 5779 3668 E80E add r14,r24 5780 366a F91E adc r15,r25 5781 366c 0A1F adc r16,r26 5782 366e 1B1F adc r17,r27 5783 3670 E092 0000 sts IPartRoll.2409,r14 5784 3674 F092 0000 sts (IPartRoll.2409)+1,r15 5785 3678 0093 0000 sts (IPartRoll.2409)+2,r16 5786 367c 1093 0000 sts (IPartRoll.2409)+3,r17 5787 .L692: 5788 3680 F0E0 ldi r31,lo8(-64000) 5789 3682 EF16 cp r14,r31 5790 3684 F6E0 ldi r31,hi8(-64000) 5791 3686 FF06 cpc r15,r31 5792 3688 FFEF ldi r31,hlo8(-64000) 5793 368a 0F07 cpc r16,r31 5794 368c FFEF ldi r31,hhi8(-64000) 5795 368e 1F07 cpc r17,r31 5796 3690 04F0 brlt .+2 5797 3692 00C0 rjmp .L693 5798 3694 80E0 ldi r24,lo8(-64000) 5799 3696 96E0 ldi r25,hi8(-64000) 5800 3698 AFEF ldi r26,hlo8(-64000) 5801 369a BFEF ldi r27,hhi8(-64000) 5802 369c 8093 0000 sts IPartRoll.2409,r24 5803 36a0 9093 0000 sts (IPartRoll.2409)+1,r25 5804 36a4 A093 0000 sts (IPartRoll.2409)+2,r26 5805 36a8 B093 0000 sts (IPartRoll.2409)+3,r27 5806 36ac E12C mov r14,__zero_reg__ 5807 36ae F6E0 ldi r31,hi8(-64000) 5808 36b0 FF2E mov r15,r31 5809 36b2 FFEF ldi r31,hlo8(-64000) 5810 36b4 0F2F mov r16,r31 5811 36b6 FFEF ldi r31,hhi8(-64000) 5812 36b8 1F2F mov r17,r31 5813 .L694: 5814 36ba 9201 movw r18,r4 5815 36bc 4427 clr r20 5816 36be 37FD sbrc r19,7 5817 36c0 4095 com r20 5818 36c2 542F mov r21,r20 5819 36c4 C801 movw r24,r16 5820 36c6 B701 movw r22,r14 5821 36c8 0E94 0000 call __divmodsi4 5822 36cc 8601 movw r16,r12 5823 36ce 020F add r16,r18 5824 36d0 131F adc r17,r19 5825 36d2 9301 movw r18,r6 5826 36d4 77FC sbrc r7,7 5827 36d6 00C0 rjmp .L783 5828 .L695: 5829 36d8 3595 asr r19 5830 36da 2795 ror r18 5831 36dc 2A0D add r18,r10 5832 36de 3B1D adc r19,r11 5833 36e0 B901 movw r22,r18 5834 36e2 8827 clr r24 5835 36e4 77FD sbrc r23,7 5836 36e6 8095 com r24 5837 36e8 982F mov r25,r24 5838 36ea 2091 0000 lds r18,FCParam+27 5839 36ee 30E0 ldi r19,lo8(0) 5840 36f0 40E0 ldi r20,lo8(0) 5841 36f2 50E0 ldi r21,hi8(0) 5842 36f4 0E94 0000 call __mulsi3 5843 36f8 20E4 ldi r18,lo8(64) 5844 36fa 30E0 ldi r19,hi8(64) 5845 36fc 40E0 ldi r20,hlo8(64) 5846 36fe 50E0 ldi r21,hhi8(64) 5847 3700 0E94 0000 call __divmodsi4 5848 3704 B901 movw r22,r18 5849 3706 8827 clr r24 5850 3708 9927 clr r25 5851 370a 821B sub r24,r18 5852 370c 930B sbc r25,r19 5853 370e 8816 cp r8,r24 5854 3710 9906 cpc r9,r25 5855 3712 04F0 brlt .+2 5856 3714 00C0 rjmp .L696 5857 3716 9C01 movw r18,r24 5858 .L697: 5859 3718 0817 cp r16,r24 5860 371a 1907 cpc r17,r25 5861 371c 04F0 brlt .L700 5862 371e CB01 movw r24,r22 5863 3720 0617 cp r16,r22 5864 3722 1707 cpc r17,r23 5865 3724 04F4 brge .+2 5866 3726 00C0 rjmp .L784 5867 .L700: 5868 3728 60E0 ldi r22,lo8(Mixer+13) 5869 372a 262E mov r2,r22 5870 372c 60E0 ldi r22,hi8(Mixer+13) 5871 372e 362E mov r3,r22 5872 3730 50E0 ldi r21,lo8(Motor) 5873 3732 852E mov r8,r21 5874 3734 50E0 ldi r21,hi8(Motor) 5875 3736 952E mov r9,r21 5876 3738 40E0 ldi r20,lo8(MotorValue.2421) 5877 373a C42E mov r12,r20 5878 373c 40E0 ldi r20,hi8(MotorValue.2421) 5879 373e D42E mov r13,r20 5880 3740 A301 movw r20,r6 5881 3742 6627 clr r22 5882 3744 57FD sbrc r21,7 5883 3746 6095 com r22 5884 3748 762F mov r23,r22 5885 374a 4CA7 std Y+44,r20 5886 374c 5DA7 std Y+45,r21 5887 374e 6EA7 std Y+46,r22 5888 3750 7FA7 std Y+47,r23 5889 3752 A501 movw r20,r10 5890 3754 6627 clr r22 5891 3756 57FD sbrc r21,7 5892 3758 6095 com r22 5893 375a 762F mov r23,r22 5894 375c 48AB std Y+48,r20 5895 375e 59AB std Y+49,r21 5896 3760 6AAB std Y+50,r22 5897 3762 7BAB std Y+51,r23 5898 3764 A901 movw r20,r18 5899 3766 6627 clr r22 5900 3768 57FD sbrc r21,7 5901 376a 6095 com r22 5902 376c 762F mov r23,r22 5903 376e 4CAB std Y+52,r20 5904 3770 5DAB std Y+53,r21 5905 3772 6EAB std Y+54,r22 5906 3774 7FAB std Y+55,r23 5907 3776 2C01 movw r4,r24 5908 3778 6624 clr r6 5909 377a 57FC sbrc r5,7 5910 377c 6094 com r6 5911 377e 762C mov r7,r6 5912 3780 00C0 rjmp .L707 5913 .L787: 5914 3782 1396 adiw r26,3 5915 3784 2C91 ld r18,X 5916 3786 3327 clr r19 5917 3788 27FD sbrc r18,7 5918 378a 3095 com r19 5919 378c 432F mov r20,r19 5920 378e 532F mov r21,r19 5921 3790 6CA5 ldd r22,Y+44 5922 3792 7DA5 ldd r23,Y+45 5923 3794 8EA5 ldd r24,Y+46 5924 3796 9FA5 ldd r25,Y+47 5925 3798 0E94 0000 call __mulsi3 5926 379c 20E4 ldi r18,lo8(64) 5927 379e 30E0 ldi r19,hi8(64) 5928 37a0 40E0 ldi r20,hlo8(64) 5929 37a2 50E0 ldi r21,hhi8(64) 5930 37a4 0E94 0000 call __divmodsi4 5931 37a8 7901 movw r14,r18 5932 37aa 8A01 movw r16,r20 5933 37ac 2A2D mov r18,r10 5934 37ae 3327 clr r19 5935 37b0 27FD sbrc r18,7 5936 37b2 3095 com r19 5937 37b4 432F mov r20,r19 5938 37b6 532F mov r21,r19 5939 37b8 68A9 ldd r22,Y+48 5940 37ba 79A9 ldd r23,Y+49 5941 37bc 8AA9 ldd r24,Y+50 5942 37be 9BA9 ldd r25,Y+51 5943 37c0 0E94 0000 call __mulsi3 5944 37c4 20E4 ldi r18,lo8(64) 5945 37c6 30E0 ldi r19,hi8(64) 5946 37c8 40E0 ldi r20,hlo8(64) 5947 37ca 50E0 ldi r21,hhi8(64) 5948 37cc 0E94 0000 call __divmodsi4 5949 37d0 E20E add r14,r18 5950 37d2 F31E adc r15,r19 5951 37d4 F101 movw r30,r2 5952 37d6 2181 ldd r18,Z+1 5953 37d8 3327 clr r19 5954 37da 27FD sbrc r18,7 5955 37dc 3095 com r19 5956 37de 432F mov r20,r19 5957 37e0 532F mov r21,r19 5958 37e2 6CA9 ldd r22,Y+52 5959 37e4 7DA9 ldd r23,Y+53 5960 37e6 8EA9 ldd r24,Y+54 5961 37e8 9FA9 ldd r25,Y+55 5962 37ea 0E94 0000 call __mulsi3 5963 37ee 20E4 ldi r18,lo8(64) 5964 37f0 30E0 ldi r19,hi8(64) 5965 37f2 40E0 ldi r20,hlo8(64) 5966 37f4 50E0 ldi r21,hhi8(64) 5967 37f6 0E94 0000 call __divmodsi4 5968 37fa E20E add r14,r18 5969 37fc F31E adc r15,r19 5970 37fe D101 movw r26,r2 5971 3800 1296 adiw r26,2 5972 3802 2C91 ld r18,X 5973 3804 3327 clr r19 5974 3806 27FD sbrc r18,7 5975 3808 3095 com r19 5976 380a 432F mov r20,r19 5977 380c 532F mov r21,r19 5978 380e C301 movw r24,r6 5979 3810 B201 movw r22,r4 5980 3812 0E94 0000 call __mulsi3 5981 3816 20E4 ldi r18,lo8(64) 5982 3818 30E0 ldi r19,hi8(64) 5983 381a 40E0 ldi r20,hlo8(64) 5984 381c 50E0 ldi r21,hhi8(64) 5985 381e 0E94 0000 call __divmodsi4 5986 3822 E20E add r14,r18 5987 3824 F31E adc r15,r19 5988 3826 F601 movw r30,r12 5989 3828 6081 ld r22,Z 5990 382a 7181 ldd r23,Z+1 5991 382c C701 movw r24,r14 5992 382e 0E94 0000 call MotorSmoothing 5993 3832 D601 movw r26,r12 5994 3834 8C93 st X,r24 5995 3836 1196 adiw r26,1 5996 3838 9C93 st X,r25 5997 383a 97FD sbrc r25,7 5998 383c 00C0 rjmp .L785 5999 .L703: 6000 383e AC01 movw r20,r24 6001 3840 5595 asr r21 6002 3842 4795 ror r20 6003 3844 5595 asr r21 6004 3846 4795 ror r20 6005 3848 8091 0000 lds r24,ParamSet+21 6006 384c 282F mov r18,r24 6007 384e 30E0 ldi r19,lo8(0) 6008 3850 4217 cp r20,r18 6009 3852 5307 cpc r21,r19 6010 3854 04F0 brlt .L704 6011 3856 8091 0000 lds r24,ParamSet+22 6012 385a 90E0 ldi r25,lo8(0) 6013 385c 9C01 movw r18,r24 6014 385e 4217 cp r20,r18 6015 3860 5307 cpc r21,r19 6016 3862 04F4 brge .L704 6017 3864 9A01 movw r18,r20 6018 .L704: 6019 3866 F401 movw r30,r8 6020 3868 2083 st Z,r18 6021 .L706: 6022 386a E4E0 ldi r30,lo8(4) 6023 386c F0E0 ldi r31,hi8(4) 6024 386e 2E0E add r2,r30 6025 3870 3F1E adc r3,r31 6026 3872 95E0 ldi r25,lo8(5) 6027 3874 A92E mov r10,r25 6028 3876 B12C mov r11,__zero_reg__ 6029 3878 8A0C add r8,r10 6030 387a 9B1C adc r9,r11 6031 387c 22E0 ldi r18,lo8(2) 6032 387e 30E0 ldi r19,hi8(2) 6033 3880 C20E add r12,r18 6034 3882 D31E adc r13,r19 6035 3884 30E0 ldi r19,lo8(Mixer+61) 6036 3886 2316 cp r2,r19 6037 3888 30E0 ldi r19,hi8(Mixer+61) 6038 388a 3306 cpc r3,r19 6039 388c 01F0 breq .L786 6040 .L707: 6041 388e D101 movw r26,r2 6042 3890 AC90 ld r10,X 6043 3892 1A14 cp __zero_reg__,r10 6044 3894 04F4 brge .+2 6045 3896 00C0 rjmp .L787 6046 3898 D401 movw r26,r8 6047 389a 1C92 st X,__zero_reg__ 6048 389c 00C0 rjmp .L706 6049 .L785: 6050 389e 0396 adiw r24,3 6051 38a0 00C0 rjmp .L703 6052 .L786: 6053 /* epilogue start */ 6054 38a2 E996 adiw r28,57 6055 38a4 0FB6 in __tmp_reg__,__SREG__ 6056 38a6 F894 cli 6057 38a8 DEBF out __SP_H__,r29 6058 38aa 0FBE out __SREG__,__tmp_reg__ 6059 38ac CDBF out __SP_L__,r28 6060 38ae CF91 pop r28 6061 38b0 DF91 pop r29 6062 38b2 1F91 pop r17 6063 38b4 0F91 pop r16 6064 38b6 FF90 pop r15 6065 38b8 EF90 pop r14 6066 38ba DF90 pop r13 6067 38bc CF90 pop r12 6068 38be BF90 pop r11 6069 38c0 AF90 pop r10 6070 38c2 9F90 pop r9 6071 38c4 8F90 pop r8 6072 38c6 7F90 pop r7 6073 38c8 6F90 pop r6 6074 38ca 5F90 pop r5 6075 38cc 4F90 pop r4 6076 38ce 3F90 pop r3 6077 38d0 2F90 pop r2 6078 38d2 0895 ret 6079 .L696: 6080 38d4 8216 cp r8,r18 6081 38d6 9306 cpc r9,r19 6082 38d8 04F0 brlt .+2 6083 38da 00C0 rjmp .L697 6084 38dc 9401 movw r18,r8 6085 38de 00C0 rjmp .L697 6086 .L693: 6087 38e0 21E0 ldi r18,lo8(64001) 6088 38e2 E216 cp r14,r18 6089 38e4 2AEF ldi r18,hi8(64001) 6090 38e6 F206 cpc r15,r18 6091 38e8 20E0 ldi r18,hlo8(64001) 6092 38ea 0207 cpc r16,r18 6093 38ec 20E0 ldi r18,hhi8(64001) 6094 38ee 1207 cpc r17,r18 6095 38f0 04F4 brge .+2 6096 38f2 00C0 rjmp .L694 6097 38f4 80E0 ldi r24,lo8(64000) 6098 38f6 9AEF ldi r25,hi8(64000) 6099 38f8 A0E0 ldi r26,hlo8(64000) 6100 38fa B0E0 ldi r27,hhi8(64000) 6101 38fc 8093 0000 sts IPartRoll.2409,r24 6102 3900 9093 0000 sts (IPartRoll.2409)+1,r25 6103 3904 A093 0000 sts (IPartRoll.2409)+2,r26 6104 3908 B093 0000 sts (IPartRoll.2409)+3,r27 6105 390c E12C mov r14,__zero_reg__ 6106 390e EAEF ldi r30,hi8(64000) 6107 3910 FE2E mov r15,r30 6108 3912 012D mov r16,__zero_reg__ 6109 3914 112D mov r17,__zero_reg__ 6110 3916 00C0 rjmp .L694 6111 .L691: 6112 3918 C601 movw r24,r12 6113 391a AA27 clr r26 6114 391c 97FD sbrc r25,7 6115 391e A095 com r26 6116 3920 BA2F mov r27,r26 6117 3922 E090 0000 lds r14,IPartRoll.2409 6118 3926 F090 0000 lds r15,(IPartRoll.2409)+1 6119 392a 0091 0000 lds r16,(IPartRoll.2409)+2 6120 392e 1091 0000 lds r17,(IPartRoll.2409)+3 6121 3932 E80E add r14,r24 6122 3934 F91E adc r15,r25 6123 3936 0A1F adc r16,r26 6124 3938 1B1F adc r17,r27 6125 393a E092 0000 sts IPartRoll.2409,r14 6126 393e F092 0000 sts (IPartRoll.2409)+1,r15 6127 3942 0093 0000 sts (IPartRoll.2409)+2,r16 6128 3946 1093 0000 sts (IPartRoll.2409)+3,r17 6129 394a 00C0 rjmp .L692 6130 .L687: 6131 394c 61E0 ldi r22,lo8(64001) 6132 394e E616 cp r14,r22 6133 3950 6AEF ldi r22,hi8(64001) 6134 3952 F606 cpc r15,r22 6135 3954 60E0 ldi r22,hlo8(64001) 6136 3956 0607 cpc r16,r22 6137 3958 60E0 ldi r22,hhi8(64001) 6138 395a 1607 cpc r17,r22 6139 395c 04F4 brge .+2 6140 395e 00C0 rjmp .L688 6141 3960 80E0 ldi r24,lo8(64000) 6142 3962 9AEF ldi r25,hi8(64000) 6143 3964 A0E0 ldi r26,hlo8(64000) 6144 3966 B0E0 ldi r27,hhi8(64000) 6145 3968 8093 0000 sts IPartNick.2408,r24 6146 396c 9093 0000 sts (IPartNick.2408)+1,r25 6147 3970 A093 0000 sts (IPartNick.2408)+2,r26 6148 3974 B093 0000 sts (IPartNick.2408)+3,r27 6149 3978 E12C mov r14,__zero_reg__ 6150 397a 2AEF ldi r18,hi8(64000) 6151 397c F22E mov r15,r18 6152 397e 012D mov r16,__zero_reg__ 6153 3980 112D mov r17,__zero_reg__ 6154 3982 00C0 rjmp .L688 6155 .L685: 6156 3984 C601 movw r24,r12 6157 3986 AA27 clr r26 6158 3988 97FD sbrc r25,7 6159 398a A095 com r26 6160 398c BA2F mov r27,r26 6161 398e E090 0000 lds r14,IPartNick.2408 6162 3992 F090 0000 lds r15,(IPartNick.2408)+1 6163 3996 0091 0000 lds r16,(IPartNick.2408)+2 6164 399a 1091 0000 lds r17,(IPartNick.2408)+3 6165 399e E80E add r14,r24 6166 39a0 F91E adc r15,r25 6167 39a2 0A1F adc r16,r26 6168 39a4 1B1F adc r17,r27 6169 39a6 E092 0000 sts IPartNick.2408,r14 6170 39aa F092 0000 sts (IPartNick.2408)+1,r15 6171 39ae 0093 0000 sts (IPartNick.2408)+2,r16 6172 39b2 1093 0000 sts (IPartNick.2408)+3,r17 6173 39b6 00C0 rjmp .L686 6174 .L675: 6175 39b8 C701 movw r24,r14 6176 39ba 8135 cpi r24,81 6177 39bc 9105 cpc r25,__zero_reg__ 6178 39be 04F0 brlt .+2 6179 39c0 00C0 rjmp .L788 6180 39c2 9C01 movw r18,r24 6181 39c4 5FEF ldi r21,hi8(-80) 6182 39c6 803B cpi r24,lo8(-80) 6183 39c8 9507 cpc r25,r21 6184 39ca 04F0 brlt .+2 6185 39cc 00C0 rjmp .L677 6186 .L790: 6187 39ce 20EB ldi r18,lo8(-80) 6188 39d0 3FEF ldi r19,hi8(-80) 6189 39d2 00C0 rjmp .L677 6190 .L601: 6191 39d4 6091 0000 lds r22,GyroIFactor 6192 39d8 70E0 ldi r23,lo8(0) 6193 39da 80E0 ldi r24,lo8(0) 6194 39dc 90E0 ldi r25,hi8(0) 6195 39de 2091 0000 lds r18,IntegralGyroRoll 6196 39e2 3091 0000 lds r19,(IntegralGyroRoll)+1 6197 39e6 4091 0000 lds r20,(IntegralGyroRoll)+2 6198 39ea 5091 0000 lds r21,(IntegralGyroRoll)+3 6199 39ee 0E94 0000 call __mulsi3 6200 39f2 28EF ldi r18,lo8(11000) 6201 39f4 3AE2 ldi r19,hi8(11000) 6202 39f6 40E0 ldi r20,hlo8(11000) 6203 39f8 50E0 ldi r21,hhi8(11000) 6204 39fa 0E94 0000 call __divmodsi4 6205 39fe 3887 std Y+8,r19 6206 3a00 2F83 std Y+7,r18 6207 3a02 3E83 std Y+6,r19 6208 3a04 2D83 std Y+5,r18 6209 3a06 00C0 rjmp .L602 6210 .L599: 6211 3a08 6091 0000 lds r22,GyroIFactor 6212 3a0c 70E0 ldi r23,lo8(0) 6213 3a0e 80E0 ldi r24,lo8(0) 6214 3a10 90E0 ldi r25,hi8(0) 6215 3a12 2091 0000 lds r18,IntegralGyroNick 6216 3a16 3091 0000 lds r19,(IntegralGyroNick)+1 6217 3a1a 4091 0000 lds r20,(IntegralGyroNick)+2 6218 3a1e 5091 0000 lds r21,(IntegralGyroNick)+3 6219 3a22 0E94 0000 call __mulsi3 6220 3a26 28EF ldi r18,lo8(11000) 6221 3a28 3AE2 ldi r19,hi8(11000) 6222 3a2a 40E0 ldi r20,hlo8(11000) 6223 3a2c 50E0 ldi r21,hhi8(11000) 6224 3a2e 0E94 0000 call __divmodsi4 6225 3a32 2901 movw r4,r18 6226 3a34 3C83 std Y+4,r19 6227 3a36 2B83 std Y+3,r18 6228 3a38 00C0 rjmp .L600 6229 .L596: 6230 3a3a EE8D ldd r30,Y+30 6231 3a3c FF8D ldd r31,Y+31 6232 3a3e E93C cpi r30,201 6233 3a40 F105 cpc r31,__zero_reg__ 6234 3a42 04F4 brge .+2 6235 3a44 00C0 rjmp .L597 6236 3a46 88EC ldi r24,lo8(200) 6237 3a48 90E0 ldi r25,hi8(200) 6238 3a4a 9093 0000 sts (TrimRoll)+1,r25 6239 3a4e 8093 0000 sts TrimRoll,r24 6240 3a52 9F8F std Y+31,r25 6241 3a54 8E8F std Y+30,r24 6242 3a56 00C0 rjmp .L597 6243 .L594: 6244 3a58 69EC ldi r22,lo8(201) 6245 3a5a 8616 cp r8,r22 6246 3a5c 9104 cpc r9,__zero_reg__ 6247 3a5e 04F4 brge .+2 6248 3a60 00C0 rjmp .L595 6249 3a62 88EC ldi r24,lo8(200) 6250 3a64 90E0 ldi r25,hi8(200) 6251 3a66 9093 0000 sts (TrimNick)+1,r25 6252 3a6a 8093 0000 sts TrimNick,r24 6253 3a6e E8EC ldi r30,lo8(200) 6254 3a70 8E2E mov r8,r30 6255 3a72 912C mov r9,__zero_reg__ 6256 3a74 00C0 rjmp .L595 6257 .L573: 6258 3a76 2155 subi r18,lo8(50001) 6259 3a78 334C sbci r19,hi8(50001) 6260 3a7a 4040 sbci r20,hlo8(50001) 6261 3a7c 5040 sbci r21,hhi8(50001) 6262 3a7e 04F4 brge .+4 6263 3a80 0C94 0000 jmp .L574 6264 3a84 80E5 ldi r24,lo8(50000) 6265 3a86 93EC ldi r25,hi8(50000) 6266 3a88 A0E0 ldi r26,hlo8(50000) 6267 3a8a B0E0 ldi r27,hhi8(50000) 6268 3a8c 8093 0000 sts ReadingIntegralGyroYaw,r24 6269 3a90 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25 6270 3a94 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26 6271 3a98 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27 6272 3a9c 0C94 0000 jmp .L574 6273 .L740: 6274 3aa0 9B01 movw r18,r22 6275 3aa2 77FD sbrc r23,7 6276 3aa4 00C0 rjmp .L789 6277 .L484: 6278 3aa6 C901 movw r24,r18 6279 3aa8 9595 asr r25 6280 3aaa 8795 ror r24 6281 3aac 9595 asr r25 6282 3aae 8795 ror r24 6283 3ab0 9093 0000 sts (MaxStickNick)+1,r25 6284 3ab4 8093 0000 sts MaxStickNick,r24 6285 3ab8 8536 cpi r24,101 6286 3aba 9105 cpc r25,__zero_reg__ 6287 3abc 04F4 brge .+4 6288 3abe 0C94 0000 jmp .L485 6289 3ac2 84E6 ldi r24,lo8(100) 6290 3ac4 90E0 ldi r25,hi8(100) 6291 3ac6 9093 0000 sts (MaxStickNick)+1,r25 6292 3aca 8093 0000 sts MaxStickNick,r24 6293 3ace 0C94 0000 jmp .L485 6294 .L737: 6295 3ad2 1092 0000 sts GyroIFactor,__zero_reg__ 6296 3ad6 0C94 0000 jmp .L480 6297 .L778: 6298 3ada 5C01 movw r10,r24 6299 3adc 00C0 rjmp .L669 6300 .L788: 6301 3ade 80E5 ldi r24,lo8(80) 6302 3ae0 90E0 ldi r25,hi8(80) 6303 3ae2 9C01 movw r18,r24 6304 3ae4 5FEF ldi r21,hi8(-80) 6305 3ae6 803B cpi r24,lo8(-80) 6306 3ae8 9507 cpc r25,r21 6307 3aea 04F0 brlt .+2 6308 3aec 00C0 rjmp .L677 6309 3aee 00C0 rjmp .L790 6310 .L784: 6311 3af0 C801 movw r24,r16 6312 3af2 00C0 rjmp .L700 6313 .L782: 6314 3af4 3C01 movw r6,r24 6315 3af6 00C0 rjmp .L681 6316 .L781: 6317 3af8 9501 movw r18,r10 6318 3afa 3595 asr r19 6319 3afc 2795 ror r18 6320 3afe E216 cp r14,r18 6321 3b00 F306 cpc r15,r19 6322 3b02 04F0 brlt .+2 6323 3b04 00C0 rjmp .L677 6324 3b06 9701 movw r18,r14 6325 3b08 00C0 rjmp .L677 6326 .L779: 6327 3b0a 1A14 cp __zero_reg__,r10 6328 3b0c 1B04 cpc __zero_reg__,r11 6329 3b0e 04F0 brlt .+2 6330 3b10 00C0 rjmp .L672 6331 3b12 81E0 ldi r24,lo8(1) 6332 3b14 90E0 ldi r25,hi8(1) 6333 3b16 9093 0000 sts (ModelIsFlying)+1,r25 6334 3b1a 8093 0000 sts ModelIsFlying,r24 6335 3b1e 8091 0000 lds r24,ParamSet+21 6336 3b22 282F mov r18,r24 6337 3b24 30E0 ldi r19,lo8(0) 6338 3b26 00C0 rjmp .L672 6339 .L756: 6340 3b28 81E0 ldi r24,lo8(1) 6341 3b2a 90E0 ldi r25,hi8(1) 6342 3b2c 9093 0000 sts (UpdateCompassCourse.2420)+1,r25 6343 3b30 8093 0000 sts UpdateCompassCourse.2420,r24 6344 3b34 0C94 0000 jmp .L569 6345 .L777: 6346 3b38 9D8D ldd r25,Y+29 6347 3b3a 9923 tst r25 6348 3b3c 01F0 breq .+2 6349 3b3e 00C0 rjmp .L604 6350 3b40 8091 0000 lds r24,HoverGas.2445 6351 3b44 9091 0000 lds r25,(HoverGas.2445)+1 6352 3b48 9093 0000 sts (DebugOut+44)+1,r25 6353 3b4c 8093 0000 sts DebugOut+44,r24 6354 3b50 8091 0000 lds r24,ReadingVario 6355 3b54 9091 0000 lds r25,(ReadingVario)+1 6356 3b58 9093 0000 sts (DebugOut+38)+1,r25 6357 3b5c 8093 0000 sts DebugOut+38,r24 6358 3b60 2091 0000 lds r18,BaroExpandActive.2451 6359 3b64 3091 0000 lds r19,(BaroExpandActive.2451)+1 6360 3b68 2115 cp r18,__zero_reg__ 6361 3b6a 3105 cpc r19,__zero_reg__ 6362 3b6c 01F4 brne .+4 6363 3b6e 0C94 0000 jmp .L605 6364 3b72 8091 0000 lds r24,ReadingHeight 6365 3b76 9091 0000 lds r25,(ReadingHeight)+1 6366 3b7a A091 0000 lds r26,(ReadingHeight)+2 6367 3b7e B091 0000 lds r27,(ReadingHeight)+3 6368 3b82 880F lsl r24 6369 3b84 991F rol r25 6370 3b86 AA1F rol r26 6371 3b88 BB1F rol r27 6372 3b8a 880F lsl r24 6373 3b8c 991F rol r25 6374 3b8e AA1F rol r26 6375 3b90 BB1F rol r27 6376 3b92 880F lsl r24 6377 3b94 991F rol r25 6378 3b96 AA1F rol r26 6379 3b98 BB1F rol r27 6380 3b9a 880F lsl r24 6381 3b9c 991F rol r25 6382 3b9e AA1F rol r26 6383 3ba0 BB1F rol r27 6384 3ba2 8093 0000 sts SumHeight,r24 6385 3ba6 9093 0000 sts (SumHeight)+1,r25 6386 3baa A093 0000 sts (SumHeight)+2,r26 6387 3bae B093 0000 sts (SumHeight)+3,r27 6388 3bb2 1092 0000 sts (ReadingVario)+1,__zero_reg__ 6389 3bb6 1092 0000 sts ReadingVario,__zero_reg__ 6390 3bba 2150 subi r18,lo8(-(-1)) 6391 3bbc 3040 sbci r19,hi8(-(-1)) 6392 3bbe 3093 0000 sts (BaroExpandActive.2451)+1,r19 6393 3bc2 2093 0000 sts BaroExpandActive.2451,r18 6394 .L606: 6395 3bc6 8091 0000 lds r24,ParamSet+8 6396 3bca 81FF sbrs r24,1 6397 3bcc 00C0 rjmp .L611 6398 3bce 8091 0000 lds r24,FCParam+1 6399 3bd2 8233 cpi r24,lo8(50) 6400 3bd4 00F0 brlo .+4 6401 3bd6 0C94 0000 jmp .L612 6402 3bda 8091 0000 lds r24,delay.2449 6403 3bde 8150 subi r24,lo8(-(-1)) 6404 3be0 8093 0000 sts delay.2449,r24 6405 3be4 8F5F subi r24,lo8(-(1)) 6406 3be6 01F0 breq .+2 6407 3be8 00C0 rjmp .L613 6408 3bea 8091 0000 lds r24,ReadingHeight 6409 3bee 9091 0000 lds r25,(ReadingHeight)+1 6410 3bf2 A091 0000 lds r26,(ReadingHeight)+2 6411 3bf6 B091 0000 lds r27,(ReadingHeight)+3 6412 3bfa 8093 0000 sts SetPointHeight,r24 6413 3bfe 9093 0000 sts (SetPointHeight)+1,r25 6414 3c02 A093 0000 sts (SetPointHeight)+2,r26 6415 3c06 B093 0000 sts (SetPointHeight)+3,r27 6416 3c0a 1092 0000 sts HCActive.2443,__zero_reg__ 6417 3c0e 81E0 ldi r24,lo8(1) 6418 3c10 8093 0000 sts delay.2449,r24 6419 3c14 00C0 rjmp .L613 6420 .L732: 6421 3c16 0197 sbiw r24,1 6422 3c18 9093 0000 sts (RcLostTimer.2415)+1,r25 6423 3c1c 8093 0000 sts RcLostTimer.2415,r24 6424 3c20 AAE0 ldi r26,lo8(10) 6425 3c22 BA2E mov r11,r26 6426 3c24 DB14 cp r13,r11 6427 3c26 01F0 breq .+4 6428 3c28 0C94 0000 jmp .L791 6429 .L406: 6430 3c2c 289A sbi 37-32,0 6431 3c2e 0C94 0000 jmp .L408 6432 .L754: 6433 3c32 8091 0000 lds r24,LoopingRoll 6434 3c36 8823 tst r24 6435 3c38 01F0 breq .+4 6436 3c3a 0C94 0000 jmp .L530 6437 3c3e 8091 0000 lds r24,FunnelCourse 6438 3c42 8823 tst r24 6439 3c44 01F0 breq .+4 6440 3c46 0C94 0000 jmp .L530 6441 3c4a 8091 0000 lds r24,ParamSet+57 6442 3c4e 8823 tst r24 6443 3c50 01F4 brne .+4 6444 3c52 0C94 0000 jmp .L530 6445 3c56 8091 0000 lds r24,MeanIntegralGyroNick 6446 3c5a 9091 0000 lds r25,(MeanIntegralGyroNick)+1 6447 3c5e A091 0000 lds r26,(MeanIntegralGyroNick)+2 6448 3c62 B091 0000 lds r27,(MeanIntegralGyroNick)+3 6449 3c66 BC01 movw r22,r24 6450 3c68 CD01 movw r24,r26 6451 3c6a 20E0 ldi r18,lo8(256) 6452 3c6c 31E0 ldi r19,hi8(256) 6453 3c6e 40E0 ldi r20,hlo8(256) 6454 3c70 50E0 ldi r21,hhi8(256) 6455 3c72 0E94 0000 call __divmodsi4 6456 3c76 2D8B std Y+21,r18 6457 3c78 3E8B std Y+22,r19 6458 3c7a 4F8B std Y+23,r20 6459 3c7c 588F std Y+24,r21 6460 3c7e 28A3 std Y+32,r18 6461 3c80 39A3 std Y+33,r19 6462 3c82 4AA3 std Y+34,r20 6463 3c84 5BA3 std Y+35,r21 6464 3c86 2093 0000 sts MeanIntegralGyroNick,r18 6465 3c8a 3093 0000 sts (MeanIntegralGyroNick)+1,r19 6466 3c8e 4093 0000 sts (MeanIntegralGyroNick)+2,r20 6467 3c92 5093 0000 sts (MeanIntegralGyroNick)+3,r21 6468 3c96 2090 0000 lds r2,MeanIntegralGyroRoll 6469 3c9a 3090 0000 lds r3,(MeanIntegralGyroRoll)+1 6470 3c9e 4090 0000 lds r4,(MeanIntegralGyroRoll)+2 6471 3ca2 5090 0000 lds r5,(MeanIntegralGyroRoll)+3 6472 3ca6 C201 movw r24,r4 6473 3ca8 B101 movw r22,r2 6474 3caa 20E0 ldi r18,lo8(256) 6475 3cac 31E0 ldi r19,hi8(256) 6476 3cae 40E0 ldi r20,hlo8(256) 6477 3cb0 50E0 ldi r21,hhi8(256) 6478 3cb2 0E94 0000 call __divmodsi4 6479 3cb6 298F std Y+25,r18 6480 3cb8 3A8F std Y+26,r19 6481 3cba 4B8F std Y+27,r20 6482 3cbc 5C8F std Y+28,r21 6483 3cbe 1901 movw r2,r18 6484 3cc0 2A01 movw r4,r20 6485 3cc2 2093 0000 sts MeanIntegralGyroRoll,r18 6486 3cc6 3093 0000 sts (MeanIntegralGyroRoll)+1,r19 6487 3cca 4093 0000 sts (MeanIntegralGyroRoll)+2,r20 6488 3cce 5093 0000 sts (MeanIntegralGyroRoll)+3,r21 6489 3cd2 E090 0000 lds r14,ParamSet+23 6490 3cd6 FF24 clr r15 6491 3cd8 00E0 ldi r16,lo8(0) 6492 3cda 10E0 ldi r17,hi8(0) 6493 3cdc 2091 0000 lds r18,MeanAccNick 6494 3ce0 3091 0000 lds r19,(MeanAccNick)+1 6495 3ce4 4091 0000 lds r20,(MeanAccNick)+2 6496 3ce8 5091 0000 lds r21,(MeanAccNick)+3 6497 3cec C801 movw r24,r16 6498 3cee B701 movw r22,r14 6499 3cf0 0E94 0000 call __mulsi3 6500 3cf4 20E0 ldi r18,lo8(256) 6501 3cf6 31E0 ldi r19,hi8(256) 6502 3cf8 40E0 ldi r20,hlo8(256) 6503 3cfa 50E0 ldi r21,hhi8(256) 6504 3cfc 0E94 0000 call __divmodsi4 6505 3d00 5901 movw r10,r18 6506 3d02 6A01 movw r12,r20 6507 3d04 2093 0000 sts MeanAccNick,r18 6508 3d08 3093 0000 sts (MeanAccNick)+1,r19 6509 3d0c 4093 0000 sts (MeanAccNick)+2,r20 6510 3d10 5093 0000 sts (MeanAccNick)+3,r21 6511 3d14 2091 0000 lds r18,MeanAccRoll 6512 3d18 3091 0000 lds r19,(MeanAccRoll)+1 6513 3d1c 4091 0000 lds r20,(MeanAccRoll)+2 6514 3d20 5091 0000 lds r21,(MeanAccRoll)+3 6515 3d24 C801 movw r24,r16 6516 3d26 B701 movw r22,r14 6517 3d28 0E94 0000 call __mulsi3 6518 3d2c 20E0 ldi r18,lo8(256) 6519 3d2e 31E0 ldi r19,hi8(256) 6520 3d30 40E0 ldi r20,hlo8(256) 6521 3d32 50E0 ldi r21,hhi8(256) 6522 3d34 0E94 0000 call __divmodsi4 6523 3d38 3901 movw r6,r18 6524 3d3a 4A01 movw r8,r20 6525 3d3c 2093 0000 sts MeanAccRoll,r18 6526 3d40 3093 0000 sts (MeanAccRoll)+1,r19 6527 3d44 4093 0000 sts (MeanAccRoll)+2,r20 6528 3d48 5093 0000 sts (MeanAccRoll)+3,r21 6529 3d4c 6D89 ldd r22,Y+21 6530 3d4e 7E89 ldd r23,Y+22 6531 3d50 8F89 ldd r24,Y+23 6532 3d52 988D ldd r25,Y+24 6533 3d54 6A19 sub r22,r10 6534 3d56 7B09 sbc r23,r11 6535 3d58 8C09 sbc r24,r12 6536 3d5a 9D09 sbc r25,r13 6537 3d5c 6093 0000 sts IntegralGyroNickError.2411,r22 6538 3d60 7093 0000 sts (IntegralGyroNickError.2411)+1,r23 6539 3d64 8093 0000 sts (IntegralGyroNickError.2411)+2,r24 6540 3d68 9093 0000 sts (IntegralGyroNickError.2411)+3,r25 6541 3d6c E090 0000 lds r14,ParamSet+56 6542 3d70 FF24 clr r15 6543 3d72 00E0 ldi r16,lo8(0) 6544 3d74 10E0 ldi r17,hi8(0) 6545 3d76 A801 movw r20,r16 6546 3d78 9701 movw r18,r14 6547 3d7a 0E94 0000 call __divmodsi4 6548 3d7e CA01 movw r24,r20 6549 3d80 2093 0000 sts CorrectionNick.2413,r18 6550 3d84 3093 0000 sts CorrectionNick.2413+1,r19 6551 3d88 4093 0000 sts CorrectionNick.2413+2,r20 6552 3d8c 9093 0000 sts CorrectionNick.2413+3,r25 6553 3d90 B901 movw r22,r18 6554 3d92 20E0 ldi r18,lo8(256) 6555 3d94 31E0 ldi r19,hi8(256) 6556 3d96 40E0 ldi r20,hlo8(256) 6557 3d98 50E0 ldi r21,hhi8(256) 6558 3d9a 0E94 0000 call __divmodsi4 6559 3d9e 3093 0000 sts (AttitudeCorrectionNick)+1,r19 6560 3da2 2093 0000 sts AttitudeCorrectionNick,r18 6561 3da6 C201 movw r24,r4 6562 3da8 B101 movw r22,r2 6563 3daa 6619 sub r22,r6 6564 3dac 7709 sbc r23,r7 6565 3dae 8809 sbc r24,r8 6566 3db0 9909 sbc r25,r9 6567 3db2 6093 0000 sts IntegralGyroRollError.2412,r22 6568 3db6 7093 0000 sts (IntegralGyroRollError.2412)+1,r23 6569 3dba 8093 0000 sts (IntegralGyroRollError.2412)+2,r24 6570 3dbe 9093 0000 sts (IntegralGyroRollError.2412)+3,r25 6571 3dc2 A801 movw r20,r16 6572 3dc4 9701 movw r18,r14 6573 3dc6 0E94 0000 call __divmodsi4 6574 3dca CA01 movw r24,r20 6575 3dcc 2093 0000 sts CorrectionRoll.2414,r18 6576 3dd0 3093 0000 sts CorrectionRoll.2414+1,r19 6577 3dd4 4093 0000 sts CorrectionRoll.2414+2,r20 6578 3dd8 9093 0000 sts CorrectionRoll.2414+3,r25 6579 3ddc B901 movw r22,r18 6580 3dde 20E0 ldi r18,lo8(256) 6581 3de0 31E0 ldi r19,hi8(256) 6582 3de2 40E0 ldi r20,hlo8(256) 6583 3de4 50E0 ldi r21,hhi8(256) 6584 3de6 0E94 0000 call __divmodsi4 6585 3dea 3093 0000 sts (AttitudeCorrectionRoll)+1,r19 6586 3dee 2093 0000 sts AttitudeCorrectionRoll,r18 6587 3df2 8091 0000 lds r24,MaxStickNick 6588 3df6 9091 0000 lds r25,(MaxStickNick)+1 6589 3dfa 8134 cpi r24,65 6590 3dfc 9105 cpc r25,__zero_reg__ 6591 3dfe 04F4 brge .L531 6592 3e00 8091 0000 lds r24,MaxStickRoll 6593 3e04 9091 0000 lds r25,(MaxStickRoll)+1 6594 3e08 8134 cpi r24,65 6595 3e0a 9105 cpc r25,__zero_reg__ 6596 3e0c 04F4 brge .L531 6597 3e0e E091 0000 lds r30,ParamSet+3 6598 3e12 F0E0 ldi r31,lo8(0) 6599 3e14 EE0F lsl r30 6600 3e16 FF1F rol r31 6601 3e18 E050 subi r30,lo8(-(PPM_in)) 6602 3e1a F040 sbci r31,hi8(-(PPM_in)) 6603 3e1c 0190 ld __tmp_reg__,Z+ 6604 3e1e F081 ld r31,Z 6605 3e20 E02D mov r30,__tmp_reg__ 6606 3e22 F7FF sbrs r31,7 6607 3e24 00C0 rjmp .+4 6608 3e26 0C94 0000 jmp .L792 6609 .L533: 6610 3e2a 7A97 sbiw r30,26 6611 3e2c 04F0 brlt .L532 6612 .L531: 6613 3e2e 8091 0000 lds r24,FCParam+31 6614 3e32 8F3F cpi r24,lo8(-1) 6615 3e34 01F4 brne .+4 6616 3e36 0C94 0000 jmp .L793 6617 .L532: 6618 3e3a 2091 0000 lds r18,IntegralGyroNick2 6619 3e3e 3091 0000 lds r19,(IntegralGyroNick2)+1 6620 3e42 4091 0000 lds r20,(IntegralGyroNick2)+2 6621 3e46 5091 0000 lds r21,(IntegralGyroNick2)+3 6622 3e4a A8A4 ldd r10,Y+40 6623 3e4c B9A4 ldd r11,Y+41 6624 3e4e CAA4 ldd r12,Y+42 6625 3e50 DBA4 ldd r13,Y+43 6626 3e52 2A19 sub r18,r10 6627 3e54 3B09 sbc r19,r11 6628 3e56 4C09 sbc r20,r12 6629 3e58 5D09 sbc r21,r13 6630 3e5a 2093 0000 sts IntegralGyroNickError.2411,r18 6631 3e5e 3093 0000 sts (IntegralGyroNickError.2411)+1,r19 6632 3e62 4093 0000 sts (IntegralGyroNickError.2411)+2,r20 6633 3e66 5093 0000 sts (IntegralGyroNickError.2411)+3,r21 6634 3e6a 8091 0000 lds r24,ReadingIntegralGyroNick2 6635 3e6e 9091 0000 lds r25,(ReadingIntegralGyroNick2)+1 6636 3e72 A091 0000 lds r26,(ReadingIntegralGyroNick2)+2 6637 3e76 B091 0000 lds r27,(ReadingIntegralGyroNick2)+3 6638 3e7a 821B sub r24,r18 6639 3e7c 930B sbc r25,r19 6640 3e7e A40B sbc r26,r20 6641 3e80 B50B sbc r27,r21 6642 3e82 8093 0000 sts ReadingIntegralGyroNick2,r24 6643 3e86 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 6644 3e8a A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 6645 3e8e B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 6646 3e92 2091 0000 lds r18,IntegralGyroRoll2 6647 3e96 3091 0000 lds r19,(IntegralGyroRoll2)+1 6648 3e9a 4091 0000 lds r20,(IntegralGyroRoll2)+2 6649 3e9e 5091 0000 lds r21,(IntegralGyroRoll2)+3 6650 3ea2 6CA1 ldd r22,Y+36 6651 3ea4 7DA1 ldd r23,Y+37 6652 3ea6 8EA1 ldd r24,Y+38 6653 3ea8 9FA1 ldd r25,Y+39 6654 3eaa 261B sub r18,r22 6655 3eac 370B sbc r19,r23 6656 3eae 480B sbc r20,r24 6657 3eb0 590B sbc r21,r25 6658 3eb2 2093 0000 sts IntegralGyroRollError.2412,r18 6659 3eb6 3093 0000 sts (IntegralGyroRollError.2412)+1,r19 6660 3eba 4093 0000 sts (IntegralGyroRollError.2412)+2,r20 6661 3ebe 5093 0000 sts (IntegralGyroRollError.2412)+3,r21 6662 3ec2 8091 0000 lds r24,ReadingIntegralGyroRoll2 6663 3ec6 9091 0000 lds r25,(ReadingIntegralGyroRoll2)+1 6664 3eca A091 0000 lds r26,(ReadingIntegralGyroRoll2)+2 6665 3ece B091 0000 lds r27,(ReadingIntegralGyroRoll2)+3 6666 3ed2 821B sub r24,r18 6667 3ed4 930B sbc r25,r19 6668 3ed6 A40B sbc r26,r20 6669 3ed8 B50B sbc r27,r21 6670 3eda 8093 0000 sts ReadingIntegralGyroRoll2,r24 6671 3ede 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 6672 3ee2 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 6673 3ee6 B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 6674 3eea 8091 0000 lds r24,ParamSet+57 6675 3eee 8823 tst r24 6676 3ef0 01F0 breq .L536 6677 3ef2 2091 0000 lds r18,YawGyroDrift 6678 3ef6 3091 0000 lds r19,(YawGyroDrift)+1 6679 3efa 2138 cpi r18,129 6680 3efc 3105 cpc r19,__zero_reg__ 6681 3efe 04F0 brlt .L537 6682 3f00 8091 0000 lds r24,AdBiasGyroYaw 6683 3f04 9091 0000 lds r25,(AdBiasGyroYaw)+1 6684 3f08 0196 adiw r24,1 6685 3f0a 9093 0000 sts (AdBiasGyroYaw)+1,r25 6686 3f0e 8093 0000 sts AdBiasGyroYaw,r24 6687 .L537: 6688 3f12 2058 subi r18,lo8(-128) 6689 3f14 3F4F sbci r19,hi8(-128) 6690 3f16 04F4 brge .L536 6691 3f18 8091 0000 lds r24,AdBiasGyroYaw 6692 3f1c 9091 0000 lds r25,(AdBiasGyroYaw)+1 6693 3f20 0197 sbiw r24,1 6694 3f22 9093 0000 sts (AdBiasGyroYaw)+1,r25 6695 3f26 8093 0000 sts AdBiasGyroYaw,r24 6696 .L536: 6697 3f2a 1092 0000 sts (YawGyroDrift)+1,__zero_reg__ 6698 3f2e 1092 0000 sts YawGyroDrift,__zero_reg__ 6699 3f32 81E0 ldi r24,lo8(1) 6700 3f34 90E0 ldi r25,hi8(1) 6701 3f36 9093 0000 sts (cnt.2426)+1,r25 6702 3f3a 8093 0000 sts cnt.2426,r24 6703 3f3e E090 0000 lds r14,IntegralGyroNickError.2411 6704 3f42 F090 0000 lds r15,(IntegralGyroNickError.2411)+1 6705 3f46 0091 0000 lds r16,(IntegralGyroNickError.2411)+2 6706 3f4a 1091 0000 lds r17,(IntegralGyroNickError.2411)+3 6707 3f4e 81E0 ldi r24,lo8(513) 6708 3f50 E816 cp r14,r24 6709 3f52 82E0 ldi r24,hi8(513) 6710 3f54 F806 cpc r15,r24 6711 3f56 80E0 ldi r24,hlo8(513) 6712 3f58 0807 cpc r16,r24 6713 3f5a 80E0 ldi r24,hhi8(513) 6714 3f5c 1807 cpc r17,r24 6715 3f5e 04F0 brlt .L538 6716 3f60 84E0 ldi r24,lo8(4) 6717 3f62 90E0 ldi r25,hi8(4) 6718 3f64 9093 0000 sts (cnt.2426)+1,r25 6719 3f68 8093 0000 sts cnt.2426,r24 6720 .L538: 6721 3f6c 1092 0000 sts CorrectionNick.2413,__zero_reg__ 6722 3f70 1092 0000 sts (CorrectionNick.2413)+1,__zero_reg__ 6723 3f74 1092 0000 sts (CorrectionNick.2413)+2,__zero_reg__ 6724 3f78 1092 0000 sts (CorrectionNick.2413)+3,__zero_reg__ 6725 3f7c 8091 0000 lds r24,MeanIntegralGyroNick_old.2431 6726 3f80 9091 0000 lds r25,(MeanIntegralGyroNick_old.2431)+1 6727 3f84 A091 0000 lds r26,(MeanIntegralGyroNick_old.2431)+2 6728 3f88 B091 0000 lds r27,(MeanIntegralGyroNick_old.2431)+3 6729 3f8c AD88 ldd r10,Y+21 6730 3f8e BE88 ldd r11,Y+22 6731 3f90 CF88 ldd r12,Y+23 6732 3f92 D88C ldd r13,Y+24 6733 3f94 8A19 sub r24,r10 6734 3f96 9B09 sbc r25,r11 6735 3f98 AC09 sbc r26,r12 6736 3f9a BD09 sbc r27,r13 6737 3f9c 815E subi r24,lo8(-(19999)) 6738 3f9e 914B sbci r25,hi8(-(19999)) 6739 3fa0 AF4F sbci r26,hlo8(-(19999)) 6740 3fa2 BF4F sbci r27,hhi8(-(19999)) 6741 3fa4 8F53 subi r24,lo8(39999) 6742 3fa6 9C49 sbci r25,hi8(39999) 6743 3fa8 A040 sbci r26,hlo8(39999) 6744 3faa B040 sbci r27,hhi8(39999) 6745 3fac 00F4 brsh .+4 6746 3fae 0C94 0000 jmp .L539 6747 3fb2 8091 0000 lds r24,FCParam+32 6748 3fb6 8931 cpi r24,lo8(25) 6749 3fb8 04F0 brlt .+4 6750 3fba 0C94 0000 jmp .L539 6751 3fbe 1092 0000 sts (cnt.2426)+1,__zero_reg__ 6752 3fc2 1092 0000 sts cnt.2426,__zero_reg__ 6753 3fc6 88EE ldi r24,lo8(1000) 6754 3fc8 93E0 ldi r25,hi8(1000) 6755 3fca 9093 0000 sts (BadCompassHeading)+1,r25 6756 3fce 8093 0000 sts BadCompassHeading,r24 6757 3fd2 AA24 clr r10 6758 3fd4 BB24 clr r11 6759 .L548: 6760 3fd6 8091 0000 lds r24,ParamSet+57 6761 3fda 90E0 ldi r25,lo8(0) 6762 3fdc 8A15 cp r24,r10 6763 3fde 9B05 cpc r25,r11 6764 3fe0 04F4 brge .L549 6765 3fe2 9093 0000 sts (cnt.2426)+1,r25 6766 3fe6 8093 0000 sts cnt.2426,r24 6767 .L549: 6768 3fea D090 0000 lds r13,FCParam+32 6769 3fee DD20 tst r13 6770 3ff0 01F0 breq .L550 6771 3ff2 2D2D mov r18,r13 6772 3ff4 3327 clr r19 6773 3ff6 27FD sbrc r18,7 6774 3ff8 3095 com r19 6775 3ffa 8091 0000 lds r24,cnt.2426 6776 3ffe 9091 0000 lds r25,(cnt.2426)+1 6777 4002 2817 cp r18,r24 6778 4004 3907 cpc r19,r25 6779 4006 04F4 brge .L550 6780 4008 3093 0000 sts (cnt.2426)+1,r19 6781 400c 2093 0000 sts cnt.2426,r18 6782 .L550: 6783 4010 71E8 ldi r23,lo8(129) 6784 4012 E716 cp r14,r23 6785 4014 F104 cpc r15,__zero_reg__ 6786 4016 0105 cpc r16,__zero_reg__ 6787 4018 1105 cpc r17,__zero_reg__ 6788 401a 04F0 brlt .L551 6789 401c 8091 0000 lds r24,BiasHiResGyroNick 6790 4020 9091 0000 lds r25,(BiasHiResGyroNick)+1 6791 4024 2091 0000 lds r18,cnt.2426 6792 4028 3091 0000 lds r19,(cnt.2426)+1 6793 402c 820F add r24,r18 6794 402e 931F adc r25,r19 6795 4030 9093 0000 sts (BiasHiResGyroNick)+1,r25 6796 4034 8093 0000 sts BiasHiResGyroNick,r24 6797 .L551: 6798 4038 80E8 ldi r24,lo8(-128) 6799 403a E816 cp r14,r24 6800 403c 8FEF ldi r24,hi8(-128) 6801 403e F806 cpc r15,r24 6802 4040 8FEF ldi r24,hlo8(-128) 6803 4042 0807 cpc r16,r24 6804 4044 8FEF ldi r24,hhi8(-128) 6805 4046 1807 cpc r17,r24 6806 4048 04F4 brge .L552 6807 404a 8091 0000 lds r24,BiasHiResGyroNick 6808 404e 9091 0000 lds r25,(BiasHiResGyroNick)+1 6809 4052 2091 0000 lds r18,cnt.2426 6810 4056 3091 0000 lds r19,(cnt.2426)+1 6811 405a 821B sub r24,r18 6812 405c 930B sbc r25,r19 6813 405e 9093 0000 sts (BiasHiResGyroNick)+1,r25 6814 4062 8093 0000 sts BiasHiResGyroNick,r24 6815 .L552: 6816 4066 81E0 ldi r24,lo8(1) 6817 4068 90E0 ldi r25,hi8(1) 6818 406a 9093 0000 sts (cnt.2426)+1,r25 6819 406e 8093 0000 sts cnt.2426,r24 6820 4072 E090 0000 lds r14,IntegralGyroRollError.2412 6821 4076 F090 0000 lds r15,(IntegralGyroRollError.2412)+1 6822 407a 0091 0000 lds r16,(IntegralGyroRollError.2412)+2 6823 407e 1091 0000 lds r17,(IntegralGyroRollError.2412)+3 6824 4082 91E0 ldi r25,lo8(513) 6825 4084 E916 cp r14,r25 6826 4086 92E0 ldi r25,hi8(513) 6827 4088 F906 cpc r15,r25 6828 408a 90E0 ldi r25,hlo8(513) 6829 408c 0907 cpc r16,r25 6830 408e 90E0 ldi r25,hhi8(513) 6831 4090 1907 cpc r17,r25 6832 4092 04F0 brlt .L553 6833 4094 84E0 ldi r24,lo8(4) 6834 4096 90E0 ldi r25,hi8(4) 6835 4098 9093 0000 sts (cnt.2426)+1,r25 6836 409c 8093 0000 sts cnt.2426,r24 6837 .L553: 6838 40a0 1092 0000 sts CorrectionRoll.2414,__zero_reg__ 6839 40a4 1092 0000 sts (CorrectionRoll.2414)+1,__zero_reg__ 6840 40a8 1092 0000 sts (CorrectionRoll.2414)+2,__zero_reg__ 6841 40ac 1092 0000 sts (CorrectionRoll.2414)+3,__zero_reg__ 6842 40b0 8091 0000 lds r24,MeanIntegralGyroRoll_old.2432 6843 40b4 9091 0000 lds r25,(MeanIntegralGyroRoll_old.2432)+1 6844 40b8 A091 0000 lds r26,(MeanIntegralGyroRoll_old.2432)+2 6845 40bc B091 0000 lds r27,(MeanIntegralGyroRoll_old.2432)+3 6846 40c0 298D ldd r18,Y+25 6847 40c2 3A8D ldd r19,Y+26 6848 40c4 4B8D ldd r20,Y+27 6849 40c6 5C8D ldd r21,Y+28 6850 40c8 821B sub r24,r18 6851 40ca 930B sbc r25,r19 6852 40cc A40B sbc r26,r20 6853 40ce B50B sbc r27,r21 6854 40d0 815E subi r24,lo8(-(19999)) 6855 40d2 914B sbci r25,hi8(-(19999)) 6856 40d4 AF4F sbci r26,hlo8(-(19999)) 6857 40d6 BF4F sbci r27,hhi8(-(19999)) 6858 40d8 8F53 subi r24,lo8(39999) 6859 40da 9C49 sbci r25,hi8(39999) 6860 40dc A040 sbci r26,hlo8(39999) 6861 40de B040 sbci r27,hhi8(39999) 6862 40e0 00F4 brsh .+4 6863 40e2 0C94 0000 jmp .L554 6864 40e6 48E1 ldi r20,lo8(24) 6865 40e8 4D15 cp r20,r13 6866 40ea 04F4 brge .+4 6867 40ec 0C94 0000 jmp .L554 6868 40f0 1092 0000 sts (cnt.2426)+1,__zero_reg__ 6869 40f4 1092 0000 sts cnt.2426,__zero_reg__ 6870 40f8 88EE ldi r24,lo8(1000) 6871 40fa 93E0 ldi r25,hi8(1000) 6872 40fc 9093 0000 sts (BadCompassHeading)+1,r25 6873 4100 8093 0000 sts BadCompassHeading,r24 6874 4104 AA24 clr r10 6875 4106 BB24 clr r11 6876 .L563: 6877 4108 8091 0000 lds r24,ParamSet+57 6878 410c 90E0 ldi r25,lo8(0) 6879 410e 8A15 cp r24,r10 6880 4110 9B05 cpc r25,r11 6881 4112 04F4 brge .L564 6882 4114 9093 0000 sts (cnt.2426)+1,r25 6883 4118 8093 0000 sts cnt.2426,r24 6884 .L564: 6885 411c DD20 tst r13 6886 411e 01F0 breq .L565 6887 4120 2D2D mov r18,r13 6888 4122 3327 clr r19 6889 4124 27FD sbrc r18,7 6890 4126 3095 com r19 6891 4128 8091 0000 lds r24,cnt.2426 6892 412c 9091 0000 lds r25,(cnt.2426)+1 6893 4130 2817 cp r18,r24 6894 4132 3907 cpc r19,r25 6895 4134 04F4 brge .L565 6896 4136 3093 0000 sts (cnt.2426)+1,r19 6897 413a 2093 0000 sts cnt.2426,r18 6898 .L565: 6899 413e F1E8 ldi r31,lo8(129) 6900 4140 EF16 cp r14,r31 6901 4142 F104 cpc r15,__zero_reg__ 6902 4144 0105 cpc r16,__zero_reg__ 6903 4146 1105 cpc r17,__zero_reg__ 6904 4148 04F0 brlt .L566 6905 414a 8091 0000 lds r24,BiasHiResGyroRoll 6906 414e 9091 0000 lds r25,(BiasHiResGyroRoll)+1 6907 4152 2091 0000 lds r18,cnt.2426 6908 4156 3091 0000 lds r19,(cnt.2426)+1 6909 415a 820F add r24,r18 6910 415c 931F adc r25,r19 6911 415e 9093 0000 sts (BiasHiResGyroRoll)+1,r25 6912 4162 8093 0000 sts BiasHiResGyroRoll,r24 6913 .L566: 6914 4166 20E8 ldi r18,lo8(-128) 6915 4168 E216 cp r14,r18 6916 416a 2FEF ldi r18,hi8(-128) 6917 416c F206 cpc r15,r18 6918 416e 2FEF ldi r18,hlo8(-128) 6919 4170 0207 cpc r16,r18 6920 4172 2FEF ldi r18,hhi8(-128) 6921 4174 1207 cpc r17,r18 6922 4176 04F0 brlt .+4 6923 4178 0C94 0000 jmp .L567 6924 417c 8091 0000 lds r24,BiasHiResGyroRoll 6925 4180 9091 0000 lds r25,(BiasHiResGyroRoll)+1 6926 4184 2091 0000 lds r18,cnt.2426 6927 4188 3091 0000 lds r19,(cnt.2426)+1 6928 418c 821B sub r24,r18 6929 418e 930B sbc r25,r19 6930 4190 9093 0000 sts (BiasHiResGyroRoll)+1,r25 6931 4194 8093 0000 sts BiasHiResGyroRoll,r24 6932 4198 0C94 0000 jmp .L567 6933 .L753: 6934 419c 8091 0000 lds r24,LoopingRoll 6935 41a0 8823 tst r24 6936 41a2 01F0 breq .+4 6937 41a4 0C94 0000 jmp .L510 6938 .L509: 6939 41a8 8091 0000 lds r24,AdValueAccZ 6940 41ac 9091 0000 lds r25,(AdValueAccZ)+1 6941 41b0 8150 subi r24,lo8(513) 6942 41b2 9240 sbci r25,hi8(513) 6943 41b4 04F4 brge .+2 6944 41b6 00C0 rjmp .L794 6945 .L511: 6946 41b8 8091 0000 lds r24,FCParam+31 6947 41bc 8F3F cpi r24,lo8(-1) 6948 41be 01F4 brne .+2 6949 41c0 00C0 rjmp .L512 6950 41c2 E090 0000 lds r14,ParamSet+23 6951 41c6 FF24 clr r15 6952 41c8 00E0 ldi r16,lo8(0) 6953 41ca 10E0 ldi r17,hi8(0) 6954 41cc A82E mov r10,r24 6955 41ce BB24 clr r11 6956 41d0 A7FC sbrc r10,7 6957 41d2 B094 com r11 6958 41d4 CB2C mov r12,r11 6959 41d6 DB2C mov r13,r11 6960 41d8 68A5 ldd r22,Y+40 6961 41da 79A5 ldd r23,Y+41 6962 41dc 8AA5 ldd r24,Y+42 6963 41de 9BA5 ldd r25,Y+43 6964 41e0 A801 movw r20,r16 6965 41e2 9701 movw r18,r14 6966 41e4 0E94 0000 call __divmodsi4 6967 41e8 8091 0000 lds r24,AccNick 6968 41ec 9091 0000 lds r25,(AccNick)+1 6969 41f0 AA27 clr r26 6970 41f2 97FD sbrc r25,7 6971 41f4 A095 com r26 6972 41f6 BA2F mov r27,r26 6973 41f8 281B sub r18,r24 6974 41fa 390B sbc r19,r25 6975 41fc 4A0B sbc r20,r26 6976 41fe 5B0B sbc r21,r27 6977 4200 CA01 movw r24,r20 6978 4202 B901 movw r22,r18 6979 4204 A601 movw r20,r12 6980 4206 9501 movw r18,r10 6981 4208 0E94 0000 call __mulsi3 6982 420c 20E0 ldi r18,lo8(512) 6983 420e 32E0 ldi r19,hi8(512) 6984 4210 40E0 ldi r20,hlo8(512) 6985 4212 50E0 ldi r21,hhi8(512) 6986 4214 0E94 0000 call __divmodsi4 6987 4218 522E mov r5,r18 6988 421a 432E mov r4,r19 6989 421c 342E mov r3,r20 6990 421e 252E mov r2,r21 6991 4220 822F mov r24,r18 6992 4222 932F mov r25,r19 6993 4224 A42F mov r26,r20 6994 4226 B52F mov r27,r21 6995 4228 3C01 movw r6,r24 6996 422a 4D01 movw r8,r26 6997 422c 6CA1 ldd r22,Y+36 6998 422e 7DA1 ldd r23,Y+37 6999 4230 8EA1 ldd r24,Y+38 7000 4232 9FA1 ldd r25,Y+39 7001 4234 A801 movw r20,r16 7002 4236 9701 movw r18,r14 7003 4238 0E94 0000 call __divmodsi4 7004 423c 8091 0000 lds r24,AccRoll 7005 4240 9091 0000 lds r25,(AccRoll)+1 7006 4244 AA27 clr r26 7007 4246 97FD sbrc r25,7 7008 4248 A095 com r26 7009 424a BA2F mov r27,r26 7010 424c 281B sub r18,r24 7011 424e 390B sbc r19,r25 7012 4250 4A0B sbc r20,r26 7013 4252 5B0B sbc r21,r27 7014 4254 CA01 movw r24,r20 7015 4256 B901 movw r22,r18 7016 4258 A601 movw r20,r12 7017 425a 9501 movw r18,r10 7018 425c 0E94 0000 call __mulsi3 7019 4260 20E0 ldi r18,lo8(512) 7020 4262 32E0 ldi r19,hi8(512) 7021 4264 40E0 ldi r20,hlo8(512) 7022 4266 50E0 ldi r21,hhi8(512) 7023 4268 0E94 0000 call __divmodsi4 7024 426c 122F mov r17,r18 7025 426e 032F mov r16,r19 7026 4270 F42E mov r15,r20 7027 4272 E52E mov r14,r21 7028 4274 822F mov r24,r18 7029 4276 932F mov r25,r19 7030 4278 A42F mov r26,r20 7031 427a B52F mov r27,r21 7032 427c 5C01 movw r10,r24 7033 427e 6D01 movw r12,r26 7034 4280 8091 0000 lds r24,MaxStickNick 7035 4284 9091 0000 lds r25,(MaxStickNick)+1 7036 4288 8134 cpi r24,65 7037 428a 9105 cpc r25,__zero_reg__ 7038 428c 04F0 brlt .+2 7039 428e 00C0 rjmp .L513 7040 4290 8091 0000 lds r24,MaxStickRoll 7041 4294 9091 0000 lds r25,(MaxStickRoll)+1 7042 4298 8134 cpi r24,65 7043 429a 9105 cpc r25,__zero_reg__ 7044 429c 04F0 brlt .+2 7045 429e 00C0 rjmp .L513 7046 .L514: 7047 42a0 E091 0000 lds r30,ParamSet+3 7048 42a4 F0E0 ldi r31,lo8(0) 7049 42a6 EE0F lsl r30 7050 42a8 FF1F rol r31 7051 42aa E050 subi r30,lo8(-(PPM_in)) 7052 42ac F040 sbci r31,hi8(-(PPM_in)) 7053 42ae 0190 ld __tmp_reg__,Z+ 7054 42b0 F081 ld r31,Z 7055 42b2 E02D mov r30,__tmp_reg__ 7056 42b4 F7FF sbrs r31,7 7057 42b6 00C0 rjmp .+4 7058 42b8 0C94 0000 jmp .L795 7059 .L516: 7060 42bc 7A97 sbiw r30,26 7061 42be 04F0 brlt .L515 7062 42c0 C401 movw r24,r8 7063 42c2 B301 movw r22,r6 7064 42c4 23E0 ldi r18,lo8(3) 7065 42c6 30E0 ldi r19,hi8(3) 7066 42c8 40E0 ldi r20,hlo8(3) 7067 42ca 50E0 ldi r21,hhi8(3) 7068 42cc 0E94 0000 call __divmodsi4 7069 42d0 C901 movw r24,r18 7070 42d2 DA01 movw r26,r20 7071 42d4 3C01 movw r6,r24 7072 42d6 4D01 movw r8,r26 7073 42d8 C601 movw r24,r12 7074 42da B501 movw r22,r10 7075 42dc 23E0 ldi r18,lo8(3) 7076 42de 30E0 ldi r19,hi8(3) 7077 42e0 40E0 ldi r20,hlo8(3) 7078 42e2 50E0 ldi r21,hhi8(3) 7079 42e4 0E94 0000 call __divmodsi4 7080 42e8 C901 movw r24,r18 7081 42ea DA01 movw r26,r20 7082 42ec 5C01 movw r10,r24 7083 42ee 6D01 movw r12,r26 7084 .L515: 7085 42f0 8091 0000 lds r24,FCParam+33 7086 42f4 9927 clr r25 7087 42f6 87FD sbrc r24,7 7088 42f8 9095 com r25 7089 42fa A92F mov r26,r25 7090 42fc B92F mov r27,r25 7091 42fe 2227 clr r18 7092 4300 3327 clr r19 7093 4302 A901 movw r20,r18 7094 4304 281B sub r18,r24 7095 4306 390B sbc r19,r25 7096 4308 4A0B sbc r20,r26 7097 430a 5B0B sbc r21,r27 7098 430c 6216 cp r6,r18 7099 430e 7306 cpc r7,r19 7100 4310 8406 cpc r8,r20 7101 4312 9506 cpc r9,r21 7102 4314 04F0 brlt .+2 7103 4316 00C0 rjmp .L517 7104 4318 7901 movw r14,r18 7105 431a 8A01 movw r16,r20 7106 .L518: 7107 431c A216 cp r10,r18 7108 431e B306 cpc r11,r19 7109 4320 C406 cpc r12,r20 7110 4322 D506 cpc r13,r21 7111 4324 04F0 brlt .+2 7112 4326 00C0 rjmp .L520 7113 4328 5901 movw r10,r18 7114 432a 6A01 movw r12,r20 7115 .L521: 7116 432c 8091 0000 lds r24,ReadingIntegralGyroNick 7117 4330 9091 0000 lds r25,(ReadingIntegralGyroNick)+1 7118 4334 A091 0000 lds r26,(ReadingIntegralGyroNick)+2 7119 4338 B091 0000 lds r27,(ReadingIntegralGyroNick)+3 7120 433c 8E19 sub r24,r14 7121 433e 9F09 sbc r25,r15 7122 4340 A00B sbc r26,r16 7123 4342 B10B sbc r27,r17 7124 4344 8093 0000 sts ReadingIntegralGyroNick,r24 7125 4348 9093 0000 sts (ReadingIntegralGyroNick)+1,r25 7126 434c A093 0000 sts (ReadingIntegralGyroNick)+2,r26 7127 4350 B093 0000 sts (ReadingIntegralGyroNick)+3,r27 7128 4354 8091 0000 lds r24,ReadingIntegralGyroRoll 7129 4358 9091 0000 lds r25,(ReadingIntegralGyroRoll)+1 7130 435c A091 0000 lds r26,(ReadingIntegralGyroRoll)+2 7131 4360 B091 0000 lds r27,(ReadingIntegralGyroRoll)+3 7132 4364 8A19 sub r24,r10 7133 4366 9B09 sbc r25,r11 7134 4368 AC09 sbc r26,r12 7135 436a BD09 sbc r27,r13 7136 436c 8093 0000 sts ReadingIntegralGyroRoll,r24 7137 4370 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25 7138 4374 A093 0000 sts (ReadingIntegralGyroRoll)+2,r26 7139 4378 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27 7140 437c 0C94 0000 jmp .L510 7141 .L486: 7142 4380 4150 subi r20,lo8(-(-1)) 7143 4382 5040 sbci r21,hi8(-(-1)) 7144 4384 5093 0000 sts (MaxStickRoll)+1,r21 7145 4388 4093 0000 sts MaxStickRoll,r20 7146 438c 0C94 0000 jmp .L490 7147 .L468: 7148 4390 1092 0000 sts delay_startmotors.2417,__zero_reg__ 7149 .L469: 7150 4394 E091 0000 lds r30,ParamSet+3 7151 4398 F0E0 ldi r31,lo8(0) 7152 439a EE0F lsl r30 7153 439c FF1F rol r31 7154 439e E050 subi r30,lo8(-(PPM_in)) 7155 43a0 F040 sbci r31,hi8(-(PPM_in)) 7156 43a2 8081 ld r24,Z 7157 43a4 9181 ldd r25,Z+1 7158 43a6 8C34 cpi r24,76 7159 43a8 9105 cpc r25,__zero_reg__ 7160 43aa 04F4 brge .+2 7161 43ac 00C0 rjmp .L470 7162 43ae 8091 0000 lds r24,delay_stopmotors.2418 7163 43b2 8F5F subi r24,lo8(-(1)) 7164 43b4 8093 0000 sts delay_stopmotors.2418,r24 7165 43b8 893C cpi r24,lo8(-55) 7166 43ba 00F4 brsh .+4 7167 43bc 0C94 0000 jmp .L410 7168 43c0 88EC ldi r24,lo8(-56) 7169 43c2 8093 0000 sts delay_stopmotors.2418,r24 7170 43c6 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ 7171 43ca 1092 0000 sts ModelIsFlying,__zero_reg__ 7172 .L727: 7173 43ce 8091 0000 lds r24,MKFlags 7174 43d2 8E7F andi r24,lo8(-2) 7175 43d4 8093 0000 sts MKFlags,r24 7176 43d8 0C94 0000 jmp .L410 7177 .L582: 7178 43dc 6B01 movw r12,r22 7179 43de CC0C lsl r12 7180 43e0 DD1C rol r13 7181 43e2 CC0C lsl r12 7182 43e4 DD1C rol r13 7183 43e6 CC0C lsl r12 7184 43e8 DD1C rol r13 7185 43ea 0C94 0000 jmp .L584 7186 .L755: 7187 43ee 8827 clr r24 7188 43f0 9927 clr r25 7189 43f2 8E19 sub r24,r14 7190 43f4 9F09 sbc r25,r15 7191 43f6 0C94 0000 jmp .L570 7192 .L783: 7193 43fa 2227 clr r18 7194 43fc 3327 clr r19 7195 43fe 2619 sub r18,r6 7196 4400 3709 sbc r19,r7 7197 4402 00C0 rjmp .L695 7198 .L758: 7199 4404 6D5F subi r22,lo8(-(3)) 7200 4406 7F4F sbci r23,hi8(-(3)) 7201 4408 0C94 0000 jmp .L572 7202 .L757: 7203 440c EE27 clr r30 7204 440e FF27 clr r31 7205 4410 EE19 sub r30,r14 7206 4412 FF09 sbc r31,r15 7207 4414 0C94 0000 jmp .L571 7208 .L736: 7209 4418 1092 0000 sts (StickGas)+1,__zero_reg__ 7210 441c 1092 0000 sts StickGas,__zero_reg__ 7211 4420 8091 0000 lds r24,ParamSet+8 7212 4424 82FD sbrc r24,2 7213 4426 00C0 rjmp .+4 7214 4428 0C94 0000 jmp .L480 7215 442c 00C0 rjmp .L737 7216 .L735: 7217 442e 2250 subi r18,lo8(-(-2)) 7218 4430 3040 sbci r19,hi8(-(-2)) 7219 4432 3093 0000 sts (StickYaw)+1,r19 7220 4436 2093 0000 sts StickYaw,r18 7221 443a 0C94 0000 jmp .L475 7222 .L744: 7223 443e 84E6 ldi r24,lo8(100) 7224 4440 90E0 ldi r25,hi8(100) 7225 4442 9093 0000 sts (MaxStickRoll)+1,r25 7226 4446 8093 0000 sts MaxStickRoll,r24 7227 444a 0C94 0000 jmp .L490 7228 .L503: 7229 444e 1092 0000 sts LoopingNick,__zero_reg__ 7230 4452 0C94 0000 jmp .L472 7231 .L500: 7232 4456 1092 0000 sts LoopingRoll,__zero_reg__ 7233 445a 0C94 0000 jmp .L501 7234 .L751: 7235 445e 81E0 ldi r24,lo8(1) 7236 4460 8093 0000 sts LoopingDown,r24 7237 4464 0C94 0000 jmp .L498 7238 .L749: 7239 4468 81E0 ldi r24,lo8(1) 7240 446a 8093 0000 sts LoopingTop,r24 7241 446e 8091 0000 lds r24,ParamSet+49 7242 4472 482F mov r20,r24 7243 4474 50E0 ldi r21,lo8(0) 7244 4476 61E0 ldi r22,lo8(1) 7245 4478 0C94 0000 jmp .L496 7246 .L747: 7247 447c 81E0 ldi r24,lo8(1) 7248 447e 8093 0000 sts LoopingRight,r24 7249 4482 8091 0000 lds r24,ParamSet+49 7250 4486 482F mov r20,r24 7251 4488 50E0 ldi r21,lo8(0) 7252 448a 0C94 0000 jmp .L494 7253 .L745: 7254 448e 81E0 ldi r24,lo8(1) 7255 4490 8093 0000 sts LoopingLeft,r24 7256 4494 71E0 ldi r23,lo8(1) 7257 4496 0C94 0000 jmp .L492 7258 .L588: 7259 449a 0894 sec 7260 449c 4108 sbc r4,__zero_reg__ 7261 449e 5108 sbc r5,__zero_reg__ 7262 44a0 5092 0000 sts (BadCompassHeading)+1,r5 7263 44a4 4092 0000 sts BadCompassHeading,r4 7264 44a8 0C94 0000 jmp .L575 7265 .L794: 7266 44ac 8091 0000 lds r24,MKFlags 7267 44b0 80FD sbrc r24,0 7268 44b2 00C0 rjmp .+4 7269 44b4 0C94 0000 jmp .L510 7270 44b8 00C0 rjmp .L511 7271 .L748: 7272 44ba 1092 0000 sts LoopingRight,__zero_reg__ 7273 44be 8091 0000 lds r24,ParamSet+49 7274 44c2 482F mov r20,r24 7275 44c4 50E0 ldi r21,lo8(0) 7276 44c6 0C94 0000 jmp .L494 7277 .L746: 7278 44ca 1092 0000 sts LoopingLeft,__zero_reg__ 7279 44ce 8091 0000 lds r24,ParamSet+1 7280 44d2 A82F mov r26,r24 7281 44d4 B0E0 ldi r27,lo8(0) 7282 44d6 8091 0000 lds r24,ParamSet+49 7283 44da 482F mov r20,r24 7284 44dc 50E0 ldi r21,lo8(0) 7285 44de AA0F lsl r26 7286 44e0 BB1F rol r27 7287 44e2 70E0 ldi r23,lo8(0) 7288 44e4 0C94 0000 jmp .L492 7289 .L752: 7290 44e8 1092 0000 sts LoopingDown,__zero_reg__ 7291 44ec 0C94 0000 jmp .L498 7292 .L750: 7293 44f0 1092 0000 sts LoopingTop,__zero_reg__ 7294 44f4 8091 0000 lds r24,ParamSet 7295 44f8 A82F mov r26,r24 7296 44fa B0E0 ldi r27,lo8(0) 7297 44fc 8091 0000 lds r24,ParamSet+49 7298 4500 482F mov r20,r24 7299 4502 50E0 ldi r21,lo8(0) 7300 4504 AA0F lsl r26 7301 4506 BB1F rol r27 7302 4508 60E0 ldi r22,lo8(0) 7303 450a 0C94 0000 jmp .L496 7304 .L412: 7305 450e 8091 0000 lds r24,MKFlags 7306 4512 8260 ori r24,lo8(2) 7307 4514 8093 0000 sts MKFlags,r24 7308 4518 0C94 0000 jmp .L413 7309 .L780: 7310 451c 0196 adiw r24,1 7311 451e 0C94 0000 jmp .L676 7312 .L764: 7313 4522 8091 0000 lds r24,YawGyroDrift 7314 4526 9091 0000 lds r25,(YawGyroDrift)+1 7315 452a 860F add r24,r22 7316 452c 971F adc r25,r23 7317 452e 9093 0000 sts (YawGyroDrift)+1,r25 7318 4532 8093 0000 sts YawGyroDrift,r24 7319 4536 8091 0000 lds r24,UpdateCompassCourse.2420 7320 453a 9091 0000 lds r25,(UpdateCompassCourse.2420)+1 7321 453e 892B or r24,r25 7322 4540 01F4 brne .+4 7323 4542 0C94 0000 jmp .L585 7324 4546 E090 0000 lds r14,ParamSet+23 7325 454a 8AE2 ldi r24,lo8(42) 7326 454c E89E mul r14,r24 7327 454e 7001 movw r14,r0 7328 4550 1124 clr r1 7329 4552 0027 clr r16 7330 4554 F7FC sbrc r15,7 7331 4556 0095 com r16 7332 4558 102F mov r17,r16 7333 455a 9501 movw r18,r10 7334 455c 4427 clr r20 7335 455e 37FD sbrc r19,7 7336 4560 4095 com r20 7337 4562 542F mov r21,r20 7338 4564 C801 movw r24,r16 7339 4566 B701 movw r22,r14 7340 4568 0E94 0000 call __mulsi3 7341 456c 6093 0000 sts YawGyroHeading,r22 7342 4570 7093 0000 sts (YawGyroHeading)+1,r23 7343 4574 8093 0000 sts (YawGyroHeading)+2,r24 7344 4578 9093 0000 sts (YawGyroHeading)+3,r25 7345 457c A801 movw r20,r16 7346 457e 9701 movw r18,r14 7347 4580 0E94 0000 call __divmodsi4 7348 4584 3093 0000 sts (CompassCourse)+1,r19 7349 4588 2093 0000 sts CompassCourse,r18 7350 458c 1092 0000 sts (UpdateCompassCourse.2420)+1,__zero_reg__ 7351 4590 1092 0000 sts UpdateCompassCourse.2420,__zero_reg__ 7352 4594 0C94 0000 jmp .L585 7353 .L513: 7354 4598 652D mov r22,r5 7355 459a 742D mov r23,r4 7356 459c 832D mov r24,r3 7357 459e 922D mov r25,r2 7358 45a0 22E0 ldi r18,lo8(2) 7359 45a2 30E0 ldi r19,hi8(2) 7360 45a4 40E0 ldi r20,hlo8(2) 7361 45a6 50E0 ldi r21,hhi8(2) 7362 45a8 0E94 0000 call __divmodsi4 7363 45ac C901 movw r24,r18 7364 45ae DA01 movw r26,r20 7365 45b0 3C01 movw r6,r24 7366 45b2 4D01 movw r8,r26 7367 45b4 612F mov r22,r17 7368 45b6 702F mov r23,r16 7369 45b8 8F2D mov r24,r15 7370 45ba 9E2D mov r25,r14 7371 45bc 22E0 ldi r18,lo8(2) 7372 45be 30E0 ldi r19,hi8(2) 7373 45c0 40E0 ldi r20,hlo8(2) 7374 45c2 50E0 ldi r21,hhi8(2) 7375 45c4 0E94 0000 call __divmodsi4 7376 45c8 C901 movw r24,r18 7377 45ca DA01 movw r26,r20 7378 45cc 5C01 movw r10,r24 7379 45ce 6D01 movw r12,r26 7380 45d0 00C0 rjmp .L514 7381 .L451: 7382 45d2 8091 0000 lds r24,ParamSet+1 7383 45d6 A82F mov r26,r24 7384 45d8 B0E0 ldi r27,lo8(0) 7385 45da AA0F lsl r26 7386 45dc BB1F rol r27 7387 45de FD01 movw r30,r26 7388 45e0 E050 subi r30,lo8(-(PPM_in)) 7389 45e2 F040 sbci r31,hi8(-(PPM_in)) 7390 45e4 0190 ld __tmp_reg__,Z+ 7391 45e6 F081 ld r31,Z 7392 45e8 E02D mov r30,__tmp_reg__ 7393 45ea F7FF sbrs r31,7 7394 45ec 00C0 rjmp .+4 7395 45ee 0C94 0000 jmp .L796 7396 .L454: 7397 45f2 E734 cpi r30,71 7398 45f4 F105 cpc r31,__zero_reg__ 7399 45f6 04F0 brlt .+4 7400 45f8 0C94 0000 jmp .L452 7401 45fc 8091 0000 lds r24,ParamSet+8 7402 4600 8872 andi r24,lo8(40) 7403 4602 01F4 brne .+4 7404 4604 0C94 0000 jmp .L461 7405 4608 A050 subi r26,lo8(-(PPM_in)) 7406 460a B040 sbci r27,hi8(-(PPM_in)) 7407 460c 8D91 ld r24,X+ 7408 460e 9C91 ld r25,X 7409 4610 97FF sbrs r25,7 7410 4612 00C0 rjmp .+4 7411 4614 0C94 0000 jmp .L797 7412 .L463: 7413 4618 4E97 sbiw r24,30 7414 461a 04F0 brlt .+4 7415 461c 0C94 0000 jmp .L461 7416 4620 4050 subi r20,lo8(-(PPM_in)) 7417 4622 5040 sbci r21,hi8(-(PPM_in)) 7418 4624 FA01 movw r30,r20 7419 4626 8081 ld r24,Z 7420 4628 9181 ldd r25,Z+1 7421 462a 8A5B subi r24,lo8(-70) 7422 462c 9F4F sbci r25,hi8(-70) 7423 462e 04F0 brlt .+4 7424 4630 0C94 0000 jmp .L461 7425 4634 81E0 ldi r24,lo8(1) 7426 4636 8093 0000 sts CompassCalState,r24 7427 463a 88EE ldi r24,lo8(1000) 7428 463c 93E0 ldi r25,hi8(1000) 7429 463e 9093 0000 sts (BeepTime)+1,r25 7430 4642 8093 0000 sts BeepTime,r24 7431 4646 8091 0000 lds r24,ParamSet+2 7432 464a A82F mov r26,r24 7433 464c B0E0 ldi r27,lo8(0) 7434 464e AA0F lsl r26 7435 4650 BB1F rol r27 7436 .L446: 7437 4652 A050 subi r26,lo8(-(PPM_in)) 7438 4654 B040 sbci r27,hi8(-(PPM_in)) 7439 4656 8D91 ld r24,X+ 7440 4658 9C91 ld r25,X 7441 465a 8B5A subi r24,lo8(-85) 7442 465c 9F4F sbci r25,hi8(-85) 7443 465e 04F0 brlt .+4 7444 4660 0C94 0000 jmp .L410 7445 4664 E091 0000 lds r30,ParamSet+3 7446 4668 F0E0 ldi r31,lo8(0) 7447 466a EE0F lsl r30 7448 466c FF1F rol r31 7449 466e E050 subi r30,lo8(-(PPM_in)) 7450 4670 F040 sbci r31,hi8(-(PPM_in)) 7451 4672 8081 ld r24,Z 7452 4674 9181 ldd r25,Z+1 7453 4676 855B subi r24,lo8(-75) 7454 4678 9F4F sbci r25,hi8(-75) 7455 467a 04F0 brlt .+2 7456 467c 00C0 rjmp .L468 7457 467e 8091 0000 lds r24,delay_startmotors.2417 7458 4682 8F5F subi r24,lo8(-(1)) 7459 4684 8093 0000 sts delay_startmotors.2417,r24 7460 4688 893C cpi r24,lo8(-55) 7461 468a 00F4 brsh .+2 7462 468c 00C0 rjmp .L469 7463 468e 88EC ldi r24,lo8(-56) 7464 4690 8093 0000 sts delay_startmotors.2417,r24 7465 4694 81E0 ldi r24,lo8(1) 7466 4696 90E0 ldi r25,hi8(1) 7467 4698 9093 0000 sts (ModelIsFlying)+1,r25 7468 469c 8093 0000 sts ModelIsFlying,r24 7469 46a0 8091 0000 lds r24,MKFlags 7470 46a4 8960 ori r24,lo8(9) 7471 46a6 8093 0000 sts MKFlags,r24 7472 46aa 1092 0000 sts SetPointYaw.2410,__zero_reg__ 7473 46ae 1092 0000 sts (SetPointYaw.2410)+1,__zero_reg__ 7474 46b2 1092 0000 sts (SetPointYaw.2410)+2,__zero_reg__ 7475 46b6 1092 0000 sts (SetPointYaw.2410)+3,__zero_reg__ 7476 46ba 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ 7477 46be 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ 7478 46c2 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ 7479 46c6 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ 7480 46ca 6091 0000 lds r22,AccNick 7481 46ce 7091 0000 lds r23,(AccNick)+1 7482 46d2 8827 clr r24 7483 46d4 77FD sbrc r23,7 7484 46d6 8095 com r24 7485 46d8 982F mov r25,r24 7486 46da E090 0000 lds r14,ParamSet+23 7487 46de FF24 clr r15 7488 46e0 00E0 ldi r16,lo8(0) 7489 46e2 10E0 ldi r17,hi8(0) 7490 46e4 A801 movw r20,r16 7491 46e6 9701 movw r18,r14 7492 46e8 0E94 0000 call __mulsi3 7493 46ec 6093 0000 sts ReadingIntegralGyroNick,r22 7494 46f0 7093 0000 sts (ReadingIntegralGyroNick)+1,r23 7495 46f4 8093 0000 sts (ReadingIntegralGyroNick)+2,r24 7496 46f8 9093 0000 sts (ReadingIntegralGyroNick)+3,r25 7497 46fc 6091 0000 lds r22,AccRoll 7498 4700 7091 0000 lds r23,(AccRoll)+1 7499 4704 8827 clr r24 7500 4706 77FD sbrc r23,7 7501 4708 8095 com r24 7502 470a 982F mov r25,r24 7503 470c 0E94 0000 call __mulsi3 7504 4710 6093 0000 sts ReadingIntegralGyroRoll,r22 7505 4714 7093 0000 sts (ReadingIntegralGyroRoll)+1,r23 7506 4718 8093 0000 sts (ReadingIntegralGyroRoll)+2,r24 7507 471c 9093 0000 sts (ReadingIntegralGyroRoll)+3,r25 7508 4720 8091 0000 lds r24,IntegralGyroNick 7509 4724 9091 0000 lds r25,(IntegralGyroNick)+1 7510 4728 A091 0000 lds r26,(IntegralGyroNick)+2 7511 472c B091 0000 lds r27,(IntegralGyroNick)+3 7512 4730 8093 0000 sts ReadingIntegralGyroNick2,r24 7513 4734 9093 0000 sts (ReadingIntegralGyroNick2)+1,r25 7514 4738 A093 0000 sts (ReadingIntegralGyroNick2)+2,r26 7515 473c B093 0000 sts (ReadingIntegralGyroNick2)+3,r27 7516 4740 8091 0000 lds r24,IntegralGyroRoll 7517 4744 9091 0000 lds r25,(IntegralGyroRoll)+1 7518 4748 A091 0000 lds r26,(IntegralGyroRoll)+2 7519 474c B091 0000 lds r27,(IntegralGyroRoll)+3 7520 4750 8093 0000 sts ReadingIntegralGyroRoll2,r24 7521 4754 9093 0000 sts (ReadingIntegralGyroRoll2)+1,r25 7522 4758 A093 0000 sts (ReadingIntegralGyroRoll2)+2,r26 7523 475c B093 0000 sts (ReadingIntegralGyroRoll2)+3,r27 7524 4760 1092 0000 sts IPartNick.2408,__zero_reg__ 7525 4764 1092 0000 sts (IPartNick.2408)+1,__zero_reg__ 7526 4768 1092 0000 sts (IPartNick.2408)+2,__zero_reg__ 7527 476c 1092 0000 sts (IPartNick.2408)+3,__zero_reg__ 7528 4770 1092 0000 sts IPartRoll.2409,__zero_reg__ 7529 4774 1092 0000 sts (IPartRoll.2409)+1,__zero_reg__ 7530 4778 1092 0000 sts (IPartRoll.2409)+2,__zero_reg__ 7531 477c 1092 0000 sts (IPartRoll.2409)+3,__zero_reg__ 7532 4780 00C0 rjmp .L469 7533 .L739: 7534 4782 3095 com r19 7535 4784 2195 neg r18 7536 4786 3F4F sbci r19,lo8(-1) 7537 4788 0C94 0000 jmp .L483 7538 .L742: 7539 478c 3095 com r19 7540 478e 2195 neg r18 7541 4790 3F4F sbci r19,lo8(-1) 7542 4792 0C94 0000 jmp .L488 7543 .L741: 7544 4796 2D5F subi r18,lo8(-(3)) 7545 4798 3F4F sbci r19,hi8(-(3)) 7546 479a 0C94 0000 jmp .L487 7547 .L733: 7548 479e 4D5F subi r20,lo8(-(3)) 7549 47a0 5F4F sbci r21,hi8(-(3)) 7550 47a2 0C94 0000 jmp .L473 7551 .L734: 7552 47a6 4D5F subi r20,lo8(-(3)) 7553 47a8 5F4F sbci r21,hi8(-(3)) 7554 47aa 0C94 0000 jmp .L474 7555 .L738: 7556 47ae 2D5F subi r18,lo8(-(3)) 7557 47b0 3F4F sbci r19,hi8(-(3)) 7558 47b2 0C94 0000 jmp .L482 7559 .L762: 7560 47b6 60E0 ldi r22,lo8(0) 7561 47b8 70E0 ldi r23,hi8(0) 7562 47ba 0C94 0000 jmp .L581 7563 .L587: 7564 47be 84EF ldi r24,lo8(500) 7565 47c0 91E0 ldi r25,hi8(500) 7566 47c2 9093 0000 sts (BadCompassHeading)+1,r25 7567 47c6 8093 0000 sts BadCompassHeading,r24 7568 47ca 0C94 0000 jmp .L575 7569 .L428: 7570 47ce 8091 0000 lds r24,PPM_in+24 7571 47d2 9091 0000 lds r25,(PPM_in+24)+1 7572 47d6 8259 subi r24,lo8(-(110)) 7573 47d8 9F4F sbci r25,hi8(-(110)) 7574 47da 8217 cp r24,r18 7575 47dc 9307 cpc r25,r19 7576 47de 04F0 brlt .+4 7577 47e0 0C94 0000 jmp .L429 7578 47e4 2115 cp r18,__zero_reg__ 7579 47e6 3105 cpc r19,__zero_reg__ 7580 47e8 01F4 brne .+4 7581 47ea 0C94 0000 jmp .L429 7582 47ee C901 movw r24,r18 7583 47f0 0197 sbiw r24,1 7584 47f2 9093 0000 sts (Poti8)+1,r25 7585 47f6 8093 0000 sts Poti8,r24 7586 47fa 9C01 movw r18,r24 7587 47fc 0C94 0000 jmp .L429 7588 .L426: 7589 4800 8091 0000 lds r24,PPM_in+22 7590 4804 9091 0000 lds r25,(PPM_in+22)+1 7591 4808 8259 subi r24,lo8(-(110)) 7592 480a 9F4F sbci r25,hi8(-(110)) 7593 480c 8417 cp r24,r20 7594 480e 9507 cpc r25,r21 7595 4810 04F0 brlt .+4 7596 4812 0C94 0000 jmp .L427 7597 4816 4115 cp r20,__zero_reg__ 7598 4818 5105 cpc r21,__zero_reg__ 7599 481a 01F4 brne .+4 7600 481c 0C94 0000 jmp .L427 7601 4820 CA01 movw r24,r20 7602 4822 0197 sbiw r24,1 7603 4824 9093 0000 sts (Poti7)+1,r25 7604 4828 8093 0000 sts Poti7,r24 7605 482c AC01 movw r20,r24 7606 482e 0C94 0000 jmp .L427 7607 .L424: 7608 4832 8091 0000 lds r24,PPM_in+20 7609 4836 9091 0000 lds r25,(PPM_in+20)+1 7610 483a 8259 subi r24,lo8(-(110)) 7611 483c 9F4F sbci r25,hi8(-(110)) 7612 483e 8617 cp r24,r22 7613 4840 9707 cpc r25,r23 7614 4842 04F0 brlt .+4 7615 4844 0C94 0000 jmp .L425 7616 4848 6115 cp r22,__zero_reg__ 7617 484a 7105 cpc r23,__zero_reg__ 7618 484c 01F4 brne .+4 7619 484e 0C94 0000 jmp .L425 7620 4852 CB01 movw r24,r22 7621 4854 0197 sbiw r24,1 7622 4856 9093 0000 sts (Poti6)+1,r25 7623 485a 8093 0000 sts Poti6,r24 7624 485e BC01 movw r22,r24 7625 4860 0C94 0000 jmp .L425 7626 .L422: 7627 4864 8091 0000 lds r24,PPM_in+18 7628 4868 9091 0000 lds r25,(PPM_in+18)+1 7629 486c 8259 subi r24,lo8(-(110)) 7630 486e 9F4F sbci r25,hi8(-(110)) 7631 4870 8E17 cp r24,r30 7632 4872 9F07 cpc r25,r31 7633 4874 04F0 brlt .+4 7634 4876 0C94 0000 jmp .L423 7635 487a 3097 sbiw r30,0 7636 487c 01F4 brne .+4 7637 487e 0C94 0000 jmp .L423 7638 4882 CF01 movw r24,r30 7639 4884 0197 sbiw r24,1 7640 4886 9093 0000 sts (Poti5)+1,r25 7641 488a 8093 0000 sts Poti5,r24 7642 488e FC01 movw r30,r24 7643 4890 0C94 0000 jmp .L423 7644 .L420: 7645 4894 8081 ld r24,Z 7646 4896 9181 ldd r25,Z+1 7647 4898 8259 subi r24,lo8(-(110)) 7648 489a 9F4F sbci r25,hi8(-(110)) 7649 489c 8A17 cp r24,r26 7650 489e 9B07 cpc r25,r27 7651 48a0 04F0 brlt .+4 7652 48a2 0C94 0000 jmp .L421 7653 48a6 1097 sbiw r26,0 7654 48a8 01F4 brne .+4 7655 48aa 0C94 0000 jmp .L421 7656 48ae CD01 movw r24,r26 7657 48b0 0197 sbiw r24,1 7658 48b2 9093 0000 sts (Poti4)+1,r25 7659 48b6 8093 0000 sts Poti4,r24 7660 48ba DC01 movw r26,r24 7661 48bc 0C94 0000 jmp .L421 7662 .L418: 7663 48c0 8081 ld r24,Z 7664 48c2 9181 ldd r25,Z+1 7665 48c4 8259 subi r24,lo8(-(110)) 7666 48c6 9F4F sbci r25,hi8(-(110)) 7667 48c8 8A15 cp r24,r10 7668 48ca 9B05 cpc r25,r11 7669 48cc 04F0 brlt .+4 7670 48ce 0C94 0000 jmp .L419 7671 48d2 A114 cp r10,__zero_reg__ 7672 48d4 B104 cpc r11,__zero_reg__ 7673 48d6 01F4 brne .+4 7674 48d8 0C94 0000 jmp .L419 7675 48dc C501 movw r24,r10 7676 48de 0197 sbiw r24,1 7677 48e0 9093 0000 sts (Poti3)+1,r25 7678 48e4 8093 0000 sts Poti3,r24 7679 48e8 5C01 movw r10,r24 7680 48ea 0C94 0000 jmp .L419 7681 .L416: 7682 48ee 8081 ld r24,Z 7683 48f0 9181 ldd r25,Z+1 7684 48f2 8259 subi r24,lo8(-(110)) 7685 48f4 9F4F sbci r25,hi8(-(110)) 7686 48f6 8017 cp r24,r16 7687 48f8 9107 cpc r25,r17 7688 48fa 04F0 brlt .+4 7689 48fc 0C94 0000 jmp .L417 7690 4900 0115 cp r16,__zero_reg__ 7691 4902 1105 cpc r17,__zero_reg__ 7692 4904 01F4 brne .+4 7693 4906 0C94 0000 jmp .L417 7694 490a C801 movw r24,r16 7695 490c 0197 sbiw r24,1 7696 490e 9093 0000 sts (Poti2)+1,r25 7697 4912 8093 0000 sts Poti2,r24 7698 4916 8C01 movw r16,r24 7699 4918 0C94 0000 jmp .L417 7700 .L414: 7701 491c 8081 ld r24,Z 7702 491e 9181 ldd r25,Z+1 7703 4920 8259 subi r24,lo8(-(110)) 7704 4922 9F4F sbci r25,hi8(-(110)) 7705 4924 8E15 cp r24,r14 7706 4926 9F05 cpc r25,r15 7707 4928 04F0 brlt .+4 7708 492a 0C94 0000 jmp .L415 7709 492e E114 cp r14,__zero_reg__ 7710 4930 F104 cpc r15,__zero_reg__ 7711 4932 01F4 brne .+4 7712 4934 0C94 0000 jmp .L415 7713 4938 C701 movw r24,r14 7714 493a 0197 sbiw r24,1 7715 493c 9093 0000 sts (Poti1)+1,r25 7716 4940 8093 0000 sts Poti1,r24 7717 4944 7C01 movw r14,r24 7718 4946 0C94 0000 jmp .L415 7719 .L765: 7720 494a 4F96 adiw r24,31 7721 494c 0C94 0000 jmp .L586 7722 .L763: 7723 4950 8827 clr r24 7724 4952 9927 clr r25 7725 4954 8219 sub r24,r2 7726 4956 9309 sbc r25,r3 7727 4958 0C94 0000 jmp .L583 7728 .L761: 7729 495c 9094 com r9 7730 495e 8194 neg r8 7731 4960 9108 sbc r9,__zero_reg__ 7732 4962 9394 inc r9 7733 4964 0C94 0000 jmp .L578 7734 .L760: 7735 4968 1095 com r17 7736 496a 0195 neg r16 7737 496c 1F4F sbci r17,lo8(-1) 7738 496e 0C94 0000 jmp .L577 7739 .L477: 7740 4972 1092 0000 sts (StickYaw)+1,__zero_reg__ 7741 4976 1092 0000 sts StickYaw,__zero_reg__ 7742 497a 0C94 0000 jmp .L475 7743 .L776: 7744 497e 1092 0000 sts (Poti8)+1,__zero_reg__ 7745 4982 1092 0000 sts Poti8,__zero_reg__ 7746 4986 0C94 0000 jmp .L445 7747 .L775: 7748 498a 1092 0000 sts (Poti7)+1,__zero_reg__ 7749 498e 1092 0000 sts Poti7,__zero_reg__ 7750 4992 0C94 0000 jmp .L443 7751 .L774: 7752 4996 1092 0000 sts (Poti6)+1,__zero_reg__ 7753 499a 1092 0000 sts Poti6,__zero_reg__ 7754 499e 0C94 0000 jmp .L441 7755 .L773: 7756 49a2 1092 0000 sts (Poti5)+1,__zero_reg__ 7757 49a6 1092 0000 sts Poti5,__zero_reg__ 7758 49aa 0C94 0000 jmp .L439 7759 .L772: 7760 49ae 1092 0000 sts (Poti4)+1,__zero_reg__ 7761 49b2 1092 0000 sts Poti4,__zero_reg__ 7762 49b6 0C94 0000 jmp .L437 7763 .L771: 7764 49ba 1092 0000 sts (Poti3)+1,__zero_reg__ 7765 49be 1092 0000 sts Poti3,__zero_reg__ 7766 49c2 0C94 0000 jmp .L435 7767 .L770: 7768 49c6 1092 0000 sts (Poti2)+1,__zero_reg__ 7769 49ca 1092 0000 sts Poti2,__zero_reg__ 7770 49ce 0C94 0000 jmp .L433 7771 .L769: 7772 49d2 1092 0000 sts (Poti1)+1,__zero_reg__ 7773 49d6 1092 0000 sts Poti1,__zero_reg__ 7774 49da 0C94 0000 jmp .L431 7775 .L590: 7776 49de 4427 clr r20 7777 49e0 5527 clr r21 7778 49e2 481B sub r20,r24 7779 49e4 590B sbc r21,r25 7780 49e6 B901 movw r22,r18 7781 49e8 2417 cp r18,r20 7782 49ea 3507 cpc r19,r21 7783 49ec 04F0 brlt .+4 7784 49ee 0C94 0000 jmp .L591 7785 49f2 BA01 movw r22,r20 7786 49f4 0C94 0000 jmp .L591 7787 .L743: 7788 49f8 3095 com r19 7789 49fa 2195 neg r18 7790 49fc 3F4F sbci r19,lo8(-1) 7791 49fe 0C94 0000 jmp .L489 7792 .L789: 7793 4a02 3095 com r19 7794 4a04 2195 neg r18 7795 4a06 3F4F sbci r19,lo8(-1) 7796 4a08 00C0 rjmp .L484 7797 .L520: 7798 4a0a 8A15 cp r24,r10 7799 4a0c 9B05 cpc r25,r11 7800 4a0e AC05 cpc r26,r12 7801 4a10 BD05 cpc r27,r13 7802 4a12 04F0 brlt .+2 7803 4a14 00C0 rjmp .L521 7804 4a16 5C01 movw r10,r24 7805 4a18 6D01 movw r12,r26 7806 4a1a 00C0 rjmp .L521 7807 .L517: 7808 4a1c 8401 movw r16,r8 7809 4a1e 7301 movw r14,r6 7810 4a20 8615 cp r24,r6 7811 4a22 9705 cpc r25,r7 7812 4a24 A805 cpc r26,r8 7813 4a26 B905 cpc r27,r9 7814 4a28 04F0 brlt .+2 7815 4a2a 00C0 rjmp .L518 7816 4a2c 7C01 movw r14,r24 7817 4a2e 8D01 movw r16,r26 7818 4a30 00C0 rjmp .L518 7819 .L611: 7820 4a32 8091 0000 lds r24,BaroFlags.2450 7821 4a36 90E0 ldi r25,lo8(0) 7822 4a38 8370 andi r24,lo8(3) 7823 4a3a 9070 andi r25,hi8(3) 7824 4a3c 892B or r24,r25 7825 4a3e 01F4 brne .+2 7826 4a40 00C0 rjmp .L798 7827 .L614: 7828 4a42 81E0 ldi r24,lo8(1) 7829 4a44 8093 0000 sts HCActive.2443,r24 7830 .L613: 7831 4a48 E090 0000 lds r14,ParamSet+23 7832 4a4c 8AE2 ldi r24,lo8(42) 7833 4a4e E89E mul r14,r24 7834 4a50 7001 movw r14,r0 7835 4a52 1124 clr r1 7836 4a54 0027 clr r16 7837 4a56 F7FC sbrc r15,7 7838 4a58 0095 com r16 7839 4a5a 102F mov r17,r16 7840 4a5c 6091 0000 lds r22,IntegralGyroNick 7841 4a60 7091 0000 lds r23,(IntegralGyroNick)+1 7842 4a64 8091 0000 lds r24,(IntegralGyroNick)+2 7843 4a68 9091 0000 lds r25,(IntegralGyroNick)+3 7844 4a6c A801 movw r20,r16 7845 4a6e 9701 movw r18,r14 7846 4a70 0E94 0000 call __divmodsi4 7847 4a74 5901 movw r10,r18 7848 4a76 6A01 movw r12,r20 7849 4a78 6091 0000 lds r22,IntegralGyroRoll 7850 4a7c 7091 0000 lds r23,(IntegralGyroRoll)+1 7851 4a80 8091 0000 lds r24,(IntegralGyroRoll)+2 7852 4a84 9091 0000 lds r25,(IntegralGyroRoll)+3 7853 4a88 A801 movw r20,r16 7854 4a8a 9701 movw r18,r14 7855 4a8c 0E94 0000 call __divmodsi4 7856 4a90 C501 movw r24,r10 7857 4a92 B901 movw r22,r18 7858 4a94 0E94 0000 call ihypot 7859 4a98 8D33 cpi r24,61 7860 4a9a 9105 cpc r25,__zero_reg__ 7861 4a9c 04F0 brlt .L615 7862 4a9e 8CE3 ldi r24,lo8(60) 7863 4aa0 90E0 ldi r25,hi8(60) 7864 .L615: 7865 4aa2 0E94 0000 call c_cos_8192 7866 4aa6 9A83 std Y+2,r25 7867 4aa8 8983 std Y+1,r24 7868 4aaa 8091 0000 lds r24,HCActive.2443 7869 4aae 8823 tst r24 7870 4ab0 01F4 brne .+2 7871 4ab2 00C0 rjmp .L616 7872 4ab4 8091 0000 lds r24,MKFlags 7873 4ab8 84FD sbrc r24,4 7874 4aba 00C0 rjmp .L616 7875 4abc 8091 0000 lds r24,ParamSet+88 7876 4ac0 80FD sbrc r24,0 7877 4ac2 00C0 rjmp .L617 7878 4ac4 8091 0000 lds r24,ParamSet+8 7879 4ac8 81FF sbrs r24,1 7880 4aca 00C0 rjmp .L617 7881 4acc 8091 0000 lds r24,MKFlags 7882 4ad0 81FF sbrs r24,1 7883 4ad2 00C0 rjmp .L620 7884 4ad4 6091 0000 lds r22,StickGasHover.2444 7885 4ad8 7091 0000 lds r23,(StickGasHover.2444)+1 7886 4adc 4091 0000 lds r20,StickGas 7887 4ae0 5091 0000 lds r21,(StickGas)+1 7888 4ae4 CB01 movw r24,r22 7889 4ae6 0F96 adiw r24,15 7890 4ae8 8417 cp r24,r20 7891 4aea 9507 cpc r25,r21 7892 4aec 04F0 brlt .+2 7893 4aee 00C0 rjmp .L621 7894 4af0 8091 0000 lds r24,BaroFlags.2450 7895 4af4 80FD sbrc r24,0 7896 4af6 00C0 rjmp .L621 7897 4af8 2091 0000 lds r18,HeightTrimmingFlag.2452 7898 4afc 21FF sbrs r18,1 7899 4afe 00C0 rjmp .L622 7900 4b00 2D7F andi r18,lo8(-3) 7901 4b02 2093 0000 sts HeightTrimmingFlag.2452,r18 7902 4b06 8091 0000 lds r24,ReadingHeight 7903 4b0a 9091 0000 lds r25,(ReadingHeight)+1 7904 4b0e A091 0000 lds r26,(ReadingHeight)+2 7905 4b12 B091 0000 lds r27,(ReadingHeight)+3 7906 4b16 8093 0000 sts SetPointHeight,r24 7907 4b1a 9093 0000 sts (SetPointHeight)+1,r25 7908 4b1e A093 0000 sts (SetPointHeight)+2,r26 7909 4b22 B093 0000 sts (SetPointHeight)+3,r27 7910 .L622: 7911 4b26 2160 ori r18,lo8(1) 7912 4b28 2093 0000 sts HeightTrimmingFlag.2452,r18 7913 4b2c CA01 movw r24,r20 7914 4b2e 0F97 sbiw r24,15 7915 4b30 861B sub r24,r22 7916 4b32 970B sbc r25,r23 7917 4b34 97FD sbrc r25,7 7918 4b36 00C0 rjmp .L799 7919 .L623: 7920 4b38 2091 0000 lds r18,HeightTrimming.2442 7921 4b3c 3091 0000 lds r19,(HeightTrimming.2442)+1 7922 4b40 280F add r18,r24 7923 4b42 391F adc r19,r25 7924 4b44 3093 0000 sts (HeightTrimming.2442)+1,r19 7925 4b48 2093 0000 sts HeightTrimming.2442,r18 7926 .L624: 7927 4b4c C901 movw r24,r18 7928 4b4e 37FF sbrs r19,7 7929 4b50 00C0 rjmp .L629 7930 4b52 8827 clr r24 7931 4b54 9927 clr r25 7932 4b56 821B sub r24,r18 7933 4b58 930B sbc r25,r19 7934 .L629: 7935 4b5a 8150 subi r24,lo8(513) 7936 4b5c 9240 sbci r25,hi8(513) 7937 4b5e 04F4 brge .+2 7938 4b60 00C0 rjmp .L628 7939 4b62 8091 0000 lds r24,ParamSet+13 7940 4b66 90E0 ldi r25,lo8(0) 7941 4b68 5C01 movw r10,r24 7942 4b6a A29E mul r10,r18 7943 4b6c C001 movw r24,r0 7944 4b6e A39E mul r10,r19 7945 4b70 900D add r25,r0 7946 4b72 B29E mul r11,r18 7947 4b74 900D add r25,r0 7948 4b76 1124 clr r1 7949 4b78 60E0 ldi r22,lo8(1280) 7950 4b7a 75E0 ldi r23,hi8(1280) 7951 4b7c 0E94 0000 call __divmodhi4 7952 4b80 8827 clr r24 7953 4b82 77FD sbrc r23,7 7954 4b84 8095 com r24 7955 4b86 982F mov r25,r24 7956 4b88 2091 0000 lds r18,SetPointHeight 7957 4b8c 3091 0000 lds r19,(SetPointHeight)+1 7958 4b90 4091 0000 lds r20,(SetPointHeight)+2 7959 4b94 5091 0000 lds r21,(SetPointHeight)+3 7960 4b98 260F add r18,r22 7961 4b9a 371F adc r19,r23 7962 4b9c 481F adc r20,r24 7963 4b9e 591F adc r21,r25 7964 4ba0 2093 0000 sts SetPointHeight,r18 7965 4ba4 3093 0000 sts (SetPointHeight)+1,r19 7966 4ba8 4093 0000 sts (SetPointHeight)+2,r20 7967 4bac 5093 0000 sts (SetPointHeight)+3,r21 7968 4bb0 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ 7969 4bb4 1092 0000 sts HeightTrimming.2442,__zero_reg__ 7970 4bb8 8091 0000 lds r24,ParamSet+88 7971 4bbc 81FF sbrs r24,1 7972 4bbe 00C0 rjmp .L630 7973 4bc0 8BE4 ldi r24,lo8(75) 7974 4bc2 90E0 ldi r25,hi8(75) 7975 4bc4 9093 0000 sts (BeepTime)+1,r25 7976 4bc8 8093 0000 sts BeepTime,r24 7977 .L630: 7978 4bcc 8091 0000 lds r24,ParamSet+17 7979 4bd0 8823 tst r24 7980 4bd2 01F4 brne .L628 7981 4bd4 8091 0000 lds r24,HoverGas.2445 7982 4bd8 9091 0000 lds r25,HoverGas.2445+1 7983 4bdc 9C01 movw r18,r24 7984 4bde 37FD sbrc r19,7 7985 4be0 00C0 rjmp .L800 7986 .L631: 7987 4be2 3595 asr r19 7988 4be4 2795 ror r18 7989 4be6 3595 asr r19 7990 4be8 2795 ror r18 7991 4bea 3093 0000 sts (StickGasHover.2444)+1,r19 7992 4bee 2093 0000 sts StickGasHover.2444,r18 7993 4bf2 8091 0000 lds r24,UBat 7994 4bf6 9091 0000 lds r25,(UBat)+1 7995 4bfa 6091 0000 lds r22,LowVoltageWarning 7996 4bfe 6C01 movw r12,r24 7997 4c00 2C9D mul r18,r12 7998 4c02 C001 movw r24,r0 7999 4c04 2D9D mul r18,r13 8000 4c06 900D add r25,r0 8001 4c08 3C9D mul r19,r12 8002 4c0a 900D add r25,r0 8003 4c0c 1124 clr r1 8004 4c0e 70E0 ldi r23,lo8(0) 8005 4c10 0E94 0000 call __divmodhi4 8006 4c14 7093 0000 sts (StickGasHover.2444)+1,r23 8007 4c18 6093 0000 sts StickGasHover.2444,r22 8008 4c1c 6634 cpi r22,70 8009 4c1e 7105 cpc r23,__zero_reg__ 8010 4c20 04F0 brlt .+2 8011 4c22 00C0 rjmp .L632 8012 4c24 86E4 ldi r24,lo8(70) 8013 4c26 90E0 ldi r25,hi8(70) 8014 4c28 9093 0000 sts (StickGasHover.2444)+1,r25 8015 4c2c 8093 0000 sts StickGasHover.2444,r24 8016 .L628: 8017 4c30 8091 0000 lds r24,BaroExpandActive.2451 8018 4c34 9091 0000 lds r25,(BaroExpandActive.2451)+1 8019 4c38 892B or r24,r25 8020 4c3a 01F0 breq .L633 8021 4c3c 8091 0000 lds r24,ReadingHeight 8022 4c40 9091 0000 lds r25,(ReadingHeight)+1 8023 4c44 A091 0000 lds r26,(ReadingHeight)+2 8024 4c48 B091 0000 lds r27,(ReadingHeight)+3 8025 4c4c 8093 0000 sts SetPointHeight,r24 8026 4c50 9093 0000 sts (SetPointHeight)+1,r25 8027 4c54 A093 0000 sts (SetPointHeight)+2,r26 8028 4c58 B093 0000 sts (SetPointHeight)+3,r27 8029 .L633: 8030 4c5c 0091 0000 lds r16,HoverGas.2445 8031 4c60 1091 0000 lds r17,(HoverGas.2445)+1 8032 .L619: 8033 4c64 8091 0000 lds r24,ReadingHeight 8034 4c68 9091 0000 lds r25,(ReadingHeight)+1 8035 4c6c A091 0000 lds r26,(ReadingHeight)+2 8036 4c70 B091 0000 lds r27,(ReadingHeight)+3 8037 4c74 2091 0000 lds r18,SetPointHeight 8038 4c78 3091 0000 lds r19,(SetPointHeight)+1 8039 4c7c 4091 0000 lds r20,(SetPointHeight)+2 8040 4c80 5091 0000 lds r21,(SetPointHeight)+3 8041 4c84 2817 cp r18,r24 8042 4c86 3907 cpc r19,r25 8043 4c88 4A07 cpc r20,r26 8044 4c8a 5B07 cpc r21,r27 8045 4c8c 04F0 brlt .L635 8046 4c8e 8091 0000 lds r24,ParamSet+88 8047 4c92 80FD sbrc r24,0 8048 4c94 00C0 rjmp .L636 8049 .L635: 8050 4c96 8091 0000 lds r24,BaroExpandActive.2451 8051 4c9a 9091 0000 lds r25,(BaroExpandActive.2451)+1 8052 4c9e 892B or r24,r25 8053 4ca0 01F4 brne .+2 8054 4ca2 00C0 rjmp .L637 8055 4ca4 0091 0000 lds r16,HoverGas.2445 8056 4ca8 1091 0000 lds r17,(HoverGas.2445)+1 8057 .L638: 8058 4cac 6091 0000 lds r22,NCGpsZ 8059 4cb0 7727 clr r23 8060 4cb2 67FD sbrc r22,7 8061 4cb4 7095 com r23 8062 4cb6 872F mov r24,r23 8063 4cb8 972F mov r25,r23 8064 4cba 2091 0000 lds r18,ParamSet+16 8065 4cbe 30E0 ldi r19,lo8(0) 8066 4cc0 40E0 ldi r20,lo8(0) 8067 4cc2 50E0 ldi r21,hi8(0) 8068 4cc4 0E94 0000 call __mulsi3 8069 4cc8 20E8 ldi r18,lo8(128) 8070 4cca 30E0 ldi r19,hi8(128) 8071 4ccc 40E0 ldi r20,hlo8(128) 8072 4cce 50E0 ldi r21,hhi8(128) 8073 4cd0 0E94 0000 call __divmodsi4 8074 4cd4 A901 movw r20,r18 8075 4cd6 C901 movw r24,r18 8076 4cd8 2FEF ldi r18,hi8(-127) 8077 4cda 4138 cpi r20,lo8(-127) 8078 4cdc 5207 cpc r21,r18 8079 4cde 04F4 brge .L652 8080 4ce0 81E8 ldi r24,lo8(-127) 8081 4ce2 9FEF ldi r25,hi8(-127) 8082 .L652: 8083 4ce4 081B sub r16,r24 8084 4ce6 190B sbc r17,r25 8085 4ce8 B801 movw r22,r16 8086 4cea 8827 clr r24 8087 4cec 77FD sbrc r23,7 8088 4cee 8095 com r24 8089 4cf0 982F mov r25,r24 8090 4cf2 EDE0 ldi r30,13 8091 4cf4 660F 1: lsl r22 8092 4cf6 771F rol r23 8093 4cf8 881F rol r24 8094 4cfa 991F rol r25 8095 4cfc EA95 dec r30 8096 4cfe 01F4 brne 1b 8097 4d00 A981 ldd r26,Y+1 8098 4d02 BA81 ldd r27,Y+2 8099 4d04 9D01 movw r18,r26 8100 4d06 4427 clr r20 8101 4d08 37FD sbrc r19,7 8102 4d0a 4095 com r20 8103 4d0c 542F mov r21,r20 8104 4d0e 0E94 0000 call __divmodsi4 8105 4d12 8091 0000 lds r24,FilterHCGas.2441 8106 4d16 9091 0000 lds r25,(FilterHCGas.2441)+1 8107 4d1a AC01 movw r20,r24 8108 4d1c 440F lsl r20 8109 4d1e 551F rol r21 8110 4d20 480F add r20,r24 8111 4d22 591F adc r21,r25 8112 4d24 420F add r20,r18 8113 4d26 531F adc r21,r19 8114 4d28 57FD sbrc r21,7 8115 4d2a 00C0 rjmp .L801 8116 .L653: 8117 4d2c 5595 asr r21 8118 4d2e 4795 ror r20 8119 4d30 5595 asr r21 8120 4d32 4795 ror r20 8121 4d34 5093 0000 sts (FilterHCGas.2441)+1,r21 8122 4d38 4093 0000 sts FilterHCGas.2441,r20 8123 4d3c 8091 0000 lds r24,ParamSet+9 8124 4d40 282F mov r18,r24 8125 4d42 30E0 ldi r19,lo8(0) 8126 4d44 220F lsl r18 8127 4d46 331F rol r19 8128 4d48 220F lsl r18 8129 4d4a 331F rol r19 8130 4d4c 4217 cp r20,r18 8131 4d4e 5307 cpc r21,r19 8132 4d50 04F0 brlt .L731 8133 4d52 8091 0000 lds r24,ParamSet+22 8134 4d56 282F mov r18,r24 8135 4d58 30E0 ldi r19,lo8(0) 8136 4d5a 2451 subi r18,lo8(-(-20)) 8137 4d5c 3040 sbci r19,hi8(-(-20)) 8138 4d5e 220F lsl r18 8139 4d60 331F rol r19 8140 4d62 220F lsl r18 8141 4d64 331F rol r19 8142 4d66 2417 cp r18,r20 8143 4d68 3507 cpc r19,r21 8144 4d6a 04F4 brge .L655 8145 .L731: 8146 4d6c 3093 0000 sts (FilterHCGas.2441)+1,r19 8147 4d70 2093 0000 sts FilterHCGas.2441,r18 8148 .L655: 8149 4d74 8091 0000 lds r24,ParamSet+88 8150 4d78 80FD sbrc r24,0 8151 4d7a 00C0 rjmp .L725 8152 4d7c 8091 0000 lds r24,FilterHCGas.2441 8153 4d80 9091 0000 lds r25,(FilterHCGas.2441)+1 8154 .L656: 8155 4d84 99AF std Y+57,r25 8156 4d86 88AF std Y+56,r24 8157 4d88 00C0 rjmp .L636 8158 .L616: 8159 4d8a 8091 0000 lds r24,ParamSet+17 8160 4d8e 8823 tst r24 8161 4d90 01F0 breq .+2 8162 4d92 00C0 rjmp .L802 8163 4d94 8091 0000 lds r24,HoverGas.2445 8164 4d98 9091 0000 lds r25,HoverGas.2445+1 8165 4d9c 9C01 movw r18,r24 8166 4d9e 37FD sbrc r19,7 8167 4da0 00C0 rjmp .L803 8168 .L659: 8169 4da2 3595 asr r19 8170 4da4 2795 ror r18 8171 4da6 3595 asr r19 8172 4da8 2795 ror r18 8173 4daa 3093 0000 sts (StickGasHover.2444)+1,r19 8174 4dae 2093 0000 sts StickGasHover.2444,r18 8175 4db2 8091 0000 lds r24,UBat 8176 4db6 9091 0000 lds r25,(UBat)+1 8177 4dba 6091 0000 lds r22,LowVoltageWarning 8178 4dbe 5C01 movw r10,r24 8179 4dc0 2A9D mul r18,r10 8180 4dc2 C001 movw r24,r0 8181 4dc4 2B9D mul r18,r11 8182 4dc6 900D add r25,r0 8183 4dc8 3A9D mul r19,r10 8184 4dca 900D add r25,r0 8185 4dcc 1124 clr r1 8186 4dce 70E0 ldi r23,lo8(0) 8187 4dd0 0E94 0000 call __divmodhi4 8188 4dd4 7093 0000 sts (StickGasHover.2444)+1,r23 8189 4dd8 6093 0000 sts StickGasHover.2444,r22 8190 .L658: 8191 4ddc 8091 0000 lds r24,StickGasHover.2444 8192 4de0 9091 0000 lds r25,(StickGasHover.2444)+1 8193 4de4 8634 cpi r24,70 8194 4de6 9105 cpc r25,__zero_reg__ 8195 4de8 04F0 brlt .+2 8196 4dea 00C0 rjmp .L660 8197 4dec 86E4 ldi r24,lo8(70) 8198 4dee 90E0 ldi r25,hi8(70) 8199 4df0 9093 0000 sts (StickGasHover.2444)+1,r25 8200 4df4 8093 0000 sts StickGasHover.2444,r24 8201 .L661: 8202 4df8 C8AC ldd r12,Y+56 8203 4dfa D9AC ldd r13,Y+57 8204 4dfc D092 0000 sts (FilterHCGas.2441)+1,r13 8205 4e00 C092 0000 sts FilterHCGas.2441,r12 8206 .L636: 8207 4e04 8091 0000 lds r24,MKFlags 8208 4e08 81FD sbrc r24,1 8209 4e0a 00C0 rjmp .+4 8210 4e0c 0C94 0000 jmp .L604 8211 4e10 8091 0000 lds r24,MKFlags 8212 4e14 84FF sbrs r24,4 8213 4e16 00C0 rjmp .+4 8214 4e18 0C94 0000 jmp .L604 8215 4e1c 8091 0000 lds r24,HoverGasFilter.2448 8216 4e20 9091 0000 lds r25,(HoverGasFilter.2448)+1 8217 4e24 A091 0000 lds r26,(HoverGasFilter.2448)+2 8218 4e28 B091 0000 lds r27,(HoverGasFilter.2448)+3 8219 4e2c 0097 sbiw r24,0 8220 4e2e A105 cpc r26,__zero_reg__ 8221 4e30 B105 cpc r27,__zero_reg__ 8222 4e32 01F4 brne .L662 8223 4e34 28AD ldd r18,Y+56 8224 4e36 39AD ldd r19,Y+57 8225 4e38 C901 movw r24,r18 8226 4e3a AA27 clr r26 8227 4e3c 97FD sbrc r25,7 8228 4e3e A095 com r26 8229 4e40 BA2F mov r27,r26 8230 4e42 1CE0 ldi r17,12 8231 4e44 880F 1: lsl r24 8232 4e46 991F rol r25 8233 4e48 AA1F rol r26 8234 4e4a BB1F rol r27 8235 4e4c 1A95 dec r17 8236 4e4e 01F4 brne 1b 8237 4e50 8093 0000 sts HoverGasFilter.2448,r24 8238 4e54 9093 0000 sts (HoverGasFilter.2448)+1,r25 8239 4e58 A093 0000 sts (HoverGasFilter.2448)+2,r26 8240 4e5c B093 0000 sts (HoverGasFilter.2448)+3,r27 8241 .L662: 8242 4e60 8091 0000 lds r24,ReadingVario 8243 4e64 9091 0000 lds r25,(ReadingVario)+1 8244 4e68 97FD sbrc r25,7 8245 4e6a 00C0 rjmp .L804 8246 .L663: 8247 4e6c 8436 cpi r24,100 8248 4e6e 9105 cpc r25,__zero_reg__ 8249 4e70 04F0 brlt .+4 8250 4e72 0C94 0000 jmp .L604 8251 4e76 48AD ldd r20,Y+56 8252 4e78 59AD ldd r21,Y+57 8253 4e7a BA01 movw r22,r20 8254 4e7c 8827 clr r24 8255 4e7e 77FD sbrc r23,7 8256 4e80 8095 com r24 8257 4e82 982F mov r25,r24 8258 4e84 A981 ldd r26,Y+1 8259 4e86 BA81 ldd r27,Y+2 8260 4e88 9D01 movw r18,r26 8261 4e8a 4427 clr r20 8262 4e8c 37FD sbrc r19,7 8263 4e8e 4095 com r20 8264 4e90 542F mov r21,r20 8265 4e92 0E94 0000 call __mulsi3 8266 4e96 20E0 ldi r18,lo8(8192) 8267 4e98 30E2 ldi r19,hi8(8192) 8268 4e9a 40E0 ldi r20,hlo8(8192) 8269 4e9c 50E0 ldi r21,hhi8(8192) 8270 4e9e 0E94 0000 call __divmodsi4 8271 4ea2 7901 movw r14,r18 8272 4ea4 8A01 movw r16,r20 8273 4ea6 8091 0000 lds r24,ModelIsFlying 8274 4eaa 9091 0000 lds r25,(ModelIsFlying)+1 8275 4eae B7E0 ldi r27,hi8(2000) 8276 4eb0 803D cpi r24,lo8(2000) 8277 4eb2 9B07 cpc r25,r27 8278 4eb4 00F0 brlo .+2 8279 4eb6 00C0 rjmp .L664 8280 4eb8 2091 0000 lds r18,HoverGasFilter.2448 8281 4ebc 3091 0000 lds r19,(HoverGasFilter.2448)+1 8282 4ec0 4091 0000 lds r20,(HoverGasFilter.2448)+2 8283 4ec4 5091 0000 lds r21,(HoverGasFilter.2448)+3 8284 4ec8 DA01 movw r26,r20 8285 4eca C901 movw r24,r18 8286 4ecc F9E0 ldi r31,9 8287 4ece B695 1: lsr r27 8288 4ed0 A795 ror r26 8289 4ed2 9795 ror r25 8290 4ed4 8795 ror r24 8291 4ed6 FA95 dec r31 8292 4ed8 01F4 brne 1b 8293 4eda 5901 movw r10,r18 8294 4edc 6A01 movw r12,r20 8295 4ede A81A sub r10,r24 8296 4ee0 B90A sbc r11,r25 8297 4ee2 CA0A sbc r12,r26 8298 4ee4 DB0A sbc r13,r27 8299 4ee6 A801 movw r20,r16 8300 4ee8 9701 movw r18,r14 8301 4eea 220F lsl r18 8302 4eec 331F rol r19 8303 4eee 441F rol r20 8304 4ef0 551F rol r21 8305 4ef2 220F lsl r18 8306 4ef4 331F rol r19 8307 4ef6 441F rol r20 8308 4ef8 551F rol r21 8309 4efa 220F lsl r18 8310 4efc 331F rol r19 8311 4efe 441F rol r20 8312 4f00 551F rol r21 8313 4f02 2A0D add r18,r10 8314 4f04 3B1D adc r19,r11 8315 4f06 4C1D adc r20,r12 8316 4f08 5D1D adc r21,r13 8317 4f0a 2093 0000 sts HoverGasFilter.2448,r18 8318 4f0e 3093 0000 sts (HoverGasFilter.2448)+1,r19 8319 4f12 4093 0000 sts (HoverGasFilter.2448)+2,r20 8320 4f16 5093 0000 sts (HoverGasFilter.2448)+3,r21 8321 .L665: 8322 4f1a DA01 movw r26,r20 8323 4f1c C901 movw r24,r18 8324 4f1e 2CE0 ldi r18,12 8325 4f20 B695 1: lsr r27 8326 4f22 A795 ror r26 8327 4f24 9795 ror r25 8328 4f26 8795 ror r24 8329 4f28 2A95 dec r18 8330 4f2a 01F4 brne 1b 8331 4f2c FC01 movw r30,r24 8332 4f2e 9093 0000 sts (HoverGas.2445)+1,r25 8333 4f32 8093 0000 sts HoverGas.2445,r24 8334 4f36 6091 0000 lds r22,ParamSet+15 8335 4f3a 6623 tst r22 8336 4f3c 01F4 brne .+2 8337 4f3e 00C0 rjmp .L668 8338 4f40 70E0 ldi r23,lo8(0) 8339 4f42 0E94 0000 call __divmodhi4 8340 4f46 CB01 movw r24,r22 8341 4f48 9F01 movw r18,r30 8342 4f4a 261B sub r18,r22 8343 4f4c 370B sbc r19,r23 8344 4f4e 3093 0000 sts (HoverGasMin.2446)+1,r19 8345 4f52 2093 0000 sts HoverGasMin.2446,r18 8346 4f56 8E0F add r24,r30 8347 4f58 9F1F adc r25,r31 8348 4f5a 9093 0000 sts (HoverGasMax.2447)+1,r25 8349 4f5e 8093 0000 sts HoverGasMax.2447,r24 8350 4f62 0C94 0000 jmp .L604 8351 .L605: 8352 4f66 8091 0000 lds r24,AdAirPressure 8353 4f6a 9091 0000 lds r25,(AdAirPressure)+1 8354 4f6e 8C59 subi r24,lo8(924) 8355 4f70 9340 sbci r25,hi8(924) 8356 4f72 04F0 brlt .+2 8357 4f74 00C0 rjmp .L805 8358 4f76 8091 0000 lds r24,AdAirPressure 8359 4f7a 9091 0000 lds r25,(AdAirPressure)+1 8360 4f7e 8436 cpi r24,100 8361 4f80 9105 cpc r25,__zero_reg__ 8362 4f82 04F0 brlt .+2 8363 4f84 00C0 rjmp .L609 8364 4f86 87B5 in r24,71-32 8365 4f88 8B30 cpi r24,lo8(11) 8366 4f8a 00F4 brsh .+2 8367 4f8c 00C0 rjmp .L610 8368 4f8e 8091 0000 lds r24,ExpandBaro 8369 4f92 8F5F subi r24,lo8(-(1)) 8370 .L729: 8371 4f94 8093 0000 sts ExpandBaro,r24 8372 4f98 26EF ldi r18,lo8(-10) 8373 4f9a 8202 muls r24,r18 8374 4f9c C001 movw r24,r0 8375 4f9e 1124 clr r1 8376 4fa0 2091 0000 lds r18,PressureSensorOffset 8377 4fa4 280F add r18,r24 8378 4fa6 27BD out 71-32,r18 8379 4fa8 8CE2 ldi r24,lo8(300) 8380 4faa 91E0 ldi r25,hi8(300) 8381 4fac 9093 0000 sts (BeepTime)+1,r25 8382 4fb0 8093 0000 sts BeepTime,r24 8383 4fb4 8EE5 ldi r24,lo8(350) 8384 4fb6 91E0 ldi r25,hi8(350) 8385 4fb8 9093 0000 sts (BaroExpandActive.2451)+1,r25 8386 4fbc 8093 0000 sts BaroExpandActive.2451,r24 8387 4fc0 0C94 0000 jmp .L606 8388 .L470: 8389 4fc4 1092 0000 sts delay_stopmotors.2418,__zero_reg__ 8390 4fc8 0C94 0000 jmp .L410 8391 .L512: 8392 4fcc E090 0000 lds r14,ParamSet+23 8393 4fd0 FF24 clr r15 8394 4fd2 00E0 ldi r16,lo8(0) 8395 4fd4 10E0 ldi r17,hi8(0) 8396 4fd6 68A5 ldd r22,Y+40 8397 4fd8 79A5 ldd r23,Y+41 8398 4fda 8AA5 ldd r24,Y+42 8399 4fdc 9BA5 ldd r25,Y+43 8400 4fde A801 movw r20,r16 8401 4fe0 9701 movw r18,r14 8402 4fe2 0E94 0000 call __divmodsi4 8403 4fe6 8091 0000 lds r24,AccNick 8404 4fea 9091 0000 lds r25,(AccNick)+1 8405 4fee AA27 clr r26 8406 4ff0 97FD sbrc r25,7 8407 4ff2 A095 com r26 8408 4ff4 BA2F mov r27,r26 8409 4ff6 281B sub r18,r24 8410 4ff8 390B sbc r19,r25 8411 4ffa 4A0B sbc r20,r26 8412 4ffc 5B0B sbc r21,r27 8413 4ffe CA01 movw r24,r20 8414 5000 B901 movw r22,r18 8415 5002 20E1 ldi r18,lo8(16) 8416 5004 30E0 ldi r19,hi8(16) 8417 5006 40E0 ldi r20,hlo8(16) 8418 5008 50E0 ldi r21,hhi8(16) 8419 500a 0E94 0000 call __divmodsi4 8420 500e 522E mov r5,r18 8421 5010 432E mov r4,r19 8422 5012 342E mov r3,r20 8423 5014 252E mov r2,r21 8424 5016 4201 movw r8,r4 8425 5018 3101 movw r6,r2 8426 501a 6CA1 ldd r22,Y+36 8427 501c 7DA1 ldd r23,Y+37 8428 501e 8EA1 ldd r24,Y+38 8429 5020 9FA1 ldd r25,Y+39 8430 5022 A801 movw r20,r16 8431 5024 9701 movw r18,r14 8432 5026 0E94 0000 call __divmodsi4 8433 502a 8091 0000 lds r24,AccRoll 8434 502e 9091 0000 lds r25,(AccRoll)+1 8435 5032 AA27 clr r26 8436 5034 97FD sbrc r25,7 8437 5036 A095 com r26 8438 5038 BA2F mov r27,r26 8439 503a 281B sub r18,r24 8440 503c 390B sbc r19,r25 8441 503e 4A0B sbc r20,r26 8442 5040 5B0B sbc r21,r27 8443 5042 CA01 movw r24,r20 8444 5044 B901 movw r22,r18 8445 5046 20E1 ldi r18,lo8(16) 8446 5048 30E0 ldi r19,hi8(16) 8447 504a 40E0 ldi r20,hlo8(16) 8448 504c 50E0 ldi r21,hhi8(16) 8449 504e 0E94 0000 call __divmodsi4 8450 5052 122F mov r17,r18 8451 5054 032F mov r16,r19 8452 5056 F42E mov r15,r20 8453 5058 E52E mov r14,r21 8454 505a 822F mov r24,r18 8455 505c 932F mov r25,r19 8456 505e A42F mov r26,r20 8457 5060 B52F mov r27,r21 8458 5062 5C01 movw r10,r24 8459 5064 6D01 movw r12,r26 8460 5066 8091 0000 lds r24,MaxStickNick 8461 506a 9091 0000 lds r25,(MaxStickNick)+1 8462 506e 8134 cpi r24,65 8463 5070 9105 cpc r25,__zero_reg__ 8464 5072 04F4 brge .+2 8465 5074 00C0 rjmp .L806 8466 .L522: 8467 5076 652D mov r22,r5 8468 5078 742D mov r23,r4 8469 507a 832D mov r24,r3 8470 507c 922D mov r25,r2 8471 507e 23E0 ldi r18,lo8(3) 8472 5080 30E0 ldi r19,hi8(3) 8473 5082 40E0 ldi r20,hlo8(3) 8474 5084 50E0 ldi r21,hhi8(3) 8475 5086 0E94 0000 call __divmodsi4 8476 508a 922E mov r9,r18 8477 508c 832E mov r8,r19 8478 508e 742E mov r7,r20 8479 5090 652E mov r6,r21 8480 5092 612F mov r22,r17 8481 5094 702F mov r23,r16 8482 5096 8F2D mov r24,r15 8483 5098 9E2D mov r25,r14 8484 509a 23E0 ldi r18,lo8(3) 8485 509c 30E0 ldi r19,hi8(3) 8486 509e 40E0 ldi r20,hlo8(3) 8487 50a0 50E0 ldi r21,hhi8(3) 8488 50a2 0E94 0000 call __divmodsi4 8489 50a6 C901 movw r24,r18 8490 50a8 DA01 movw r26,r20 8491 50aa 5C01 movw r10,r24 8492 50ac 6D01 movw r12,r26 8493 .L523: 8494 50ae E091 0000 lds r30,ParamSet+3 8495 50b2 F0E0 ldi r31,lo8(0) 8496 50b4 EE0F lsl r30 8497 50b6 FF1F rol r31 8498 50b8 E050 subi r30,lo8(-(PPM_in)) 8499 50ba F040 sbci r31,hi8(-(PPM_in)) 8500 50bc 0190 ld __tmp_reg__,Z+ 8501 50be F081 ld r31,Z 8502 50c0 E02D mov r30,__tmp_reg__ 8503 50c2 F7FD sbrc r31,7 8504 50c4 00C0 rjmp .L807 8505 .L525: 8506 50c6 7A97 sbiw r30,26 8507 50c8 04F0 brlt .L524 8508 50ca 692D mov r22,r9 8509 50cc 782D mov r23,r8 8510 50ce 872D mov r24,r7 8511 50d0 962D mov r25,r6 8512 50d2 23E0 ldi r18,lo8(3) 8513 50d4 30E0 ldi r19,hi8(3) 8514 50d6 40E0 ldi r20,hlo8(3) 8515 50d8 50E0 ldi r21,hhi8(3) 8516 50da 0E94 0000 call __divmodsi4 8517 50de 922E mov r9,r18 8518 50e0 832E mov r8,r19 8519 50e2 742E mov r7,r20 8520 50e4 652E mov r6,r21 8521 50e6 C601 movw r24,r12 8522 50e8 B501 movw r22,r10 8523 50ea 23E0 ldi r18,lo8(3) 8524 50ec 30E0 ldi r19,hi8(3) 8525 50ee 40E0 ldi r20,hlo8(3) 8526 50f0 50E0 ldi r21,hhi8(3) 8527 50f2 0E94 0000 call __divmodsi4 8528 50f6 C901 movw r24,r18 8529 50f8 DA01 movw r26,r20 8530 50fa 5C01 movw r10,r24 8531 50fc 6D01 movw r12,r26 8532 .L524: 8533 50fe 292D mov r18,r9 8534 5100 382D mov r19,r8 8535 5102 472D mov r20,r7 8536 5104 562D mov r21,r6 8537 5106 C901 movw r24,r18 8538 5108 DA01 movw r26,r20 8539 510a BC01 movw r22,r24 8540 510c CD01 movw r24,r26 8541 510e 6132 cpi r22,lo8(33) 8542 5110 7105 cpc r23,__zero_reg__ 8543 5112 8105 cpc r24,__zero_reg__ 8544 5114 9105 cpc r25,__zero_reg__ 8545 5116 04F0 brlt .L526 8546 5118 60E2 ldi r22,lo8(32) 8547 511a 70E0 ldi r23,hi8(32) 8548 511c 80E0 ldi r24,hlo8(32) 8549 511e 90E0 ldi r25,hhi8(32) 8550 .L526: 8551 5120 7B01 movw r14,r22 8552 5122 8C01 movw r16,r24 8553 5124 603E cpi r22,lo8(-32) 8554 5126 3FEF ldi r19,hi8(-32) 8555 5128 7307 cpc r23,r19 8556 512a 3FEF ldi r19,hlo8(-32) 8557 512c 8307 cpc r24,r19 8558 512e 3FEF ldi r19,hhi8(-32) 8559 5130 9307 cpc r25,r19 8560 5132 04F4 brge .L527 8561 5134 90EE ldi r25,lo8(-32) 8562 5136 E92E mov r14,r25 8563 5138 9FEF ldi r25,hi8(-32) 8564 513a F92E mov r15,r25 8565 513c 9FEF ldi r25,hlo8(-32) 8566 513e 092F mov r16,r25 8567 5140 9FEF ldi r25,hhi8(-32) 8568 5142 192F mov r17,r25 8569 .L527: 8570 5144 40EE ldi r20,lo8(-32) 8571 5146 A416 cp r10,r20 8572 5148 4FEF ldi r20,hi8(-32) 8573 514a B406 cpc r11,r20 8574 514c 4FEF ldi r20,hlo8(-32) 8575 514e C406 cpc r12,r20 8576 5150 4FEF ldi r20,hhi8(-32) 8577 5152 D406 cpc r13,r20 8578 5154 04F0 brlt .+2 8579 5156 00C0 rjmp .L528 8580 5158 80EE ldi r24,lo8(-32) 8581 515a A82E mov r10,r24 8582 515c 8FEF ldi r24,hi8(-32) 8583 515e B82E mov r11,r24 8584 5160 8FEF ldi r24,hlo8(-32) 8585 5162 C82E mov r12,r24 8586 5164 8FEF ldi r24,hhi8(-32) 8587 5166 D82E mov r13,r24 8588 5168 00C0 rjmp .L521 8589 .L802: 8590 516a 8093 0000 sts StickGasHover.2444,r24 8591 516e 1092 0000 sts StickGasHover.2444+1,__zero_reg__ 8592 5172 00C0 rjmp .L658 8593 .L798: 8594 5174 8091 0000 lds r24,FCParam+1 8595 5178 2091 0000 lds r18,ExternHeightValue 8596 517c 3091 0000 lds r19,(ExternHeightValue)+1 8597 5180 280F add r18,r24 8598 5182 311D adc r19,__zero_reg__ 8599 5184 8091 0000 lds r24,ParamSet+13 8600 5188 90E0 ldi r25,lo8(0) 8601 518a FC01 movw r30,r24 8602 518c 2E9F mul r18,r30 8603 518e C001 movw r24,r0 8604 5190 2F9F mul r18,r31 8605 5192 900D add r25,r0 8606 5194 3E9F mul r19,r30 8607 5196 900D add r25,r0 8608 5198 1124 clr r1 8609 519a AA27 clr r26 8610 519c 97FD sbrc r25,7 8611 519e A095 com r26 8612 51a0 BA2F mov r27,r26 8613 51a2 8093 0000 sts SetPointHeight,r24 8614 51a6 9093 0000 sts (SetPointHeight)+1,r25 8615 51aa A093 0000 sts (SetPointHeight)+2,r26 8616 51ae B093 0000 sts (SetPointHeight)+3,r27 8617 51b2 00C0 rjmp .L614 8618 .L612: 8619 51b4 81E0 ldi r24,lo8(1) 8620 51b6 8093 0000 sts HCActive.2443,r24 8621 51ba 88EC ldi r24,lo8(-56) 8622 51bc 8093 0000 sts delay.2449,r24 8623 51c0 00C0 rjmp .L613 8624 .L447: 8625 51c2 8081 ld r24,Z 8626 51c4 9181 ldd r25,Z+1 8627 51c6 855B subi r24,lo8(-75) 8628 51c8 9F4F sbci r25,hi8(-75) 8629 51ca 04F0 brlt .+2 8630 51cc 00C0 rjmp .L464 8631 51ce 8091 0000 lds r24,delay_neutral.2416 8632 51d2 8F5F subi r24,lo8(-(1)) 8633 51d4 8093 0000 sts delay_neutral.2416,r24 8634 51d8 893C cpi r24,lo8(-55) 8635 51da 00F4 brsh .+2 8636 51dc 00C0 rjmp .L446 8637 51de 1092 0000 sts delay_neutral.2416,__zero_reg__ 8638 51e2 3BE0 ldi r19,lo8(11) 8639 51e4 3D15 cp r19,r13 8640 51e6 00F4 brsh .+2 8641 51e8 00C0 rjmp .L466 8642 51ea 2998 cbi 37-32,1 8643 .L467: 8644 51ec 1092 0000 sts (ModelIsFlying)+1,__zero_reg__ 8645 51f0 1092 0000 sts ModelIsFlying,__zero_reg__ 8646 51f4 0E94 0000 call Servo_Off 8647 51f8 81E0 ldi r24,lo8(1) 8648 .L726: 8649 51fa 0E94 0000 call SetNeutral 8650 51fe 0E94 0000 call Servo_On 8651 5202 0E94 0000 call GetActiveParamSet 8652 5206 68E7 ldi r22,lo8(120) 8653 5208 70E0 ldi r23,hi8(120) 8654 520a 0E94 0000 call Beep 8655 520e 8091 0000 lds r24,ParamSet+2 8656 5212 A82F mov r26,r24 8657 5214 B0E0 ldi r27,lo8(0) 8658 5216 AA0F lsl r26 8659 5218 BB1F rol r27 8660 521a 00C0 rjmp .L446 8661 .L805: 8662 521c 87B5 in r24,71-32 8663 521e 853F cpi r24,lo8(-11) 8664 5220 00F0 brlo .+2 8665 5222 00C0 rjmp .L608 8666 5224 8091 0000 lds r24,ExpandBaro 8667 5228 8150 subi r24,lo8(-(-1)) 8668 522a 00C0 rjmp .L729 8669 .L806: 8670 522c 8091 0000 lds r24,MaxStickRoll 8671 5230 9091 0000 lds r25,(MaxStickRoll)+1 8672 5234 8134 cpi r24,65 8673 5236 9105 cpc r25,__zero_reg__ 8674 5238 04F4 brge .+2 8675 523a 00C0 rjmp .L523 8676 523c 00C0 rjmp .L522 8677 .L660: 8678 523e 8739 cpi r24,151 8679 5240 9105 cpc r25,__zero_reg__ 8680 5242 04F4 brge .+2 8681 5244 00C0 rjmp .L661 8682 5246 86E9 ldi r24,lo8(150) 8683 5248 90E0 ldi r25,hi8(150) 8684 524a 9093 0000 sts (StickGasHover.2444)+1,r25 8685 524e 8093 0000 sts StickGasHover.2444,r24 8686 5252 00C0 rjmp .L661 8687 .L768: 8688 5254 81E0 ldi r24,lo8(1) 8689 5256 90E0 ldi r25,hi8(1) 8690 5258 9093 0000 sts (UpdateCompassCourse.2420)+1,r25 8691 525c 8093 0000 sts UpdateCompassCourse.2420,r24 8692 5260 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__ 8693 5264 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__ 8694 5268 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__ 8695 526c 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__ 8696 5270 1092 0000 sts SetPointYaw.2410,__zero_reg__ 8697 5274 1092 0000 sts (SetPointYaw.2410)+1,__zero_reg__ 8698 5278 1092 0000 sts (SetPointYaw.2410)+2,__zero_reg__ 8699 527c 1092 0000 sts (SetPointYaw.2410)+3,__zero_reg__ 8700 5280 0C94 0000 jmp .L413 8701 .L766: 8702 5284 27E0 ldi r18,lo8(7) 8703 5286 30E0 ldi r19,hi8(7) 8704 5288 420E add r4,r18 8705 528a 531E adc r5,r19 8706 528c 0C94 0000 jmp .L589 8707 .L528: 8708 5290 51E2 ldi r21,lo8(33) 8709 5292 A516 cp r10,r21 8710 5294 B104 cpc r11,__zero_reg__ 8711 5296 C104 cpc r12,__zero_reg__ 8712 5298 D104 cpc r13,__zero_reg__ 8713 529a 04F4 brge .+2 8714 529c 00C0 rjmp .L521 8715 529e B0E2 ldi r27,lo8(32) 8716 52a0 AB2E mov r10,r27 8717 52a2 B12C mov r11,__zero_reg__ 8718 52a4 C12C mov r12,__zero_reg__ 8719 52a6 D12C mov r13,__zero_reg__ 8720 52a8 00C0 rjmp .L521 8721 .L795: 8722 52aa F095 com r31 8723 52ac E195 neg r30 8724 52ae FF4F sbci r31,lo8(-1) 8725 52b0 0C94 0000 jmp .L516 8726 .L554: 8727 52b4 51E0 ldi r21,lo8(4097) 8728 52b6 E516 cp r14,r21 8729 52b8 50E1 ldi r21,hi8(4097) 8730 52ba F506 cpc r15,r21 8731 52bc 50E0 ldi r21,hlo8(4097) 8732 52be 0507 cpc r16,r21 8733 52c0 50E0 ldi r21,hhi8(4097) 8734 52c2 1507 cpc r17,r21 8735 52c4 04F4 brge .+2 8736 52c6 00C0 rjmp .L556 8737 52c8 8091 0000 lds r24,last_r_p.2429 8738 52cc 8823 tst r24 8739 52ce 01F4 brne .+2 8740 52d0 00C0 rjmp .L557 8741 52d2 D801 movw r26,r16 8742 52d4 C701 movw r24,r14 8743 52d6 39E0 ldi r19,9 8744 52d8 B595 1: asr r27 8745 52da A795 ror r26 8746 52dc 9795 ror r25 8747 52de 8795 ror r24 8748 52e0 3A95 dec r19 8749 52e2 01F4 brne 1b 8750 52e4 A090 0000 lds r10,cnt.2426 8751 52e8 B090 0000 lds r11,(cnt.2426)+1 8752 52ec A80E add r10,r24 8753 52ee B91E adc r11,r25 8754 52f0 B092 0000 sts (cnt.2426)+1,r11 8755 52f4 A092 0000 sts cnt.2426,r10 8756 52f8 C801 movw r24,r16 8757 52fa B701 movw r22,r14 8758 52fc 28E0 ldi r18,lo8(8) 8759 52fe 30E0 ldi r19,hi8(8) 8760 5300 40E0 ldi r20,hlo8(8) 8761 5302 50E0 ldi r21,hhi8(8) 8762 5304 0E94 0000 call __divmodsi4 8763 5308 C901 movw r24,r18 8764 530a DA01 movw r26,r20 8765 530c 8093 0000 sts CorrectionRoll.2414,r24 8766 5310 9093 0000 sts (CorrectionRoll.2414)+1,r25 8767 5314 A093 0000 sts (CorrectionRoll.2414)+2,r26 8768 5318 B093 0000 sts (CorrectionRoll.2414)+3,r27 8769 531c 8958 subi r24,lo8(5001) 8770 531e 9341 sbci r25,hi8(5001) 8771 5320 A040 sbci r26,hlo8(5001) 8772 5322 B040 sbci r27,hhi8(5001) 8773 5324 04F0 brlt .L558 8774 5326 88E8 ldi r24,lo8(5000) 8775 5328 93E1 ldi r25,hi8(5000) 8776 532a A0E0 ldi r26,hlo8(5000) 8777 532c B0E0 ldi r27,hhi8(5000) 8778 532e 8093 0000 sts CorrectionRoll.2414,r24 8779 5332 9093 0000 sts (CorrectionRoll.2414)+1,r25 8780 5336 A093 0000 sts (CorrectionRoll.2414)+2,r26 8781 533a B093 0000 sts (CorrectionRoll.2414)+3,r27 8782 .L558: 8783 533e 6091 0000 lds r22,CorrectionRoll.2414 8784 5342 7091 0000 lds r23,(CorrectionRoll.2414)+1 8785 5346 8091 0000 lds r24,(CorrectionRoll.2414)+2 8786 534a 9091 0000 lds r25,(CorrectionRoll.2414)+3 8787 534e 20E0 ldi r18,lo8(256) 8788 5350 31E0 ldi r19,hi8(256) 8789 5352 40E0 ldi r20,hlo8(256) 8790 5354 50E0 ldi r21,hhi8(256) 8791 5356 0E94 0000 call __divmodsi4 8792 535a 8091 0000 lds r24,AttitudeCorrectionRoll 8793 535e 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 8794 5362 820F add r24,r18 8795 5364 931F adc r25,r19 8796 5366 9093 0000 sts (AttitudeCorrectionRoll)+1,r25 8797 536a 8093 0000 sts AttitudeCorrectionRoll,r24 8798 .L559: 8799 536e 70E0 ldi r23,lo8(-4096) 8800 5370 E716 cp r14,r23 8801 5372 70EF ldi r23,hi8(-4096) 8802 5374 F706 cpc r15,r23 8803 5376 7FEF ldi r23,hlo8(-4096) 8804 5378 0707 cpc r16,r23 8805 537a 7FEF ldi r23,hhi8(-4096) 8806 537c 1707 cpc r17,r23 8807 537e 04F0 brlt .+2 8808 5380 00C0 rjmp .L560 8809 5382 8091 0000 lds r24,last_r_n.2430 8810 5386 8823 tst r24 8811 5388 01F4 brne .+2 8812 538a 00C0 rjmp .L561 8813 538c 8827 clr r24 8814 538e 9927 clr r25 8815 5390 DC01 movw r26,r24 8816 5392 8E19 sub r24,r14 8817 5394 9F09 sbc r25,r15 8818 5396 A00B sbc r26,r16 8819 5398 B10B sbc r27,r17 8820 539a 29E0 ldi r18,9 8821 539c B595 1: asr r27 8822 539e A795 ror r26 8823 53a0 9795 ror r25 8824 53a2 8795 ror r24 8825 53a4 2A95 dec r18 8826 53a6 01F4 brne 1b 8827 53a8 A80E add r10,r24 8828 53aa B91E adc r11,r25 8829 53ac B092 0000 sts (cnt.2426)+1,r11 8830 53b0 A092 0000 sts cnt.2426,r10 8831 53b4 C801 movw r24,r16 8832 53b6 B701 movw r22,r14 8833 53b8 28E0 ldi r18,lo8(8) 8834 53ba 30E0 ldi r19,hi8(8) 8835 53bc 40E0 ldi r20,hlo8(8) 8836 53be 50E0 ldi r21,hhi8(8) 8837 53c0 0E94 0000 call __divmodsi4 8838 53c4 C901 movw r24,r18 8839 53c6 DA01 movw r26,r20 8840 53c8 8093 0000 sts CorrectionRoll.2414,r24 8841 53cc 9093 0000 sts (CorrectionRoll.2414)+1,r25 8842 53d0 A093 0000 sts (CorrectionRoll.2414)+2,r26 8843 53d4 B093 0000 sts (CorrectionRoll.2414)+3,r27 8844 53d8 8857 subi r24,lo8(-5000) 8845 53da 9C4E sbci r25,hi8(-5000) 8846 53dc AF4F sbci r26,hlo8(-5000) 8847 53de BF4F sbci r27,hhi8(-5000) 8848 53e0 04F4 brge .L562 8849 53e2 88E7 ldi r24,lo8(-5000) 8850 53e4 9CEE ldi r25,hi8(-5000) 8851 53e6 AFEF ldi r26,hlo8(-5000) 8852 53e8 BFEF ldi r27,hhi8(-5000) 8853 53ea 8093 0000 sts CorrectionRoll.2414,r24 8854 53ee 9093 0000 sts (CorrectionRoll.2414)+1,r25 8855 53f2 A093 0000 sts (CorrectionRoll.2414)+2,r26 8856 53f6 B093 0000 sts (CorrectionRoll.2414)+3,r27 8857 .L562: 8858 53fa 6091 0000 lds r22,CorrectionRoll.2414 8859 53fe 7091 0000 lds r23,(CorrectionRoll.2414)+1 8860 5402 8091 0000 lds r24,(CorrectionRoll.2414)+2 8861 5406 9091 0000 lds r25,(CorrectionRoll.2414)+3 8862 540a 20E0 ldi r18,lo8(256) 8863 540c 31E0 ldi r19,hi8(256) 8864 540e 40E0 ldi r20,hlo8(256) 8865 5410 50E0 ldi r21,hhi8(256) 8866 5412 0E94 0000 call __divmodsi4 8867 5416 8091 0000 lds r24,AttitudeCorrectionRoll 8868 541a 9091 0000 lds r25,(AttitudeCorrectionRoll)+1 8869 541e 820F add r24,r18 8870 5420 931F adc r25,r19 8871 5422 9093 0000 sts (AttitudeCorrectionRoll)+1,r25 8872 5426 8093 0000 sts AttitudeCorrectionRoll,r24 8873 542a 0C94 0000 jmp .L563 8874 .L539: 8875 542e 31E0 ldi r19,lo8(4097) 8876 5430 E316 cp r14,r19 8877 5432 30E1 ldi r19,hi8(4097) 8878 5434 F306 cpc r15,r19 8879 5436 30E0 ldi r19,hlo8(4097) 8880 5438 0307 cpc r16,r19 8881 543a 30E0 ldi r19,hhi8(4097) 8882 543c 1307 cpc r17,r19 8883 543e 04F4 brge .+2 8884 5440 00C0 rjmp .L541 8885 5442 8091 0000 lds r24,last_n_p.2427 8886 5446 8823 tst r24 8887 5448 01F4 brne .+2 8888 544a 00C0 rjmp .L542 8889 544c D801 movw r26,r16 8890 544e C701 movw r24,r14 8891 5450 59E0 ldi r21,9 8892 5452 B595 1: asr r27 8893 5454 A795 ror r26 8894 5456 9795 ror r25 8895 5458 8795 ror r24 8896 545a 5A95 dec r21 8897 545c 01F4 brne 1b 8898 545e A090 0000 lds r10,cnt.2426 8899 5462 B090 0000 lds r11,(cnt.2426)+1 8900 5466 A80E add r10,r24 8901 5468 B91E adc r11,r25 8902 546a B092 0000 sts (cnt.2426)+1,r11 8903 546e A092 0000 sts cnt.2426,r10 8904 5472 C801 movw r24,r16 8905 5474 B701 movw r22,r14 8906 5476 28E0 ldi r18,lo8(8) 8907 5478 30E0 ldi r19,hi8(8) 8908 547a 40E0 ldi r20,hlo8(8) 8909 547c 50E0 ldi r21,hhi8(8) 8910 547e 0E94 0000 call __divmodsi4 8911 5482 C901 movw r24,r18 8912 5484 DA01 movw r26,r20 8913 5486 8093 0000 sts CorrectionNick.2413,r24 8914 548a 9093 0000 sts (CorrectionNick.2413)+1,r25 8915 548e A093 0000 sts (CorrectionNick.2413)+2,r26 8916 5492 B093 0000 sts (CorrectionNick.2413)+3,r27 8917 5496 8958 subi r24,lo8(5001) 8918 5498 9341 sbci r25,hi8(5001) 8919 549a A040 sbci r26,hlo8(5001) 8920 549c B040 sbci r27,hhi8(5001) 8921 549e 04F0 brlt .L543 8922 54a0 88E8 ldi r24,lo8(5000) 8923 54a2 93E1 ldi r25,hi8(5000) 8924 54a4 A0E0 ldi r26,hlo8(5000) 8925 54a6 B0E0 ldi r27,hhi8(5000) 8926 54a8 8093 0000 sts CorrectionNick.2413,r24 8927 54ac 9093 0000 sts (CorrectionNick.2413)+1,r25 8928 54b0 A093 0000 sts (CorrectionNick.2413)+2,r26 8929 54b4 B093 0000 sts (CorrectionNick.2413)+3,r27 8930 .L543: 8931 54b8 6091 0000 lds r22,CorrectionNick.2413 8932 54bc 7091 0000 lds r23,(CorrectionNick.2413)+1 8933 54c0 8091 0000 lds r24,(CorrectionNick.2413)+2 8934 54c4 9091 0000 lds r25,(CorrectionNick.2413)+3 8935 54c8 20E0 ldi r18,lo8(256) 8936 54ca 31E0 ldi r19,hi8(256) 8937 54cc 40E0 ldi r20,hlo8(256) 8938 54ce 50E0 ldi r21,hhi8(256) 8939 54d0 0E94 0000 call __divmodsi4 8940 54d4 8091 0000 lds r24,AttitudeCorrectionNick 8941 54d8 9091 0000 lds r25,(AttitudeCorrectionNick)+1 8942 54dc 820F add r24,r18 8943 54de 931F adc r25,r19 8944 54e0 9093 0000 sts (AttitudeCorrectionNick)+1,r25 8945 54e4 8093 0000 sts AttitudeCorrectionNick,r24 8946 .L544: 8947 54e8 50E0 ldi r21,lo8(-4096) 8948 54ea E516 cp r14,r21 8949 54ec 50EF ldi r21,hi8(-4096) 8950 54ee F506 cpc r15,r21 8951 54f0 5FEF ldi r21,hlo8(-4096) 8952 54f2 0507 cpc r16,r21 8953 54f4 5FEF ldi r21,hhi8(-4096) 8954 54f6 1507 cpc r17,r21 8955 54f8 04F0 brlt .+2 8956 54fa 00C0 rjmp .L545 8957 54fc 8091 0000 lds r24,last_n_n.2428 8958 5500 8823 tst r24 8959 5502 01F4 brne .+2 8960 5504 00C0 rjmp .L546 8961 5506 8827 clr r24 8962 5508 9927 clr r25 8963 550a DC01 movw r26,r24 8964 550c 8E19 sub r24,r14 8965 550e 9F09 sbc r25,r15 8966 5510 A00B sbc r26,r16 8967 5512 B10B sbc r27,r17 8968 5514 49E0 ldi r20,9 8969 5516 B595 1: asr r27 8970 5518 A795 ror r26 8971 551a 9795 ror r25 8972 551c 8795 ror r24 8973 551e 4A95 dec r20 8974 5520 01F4 brne 1b 8975 5522 A80E add r10,r24 8976 5524 B91E adc r11,r25 8977 5526 B092 0000 sts (cnt.2426)+1,r11 8978 552a A092 0000 sts cnt.2426,r10 8979 552e C801 movw r24,r16 8980 5530 B701 movw r22,r14 8981 5532 28E0 ldi r18,lo8(8) 8982 5534 30E0 ldi r19,hi8(8) 8983 5536 40E0 ldi r20,hlo8(8) 8984 5538 50E0 ldi r21,hhi8(8) 8985 553a 0E94 0000 call __divmodsi4 8986 553e C901 movw r24,r18 8987 5540 DA01 movw r26,r20 8988 5542 8093 0000 sts CorrectionNick.2413,r24 8989 5546 9093 0000 sts (CorrectionNick.2413)+1,r25 8990 554a A093 0000 sts (CorrectionNick.2413)+2,r26 8991 554e B093 0000 sts (CorrectionNick.2413)+3,r27 8992 5552 8857 subi r24,lo8(-5000) 8993 5554 9C4E sbci r25,hi8(-5000) 8994 5556 AF4F sbci r26,hlo8(-5000) 8995 5558 BF4F sbci r27,hhi8(-5000) 8996 555a 04F4 brge .L547 8997 555c 88E7 ldi r24,lo8(-5000) 8998 555e 9CEE ldi r25,hi8(-5000) 8999 5560 AFEF ldi r26,hlo8(-5000) 9000 5562 BFEF ldi r27,hhi8(-5000) 9001 5564 8093 0000 sts CorrectionNick.2413,r24 9002 5568 9093 0000 sts (CorrectionNick.2413)+1,r25 9003 556c A093 0000 sts (CorrectionNick.2413)+2,r26 9004 5570 B093 0000 sts (CorrectionNick.2413)+3,r27 9005 .L547: 9006 5574 6091 0000 lds r22,CorrectionNick.2413 9007 5578 7091 0000 lds r23,(CorrectionNick.2413)+1 9008 557c 8091 0000 lds r24,(CorrectionNick.2413)+2 9009 5580 9091 0000 lds r25,(CorrectionNick.2413)+3 9010 5584 20E0 ldi r18,lo8(256) 9011 5586 31E0 ldi r19,hi8(256) 9012 5588 40E0 ldi r20,hlo8(256) 9013 558a 50E0 ldi r21,hhi8(256) 9014 558c 0E94 0000 call __divmodsi4 9015 5590 8091 0000 lds r24,AttitudeCorrectionNick 9016 5594 9091 0000 lds r25,(AttitudeCorrectionNick)+1 9017 5598 820F add r24,r18 9018 559a 931F adc r25,r19 9019 559c 9093 0000 sts (AttitudeCorrectionNick)+1,r25 9020 55a0 8093 0000 sts AttitudeCorrectionNick,r24 9021 55a4 0C94 0000 jmp .L548 9022 .L617: 9023 55a8 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ 9024 55ac 1092 0000 sts HeightTrimming.2442,__zero_reg__ 9025 55b0 08AD ldd r16,Y+56 9026 55b2 19AD ldd r17,Y+57 9027 55b4 00C0 rjmp .L619 9028 .L608: 9029 55b6 8091 0000 lds r24,BaroFlags.2450 9030 55ba 8260 ori r24,lo8(2) 9031 55bc 8093 0000 sts BaroFlags.2450,r24 9032 55c0 0C94 0000 jmp .L606 9033 .L609: 9034 55c4 8091 0000 lds r24,BaroFlags.2450 9035 55c8 8C7F andi r24,lo8(-4) 9036 55ca 8093 0000 sts BaroFlags.2450,r24 9037 55ce 0C94 0000 jmp .L606 9038 .L464: 9039 55d2 1092 0000 sts delay_neutral.2416,__zero_reg__ 9040 55d6 00C0 rjmp .L446 9041 .L637: 9042 55d8 8091 0000 lds r24,ReadingHeight 9043 55dc 9091 0000 lds r25,(ReadingHeight)+1 9044 55e0 A091 0000 lds r26,(ReadingHeight)+2 9045 55e4 B091 0000 lds r27,(ReadingHeight)+3 9046 55e8 5C01 movw r10,r24 9047 55ea A21A sub r10,r18 9048 55ec B30A sbc r11,r19 9049 55ee F090 0000 lds r15,FCParam+2 9050 55f2 8091 0000 lds r24,ReadingVario 9051 55f6 9091 0000 lds r25,(ReadingVario)+1 9052 55fa 97FD sbrc r25,7 9053 55fc 00C0 rjmp .L808 9054 .L639: 9055 55fe 9595 asr r25 9056 5600 8795 ror r24 9057 5602 9595 asr r25 9058 5604 8795 ror r24 9059 5606 9595 asr r25 9060 5608 8795 ror r24 9061 560a 8930 cpi r24,9 9062 560c 9105 cpc r25,__zero_reg__ 9063 560e 04F0 brlt .+2 9064 5610 00C0 rjmp .L809 9065 5612 1816 cp __zero_reg__,r24 9066 5614 1906 cpc __zero_reg__,r25 9067 5616 04F4 brge .+2 9068 5618 00C0 rjmp .L641 9069 561a 889F mul r24,r24 9070 561c A001 movw r20,r0 9071 561e 899F mul r24,r25 9072 5620 500D add r21,r0 9073 5622 989F mul r25,r24 9074 5624 500D add r21,r0 9075 5626 1124 clr r1 9076 5628 57FF sbrs r21,7 9077 562a 00C0 rjmp .L644 9078 562c 4D5F subi r20,lo8(-(3)) 9079 562e 5F4F sbci r21,hi8(-(3)) 9080 .L644: 9081 5630 5595 asr r21 9082 5632 4795 ror r20 9083 5634 5595 asr r21 9084 5636 4795 ror r20 9085 5638 2091 0000 lds r18,ReadingVario 9086 563c 3091 0000 lds r19,(ReadingVario)+1 9087 5640 241B sub r18,r20 9088 5642 350B sbc r19,r21 9089 .L643: 9090 5644 B901 movw r22,r18 9091 5646 8827 clr r24 9092 5648 77FD sbrc r23,7 9093 564a 8095 com r24 9094 564c 982F mov r25,r24 9095 564e 2091 0000 lds r18,FCParam 9096 5652 30E0 ldi r19,lo8(0) 9097 5654 40E0 ldi r20,lo8(0) 9098 5656 50E0 ldi r21,hi8(0) 9099 5658 0E94 0000 call __mulsi3 9100 565c 20E8 ldi r18,lo8(128) 9101 565e 30E0 ldi r19,hi8(128) 9102 5660 40E0 ldi r20,hlo8(128) 9103 5662 50E0 ldi r21,hhi8(128) 9104 5664 0E94 0000 call __divmodsi4 9105 5668 A901 movw r20,r18 9106 566a 2F3F cpi r18,255 9107 566c 3105 cpc r19,__zero_reg__ 9108 566e 01F0 breq .L645 9109 5670 04F0 brlt .L645 9110 5672 4FEF ldi r20,lo8(255) 9111 5674 50E0 ldi r21,hi8(255) 9112 .L645: 9113 5676 8F2D mov r24,r15 9114 5678 90E0 ldi r25,lo8(0) 9115 567a A89E mul r10,r24 9116 567c 9001 movw r18,r0 9117 567e A99E mul r10,r25 9118 5680 300D add r19,r0 9119 5682 B89E mul r11,r24 9120 5684 300D add r19,r0 9121 5686 1124 clr r1 9122 5688 37FD sbrc r19,7 9123 568a 00C0 rjmp .L810 9124 .L646: 9125 568c 3595 asr r19 9126 568e 2795 ror r18 9127 5690 3595 asr r19 9128 5692 2795 ror r18 9129 5694 3595 asr r19 9130 5696 2795 ror r18 9131 5698 3595 asr r19 9132 569a 2795 ror r18 9133 569c B801 movw r22,r16 9134 569e 621B sub r22,r18 9135 56a0 730B sbc r23,r19 9136 56a2 9B01 movw r18,r22 9137 56a4 CA01 movw r24,r20 9138 56a6 7FEF ldi r23,hi8(-127) 9139 56a8 4138 cpi r20,lo8(-127) 9140 56aa 5707 cpc r21,r23 9141 56ac 04F4 brge .L647 9142 56ae 81E8 ldi r24,lo8(-127) 9143 56b0 9FEF ldi r25,hi8(-127) 9144 .L647: 9145 56b2 6901 movw r12,r18 9146 56b4 C81A sub r12,r24 9147 56b6 D90A sbc r13,r25 9148 56b8 6091 0000 lds r22,ReadingIntegralTop 9149 56bc 7091 0000 lds r23,(ReadingIntegralTop)+1 9150 56c0 8091 0000 lds r24,(ReadingIntegralTop)+2 9151 56c4 9091 0000 lds r25,(ReadingIntegralTop)+3 9152 56c8 E090 0000 lds r14,FCParam+3 9153 56cc FF24 clr r15 9154 56ce 00E0 ldi r16,lo8(0) 9155 56d0 10E0 ldi r17,hi8(0) 9156 56d2 20E8 ldi r18,lo8(128) 9157 56d4 30E0 ldi r19,hi8(128) 9158 56d6 40E0 ldi r20,hlo8(128) 9159 56d8 50E0 ldi r21,hhi8(128) 9160 56da 0E94 0000 call __divmodsi4 9161 56de C801 movw r24,r16 9162 56e0 B701 movw r22,r14 9163 56e2 0E94 0000 call __mulsi3 9164 56e6 20E2 ldi r18,lo8(32) 9165 56e8 30E0 ldi r19,hi8(32) 9166 56ea 40E0 ldi r20,hlo8(32) 9167 56ec 50E0 ldi r21,hhi8(32) 9168 56ee 0E94 0000 call __divmodsi4 9169 56f2 C901 movw r24,r18 9170 56f4 2F3F cpi r18,255 9171 56f6 3105 cpc r19,__zero_reg__ 9172 56f8 01F0 breq .L648 9173 56fa 04F0 brlt .L648 9174 56fc 8FEF ldi r24,lo8(255) 9175 56fe 90E0 ldi r25,hi8(255) 9176 .L648: 9177 5700 AFEF ldi r26,hi8(-127) 9178 5702 8138 cpi r24,lo8(-127) 9179 5704 9A07 cpc r25,r26 9180 5706 04F4 brge .L649 9181 5708 81E8 ldi r24,lo8(-127) 9182 570a 9FEF ldi r25,hi8(-127) 9183 .L649: 9184 570c 9601 movw r18,r12 9185 570e 281B sub r18,r24 9186 5710 390B sbc r19,r25 9187 5712 E5E9 ldi r30,lo8(149) 9188 5714 F0E0 ldi r31,hi8(149) 9189 5716 AE0E add r10,r30 9190 5718 BF1E adc r11,r31 9191 571a FBE2 ldi r31,lo8(299) 9192 571c AF16 cp r10,r31 9193 571e F1E0 ldi r31,hi8(299) 9194 5720 BF06 cpc r11,r31 9195 5722 00F0 brlo .+2 9196 5724 00C0 rjmp .L650 9197 5726 8091 0000 lds r24,HeightTrimming.2442 9198 572a 9091 0000 lds r25,(HeightTrimming.2442)+1 9199 572e 892B or r24,r25 9200 5730 01F0 breq .+2 9201 5732 00C0 rjmp .L650 9202 5734 8091 0000 lds r24,HoverGas.2445 9203 5738 9091 0000 lds r25,(HoverGas.2445)+1 9204 573c 1816 cp __zero_reg__,r24 9205 573e 1906 cpc __zero_reg__,r25 9206 5740 04F0 brlt .+2 9207 5742 00C0 rjmp .L650 9208 5744 0091 0000 lds r16,HoverGasMin.2446 9209 5748 1091 0000 lds r17,(HoverGasMin.2446)+1 9210 574c 2017 cp r18,r16 9211 574e 3107 cpc r19,r17 9212 5750 04F4 brge .+2 9213 5752 00C0 rjmp .L638 9214 5754 8091 0000 lds r24,HoverGasMax.2447 9215 5758 9091 0000 lds r25,(HoverGasMax.2447)+1 9216 575c 8901 movw r16,r18 9217 575e 8217 cp r24,r18 9218 5760 9307 cpc r25,r19 9219 5762 04F0 brlt .+2 9220 5764 00C0 rjmp .L638 9221 5766 8C01 movw r16,r24 9222 5768 00C0 rjmp .L638 9223 .L725: 9224 576a 8091 0000 lds r24,FilterHCGas.2441 9225 576e 9091 0000 lds r25,(FilterHCGas.2441)+1 9226 5772 E8AD ldd r30,Y+56 9227 5774 F9AD ldd r31,Y+57 9228 5776 E817 cp r30,r24 9229 5778 F907 cpc r31,r25 9230 577a 04F0 brlt .+2 9231 577c 00C0 rjmp .L656 9232 577e F093 0000 sts (FilterHCGas.2441)+1,r31 9233 5782 E093 0000 sts FilterHCGas.2441,r30 9234 5786 CF01 movw r24,r30 9235 5788 00C0 rjmp .L656 9236 .L545: 9237 578a 1092 0000 sts last_n_n.2428,__zero_reg__ 9238 578e 0C94 0000 jmp .L548 9239 .L541: 9240 5792 1092 0000 sts last_n_p.2427,__zero_reg__ 9241 5796 A090 0000 lds r10,cnt.2426 9242 579a B090 0000 lds r11,(cnt.2426)+1 9243 579e 00C0 rjmp .L544 9244 .L556: 9245 57a0 1092 0000 sts last_r_p.2429,__zero_reg__ 9246 57a4 A090 0000 lds r10,cnt.2426 9247 57a8 B090 0000 lds r11,(cnt.2426)+1 9248 57ac 00C0 rjmp .L559 9249 .L560: 9250 57ae 1092 0000 sts last_r_n.2430,__zero_reg__ 9251 57b2 0C94 0000 jmp .L563 9252 .L807: 9253 57b6 F095 com r31 9254 57b8 E195 neg r30 9255 57ba FF4F sbci r31,lo8(-1) 9256 57bc 00C0 rjmp .L525 9257 .L803: 9258 57be 2D5F subi r18,lo8(-(3)) 9259 57c0 3F4F sbci r19,hi8(-(3)) 9260 57c2 00C0 rjmp .L659 9261 .L793: 9262 57c4 8091 0000 lds r24,AttitudeCorrectionNick 9263 57c8 9091 0000 lds r25,AttitudeCorrectionNick+1 9264 57cc 9C01 movw r18,r24 9265 57ce 37FD sbrc r19,7 9266 57d0 00C0 rjmp .L811 9267 .L534: 9268 57d2 3595 asr r19 9269 57d4 2795 ror r18 9270 57d6 3093 0000 sts (AttitudeCorrectionNick)+1,r19 9271 57da 2093 0000 sts AttitudeCorrectionNick,r18 9272 57de 8091 0000 lds r24,AttitudeCorrectionRoll 9273 57e2 9091 0000 lds r25,AttitudeCorrectionRoll+1 9274 57e6 9C01 movw r18,r24 9275 57e8 37FD sbrc r19,7 9276 57ea 00C0 rjmp .L812 9277 .L535: 9278 57ec 3595 asr r19 9279 57ee 2795 ror r18 9280 57f0 3093 0000 sts (AttitudeCorrectionRoll)+1,r19 9281 57f4 2093 0000 sts AttitudeCorrectionRoll,r18 9282 57f8 0C94 0000 jmp .L532 9283 .L650: 9284 57fc 8901 movw r16,r18 9285 57fe 00C0 rjmp .L638 9286 .L449: 9287 5800 299A sbi 37-32,1 9288 5802 0C94 0000 jmp .L450 9289 .L668: 9290 5806 1092 0000 sts (HoverGasMin.2446)+1,__zero_reg__ 9291 580a 1092 0000 sts HoverGasMin.2446,__zero_reg__ 9292 580e 8FEF ldi r24,lo8(1023) 9293 5810 93E0 ldi r25,hi8(1023) 9294 5812 9093 0000 sts (HoverGasMax.2447)+1,r25 9295 5816 8093 0000 sts HoverGasMax.2447,r24 9296 581a 0C94 0000 jmp .L604 9297 .L456: 9298 581e 21E0 ldi r18,lo8(1) 9299 5820 0C94 0000 jmp .L457 9300 .L664: 9301 5824 2FE0 ldi r18,hi8(4000) 9302 5826 803A cpi r24,lo8(4000) 9303 5828 9207 cpc r25,r18 9304 582a 00F0 brlo .+2 9305 582c 00C0 rjmp .L666 9306 582e 2091 0000 lds r18,HoverGasFilter.2448 9307 5832 3091 0000 lds r19,(HoverGasFilter.2448)+1 9308 5836 4091 0000 lds r20,(HoverGasFilter.2448)+2 9309 583a 5091 0000 lds r21,(HoverGasFilter.2448)+3 9310 583e DA01 movw r26,r20 9311 5840 C901 movw r24,r18 9312 5842 6AE0 ldi r22,10 9313 5844 B695 1: lsr r27 9314 5846 A795 ror r26 9315 5848 9795 ror r25 9316 584a 8795 ror r24 9317 584c 6A95 dec r22 9318 584e 01F4 brne 1b 9319 5850 5901 movw r10,r18 9320 5852 6A01 movw r12,r20 9321 5854 A81A sub r10,r24 9322 5856 B90A sbc r11,r25 9323 5858 CA0A sbc r12,r26 9324 585a DB0A sbc r13,r27 9325 585c A801 movw r20,r16 9326 585e 9701 movw r18,r14 9327 5860 220F lsl r18 9328 5862 331F rol r19 9329 5864 441F rol r20 9330 5866 551F rol r21 9331 5868 220F lsl r18 9332 586a 331F rol r19 9333 586c 441F rol r20 9334 586e 551F rol r21 9335 5870 2A0D add r18,r10 9336 5872 3B1D adc r19,r11 9337 5874 4C1D adc r20,r12 9338 5876 5D1D adc r21,r13 9339 5878 2093 0000 sts HoverGasFilter.2448,r18 9340 587c 3093 0000 sts (HoverGasFilter.2448)+1,r19 9341 5880 4093 0000 sts (HoverGasFilter.2448)+2,r20 9342 5884 5093 0000 sts (HoverGasFilter.2448)+3,r21 9343 5888 00C0 rjmp .L665 9344 .L809: 9345 588a 88E0 ldi r24,lo8(8) 9346 588c 90E0 ldi r25,hi8(8) 9347 .L641: 9348 588e 889F mul r24,r24 9349 5890 9001 movw r18,r0 9350 5892 899F mul r24,r25 9351 5894 300D add r19,r0 9352 5896 989F mul r25,r24 9353 5898 300D add r19,r0 9354 589a 1124 clr r1 9355 589c 3595 asr r19 9356 589e 2795 ror r18 9357 58a0 3595 asr r19 9358 58a2 2795 ror r18 9359 58a4 8091 0000 lds r24,ReadingVario 9360 58a8 9091 0000 lds r25,(ReadingVario)+1 9361 58ac 280F add r18,r24 9362 58ae 391F adc r19,r25 9363 58b0 00C0 rjmp .L643 9364 .L610: 9365 58b2 8091 0000 lds r24,BaroFlags.2450 9366 58b6 8160 ori r24,lo8(1) 9367 58b8 8093 0000 sts BaroFlags.2450,r24 9368 58bc 0C94 0000 jmp .L606 9369 .L542: 9370 58c0 81E0 ldi r24,lo8(1) 9371 58c2 8093 0000 sts last_n_p.2427,r24 9372 58c6 A090 0000 lds r10,cnt.2426 9373 58ca B090 0000 lds r11,(cnt.2426)+1 9374 58ce 00C0 rjmp .L544 9375 .L561: 9376 58d0 81E0 ldi r24,lo8(1) 9377 58d2 8093 0000 sts last_r_n.2430,r24 9378 58d6 0C94 0000 jmp .L563 9379 .L804: 9380 58da 9095 com r25 9381 58dc 8195 neg r24 9382 58de 9F4F sbci r25,lo8(-1) 9383 58e0 00C0 rjmp .L663 9384 .L557: 9385 58e2 81E0 ldi r24,lo8(1) 9386 58e4 8093 0000 sts last_r_p.2429,r24 9387 58e8 A090 0000 lds r10,cnt.2426 9388 58ec B090 0000 lds r11,(cnt.2426)+1 9389 58f0 00C0 rjmp .L559 9390 .L546: 9391 58f2 81E0 ldi r24,lo8(1) 9392 58f4 8093 0000 sts last_n_n.2428,r24 9393 58f8 0C94 0000 jmp .L548 9394 .L801: 9395 58fc 4D5F subi r20,lo8(-(3)) 9396 58fe 5F4F sbci r21,hi8(-(3)) 9397 5900 00C0 rjmp .L653 9398 .L466: 9399 5902 299A sbi 37-32,1 9400 5904 00C0 rjmp .L467 9401 .L620: 9402 5906 8091 0000 lds r24,ReadingHeight 9403 590a 9091 0000 lds r25,(ReadingHeight)+1 9404 590e A091 0000 lds r26,(ReadingHeight)+2 9405 5912 B091 0000 lds r27,(ReadingHeight)+3 9406 5916 8059 subi r24,lo8(-(-400)) 9407 5918 9140 sbci r25,hi8(-(-400)) 9408 591a A040 sbci r26,hlo8(-(-400)) 9409 591c B040 sbci r27,hhi8(-(-400)) 9410 591e 8093 0000 sts SetPointHeight,r24 9411 5922 9093 0000 sts (SetPointHeight)+1,r25 9412 5926 A093 0000 sts (SetPointHeight)+2,r26 9413 592a B093 0000 sts (SetPointHeight)+3,r27 9414 592e 8091 0000 lds r24,ParamSet+17 9415 5932 8823 tst r24 9416 5934 01F4 brne .+2 9417 5936 00C0 rjmp .L634 9418 5938 8093 0000 sts StickGasHover.2444,r24 9419 593c 1092 0000 sts StickGasHover.2444+1,__zero_reg__ 9420 5940 00C0 rjmp .L633 9421 .L666: 9422 5942 8054 subi r24,lo8(8000) 9423 5944 9F41 sbci r25,hi8(8000) 9424 5946 00F0 brlo .+2 9425 5948 00C0 rjmp .L667 9426 594a 2091 0000 lds r18,HoverGasFilter.2448 9427 594e 3091 0000 lds r19,(HoverGasFilter.2448)+1 9428 5952 4091 0000 lds r20,(HoverGasFilter.2448)+2 9429 5956 5091 0000 lds r21,(HoverGasFilter.2448)+3 9430 595a DA01 movw r26,r20 9431 595c C901 movw r24,r18 9432 595e EBE0 ldi r30,11 9433 5960 B695 1: lsr r27 9434 5962 A795 ror r26 9435 5964 9795 ror r25 9436 5966 8795 ror r24 9437 5968 EA95 dec r30 9438 596a 01F4 brne 1b 9439 596c 5901 movw r10,r18 9440 596e 6A01 movw r12,r20 9441 5970 A81A sub r10,r24 9442 5972 B90A sbc r11,r25 9443 5974 CA0A sbc r12,r26 9444 5976 DB0A sbc r13,r27 9445 5978 A801 movw r20,r16 9446 597a 9701 movw r18,r14 9447 597c 220F lsl r18 9448 597e 331F rol r19 9449 5980 441F rol r20 9450 5982 551F rol r21 9451 5984 2A0D add r18,r10 9452 5986 3B1D adc r19,r11 9453 5988 4C1D adc r20,r12 9454 598a 5D1D adc r21,r13 9455 598c 2093 0000 sts HoverGasFilter.2448,r18 9456 5990 3093 0000 sts (HoverGasFilter.2448)+1,r19 9457 5994 4093 0000 sts (HoverGasFilter.2448)+2,r20 9458 5998 5093 0000 sts (HoverGasFilter.2448)+3,r21 9459 599c 00C0 rjmp .L665 9460 .L621: 9461 599e CB01 movw r24,r22 9462 59a0 0F97 sbiw r24,15 9463 59a2 4817 cp r20,r24 9464 59a4 5907 cpc r21,r25 9465 59a6 04F4 brge .L625 9466 59a8 8091 0000 lds r24,BaroFlags.2450 9467 59ac 81FD sbrc r24,1 9468 59ae 00C0 rjmp .L625 9469 59b0 2091 0000 lds r18,HeightTrimmingFlag.2452 9470 59b4 20FF sbrs r18,0 9471 59b6 00C0 rjmp .L626 9472 59b8 2E7F andi r18,lo8(-2) 9473 59ba 2093 0000 sts HeightTrimmingFlag.2452,r18 9474 59be 8091 0000 lds r24,ReadingHeight 9475 59c2 9091 0000 lds r25,(ReadingHeight)+1 9476 59c6 A091 0000 lds r26,(ReadingHeight)+2 9477 59ca B091 0000 lds r27,(ReadingHeight)+3 9478 59ce 8093 0000 sts SetPointHeight,r24 9479 59d2 9093 0000 sts (SetPointHeight)+1,r25 9480 59d6 A093 0000 sts (SetPointHeight)+2,r26 9481 59da B093 0000 sts (SetPointHeight)+3,r27 9482 .L626: 9483 59de 2260 ori r18,lo8(2) 9484 59e0 2093 0000 sts HeightTrimmingFlag.2452,r18 9485 59e4 CA01 movw r24,r20 9486 59e6 0F96 adiw r24,15 9487 59e8 861B sub r24,r22 9488 59ea 970B sbc r25,r23 9489 59ec 97FD sbrc r25,7 9490 59ee 00C0 rjmp .L813 9491 .L627: 9492 59f0 2091 0000 lds r18,HeightTrimming.2442 9493 59f4 3091 0000 lds r19,(HeightTrimming.2442)+1 9494 59f8 281B sub r18,r24 9495 59fa 390B sbc r19,r25 9496 59fc 3093 0000 sts (HeightTrimming.2442)+1,r19 9497 5a00 2093 0000 sts HeightTrimming.2442,r18 9498 5a04 00C0 rjmp .L624 9499 .L625: 9500 5a06 2091 0000 lds r18,HeightTrimmingFlag.2452 9501 5a0a 822F mov r24,r18 9502 5a0c 90E0 ldi r25,lo8(0) 9503 5a0e 8370 andi r24,lo8(3) 9504 5a10 9070 andi r25,hi8(3) 9505 5a12 892B or r24,r25 9506 5a14 01F0 breq .+2 9507 5a16 00C0 rjmp .L723 9508 5a18 2091 0000 lds r18,HeightTrimming.2442 9509 5a1c 3091 0000 lds r19,(HeightTrimming.2442)+1 9510 5a20 00C0 rjmp .L624 9511 .L667: 9512 5a22 2091 0000 lds r18,HoverGasFilter.2448 9513 5a26 3091 0000 lds r19,(HoverGasFilter.2448)+1 9514 5a2a 4091 0000 lds r20,(HoverGasFilter.2448)+2 9515 5a2e 5091 0000 lds r21,(HoverGasFilter.2448)+3 9516 5a32 DA01 movw r26,r20 9517 5a34 C901 movw r24,r18 9518 5a36 6CE0 ldi r22,12 9519 5a38 B695 1: lsr r27 9520 5a3a A795 ror r26 9521 5a3c 9795 ror r25 9522 5a3e 8795 ror r24 9523 5a40 6A95 dec r22 9524 5a42 01F4 brne 1b 9525 5a44 281B sub r18,r24 9526 5a46 390B sbc r19,r25 9527 5a48 4A0B sbc r20,r26 9528 5a4a 5B0B sbc r21,r27 9529 5a4c 2E0D add r18,r14 9530 5a4e 3F1D adc r19,r15 9531 5a50 401F adc r20,r16 9532 5a52 511F adc r21,r17 9533 5a54 2093 0000 sts HoverGasFilter.2448,r18 9534 5a58 3093 0000 sts (HoverGasFilter.2448)+1,r19 9535 5a5c 4093 0000 sts (HoverGasFilter.2448)+2,r20 9536 5a60 5093 0000 sts (HoverGasFilter.2448)+3,r21 9537 5a64 00C0 rjmp .L665 9538 .L808: 9539 5a66 0796 adiw r24,7 9540 5a68 00C0 rjmp .L639 9541 .L461: 9542 5a6a 0E94 0000 call GetActiveParamSet 9543 5a6e 0E94 0000 call ParamSet_ReadFromEEProm 9544 5a72 0E94 0000 call Servo_Off 9545 5a76 80E0 ldi r24,lo8(0) 9546 5a78 00C0 rjmp .L726 9547 .L810: 9548 5a7a 215F subi r18,lo8(-(15)) 9549 5a7c 3F4F sbci r19,hi8(-(15)) 9550 5a7e 00C0 rjmp .L646 9551 .L634: 9552 5a80 88E7 ldi r24,lo8(120) 9553 5a82 90E0 ldi r25,hi8(120) 9554 5a84 9093 0000 sts (StickGasHover.2444)+1,r25 9555 5a88 8093 0000 sts StickGasHover.2444,r24 9556 5a8c 00C0 rjmp .L633 9557 .L796: 9558 5a8e F095 com r31 9559 5a90 E195 neg r30 9560 5a92 FF4F sbci r31,lo8(-1) 9561 5a94 0C94 0000 jmp .L454 9562 .L812: 9563 5a98 2F5F subi r18,lo8(-(1)) 9564 5a9a 3F4F sbci r19,hi8(-(1)) 9565 5a9c 00C0 rjmp .L535 9566 .L811: 9567 5a9e 2F5F subi r18,lo8(-(1)) 9568 5aa0 3F4F sbci r19,hi8(-(1)) 9569 5aa2 00C0 rjmp .L534 9570 .L792: 9571 5aa4 F095 com r31 9572 5aa6 E195 neg r30 9573 5aa8 FF4F sbci r31,lo8(-1) 9574 5aaa 0C94 0000 jmp .L533 9575 .L723: 9576 5aae 2C7F andi r18,lo8(-4) 9577 5ab0 2093 0000 sts HeightTrimmingFlag.2452,r18 9578 5ab4 1092 0000 sts (HeightTrimming.2442)+1,__zero_reg__ 9579 5ab8 1092 0000 sts HeightTrimming.2442,__zero_reg__ 9580 5abc 8091 0000 lds r24,ReadingHeight 9581 5ac0 9091 0000 lds r25,(ReadingHeight)+1 9582 5ac4 A091 0000 lds r26,(ReadingHeight)+2 9583 5ac8 B091 0000 lds r27,(ReadingHeight)+3 9584 5acc 8093 0000 sts SetPointHeight,r24 9585 5ad0 9093 0000 sts (SetPointHeight)+1,r25 9586 5ad4 A093 0000 sts (SetPointHeight)+2,r26 9587 5ad8 B093 0000 sts (SetPointHeight)+3,r27 9588 5adc 8091 0000 lds r24,ParamSet+88 9589 5ae0 81FF sbrs r24,1 9590 5ae2 00C0 rjmp .L730 9591 5ae4 84EF ldi r24,lo8(500) 9592 5ae6 91E0 ldi r25,hi8(500) 9593 5ae8 9093 0000 sts (BeepTime)+1,r25 9594 5aec 8093 0000 sts BeepTime,r24 9595 .L730: 9596 5af0 20E0 ldi r18,lo8(0) 9597 5af2 30E0 ldi r19,hi8(0) 9598 5af4 00C0 rjmp .L624 9599 .L813: 9600 5af6 9095 com r25 9601 5af8 8195 neg r24 9602 5afa 9F4F sbci r25,lo8(-1) 9603 5afc 00C0 rjmp .L627 9604 .L799: 9605 5afe 9095 com r25 9606 5b00 8195 neg r24 9607 5b02 9F4F sbci r25,lo8(-1) 9608 5b04 00C0 rjmp .L623 9609 .L797: 9610 5b06 9095 com r25 9611 5b08 8195 neg r24 9612 5b0a 9F4F sbci r25,lo8(-1) 9613 5b0c 0C94 0000 jmp .L463 9614 .L632: 9615 5b10 6739 cpi r22,151 9616 5b12 7105 cpc r23,__zero_reg__ 9617 5b14 04F4 brge .+2 9618 5b16 00C0 rjmp .L628 9619 5b18 86E9 ldi r24,lo8(150) 9620 5b1a 90E0 ldi r25,hi8(150) 9621 5b1c 9093 0000 sts (StickGasHover.2444)+1,r25 9622 5b20 8093 0000 sts StickGasHover.2444,r24 9623 5b24 00C0 rjmp .L628 9624 .L800: 9625 5b26 2D5F subi r18,lo8(-(3)) 9626 5b28 3F4F sbci r19,hi8(-(3)) 9627 5b2a 00C0 rjmp .L631 9629 .global BiasHiResGyroNick 9630 .global BiasHiResGyroNick 9631 .section .bss 9634 BiasHiResGyroNick: 9635 0000 0000 .skip 2,0 9636 .global BiasHiResGyroRoll 9637 .global BiasHiResGyroRoll 9640 BiasHiResGyroRoll: 9641 0002 0000 .skip 2,0 9642 .global AdBiasGyroYaw 9643 .global AdBiasGyroYaw 9646 AdBiasGyroYaw: 9647 0004 0000 .skip 2,0 9648 .global AdBiasAccNick 9649 .global AdBiasAccNick 9652 AdBiasAccNick: 9653 0006 0000 .skip 2,0 9654 .global AdBiasAccRoll 9655 .global AdBiasAccRoll 9658 AdBiasAccRoll: 9659 0008 0000 .skip 2,0 9660 .global AdBiasAccTop 9661 .global AdBiasAccTop 9664 AdBiasAccTop: 9665 000a 0000 0000 .skip 4,0 9666 .global IntegralGyroNick 9667 .global IntegralGyroNick 9670 IntegralGyroNick: 9671 000e 0000 0000 .skip 4,0 9672 .global IntegralGyroNick2 9673 .global IntegralGyroNick2 9676 IntegralGyroNick2: 9677 0012 0000 0000 .skip 4,0 9678 .global IntegralGyroRoll 9679 .global IntegralGyroRoll 9682 IntegralGyroRoll: 9683 0016 0000 0000 .skip 4,0 9684 .global IntegralGyroRoll2 9685 .global IntegralGyroRoll2 9688 IntegralGyroRoll2: 9689 001a 0000 0000 .skip 4,0 9690 .global IntegralGyroYaw 9691 .global IntegralGyroYaw 9694 IntegralGyroYaw: 9695 001e 0000 0000 .skip 4,0 9696 .global ReadingIntegralGyroNick 9697 .global ReadingIntegralGyroNick 9700 ReadingIntegralGyroNick: 9701 0022 0000 0000 .skip 4,0 9702 .global ReadingIntegralGyroNick2 9703 .global ReadingIntegralGyroNick2 9706 ReadingIntegralGyroNick2: 9707 0026 0000 0000 .skip 4,0 9708 .global ReadingIntegralGyroRoll 9709 .global ReadingIntegralGyroRoll 9712 ReadingIntegralGyroRoll: 9713 002a 0000 0000 .skip 4,0 9714 .global ReadingIntegralGyroRoll2 9715 .global ReadingIntegralGyroRoll2 9718 ReadingIntegralGyroRoll2: 9719 002e 0000 0000 .skip 4,0 9720 .global ReadingIntegralGyroYaw 9721 .global ReadingIntegralGyroYaw 9724 ReadingIntegralGyroYaw: 9725 0032 0000 0000 .skip 4,0 9726 .global MeanAccNick 9727 .global MeanAccNick 9730 MeanAccNick: 9731 0036 0000 0000 .skip 4,0 9732 .global MeanAccRoll 9733 .global MeanAccRoll 9736 MeanAccRoll: 9737 003a 0000 0000 .skip 4,0 9738 .global ReadingIntegralTop 9739 .global ReadingIntegralTop 9742 ReadingIntegralTop: 9743 003e 0000 0000 .skip 4,0 9744 .global CompassHeading 9745 .data 9748 CompassHeading: 9749 0000 FFFF .word -1 9750 .global CompassCourse 9753 CompassCourse: 9754 0002 FFFF .word -1 9755 .global CompassOffCourse 9756 .global CompassOffCourse 9757 .section .bss 9760 CompassOffCourse: 9761 0042 0000 .skip 2,0 9762 .global CompassCalState 9763 .global CompassCalState 9766 CompassCalState: 9767 0044 00 .skip 1,0 9768 .global FunnelCourse 9769 .global FunnelCourse 9772 FunnelCourse: 9773 0045 00 .skip 1,0 9774 .global BadCompassHeading 9775 .data 9778 BadCompassHeading: 9779 0004 F401 .word 500 9780 .global NaviAccNick 9781 .global NaviAccNick 9782 .section .bss 9785 NaviAccNick: 9786 0046 0000 .skip 2,0 9787 .global NaviAccRoll 9788 .global NaviAccRoll 9791 NaviAccRoll: 9792 0048 0000 .skip 2,0 9793 .global NaviCntAcc 9794 .global NaviCntAcc 9797 NaviCntAcc: 9798 004a 0000 .skip 2,0 9799 .global ModelIsFlying 9800 .global ModelIsFlying 9803 ModelIsFlying: 9804 004c 0000 .skip 2,0 9805 .global MKFlags 9806 .global MKFlags 9809 MKFlags: 9810 004e 00 .skip 1,0 9811 .global TurnOver180Nick 9812 .data 9815 TurnOver180Nick: 9816 0006 90 .byte -112 9817 0007 D0 .byte -48 9818 0008 03 .byte 3 9819 0009 00 .byte 0 9820 .global TurnOver180Roll 9823 TurnOver180Roll: 9824 000a 90 .byte -112 9825 000b D0 .byte -48 9826 000c 03 .byte 3 9827 000d 00 .byte 0 9828 .global Ki 9831 Ki: 9832 000e 3801 .word 312 9833 .global Poti1 9834 .global Poti1 9835 .section .bss 9838 Poti1: 9839 004f 0000 .skip 2,0 9840 .global Poti2 9841 .global Poti2 9844 Poti2: 9845 0051 0000 .skip 2,0 9846 .global Poti3 9847 .global Poti3 9850 Poti3: 9851 0053 0000 .skip 2,0 9852 .global Poti4 9853 .global Poti4 9856 Poti4: 9857 0055 0000 .skip 2,0 9858 .global Poti5 9859 .global Poti5 9862 Poti5: 9863 0057 0000 .skip 2,0 9864 .global Poti6 9865 .global Poti6 9868 Poti6: 9869 0059 0000 .skip 2,0 9870 .global Poti7 9871 .global Poti7 9874 Poti7: 9875 005b 0000 .skip 2,0 9876 .global Poti8 9877 .global Poti8 9880 Poti8: 9881 005d 0000 .skip 2,0 9882 .global RequiredMotors 9883 .global RequiredMotors 9886 RequiredMotors: 9887 005f 00 .skip 1,0 9888 .global StickNick 9889 .global StickNick 9892 StickNick: 9893 0060 0000 .skip 2,0 9894 .global StickRoll 9895 .global StickRoll 9898 StickRoll: 9899 0062 0000 .skip 2,0 9900 .global StickYaw 9901 .global StickYaw 9904 StickYaw: 9905 0064 0000 .skip 2,0 9906 .global StickGas 9907 .global StickGas 9910 StickGas: 9911 0066 0000 .skip 2,0 9912 .global GPSStickNick 9913 .global GPSStickNick 9916 GPSStickNick: 9917 0068 0000 .skip 2,0 9918 .global GPSStickRoll 9919 .global GPSStickRoll 9922 GPSStickRoll: 9923 006a 0000 .skip 2,0 9924 .global MaxStickNick 9925 .global MaxStickNick 9928 MaxStickNick: 9929 006c 0000 .skip 2,0 9930 .global MaxStickRoll 9931 .global MaxStickRoll 9934 MaxStickRoll: 9935 006e 0000 .skip 2,0 9936 .global ExternStickNick 9937 .global ExternStickNick 9940 ExternStickNick: 9941 0070 0000 .skip 2,0 9942 .global ExternStickRoll 9943 .global ExternStickRoll 9946 ExternStickRoll: 9947 0072 0000 .skip 2,0 9948 .global ExternStickYaw 9949 .global ExternStickYaw 9952 ExternStickYaw: 9953 0074 0000 .skip 2,0 9954 .global ExternHeightValue 9955 .data 9958 ExternHeightValue: 9959 0010 ECFF .word -20 9960 .global SetPointHeight 9961 .global SetPointHeight 9962 .section .bss 9965 SetPointHeight: 9966 0076 0000 0000 .skip 4,0 9967 .global AttitudeCorrectionRoll 9968 .global AttitudeCorrectionRoll 9971 AttitudeCorrectionRoll: 9972 007a 0000 .skip 2,0 9973 .global AttitudeCorrectionNick 9974 .global AttitudeCorrectionNick 9977 AttitudeCorrectionNick: 9978 007c 0000 .skip 2,0 9979 .global LoopingNick 9980 .global LoopingNick 9983 LoopingNick: 9984 007e 00 .skip 1,0 9985 .global LoopingRoll 9986 .global LoopingRoll 9989 LoopingRoll: 9990 007f 00 .skip 1,0 9991 .global LoopingLeft 9992 .global LoopingLeft 9995 LoopingLeft: 9996 0080 00 .skip 1,0 9997 .global LoopingRight 9998 .global LoopingRight 10001 LoopingRight: 10002 0081 00 .skip 1,0 10003 .global LoopingDown 10004 .global LoopingDown 10007 LoopingDown: 10008 0082 00 .skip 1,0 10009 .global LoopingTop 10010 .global LoopingTop 10013 LoopingTop: 10014 0083 00 .skip 1,0 10015 .global FCParam 10016 .data 10019 FCParam: 10020 0012 30 .byte 48 10021 0013 FB .byte -5 10022 0014 10 .byte 16 10023 0015 3A .byte 58 10024 0016 40 .byte 64 10025 0017 40 .byte 64 10026 0018 08 .byte 8 10027 0019 96 .byte -106 10028 001a 96 .byte -106 10029 001b 96 .byte -106 10030 001c 96 .byte -106 10031 001d 02 .byte 2 10032 001e 0A .byte 10 10033 001f 00 .byte 0 10034 0020 00 .byte 0 10035 0021 00 .byte 0 10036 0022 00 .byte 0 10037 0023 00 .byte 0 10038 0024 00 .byte 0 10039 0025 00 .byte 0 10040 0026 00 .byte 0 10041 0027 64 .byte 100 10042 0028 64 .byte 100 10043 0029 46 .byte 70 10044 002a 5A .byte 90 10045 002b 41 .byte 65 10046 002c 40 .byte 64 10047 002d 64 .byte 100 10048 002e 00 .byte 0 10049 002f 00 .byte 0 10050 0030 00 .byte 0 10051 0031 0000 00 .skip 3,0 10052 .lcomm HeightTrimmingFlag.2452,1 10053 .lcomm BaroExpandActive.2451,2 10054 .lcomm BaroFlags.2450,1 10057 delay.2449: 10058 0034 64 .byte 100 10059 .lcomm HoverGasFilter.2448,4 10062 HoverGasMax.2447: 10063 0035 FF03 .word 1023 10064 .lcomm HoverGasMin.2446,2 10065 .lcomm HoverGas.2445,2 10068 StickGasHover.2444: 10069 0037 7800 .word 120 10070 .lcomm HCActive.2443,1 10071 .lcomm HeightTrimming.2442,2 10072 .lcomm FilterHCGas.2441,2 10073 .lcomm MeanIntegralGyroRoll_old.2432,4 10074 .lcomm MeanIntegralGyroNick_old.2431,4 10075 .lcomm last_r_n.2430,1 10076 .lcomm last_r_p.2429,1 10077 .lcomm last_n_n.2428,1 10078 .lcomm last_n_p.2427,1 10079 .lcomm cnt.2426,2 10080 .lcomm stick_roll.2425,2 10081 .lcomm stick_nick.2424,2 10082 .lcomm MotorValue.2421,24 10083 .lcomm UpdateCompassCourse.2420,2 10084 .lcomm TimerDebugOut.2419,1 10085 .lcomm delay_stopmotors.2418,1 10086 .lcomm delay_startmotors.2417,1 10087 .lcomm delay_neutral.2416,1 10088 .lcomm RcLostTimer.2415,2 10089 .lcomm CorrectionRoll.2414,4 10090 .lcomm CorrectionNick.2413,4 10091 .lcomm IntegralGyroRollError.2412,4 10092 .lcomm IntegralGyroNickError.2411,4 10093 .lcomm SetPointYaw.2410,4 10094 .lcomm IPartRoll.2409,4 10095 .lcomm IPartNick.2408,4 10098 stick.2382: 10099 0039 01 .byte 1 10100 .lcomm Last_GyroNick.2093,2 10101 .lcomm Last_GyroRoll.2092,2 10102 .section .progmem.data,"a",@progbits 10105 __c.1965: 10106 0000 0A0D 4143 .string "\n\rACC not calibrated!\r\n" 10106 4320 6E6F 10106 7420 6361 10106 6C69 6272 10106 6174 6564 10107 .comm GyroNick,2,1 10108 .comm GyroRoll,2,1 10109 .comm GyroYaw,2,1 10110 .comm YawGyroHeading,4,1 10111 .comm AccNick,2,1 10112 .comm AccRoll,2,1 10113 .comm AccTop,2,1 10114 .comm TrimNick,2,1 10115 .comm TrimRoll,2,1 10116 .comm MeanIntegralGyroNick,4,1 10117 .comm MeanIntegralGyroRoll,4,1 10118 .comm YawGyroDrift,2,1 10119 .comm GyroPFactor,1,1 10120 .comm GyroIFactor,1,1 10121 .comm GyroYawPFactor,1,1 10122 .comm GyroYawIFactor,1,1 10123 .global __do_copy_data 10124 .global __do_clear_bss DEFINED SYMBOLS *ABS*:00000000 fc.c C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2 *ABS*:0000003f __SREG__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3 *ABS*:0000003e __SP_H__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:4 *ABS*:0000003d __SP_L__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:5 *ABS*:00000034 __CCP__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:6 *ABS*:00000000 __tmp_reg__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:7 *ABS*:00000001 __zero_reg__ C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:11 .text:00000000 MotorSmoothing C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:43 .text:0000002c Mean *COM*:00000002 GyroNick *COM*:00000002 GyroRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9646 .bss:00000004 AdBiasGyroYaw *COM*:00000002 GyroYaw *COM*:00000002 AccNick *COM*:00000002 AccRoll *COM*:00000002 AccTop C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9730 .bss:00000036 MeanAccNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9736 .bss:0000003a MeanAccRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9785 .bss:00000046 NaviAccNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9791 .bss:00000048 NaviAccRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9797 .bss:0000004a NaviCntAcc C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9700 .bss:00000022 ReadingIntegralGyroNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9712 .bss:0000002a ReadingIntegralGyroRoll *COM*:00000004 YawGyroHeading C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9724 .bss:00000032 ReadingIntegralGyroYaw C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9983 .bss:0000007e LoopingNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9989 .bss:0000007f LoopingRoll *COM*:00000002 TrimNick *COM*:00000002 TrimRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9718 .bss:0000002e ReadingIntegralGyroRoll2 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9971 .bss:0000007a AttitudeCorrectionRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9823 .data:0000000a TurnOver180Roll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9706 .bss:00000026 ReadingIntegralGyroNick2 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9977 .bss:0000007c AttitudeCorrectionNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9815 .data:00000006 TurnOver180Nick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9694 .bss:0000001e IntegralGyroYaw C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9670 .bss:0000000e IntegralGyroNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9682 .bss:00000016 IntegralGyroRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9676 .bss:00000012 IntegralGyroNick2 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9688 .bss:0000001a IntegralGyroRoll2 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10019 .data:00000012 FCParam C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10095 .bss:000000e3 Last_GyroNick.2093 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10100 .bss:000000e5 Last_GyroRoll.2092 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9772 .bss:00000045 FunnelCourse C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:1559 .text:00000e60 ParameterMapping C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9856 .bss:00000055 Poti4 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9831 .data:0000000e Ki C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9838 .bss:0000004f Poti1 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9844 .bss:00000051 Poti2 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9850 .bss:00000053 Poti3 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2620 .text:000017da SendMotorData C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9809 .bss:0000004e MKFlags C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:2681 .text:0000185c SetNeutral C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9652 .bss:00000006 AdBiasAccNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9658 .bss:00000008 AdBiasAccRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9664 .bss:0000000a AdBiasAccTop C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9634 .bss:00000000 BiasHiResGyroNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9640 .bss:00000002 BiasHiResGyroRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9742 .bss:0000003e ReadingIntegralTop C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9748 .data:00000000 CompassHeading C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9753 .data:00000002 CompassCourse *COM*:00000002 YawGyroDrift C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9958 .data:00000010 ExternHeightValue C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9916 .bss:00000068 GPSStickNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9922 .bss:0000006a GPSStickRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10105 .progmem.data:00000000 __c.1965 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3317 .text:00001eca Beep C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3353 .text:00001f08 SetCompassCalState C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10098 .data:00000039 stick.2382 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9766 .bss:00000044 CompassCalState C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:3402 .text:00001f6a MotorControl C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10087 .bss:000000c5 RcLostTimer.2415 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9803 .bss:0000004c ModelIsFlying C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10080 .bss:000000a5 stick_nick.2424 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9892 .bss:00000060 StickNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10079 .bss:000000a3 stick_roll.2425 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9898 .bss:00000062 StickRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9904 .bss:00000064 StickYaw C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9910 .bss:00000066 StickGas *COM*:00000001 GyroPFactor *COM*:00000001 GyroIFactor *COM*:00000001 GyroYawPFactor *COM*:00000001 GyroYawIFactor C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9928 .bss:0000006c MaxStickNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9934 .bss:0000006e MaxStickRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9995 .bss:00000080 LoopingLeft C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10001 .bss:00000081 LoopingRight C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10013 .bss:00000083 LoopingTop C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10007 .bss:00000082 LoopingDown *COM*:00000004 MeanIntegralGyroNick *COM*:00000004 MeanIntegralGyroRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10073 .bss:00000099 MeanIntegralGyroNick_old.2431 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10072 .bss:00000095 MeanIntegralGyroRoll_old.2432 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9778 .data:00000004 BadCompassHeading C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10092 .bss:000000d7 SetPointYaw.2410 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10094 .bss:000000df IPartNick.2408 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10093 .bss:000000db IPartRoll.2409 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9862 .bss:00000057 Poti5 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9868 .bss:00000059 Poti6 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9874 .bss:0000005b Poti7 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9880 .bss:0000005d Poti8 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10086 .bss:000000c4 delay_neutral.2416 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10083 .bss:000000c1 TimerDebugOut.2419 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10081 .bss:000000a7 MotorValue.2421 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10082 .bss:000000bf UpdateCompassCourse.2420 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10064 .bss:0000008e HoverGas.2445 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10052 .bss:00000085 BaroExpandActive.2451 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10057 .data:00000034 delay.2449 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9965 .bss:00000076 SetPointHeight C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10065 .bss:00000090 HCActive.2443 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10091 .bss:000000d3 IntegralGyroNickError.2411 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10089 .bss:000000cb CorrectionNick.2413 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10090 .bss:000000cf IntegralGyroRollError.2412 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10088 .bss:000000c7 CorrectionRoll.2414 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10078 .bss:000000a1 cnt.2426 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10085 .bss:000000c3 delay_startmotors.2417 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10084 .bss:000000c2 delay_stopmotors.2418 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10053 .bss:00000087 BaroFlags.2450 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10068 .data:00000037 StickGasHover.2444 .bss:00000084 HeightTrimmingFlag.2452 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10070 .bss:00000091 HeightTrimming.2442 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10071 .bss:00000093 FilterHCGas.2441 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10054 .bss:00000088 HoverGasFilter.2448 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10059 .bss:0000008c HoverGasMin.2446 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10062 .data:00000035 HoverGasMax.2447 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10075 .bss:0000009e last_r_p.2429 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10074 .bss:0000009d last_r_n.2430 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10077 .bss:000000a0 last_n_p.2427 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:10076 .bss:0000009f last_n_n.2428 C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9760 .bss:00000042 CompassOffCourse C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9886 .bss:0000005f RequiredMotors C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9940 .bss:00000070 ExternStickNick C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9946 .bss:00000072 ExternStickRoll C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccstO6tO.s:9952 .bss:00000074 ExternStickYaw UNDEFINED SYMBOLS HiResGyroNick FilterHiResGyroNick AdValueGyroNick BoardRelease HiResGyroRoll FilterHiResGyroRoll AdValueGyroRoll AdValueGyroYaw AdValueAccNick __mulsi3 __divmodsi4 AdValueAccRoll AdValueAccTop ADReady ParamSet RC_Quality __divmodhi4 MotorTest_Active Motor MotorTest DebugOut I2C_Start ExpandBaro Delay_ms_Mess AdAirPressure AdValueAccZ __floatsisf SetParamWord __fixunssfsi AirPressure StartAirPressure ReadingVario BeepTime PPM_in SearchAirPressureOffset GetParamWord _printf_P __gtsf2 SearchDacGyroOffset Delay_ms PPM_diff NewPpmData ExternControl MeasurementCounter SetActiveParamSet GetActiveParamSet ParamSet_ReadFromEEProm Servo_Off ReadingHeight UBat ServoNickValue LowVoltageWarning __udivmodhi4 MissingMotor Mixer SumHeight ihypot c_cos_8192 NCGpsZ PressureSensorOffset Servo_On __do_copy_data __do_clear_bss