Tricopter_Flightcontrol.elf: file format elf32-avr Sections: Idx Name Size VMA LMA File off Algn 0 .text 000031c8 00000000 00000000 00000094 2**1 CONTENTS, ALLOC, LOAD, READONLY, CODE 1 .data 00000008 00802000 000031c8 0000325c 2**0 CONTENTS, ALLOC, LOAD, DATA 2 .bss 00000081 00802008 00802008 00003264 2**0 ALLOC 3 .stab 000007a4 00000000 00000000 00003264 2**2 CONTENTS, READONLY, DEBUGGING 4 .stabstr 00000054 00000000 00000000 00003a08 2**0 CONTENTS, READONLY, DEBUGGING 5 .debug_aranges 00000160 00000000 00000000 00003a5c 2**0 CONTENTS, READONLY, DEBUGGING 6 .debug_pubnames 000008ac 00000000 00000000 00003bbc 2**0 CONTENTS, READONLY, DEBUGGING 7 .debug_info 00003604 00000000 00000000 00004468 2**0 CONTENTS, READONLY, DEBUGGING 8 .debug_abbrev 00000ec6 00000000 00000000 00007a6c 2**0 CONTENTS, READONLY, DEBUGGING 9 .debug_line 00001cf7 00000000 00000000 00008932 2**0 CONTENTS, READONLY, DEBUGGING 10 .debug_frame 00000480 00000000 00000000 0000a62c 2**2 CONTENTS, READONLY, DEBUGGING 11 .debug_str 0000166e 00000000 00000000 0000aaac 2**0 CONTENTS, READONLY, DEBUGGING 12 .debug_loc 00000f75 00000000 00000000 0000c11a 2**0 CONTENTS, READONLY, DEBUGGING 13 .debug_ranges 000000a8 00000000 00000000 0000d08f 2**0 CONTENTS, READONLY, DEBUGGING Disassembly of section .text: 00000000 <__vectors>: 0: 0c 94 13 0a jmp 0x1426 ; 0x1426 <__ctors_end> 4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 10: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 14: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 18: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 20: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 24: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 28: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 2c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 30: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 34: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 38: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 3c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 40: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 44: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 48: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 4c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 50: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 54: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 58: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 5c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 60: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 64: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 68: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 6c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 70: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 74: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 78: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 7c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 80: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 84: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 88: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 8c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 90: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 94: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 98: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 9c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> a0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> a4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> a8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> ac: 0c 94 e4 10 jmp 0x21c8 ; 0x21c8 <__vector_43> b0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> b4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> b8: 0c 94 0f 13 jmp 0x261e ; 0x261e <__vector_46> bc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> c0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> c4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> c8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> cc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> d0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> d4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> d8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> dc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> e0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> e4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> e8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> ec: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> f0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> f4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> f8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> fc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 100: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 104: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 108: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 10c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 110: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 114: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 118: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 11c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 120: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 124: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 128: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 12c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 130: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 134: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 138: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 13c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 140: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 144: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 148: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 14c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 150: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 154: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 158: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 15c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 160: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 164: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 168: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 16c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 170: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 174: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 178: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 17c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 180: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 184: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 188: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 18c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 190: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 194: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 198: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 19c: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1a0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1a4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1a8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1ac: 0c 94 7b 15 jmp 0x2af6 ; 0x2af6 <__vector_107> 1b0: 0c 94 b9 13 jmp 0x2772 ; 0x2772 <__vector_108> 1b4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1b8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1bc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1c0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1c4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1c8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1cc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1d0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1d4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1d8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1dc: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1e0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1e4: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1e8: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1ec: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 1f0: 0c 94 39 0a jmp 0x1472 ; 0x1472 <__bad_interrupt> 000001f4 <__c.3589>: 1f4: 0a 00 .. 000001f6 <__c.3584>: 1f6: 20 20 20 46 69 6c 74 65 72 41 6e 67 6c 65 73 3a FilterAngles: 206: 20 00 . 00000208 <__c.3579>: 208: 20 20 20 41 63 63 65 6c 41 6e 67 6c 65 73 3a 20 AccelAngles: ... 00000219 <__c.3574>: 219: 20 20 20 61 78 2c 61 79 2c 61 7a 3a 20 00 ax,ay,az: . 00000227 <__c.3569>: 227: 70 2c 71 2c 72 3a 20 00 p,q,r: . 0000022f <__c.3451>: 22f: 73 74 61 72 74 20 69 74 67 0a 00 start itg.. 0000023a <__c.3449>: 23a: 73 74 61 72 74 20 6c 69 73 0a 00 start lis.. 00000245 <__c.3447>: 245: 45 52 52 4f 52 20 69 6e 69 74 69 61 6c 69 7a 69 ERROR initializi 255: 6e 67 20 4c 49 53 20 73 65 6e 73 6f 72 20 2e 2e ng LIS sensor .. 265: 2e 0a 00 ... 00000268 <__c.3445>: 268: 45 52 52 4f 52 20 69 6e 69 74 69 61 6c 69 7a 69 ERROR initializi 278: 6e 67 20 49 54 47 20 73 65 6e 73 6f 72 20 2e 2e ng ITG sensor .. 288: 2e 0a 00 ... 0000028b <__c.3443>: 28b: 53 54 41 52 54 49 4e 47 20 46 4c 49 47 48 54 43 STARTING FLIGHTC 29b: 4f 4e 54 52 4f 4c 0a 0a 00 ONTROL... 000002a4 <__c.3441>: 2a4: 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a **************** 2b4: 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a **************** 2c4: 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a 2a **************** 2d4: 2a 2a 2a 2a 2a 0a 00 *****.. 000002db <__c.3512>: 2db: 42 4c 4f 43 4b 00 BLOCK. 000002e1 <__c.3471>: 2e1: 20 2d 20 46 41 49 4c 55 52 45 0a 00 - FAILURE.. 000002ed <__c.3469>: 2ed: 0a 0a 00 ... 000002f0 <__c.3467>: 2f0: 20 20 2d 53 61 6d 70 6c 65 20 72 61 74 65 20 64 -Sample rate d 300: 69 76 69 64 65 72 3a 20 00 ivider: . 00000309 <__c.3465>: 309: 0a 00 .. 0000030b <__c.3463>: 30b: 20 20 2d 53 61 6d 70 6c 65 20 72 61 74 65 20 5b -Sample rate [ 31b: 48 7a 5d 3a 20 00 Hz]: . 00000321 <__c.3461>: 321: 20 20 2d 49 54 47 2d 33 32 30 30 20 33 20 61 78 -ITG-3200 3 ax 331: 65 73 20 67 79 72 6f 20 63 6f 6e 66 69 67 75 72 es gyro configur 341: 65 64 20 2e 2e 2e 0a 00 ed ..... 00000349 <__c.3459>: 349: 43 6f 6e 66 69 67 75 72 65 20 49 54 47 20 73 65 Configure ITG se 359: 6e 73 6f 72 3a 0a 00 nsor:.. 00000360 <__c.3457>: 360: 20 20 2d 20 46 41 49 4c 55 52 45 0a 00 - FAILURE.. 0000036d <__c.3455>: 36d: 20 20 2d 49 54 47 2d 33 32 30 30 20 33 20 61 78 -ITG-3200 3 ax 37d: 65 73 20 67 79 72 6f 20 66 6f 75 6e 64 20 2e 2e es gyro found .. 38d: 2e 0a 00 ... 00000390 <__c.3453>: 390: 43 68 65 63 6b 69 6e 67 20 49 54 47 20 63 6f 6d Checking ITG com 3a0: 6d 75 6e 69 63 61 74 69 6f 6e 73 3a 0a 00 munications:.. 000003ae <__c.3439>: 3ae: 52 65 63 6f 76 65 72 5f 49 54 47 5f 54 57 49 5f Recover_ITG_TWI_ 3be: 66 72 6f 6d 5f 41 52 42 4c 6f 73 74 0a 00 from_ARBLost.. 000003cc <__c.3434>: 3cc: 49 54 47 20 52 45 43 4f 56 45 52 00 ITG RECOVER. 000003d8 <__c.3427>: 3d8: 49 54 47 20 41 52 42 4c 4f 53 54 0a 00 ITG ARBLOST.. 000003e5 <__c.3420>: 3e5: 49 54 47 20 42 55 53 45 52 52 0a 00 ITG BUSERR.. 000003f1 <__c.3360>: 3f1: 49 54 47 20 4e 41 43 4b 0a 00 ITG NACK.. 000003fb <__c.3350>: 3fb: 49 54 47 20 75 6e 65 78 70 65 63 74 65 64 20 63 ITG unexpected c 40b: 6f 6e 64 69 74 69 6f 6e 0a 00 ondition.. 00000415 <__c.3571>: 415: 42 4c 4f 43 4b 00 BLOCK. 0000041b <__c.3532>: 41b: 20 20 2d 20 46 41 49 4c 55 52 45 0a 00 - FAILURE.. 00000428 <__c.3530>: 428: 20 20 2d 20 4c 49 53 33 4c 56 30 32 44 51 20 65 - LIS3LV02DQ e 438: 6e 61 62 6c 65 64 20 2e 2e 2e 0a 0a 00 nabled ...... 00000445 <__c.3528>: 445: 45 6e 61 62 6c 65 20 4c 49 53 20 73 65 6e 73 6f Enable LIS senso 455: 72 3a 0a 00 r:.. 00000459 <__c.3526>: 459: 20 20 2d 20 46 41 49 4c 55 52 45 0a 00 - FAILURE.. 00000466 <__c.3524>: 466: 20 20 2d 20 4c 49 53 33 4c 56 30 32 44 51 20 63 - LIS3LV02DQ c 476: 6f 6e 66 69 67 75 72 65 64 20 2e 2e 2e 0a 00 onfigured ..... 00000485 <__c.3522>: 485: 43 6f 6e 66 69 67 75 72 65 20 4c 49 53 20 73 65 Configure LIS se 495: 6e 73 6f 72 3a 0a 00 nsor:.. 0000049c <__c.3520>: 49c: 20 20 2d 20 46 41 49 4c 55 52 45 0a 00 - FAILURE.. 000004a9 <__c.3518>: 4a9: 20 20 2d 20 4c 49 53 33 4c 56 30 32 44 51 20 66 - LIS3LV02DQ f 4b9: 6f 75 6e 64 20 2e 2e 2e 0a 00 ound ..... 000004c3 <__c.3516>: 4c3: 43 68 65 63 6b 69 6e 67 20 4c 49 53 20 63 6f 6d Checking LIS com 4d3: 6d 75 6e 69 63 61 74 69 6f 6e 73 3a 0a 00 munications:.. 000004e1 <__c.3508>: 4e1: 52 65 63 6f 76 65 72 5f 4c 49 53 5f 54 57 49 5f Recover_LIS_TWI_ 4f1: 66 72 6f 6d 5f 41 52 42 4c 6f 73 74 0a 00 from_ARBLost.. 000004ff <__c.3503>: 4ff: 4c 49 53 20 52 45 43 4f 56 45 52 00 LIS RECOVER. 0000050b <__c.3422>: 50b: 4c 49 53 20 42 55 53 65 72 72 6f 72 0a 00 LIS BUSerror.. 00000519 <__c.3415>: 519: 4c 49 53 20 41 52 42 6c 6f 73 74 0a 00 LIS ARBlost.. 00000526 <__c.3411>: 526: 4c 49 53 20 4e 41 43 4b 0a 00 LIS NACK.. 00000530 : 530: 00 00 39 00 72 00 ab 00 e5 00 1e 01 57 01 91 01 ..9.r.......W... 540: ca 01 03 02 3c 02 76 02 af 02 e8 02 21 03 5b 03 ....<.v.....!.[. 550: 94 03 cd 03 06 04 40 04 79 04 b2 04 eb 04 24 05 ......@.y.....$. 560: 5e 05 97 05 d0 05 09 06 42 06 7b 06 b4 06 ee 06 ^.......B.{..... 570: 27 07 60 07 99 07 d2 07 0b 08 44 08 7d 08 b6 08 '.`.......D.}... 580: ef 08 28 09 61 09 99 09 d2 09 0b 0a 44 0a 7d 0a ..(.a.......D.}. 590: b6 0a ee 0a 27 0b 60 0b 99 0b d1 0b 0a 0c 43 0c ....'.`.......C. 5a0: 7b 0c b4 0c ed 0c 25 0d 5e 0d 96 0d cf 0d 07 0e {.....%.^....... 5b0: 40 0e 78 0e b0 0e e9 0e 21 0f 59 0f 91 0f ca 0f @.x.....!.Y..... 5c0: 02 10 3a 10 72 10 aa 10 e2 10 1a 11 52 11 8a 11 ..:.r.......R... 5d0: c2 11 fa 11 32 12 6a 12 a2 12 da 12 11 13 49 13 ....2.j.......I. 5e0: 81 13 b8 13 f0 13 28 14 5f 14 97 14 ce 14 05 15 ......(._....... 5f0: 3d 15 74 15 ab 15 e3 15 1a 16 51 16 88 16 bf 16 =.t.......Q..... 600: f6 16 2d 17 64 17 9b 17 d2 17 09 18 40 18 76 18 ..-.d.......@.v. 610: ad 18 e4 18 1a 19 51 19 87 19 be 19 f4 19 2b 1a ......Q.......+. 620: 61 1a 97 1a ce 1a 04 1b 3a 1b 70 1b a6 1b dc 1b a.......:.p..... 630: 12 1c 48 1c 7e 1c b4 1c e9 1c 1f 1d 55 1d 8a 1d ..H.~.......U... 640: c0 1d f5 1d 2b 1e 60 1e 96 1e cb 1e 00 1f 35 1f ....+.`.......5. 650: 6a 1f 9f 1f d4 1f 09 20 3e 20 73 20 a8 20 dd 20 j...... > s . . 660: 11 21 46 21 7b 21 af 21 e4 21 18 22 4c 22 81 22 .!F!{!.!.!."L"." 670: b5 22 e9 22 1d 23 51 23 85 23 b9 23 ed 23 21 24 .".".#Q#.#.#.#!$ 680: 54 24 88 24 bc 24 ef 24 23 25 56 25 8a 25 bd 25 T$.$.$.$#%V%.%.% 690: f0 25 24 26 57 26 8a 26 bd 26 f0 26 23 27 56 27 .%$&W&.&.&.&#'V' 6a0: 88 27 bb 27 ee 27 20 28 53 28 85 28 b8 28 ea 28 .'.'.' (S(.(.(.( 6b0: 1c 29 4e 29 81 29 b3 29 e5 29 17 2a 48 2a 7a 2a .)N).).).).*H*z* 6c0: ac 2a de 2a 0f 2b 41 2b 72 2b a4 2b d5 2b 06 2c .*.*.+A+r+.+.+., 6d0: 38 2c 69 2c 9a 2c cb 2c fc 2c 2d 2d 5d 2d 8e 2d 8,i,.,.,.,--]-.- 6e0: bf 2d ef 2d 20 2e 50 2e 81 2e b1 2e e1 2e 12 2f .-.- .P......../ 6f0: 42 2f 72 2f a2 2f d2 2f 02 30 31 30 61 30 91 30 B/r/././.010a0.0 700: c0 30 f0 30 1f 31 4f 31 7e 31 ad 31 dc 31 0b 32 .0.0.1O1~1.1.1.2 710: 3a 32 69 32 98 32 c7 32 f6 32 24 33 53 33 81 33 :2i2.2.2.2$3S3.3 720: b0 33 de 33 0c 34 3b 34 69 34 97 34 c5 34 f3 34 .3.3.4;4i4.4.4.4 730: 21 35 4f 35 7c 35 aa 35 d8 35 05 36 33 36 60 36 !5O5|5.5.5.636`6 740: 8d 36 ba 36 e8 36 15 37 42 37 6f 37 9b 37 c8 37 .6.6.6.7B7o7.7.7 750: f5 37 22 38 4e 38 7b 38 a7 38 d4 38 00 39 2c 39 .7"8N8{8.8.8.9,9 760: 58 39 84 39 b0 39 dc 39 08 3a 34 3a 5f 3a 8b 3a X9.9.9.9.:4:_:.: 770: b7 3a e2 3a 0e 3b 39 3b 64 3b 8f 3b bb 3b e6 3b .:.:.;9;d;.;.;.; 780: 11 3c 3b 3c 66 3c 91 3c bc 3c e6 3c 11 3d 3b 3d .<;9>c>.> 7a0: b6 3e e0 3e 0a 3f 33 3f 5d 3f 86 3f b0 3f d9 3f .>.>.?3?]?.?.?.? 7b0: 02 40 2b 40 55 40 7e 40 a6 40 cf 40 f8 40 21 41 .@+@U@~@.@.@.@!A 7c0: 4a 41 72 41 9b 41 c3 41 eb 41 14 42 3c 42 64 42 JArA.A.A.A.BR 8b0: 5f 52 7f 52 9f 52 c0 52 e0 52 00 53 20 53 40 53 _R.R.R.R.R.S S@S 8c0: 60 53 80 53 a0 53 c0 53 e0 53 ff 53 1f 54 3e 54 `S.S.S.S.S.S.T>T 8d0: 5e 54 7d 54 9d 54 bc 54 db 54 fa 54 1a 55 39 55 ^T}T.T.T.T.T.U9U 8e0: 58 55 77 55 95 55 b4 55 d3 55 f2 55 10 56 2f 56 XUwU.U.U.U.U.V/V 8f0: 4e 56 6c 56 8a 56 a9 56 c7 56 e5 56 03 57 22 57 NVlV.V.V.V.V.W"W 900: 40 57 5e 57 7c 57 99 57 b7 57 d5 57 f3 57 10 58 @W^W|W.W.W.W.W.X 910: 2e 58 4b 58 69 58 86 58 a4 58 c1 58 de 58 fb 58 .XKXiX.X.X.X.X.X 920: 19 59 36 59 53 59 70 59 8c 59 a9 59 c6 59 e3 59 .Y6YSYpY.Y.Y.Y.Y 930: 00 5a .Z 00000932 : 932: 00 00 02 00 04 00 07 00 09 00 0b 00 0d 00 10 00 ................ 942: 12 00 14 00 16 00 19 00 1b 00 1d 00 1f 00 22 00 ..............". 952: 24 00 26 00 28 00 2a 00 2d 00 2f 00 31 00 33 00 $.&.(.*.-./.1.3. 962: 36 00 38 00 3a 00 3c 00 3f 00 41 00 43 00 45 00 6.8.:.<.?.A.C.E. 972: 47 00 4a 00 4c 00 4e 00 50 00 53 00 55 00 57 00 G.J.L.N.P.S.U.W. 982: 59 00 5b 00 5e 00 60 00 62 00 64 00 67 00 69 00 Y.[.^.`.b.d.g.i. 992: 6b 00 6d 00 6f 00 72 00 74 00 76 00 78 00 7b 00 k.m.o.r.t.v.x.{. 9a2: 7d 00 7f 00 81 00 83 00 86 00 88 00 8a 00 8c 00 }............... 9b2: 8f 00 91 00 93 00 95 00 97 00 9a 00 9c 00 9e 00 ................ 9c2: a0 00 a2 00 a5 00 a7 00 a9 00 ab 00 ad 00 b0 00 ................ 9d2: b2 00 b4 00 b6 00 b8 00 bb 00 bd 00 bf 00 c1 00 ................ 9e2: c3 00 c6 00 c8 00 ca 00 cc 00 ce 00 d1 00 d3 00 ................ 9f2: d5 00 d7 00 d9 00 db 00 de 00 e0 00 e2 00 e4 00 ................ a02: e6 00 e9 00 eb 00 ed 00 ef 00 f1 00 f3 00 f6 00 ................ a12: f8 00 fa 00 fc 00 fe 00 00 01 03 01 05 01 07 01 ................ a22: 09 01 0b 01 0d 01 0f 01 12 01 14 01 16 01 18 01 ................ a32: 1a 01 1c 01 1f 01 21 01 23 01 25 01 27 01 29 01 ......!.#.%.'.). a42: 2b 01 2e 01 30 01 32 01 34 01 36 01 38 01 3a 01 +...0.2.4.6.8.:. a52: 3c 01 3f 01 41 01 43 01 45 01 47 01 49 01 4b 01 <.?.A.C.E.G.I.K. a62: 4d 01 4f 01 52 01 54 01 56 01 58 01 5a 01 5c 01 M.O.R.T.V.X.Z.\. a72: 5e 01 60 01 62 01 65 01 67 01 69 01 6b 01 6d 01 ^.`.b.e.g.i.k.m. a82: 6f 01 71 01 73 01 75 01 77 01 79 01 7b 01 7e 01 o.q.s.u.w.y.{.~. a92: 80 01 82 01 84 01 86 01 88 01 8a 01 8c 01 8e 01 ................ aa2: 90 01 92 01 94 01 96 01 98 01 9a 01 9c 01 9e 01 ................ ab2: a0 01 a3 01 a5 01 a7 01 a9 01 ab 01 ad 01 af 01 ................ ac2: b1 01 b3 01 b5 01 b7 01 b9 01 bb 01 bd 01 bf 01 ................ ad2: c1 01 c3 01 c5 01 c7 01 c9 01 cb 01 cd 01 cf 01 ................ ae2: d1 01 d3 01 d5 01 d7 01 d9 01 db 01 dd 01 df 01 ................ af2: e1 01 e3 01 e5 01 e7 01 e9 01 eb 01 ed 01 ee 01 ................ b02: f0 01 f2 01 f4 01 f6 01 f8 01 fa 01 fc 01 fe 01 ................ b12: 00 02 02 02 04 02 06 02 08 02 0a 02 0c 02 0d 02 ................ b22: 0f 02 11 02 13 02 15 02 17 02 19 02 1b 02 1d 02 ................ b32: 1f 02 21 02 22 02 24 02 26 02 28 02 2a 02 2c 02 ..!.".$.&.(.*.,. b42: 2e 02 30 02 31 02 33 02 35 02 37 02 39 02 3b 02 ..0.1.3.5.7.9.;. b52: 3d 02 3e 02 40 02 42 02 44 02 46 02 48 02 4a 02 =.>.@.B.D.F.H.J. b62: 4b 02 4d 02 4f 02 51 02 53 02 54 02 56 02 58 02 K.M.O.Q.S.T.V.X. b72: 5a 02 5c 02 5e 02 5f 02 61 02 63 02 65 02 66 02 Z.\.^._.a.c.e.f. b82: 68 02 6a 02 6c 02 6e 02 6f 02 71 02 73 02 75 02 h.j.l.n.o.q.s.u. b92: 76 02 78 02 7a 02 7c 02 7d 02 7f 02 81 02 83 02 v.x.z.|.}....... ba2: 84 02 86 02 88 02 8a 02 8b 02 8d 02 8f 02 91 02 ................ bb2: 92 02 94 02 96 02 97 02 99 02 9b 02 9c 02 9e 02 ................ bc2: a0 02 a1 02 a3 02 a5 02 a7 02 a8 02 aa 02 ac 02 ................ bd2: ad 02 af 02 b1 02 b2 02 b4 02 b5 02 b7 02 b9 02 ................ be2: ba 02 bc 02 be 02 bf 02 c1 02 c2 02 c4 02 c6 02 ................ bf2: c7 02 c9 02 cb 02 cc 02 ce 02 cf 02 d1 02 d2 02 ................ c02: d4 02 d6 02 d7 02 d9 02 da 02 dc 02 dd 02 df 02 ................ c12: e1 02 e2 02 e4 02 e5 02 e7 02 e8 02 ea 02 eb 02 ................ c22: ed 02 ee 02 f0 02 f1 02 f3 02 f4 02 f6 02 f7 02 ................ c32: f9 02 fa 02 fc 02 fd 02 ff 02 00 03 02 03 03 03 ................ c42: 05 03 06 03 08 03 09 03 0b 03 0c 03 0e 03 0f 03 ................ c52: 10 03 12 03 13 03 15 03 16 03 18 03 19 03 1a 03 ................ c62: 1c 03 1d 03 1f 03 20 03 21 03 23 03 24 03 26 03 ...... .!.#.$.&. c72: 27 03 28 03 2a 03 2b 03 2c 03 2e 03 2f 03 30 03 '.(.*.+.,.../.0. c82: 32 03 33 03 34 03 36 03 37 03 38 03 3a 03 3b 03 2.3.4.6.7.8.:.;. c92: 3c 03 3e 03 3f 03 40 03 42 03 43 03 44 03 46 03 <.>.?.@.B.C.D.F. ca2: 47 03 48 03 49 03 4b 03 4c 03 4d 03 4e 03 50 03 G.H.I.K.L.M.N.P. cb2: 51 03 52 03 53 03 55 03 56 03 57 03 58 03 5a 03 Q.R.S.U.V.W.X.Z. cc2: 5b 03 5c 03 5d 03 5e 03 60 03 61 03 62 03 63 03 [.\.].^.`.a.b.c. cd2: 64 03 66 03 67 03 68 03 69 03 6a 03 6b 03 6d 03 d.f.g.h.i.j.k.m. ce2: 6e 03 6f 03 70 03 71 03 72 03 73 03 75 03 76 03 n.o.p.q.r.s.u.v. cf2: 77 03 78 03 79 03 7a 03 7b 03 7c 03 7d 03 7f 03 w.x.y.z.{.|.}... d02: 80 03 81 03 82 03 83 03 84 03 85 03 86 03 87 03 ................ d12: 88 03 89 03 8a 03 8b 03 8c 03 8d 03 8e 03 8f 03 ................ d22: 90 03 91 03 92 03 93 03 94 03 95 03 96 03 97 03 ................ d32: 98 03 99 03 9a 03 9b 03 9c 03 9d 03 9e 03 9f 03 ................ d42: a0 03 a1 03 a2 03 a3 03 a4 03 a5 03 a6 03 a7 03 ................ d52: a7 03 a8 03 a9 03 aa 03 ab 03 ac 03 ad 03 ae 03 ................ d62: af 03 af 03 b0 03 b1 03 b2 03 b3 03 b4 03 b5 03 ................ d72: b5 03 b6 03 b7 03 b8 03 b9 03 ba 03 ba 03 bb 03 ................ d82: bc 03 bd 03 be 03 be 03 bf 03 c0 03 c1 03 c1 03 ................ d92: c2 03 c3 03 c4 03 c5 03 c5 03 c6 03 c7 03 c7 03 ................ da2: c8 03 c9 03 ca 03 ca 03 cb 03 cc 03 cc 03 cd 03 ................ db2: ce 03 cf 03 cf 03 d0 03 d1 03 d1 03 d2 03 d3 03 ................ dc2: d3 03 d4 03 d5 03 d5 03 d6 03 d6 03 d7 03 d8 03 ................ dd2: d8 03 d9 03 da 03 da 03 db 03 db 03 dc 03 dd 03 ................ de2: dd 03 de 03 de 03 df 03 df 03 e0 03 e0 03 e1 03 ................ df2: e2 03 e2 03 e3 03 e3 03 e4 03 e4 03 e5 03 e5 03 ................ e02: e6 03 e6 03 e7 03 e7 03 e8 03 e8 03 e9 03 e9 03 ................ e12: ea 03 ea 03 eb 03 eb 03 eb 03 ec 03 ec 03 ed 03 ................ e22: ed 03 ee 03 ee 03 ee 03 ef 03 ef 03 f0 03 f0 03 ................ e32: f0 03 f1 03 f1 03 f2 03 f2 03 f2 03 f3 03 f3 03 ................ e42: f3 03 f4 03 f4 03 f4 03 f5 03 f5 03 f5 03 f6 03 ................ e52: f6 03 f6 03 f7 03 f7 03 f7 03 f8 03 f8 03 f8 03 ................ e62: f8 03 f9 03 f9 03 f9 03 f9 03 fa 03 fa 03 fa 03 ................ e72: fa 03 fb 03 fb 03 fb 03 fb 03 fb 03 fc 03 fc 03 ................ e82: fc 03 fc 03 fc 03 fd 03 fd 03 fd 03 fd 03 fd 03 ................ e92: fe 03 fe 03 fe 03 fe 03 fe 03 fe 03 fe 03 fe 03 ................ ea2: ff 03 ff 03 ff 03 ff 03 ff 03 ff 03 ff 03 ff 03 ................ eb2: ff 03 ff 03 00 04 00 04 00 04 00 04 00 04 00 04 ................ ec2: 00 04 00 04 00 04 00 04 00 04 00 04 00 04 00 04 ................ ed2: 00 04 .. 00000ed4 : ed4: 00 00 02 00 04 00 07 00 09 00 0b 00 0d 00 10 00 ................ ee4: 12 00 14 00 16 00 19 00 1b 00 1d 00 1f 00 22 00 ..............". ef4: 24 00 26 00 28 00 2a 00 2d 00 2f 00 31 00 33 00 $.&.(.*.-./.1.3. f04: 36 00 38 00 3a 00 3c 00 3f 00 41 00 43 00 45 00 6.8.:.<.?.A.C.E. f14: 48 00 4a 00 4c 00 4e 00 51 00 53 00 55 00 57 00 H.J.L.N.Q.S.U.W. f24: 5a 00 5c 00 5e 00 60 00 63 00 65 00 67 00 69 00 Z.\.^.`.c.e.g.i. f34: 6c 00 6e 00 70 00 72 00 75 00 77 00 79 00 7b 00 l.n.p.r.u.w.y.{. f44: 7e 00 80 00 82 00 85 00 87 00 89 00 8b 00 8e 00 ~............... f54: 90 00 92 00 94 00 97 00 99 00 9b 00 9e 00 a0 00 ................ f64: a2 00 a4 00 a7 00 a9 00 ab 00 ae 00 b0 00 b2 00 ................ f74: b5 00 b7 00 b9 00 bb 00 be 00 c0 00 c2 00 c5 00 ................ f84: c7 00 c9 00 cc 00 ce 00 d0 00 d3 00 d5 00 d7 00 ................ f94: da 00 dc 00 de 00 e1 00 e3 00 e5 00 e8 00 ea 00 ................ fa4: ec 00 ef 00 f1 00 f3 00 f6 00 f8 00 fb 00 fd 00 ................ fb4: ff 00 02 01 04 01 06 01 09 01 0b 01 0e 01 10 01 ................ fc4: 12 01 15 01 17 01 1a 01 1c 01 1e 01 21 01 23 01 ............!.#. fd4: 26 01 28 01 2a 01 2d 01 2f 01 32 01 34 01 37 01 &.(.*.-./.2.4.7. fe4: 39 01 3c 01 3e 01 40 01 43 01 45 01 48 01 4a 01 9.<.>.@.C.E.H.J. ff4: 4d 01 4f 01 52 01 54 01 57 01 59 01 5c 01 5e 01 M.O.R.T.W.Y.\.^. 1004: 61 01 63 01 66 01 68 01 6b 01 6d 01 70 01 72 01 a.c.f.h.k.m.p.r. 1014: 75 01 77 01 7a 01 7c 01 7f 01 81 01 84 01 87 01 u.w.z.|......... 1024: 89 01 8c 01 8e 01 91 01 93 01 96 01 99 01 9b 01 ................ 1034: 9e 01 a0 01 a3 01 a6 01 a8 01 ab 01 ad 01 b0 01 ................ 1044: b3 01 b5 01 b8 01 bb 01 bd 01 c0 01 c3 01 c5 01 ................ 1054: c8 01 cb 01 cd 01 d0 01 d3 01 d5 01 d8 01 db 01 ................ 1064: dd 01 e0 01 e3 01 e6 01 e8 01 eb 01 ee 01 f1 01 ................ 1074: f3 01 f6 01 f9 01 fc 01 ff 01 01 02 04 02 07 02 ................ 1084: 0a 02 0d 02 0f 02 12 02 15 02 18 02 1b 02 1e 02 ................ 1094: 20 02 23 02 26 02 29 02 2c 02 2f 02 32 02 35 02 .#.&.).,./.2.5. 10a4: 38 02 3b 02 3d 02 40 02 43 02 46 02 49 02 4c 02 8.;.=.@.C.F.I.L. 10b4: 4f 02 52 02 55 02 58 02 5b 02 5e 02 61 02 64 02 O.R.U.X.[.^.a.d. 10c4: 67 02 6a 02 6d 02 70 02 74 02 77 02 7a 02 7d 02 g.j.m.p.t.w.z.}. 10d4: 80 02 83 02 86 02 89 02 8c 02 90 02 93 02 96 02 ................ 10e4: 99 02 9c 02 9f 02 a3 02 a6 02 a9 02 ac 02 af 02 ................ 10f4: b3 02 b6 02 b9 02 bc 02 c0 02 c3 02 c6 02 ca 02 ................ 1104: cd 02 d0 02 d4 02 d7 02 da 02 de 02 e1 02 e5 02 ................ 1114: e8 02 eb 02 ef 02 f2 02 f6 02 f9 02 fd 02 00 03 ................ 1124: 04 03 07 03 0b 03 0e 03 12 03 15 03 19 03 1c 03 ................ 1134: 20 03 24 03 27 03 2b 03 2f 03 32 03 36 03 3a 03 .$.'.+./.2.6.:. 1144: 3d 03 41 03 45 03 48 03 4c 03 50 03 54 03 57 03 =.A.E.H.L.P.T.W. 1154: 5b 03 5f 03 63 03 67 03 6b 03 6e 03 72 03 76 03 [._.c.g.k.n.r.v. 1164: 7a 03 7e 03 82 03 86 03 8a 03 8e 03 92 03 96 03 z.~............. 1174: 9a 03 9e 03 a2 03 a6 03 aa 03 ae 03 b3 03 b7 03 ................ 1184: bb 03 bf 03 c3 03 c8 03 cc 03 d0 03 d4 03 d9 03 ................ 1194: dd 03 e1 03 e6 03 ea 03 ee 03 f3 03 f7 03 fc 03 ................ 11a4: 00 04 04 04 09 04 0d 04 12 04 17 04 1b 04 20 04 .............. . 11b4: 24 04 29 04 2e 04 32 04 37 04 3c 04 41 04 45 04 $.)...2.7.<.A.E. 11c4: 4a 04 4f 04 54 04 59 04 5d 04 62 04 67 04 6c 04 J.O.T.Y.].b.g.l. 11d4: 71 04 76 04 7b 04 80 04 85 04 8b 04 90 04 95 04 q.v.{........... 11e4: 9a 04 9f 04 a4 04 aa 04 af 04 b4 04 ba 04 bf 04 ................ 11f4: c4 04 ca 04 cf 04 d5 04 da 04 e0 04 e5 04 eb 04 ................ 1204: f1 04 f6 04 fc 04 02 05 07 05 0d 05 13 05 19 05 ................ 1214: 1f 05 25 05 2b 05 30 05 37 05 3d 05 43 05 49 05 ..%.+.0.7.=.C.I. 1224: 4f 05 55 05 5b 05 62 05 68 05 6e 05 75 05 7b 05 O.U.[.b.h.n.u.{. 1234: 81 05 88 05 8e 05 95 05 9c 05 a2 05 a9 05 b0 05 ................ 1244: b6 05 bd 05 c4 05 cb 05 d2 05 d9 05 e0 05 e7 05 ................ 1254: ee 05 f5 05 fd 05 04 06 0b 06 12 06 1a 06 21 06 ..............!. 1264: 29 06 30 06 38 06 40 06 47 06 4f 06 57 06 5f 06 ).0.8.@.G.O.W._. 1274: 67 06 6f 06 77 06 7f 06 87 06 8f 06 97 06 a0 06 g.o.w........... 1284: a8 06 b1 06 b9 06 c2 06 ca 06 d3 06 dc 06 e5 06 ................ 1294: ee 06 f7 06 00 07 09 07 12 07 1b 07 24 07 2e 07 ............$... 12a4: 37 07 41 07 4b 07 54 07 5e 07 68 07 72 07 7c 07 7.A.K.T.^.h.r.|. 12b4: 86 07 90 07 9a 07 a5 07 af 07 ba 07 c4 07 cf 07 ................ 12c4: da 07 e5 07 f0 07 fb 07 06 08 11 08 1c 08 28 08 ..............(. 12d4: 34 08 3f 08 4b 08 57 08 63 08 6f 08 7b 08 88 08 4.?.K.W.c.o.{... 12e4: 94 08 a1 08 ad 08 ba 08 c7 08 d4 08 e1 08 ef 08 ................ 12f4: fc 08 0a 09 17 09 25 09 33 09 41 09 4f 09 5e 09 ......%.3.A.O.^. 1304: 6c 09 7b 09 8a 09 99 09 a8 09 b7 09 c7 09 d7 09 l.{............. 1314: e6 09 f6 09 07 0a 17 0a 28 0a 38 0a 49 0a 5a 0a ........(.8.I.Z. 1324: 6c 0a 7d 0a 8f 0a a1 0a b3 0a c5 0a d8 0a ea 0a l.}............. 1334: fd 0a 11 0b 24 0b 38 0b 4c 0b 60 0b 74 0b 89 0b ....$.8.L.`.t... 1344: 9e 0b b3 0b c9 0b de 0b f4 0b 0b 0c 21 0c 38 0c ............!.8. 1354: 50 0c 67 0c 7f 0c 97 0c b0 0c c9 0c e2 0c fb 0c P.g............. 1364: 15 0d 30 0d 4a 0d 65 0d 81 0d 9d 0d b9 0d d6 0d ..0.J.e......... 1374: f3 0d 11 0e 2f 0e 4d 0e 6c 0e 8c 0e ac 0e cd 0e ..../.M.l....... 1384: ee 0e 0f 0f 31 0f 54 0f 78 0f 9b 0f c0 0f e5 0f ....1.T.x....... 1394: 0b 10 32 10 59 10 81 10 a9 10 d3 10 fd 10 28 11 ..2.Y.........(. 13a4: 53 11 80 11 ad 11 dc 11 0b 12 3b 12 6c 12 9e 12 S.........;.l... 13b4: d2 12 06 13 3b 13 72 13 a9 13 e2 13 1c 14 57 14 ....;.r.......W. 13c4: 94 14 d2 14 12 15 53 15 95 15 d9 15 1f 16 66 16 ......S.......f. 13d4: af 16 fa 16 47 17 96 17 e7 17 3a 18 90 18 e7 18 ....G.....:..... 13e4: 41 19 9e 19 fd 19 5f 1a c4 1a 2c 1b 96 1b 05 1c A....._...,..... 13f4: 76 1c eb 1c 64 1d e1 1d 62 1e e7 1e 71 1f 00 20 v...d...b...q.. 1404: 94 20 2d 21 cc 21 70 22 1c 23 cd 23 86 24 46 25 . -!.!p".#.#.$F% 1414: 0f 26 e0 26 b9 27 9d 28 8b 29 83 2a 88 2b 99 2c .&.&.'.(.).*.+., 1424: b8 2d .- 00001426 <__ctors_end>: 1426: 11 24 eor r1, r1 1428: 1f be out 0x3f, r1 ; 63 142a: cf ef ldi r28, 0xFF ; 255 142c: df e3 ldi r29, 0x3F ; 63 142e: de bf out 0x3e, r29 ; 62 1430: cd bf out 0x3d, r28 ; 61 1432: 00 e0 ldi r16, 0x00 ; 0 1434: 0c bf out 0x3c, r16 ; 60 1436: 18 be out 0x38, r1 ; 56 1438: 19 be out 0x39, r1 ; 57 143a: 1a be out 0x3a, r1 ; 58 143c: 1b be out 0x3b, r1 ; 59 0000143e <__do_copy_data>: 143e: 10 e2 ldi r17, 0x20 ; 32 1440: a0 e0 ldi r26, 0x00 ; 0 1442: b0 e2 ldi r27, 0x20 ; 32 1444: e8 ec ldi r30, 0xC8 ; 200 1446: f1 e3 ldi r31, 0x31 ; 49 1448: 00 e0 ldi r16, 0x00 ; 0 144a: 0b bf out 0x3b, r16 ; 59 144c: 02 c0 rjmp .+4 ; 0x1452 <__do_copy_data+0x14> 144e: 07 90 elpm r0, Z+ 1450: 0d 92 st X+, r0 1452: a8 30 cpi r26, 0x08 ; 8 1454: b1 07 cpc r27, r17 1456: d9 f7 brne .-10 ; 0x144e <__do_copy_data+0x10> 1458: 1b be out 0x3b, r1 ; 59 0000145a <__do_clear_bss>: 145a: 10 e2 ldi r17, 0x20 ; 32 145c: a8 e0 ldi r26, 0x08 ; 8 145e: b0 e2 ldi r27, 0x20 ; 32 1460: 01 c0 rjmp .+2 ; 0x1464 <.do_clear_bss_start> 00001462 <.do_clear_bss_loop>: 1462: 1d 92 st X+, r1 00001464 <.do_clear_bss_start>: 1464: a9 38 cpi r26, 0x89 ; 137 1466: b1 07 cpc r27, r17 1468: e1 f7 brne .-8 ; 0x1462 <.do_clear_bss_loop> 146a: 0e 94 23 10 call 0x2046 ; 0x2046
146e: 0c 94 e2 18 jmp 0x31c4 ; 0x31c4 <_exit> 00001472 <__bad_interrupt>: 1472: 0c 94 00 00 jmp 0 ; 0x0 <__vectors> 00001476 : #include "debug.h" /*****************************************************************************************************************************************/ inline void InitDebugUSART () { DEBUG_PORT.DIRSET = (1 << PIN3); 1476: e0 e4 ldi r30, 0x40 ; 64 1478: f6 e0 ldi r31, 0x06 ; 6 147a: 88 e0 ldi r24, 0x08 ; 8 147c: 81 83 std Z+1, r24 ; 0x01 DEBUG_PORT.DIRCLR = (1 << PIN2); 147e: 84 e0 ldi r24, 0x04 ; 4 1480: 82 83 std Z+2, r24 ; 0x02 USART_Format_Set (&DEBUG, USART_CMODE_ASYNCHRONOUS_gc, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, 0); 1482: e0 ea ldi r30, 0xA0 ; 160 1484: f8 e0 ldi r31, 0x08 ; 8 1486: 83 e0 ldi r24, 0x03 ; 3 1488: 85 83 std Z+5, r24 ; 0x05 USART_Baudrate_Set (&DEBUG, (SYS_CLK / (16 * DEBUG_USART_BAUD_RATE) - 1), 0); 148a: 8f e0 ldi r24, 0x0F ; 15 148c: 86 83 std Z+6, r24 ; 0x06 148e: 17 82 std Z+7, r1 ; 0x07 USART_TX_Enable (&DEBUG); 1490: 84 81 ldd r24, Z+4 ; 0x04 1492: 88 60 ori r24, 0x08 ; 8 1494: 84 83 std Z+4, r24 ; 0x04 } 1496: 08 95 ret 00001498 : USART_putc (&DEBUG, 0x30 + s16 % 10); } /*****************************************************************************************************************************************/ inline void Debug_i16 (int16_t s16) { 1498: cf 93 push r28 149a: df 93 push r29 149c: ec 01 movw r28, r24 if (s16 < 0) 149e: 97 ff sbrs r25, 7 14a0: 09 c0 rjmp .+18 ; 0x14b4 { USART_putc (&DEBUG, '-'); 14a2: 80 ea ldi r24, 0xA0 ; 160 14a4: 98 e0 ldi r25, 0x08 ; 8 14a6: 6d e2 ldi r22, 0x2D ; 45 14a8: 0e 94 03 10 call 0x2006 ; 0x2006 s16 = -s16; 14ac: d0 95 com r29 14ae: c1 95 neg r28 14b0: df 4f sbci r29, 0xFF ; 255 14b2: 05 c0 rjmp .+10 ; 0x14be } else USART_putc (&DEBUG, '+'); 14b4: 80 ea ldi r24, 0xA0 ; 160 14b6: 98 e0 ldi r25, 0x08 ; 8 14b8: 6b e2 ldi r22, 0x2B ; 43 14ba: 0e 94 03 10 call 0x2006 ; 0x2006 if (s16 >= 10000) 14be: 87 e2 ldi r24, 0x27 ; 39 14c0: c0 31 cpi r28, 0x10 ; 16 14c2: d8 07 cpc r29, r24 14c4: 5c f0 brlt .+22 ; 0x14dc USART_putc (&DEBUG, 0x30 + s16 / 10000); 14c6: ce 01 movw r24, r28 14c8: 60 e1 ldi r22, 0x10 ; 16 14ca: 77 e2 ldi r23, 0x27 ; 39 14cc: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 14d0: 60 5d subi r22, 0xD0 ; 208 14d2: 80 ea ldi r24, 0xA0 ; 160 14d4: 98 e0 ldi r25, 0x08 ; 8 14d6: 0e 94 03 10 call 0x2006 ; 0x2006 14da: 09 c0 rjmp .+18 ; 0x14ee else USART_putc (&DEBUG, ' '); 14dc: 80 ea ldi r24, 0xA0 ; 160 14de: 98 e0 ldi r25, 0x08 ; 8 14e0: 60 e2 ldi r22, 0x20 ; 32 14e2: 0e 94 03 10 call 0x2006 ; 0x2006 if (s16 >= 1000) 14e6: 83 e0 ldi r24, 0x03 ; 3 14e8: c8 3e cpi r28, 0xE8 ; 232 14ea: d8 07 cpc r29, r24 14ec: 7c f0 brlt .+30 ; 0x150c USART_putc (&DEBUG, 0x30 + (s16 % 10000) / 1000); 14ee: ce 01 movw r24, r28 14f0: 60 e1 ldi r22, 0x10 ; 16 14f2: 77 e2 ldi r23, 0x27 ; 39 14f4: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 14f8: 68 ee ldi r22, 0xE8 ; 232 14fa: 73 e0 ldi r23, 0x03 ; 3 14fc: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1500: 60 5d subi r22, 0xD0 ; 208 1502: 80 ea ldi r24, 0xA0 ; 160 1504: 98 e0 ldi r25, 0x08 ; 8 1506: 0e 94 03 10 call 0x2006 ; 0x2006 150a: 08 c0 rjmp .+16 ; 0x151c else USART_putc (&DEBUG, ' '); 150c: 80 ea ldi r24, 0xA0 ; 160 150e: 98 e0 ldi r25, 0x08 ; 8 1510: 60 e2 ldi r22, 0x20 ; 32 1512: 0e 94 03 10 call 0x2006 ; 0x2006 if (s16 >= 100) 1516: c4 36 cpi r28, 0x64 ; 100 1518: d1 05 cpc r29, r1 151a: 7c f0 brlt .+30 ; 0x153a USART_putc (&DEBUG, 0x30 + (s16 % 1000) / 100); 151c: ce 01 movw r24, r28 151e: 68 ee ldi r22, 0xE8 ; 232 1520: 73 e0 ldi r23, 0x03 ; 3 1522: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1526: 64 e6 ldi r22, 0x64 ; 100 1528: 70 e0 ldi r23, 0x00 ; 0 152a: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 152e: 60 5d subi r22, 0xD0 ; 208 1530: 80 ea ldi r24, 0xA0 ; 160 1532: 98 e0 ldi r25, 0x08 ; 8 1534: 0e 94 03 10 call 0x2006 ; 0x2006 1538: 08 c0 rjmp .+16 ; 0x154a else USART_putc (&DEBUG, ' '); 153a: 80 ea ldi r24, 0xA0 ; 160 153c: 98 e0 ldi r25, 0x08 ; 8 153e: 60 e2 ldi r22, 0x20 ; 32 1540: 0e 94 03 10 call 0x2006 ; 0x2006 if (s16 >= 10) 1544: ca 30 cpi r28, 0x0A ; 10 1546: d1 05 cpc r29, r1 1548: 6c f0 brlt .+26 ; 0x1564 USART_putc (&DEBUG, 0x30 + (s16 % 100) / 10); 154a: ce 01 movw r24, r28 154c: 64 e6 ldi r22, 0x64 ; 100 154e: 70 e0 ldi r23, 0x00 ; 0 1550: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1554: 6a e0 ldi r22, 0x0A ; 10 1556: 70 e0 ldi r23, 0x00 ; 0 1558: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 155c: 60 5d subi r22, 0xD0 ; 208 155e: 80 ea ldi r24, 0xA0 ; 160 1560: 98 e0 ldi r25, 0x08 ; 8 1562: 03 c0 rjmp .+6 ; 0x156a else USART_putc (&DEBUG, ' '); 1564: 80 ea ldi r24, 0xA0 ; 160 1566: 98 e0 ldi r25, 0x08 ; 8 1568: 60 e2 ldi r22, 0x20 ; 32 156a: 0e 94 03 10 call 0x2006 ; 0x2006 USART_putc (&DEBUG, 0x30 + s16 % 10); 156e: ce 01 movw r24, r28 1570: 6a e0 ldi r22, 0x0A ; 10 1572: 70 e0 ldi r23, 0x00 ; 0 1574: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1578: 68 2f mov r22, r24 157a: 60 5d subi r22, 0xD0 ; 208 157c: 80 ea ldi r24, 0xA0 ; 160 157e: 98 e0 ldi r25, 0x08 ; 8 1580: 0e 94 03 10 call 0x2006 ; 0x2006 } 1584: df 91 pop r29 1586: cf 91 pop r28 1588: 08 95 ret 0000158a : USART_putc (&DEBUG, 0x30 + u8 % 10); } /*****************************************************************************************************************************************/ inline void Debug_i16_left (int16_t s16) { 158a: cf 93 push r28 158c: df 93 push r29 158e: ec 01 movw r28, r24 if (s16 < 0) 1590: 97 ff sbrs r25, 7 1592: 09 c0 rjmp .+18 ; 0x15a6 { USART_putc (&DEBUG, '-'); 1594: 80 ea ldi r24, 0xA0 ; 160 1596: 98 e0 ldi r25, 0x08 ; 8 1598: 6d e2 ldi r22, 0x2D ; 45 159a: 0e 94 03 10 call 0x2006 ; 0x2006 s16 = -s16; 159e: d0 95 com r29 15a0: c1 95 neg r28 15a2: df 4f sbci r29, 0xFF ; 255 15a4: 05 c0 rjmp .+10 ; 0x15b0 } else USART_putc (&DEBUG, '+'); 15a6: 80 ea ldi r24, 0xA0 ; 160 15a8: 98 e0 ldi r25, 0x08 ; 8 15aa: 6b e2 ldi r22, 0x2B ; 43 15ac: 0e 94 03 10 call 0x2006 ; 0x2006 if (s16 >= 10000) 15b0: 87 e2 ldi r24, 0x27 ; 39 15b2: c0 31 cpi r28, 0x10 ; 16 15b4: d8 07 cpc r29, r24 15b6: 5c f0 brlt .+22 ; 0x15ce USART_putc (&DEBUG, 0x30 + s16 / 10000); 15b8: ce 01 movw r24, r28 15ba: 60 e1 ldi r22, 0x10 ; 16 15bc: 77 e2 ldi r23, 0x27 ; 39 15be: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 15c2: 60 5d subi r22, 0xD0 ; 208 15c4: 80 ea ldi r24, 0xA0 ; 160 15c6: 98 e0 ldi r25, 0x08 ; 8 15c8: 0e 94 03 10 call 0x2006 ; 0x2006 15cc: 04 c0 rjmp .+8 ; 0x15d6 if (s16 >= 1000) 15ce: 83 e0 ldi r24, 0x03 ; 3 15d0: c8 3e cpi r28, 0xE8 ; 232 15d2: d8 07 cpc r29, r24 15d4: 7c f0 brlt .+30 ; 0x15f4 USART_putc (&DEBUG, 0x30 + (s16 % 10000) / 1000); 15d6: ce 01 movw r24, r28 15d8: 60 e1 ldi r22, 0x10 ; 16 15da: 77 e2 ldi r23, 0x27 ; 39 15dc: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 15e0: 68 ee ldi r22, 0xE8 ; 232 15e2: 73 e0 ldi r23, 0x03 ; 3 15e4: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 15e8: 60 5d subi r22, 0xD0 ; 208 15ea: 80 ea ldi r24, 0xA0 ; 160 15ec: 98 e0 ldi r25, 0x08 ; 8 15ee: 0e 94 03 10 call 0x2006 ; 0x2006 15f2: 03 c0 rjmp .+6 ; 0x15fa if (s16 >= 100) 15f4: c4 36 cpi r28, 0x64 ; 100 15f6: d1 05 cpc r29, r1 15f8: 7c f0 brlt .+30 ; 0x1618 USART_putc (&DEBUG, 0x30 + (s16 % 1000) / 100); 15fa: ce 01 movw r24, r28 15fc: 68 ee ldi r22, 0xE8 ; 232 15fe: 73 e0 ldi r23, 0x03 ; 3 1600: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1604: 64 e6 ldi r22, 0x64 ; 100 1606: 70 e0 ldi r23, 0x00 ; 0 1608: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 160c: 60 5d subi r22, 0xD0 ; 208 160e: 80 ea ldi r24, 0xA0 ; 160 1610: 98 e0 ldi r25, 0x08 ; 8 1612: 0e 94 03 10 call 0x2006 ; 0x2006 1616: 03 c0 rjmp .+6 ; 0x161e if (s16 >= 10) 1618: ca 30 cpi r28, 0x0A ; 10 161a: d1 05 cpc r29, r1 161c: 74 f0 brlt .+28 ; 0x163a USART_putc (&DEBUG, 0x30 + (s16 % 100) / 10); 161e: ce 01 movw r24, r28 1620: 64 e6 ldi r22, 0x64 ; 100 1622: 70 e0 ldi r23, 0x00 ; 0 1624: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1628: 6a e0 ldi r22, 0x0A ; 10 162a: 70 e0 ldi r23, 0x00 ; 0 162c: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1630: 60 5d subi r22, 0xD0 ; 208 1632: 80 ea ldi r24, 0xA0 ; 160 1634: 98 e0 ldi r25, 0x08 ; 8 1636: 0e 94 03 10 call 0x2006 ; 0x2006 USART_putc (&DEBUG, 0x30 + s16 % 10); 163a: ce 01 movw r24, r28 163c: 6a e0 ldi r22, 0x0A ; 10 163e: 70 e0 ldi r23, 0x00 ; 0 1640: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1644: 68 2f mov r22, r24 1646: 60 5d subi r22, 0xD0 ; 208 1648: 80 ea ldi r24, 0xA0 ; 160 164a: 98 e0 ldi r25, 0x08 ; 8 164c: 0e 94 03 10 call 0x2006 ; 0x2006 } 1650: df 91 pop r29 1652: cf 91 pop r28 1654: 08 95 ret 00001656 : Debug_ch(pgm_read_byte(str++)); } /*****************************************************************************************************************************************/ inline void Debug_u8 (uint8_t u8) { 1656: 1f 93 push r17 1658: 18 2f mov r17, r24 USART_putc (&DEBUG, 0x30 + u8 / 100); 165a: 64 e6 ldi r22, 0x64 ; 100 165c: 0e 94 72 18 call 0x30e4 ; 0x30e4 <__udivmodqi4> 1660: 68 2f mov r22, r24 1662: 60 5d subi r22, 0xD0 ; 208 1664: 80 ea ldi r24, 0xA0 ; 160 1666: 98 e0 ldi r25, 0x08 ; 8 1668: 0e 94 03 10 call 0x2006 ; 0x2006 USART_putc (&DEBUG, 0x30 + (u8 % 100) / 10); 166c: 81 2f mov r24, r17 166e: 64 e6 ldi r22, 0x64 ; 100 1670: 0e 94 72 18 call 0x30e4 ; 0x30e4 <__udivmodqi4> 1674: 89 2f mov r24, r25 1676: 6a e0 ldi r22, 0x0A ; 10 1678: 0e 94 72 18 call 0x30e4 ; 0x30e4 <__udivmodqi4> 167c: 68 2f mov r22, r24 167e: 60 5d subi r22, 0xD0 ; 208 1680: 80 ea ldi r24, 0xA0 ; 160 1682: 98 e0 ldi r25, 0x08 ; 8 1684: 0e 94 03 10 call 0x2006 ; 0x2006 USART_putc (&DEBUG, 0x30 + u8 % 10); 1688: 81 2f mov r24, r17 168a: 6a e0 ldi r22, 0x0A ; 10 168c: 0e 94 72 18 call 0x30e4 ; 0x30e4 <__udivmodqi4> 1690: 69 2f mov r22, r25 1692: 60 5d subi r22, 0xD0 ; 208 1694: 80 ea ldi r24, 0xA0 ; 160 1696: 98 e0 ldi r25, 0x08 ; 8 1698: 0e 94 03 10 call 0x2006 ; 0x2006 } 169c: 1f 91 pop r17 169e: 08 95 ret 000016a0 : USART_TX_Enable (&DEBUG); } /*****************************************************************************************************************************************/ inline void Debug_ch (char ch) { 16a0: 68 2f mov r22, r24 USART_putc (&DEBUG, ch); 16a2: 80 ea ldi r24, 0xA0 ; 160 16a4: 98 e0 ldi r25, 0x08 ; 8 16a6: 0e 94 03 10 call 0x2006 ; 0x2006 } 16aa: 08 95 ret 000016ac : /*****************************************************************************************************************************************/ inline void dputsP (const char * str) { 16ac: cf 93 push r28 16ae: df 93 push r29 16b0: ec 01 movw r28, r24 16b2: 07 c0 rjmp .+14 ; 0x16c2 while (pgm_read_byte(str) != 0x00) Debug_ch(pgm_read_byte(str++)); 16b4: 21 96 adiw r28, 0x01 ; 1 16b6: f9 01 movw r30, r18 16b8: 64 91 lpm r22, Z+ } /*****************************************************************************************************************************************/ inline void Debug_ch (char ch) { USART_putc (&DEBUG, ch); 16ba: 80 ea ldi r24, 0xA0 ; 160 16bc: 98 e0 ldi r25, 0x08 ; 8 16be: 0e 94 03 10 call 0x2006 ; 0x2006 16c2: 9e 01 movw r18, r28 } /*****************************************************************************************************************************************/ inline void dputsP (const char * str) { while (pgm_read_byte(str) != 0x00) 16c4: fe 01 movw r30, r28 16c6: 84 91 lpm r24, Z+ 16c8: 88 23 and r24, r24 16ca: a1 f7 brne .-24 ; 0x16b4 Debug_ch(pgm_read_byte(str++)); } 16cc: df 91 pop r29 16ce: cf 91 pop r28 16d0: 08 95 ret 000016d2 : } /*****************************************************************************************************************************************/ uint8_t UpdateITGSensorValues () { if (GetLastITGActionStatus() == ITG_ACTION_GETTEMPXYZ) 16d2: 0e 94 b7 10 call 0x216e ; 0x216e 16d6: 84 30 cpi r24, 0x04 ; 4 16d8: 11 f0 breq .+4 ; 0x16de 16da: 80 e0 ldi r24, 0x00 ; 0 16dc: 08 95 ret { Get_ITGRawValues (GyroRaw_i16_m14d375__deg_d_s); 16de: 83 e4 ldi r24, 0x43 ; 67 16e0: 90 e2 ldi r25, 0x20 ; 32 16e2: 0e 94 90 10 call 0x2120 ; 0x2120 16e6: 81 e0 ldi r24, 0x01 ; 1 return 1; } return 0; } 16e8: 08 95 ret 000016ea : } /*****************************************************************************************************************************************/ uint8_t UpdateLISSensorValues () { 16ea: 6f 92 push r6 16ec: 7f 92 push r7 16ee: 8f 92 push r8 16f0: 9f 92 push r9 16f2: af 92 push r10 16f4: bf 92 push r11 16f6: cf 92 push r12 16f8: df 92 push r13 16fa: ef 92 push r14 16fc: ff 92 push r15 16fe: 0f 93 push r16 1700: 1f 93 push r17 if (Get_LIS_LastActionStatus() == LIS_ACTION_GETXYZ) 1702: 0e 94 8c 13 call 0x2718 ; 0x2718 1706: 85 30 cpi r24, 0x05 ; 5 1708: 11 f0 breq .+4 ; 0x170e 170a: 80 e0 ldi r24, 0x00 ; 0 170c: 3f c0 rjmp .+126 ; 0x178c { Get_LISRawValues (AccelRaw_i16_m1024__1g); 170e: 8f e5 ldi r24, 0x5F ; 95 1710: 90 e2 ldi r25, 0x20 ; 32 1712: 0e 94 68 13 call 0x26d0 ; 0x26d0 // Calculate resulting acceleration vector length² AccelVectorLength = ((int32_t)AccelRaw_i16_m1024__1g[0] * (int32_t)AccelRaw_i16_m1024__1g[0]) + 1716: a0 90 5f 20 lds r10, 0x205F 171a: b0 90 60 20 lds r11, 0x2060 171e: cc 24 eor r12, r12 1720: b7 fc sbrc r11, 7 1722: c0 94 com r12 1724: dc 2c mov r13, r12 1726: 20 91 61 20 lds r18, 0x2061 172a: 30 91 62 20 lds r19, 0x2062 172e: 44 27 eor r20, r20 1730: 37 fd sbrc r19, 7 1732: 40 95 com r20 1734: 54 2f mov r21, r20 1736: 60 90 63 20 lds r6, 0x2063 173a: 70 90 64 20 lds r7, 0x2064 173e: 88 24 eor r8, r8 1740: 77 fc sbrc r7, 7 1742: 80 94 com r8 1744: 98 2c mov r9, r8 1746: ca 01 movw r24, r20 1748: b9 01 movw r22, r18 174a: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 174e: 7b 01 movw r14, r22 1750: 8c 01 movw r16, r24 1752: c6 01 movw r24, r12 1754: b5 01 movw r22, r10 1756: a6 01 movw r20, r12 1758: 95 01 movw r18, r10 175a: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 175e: e6 0e add r14, r22 1760: f7 1e adc r15, r23 1762: 08 1f adc r16, r24 1764: 19 1f adc r17, r25 1766: c4 01 movw r24, r8 1768: b3 01 movw r22, r6 176a: a4 01 movw r20, r8 176c: 93 01 movw r18, r6 176e: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1772: e6 0e add r14, r22 1774: f7 1e adc r15, r23 1776: 08 1f adc r16, r24 1778: 19 1f adc r17, r25 177a: e0 92 5b 20 sts 0x205B, r14 177e: f0 92 5c 20 sts 0x205C, r15 1782: 00 93 5d 20 sts 0x205D, r16 1786: 10 93 5e 20 sts 0x205E, r17 178a: 81 e0 ldi r24, 0x01 ; 1 return 1; } return 0; } 178c: 1f 91 pop r17 178e: 0f 91 pop r16 1790: ff 90 pop r15 1792: ef 90 pop r14 1794: df 90 pop r13 1796: cf 90 pop r12 1798: bf 90 pop r11 179a: af 90 pop r10 179c: 9f 90 pop r9 179e: 8f 90 pop r8 17a0: 7f 90 pop r7 17a2: 6f 90 pop r6 17a4: 08 95 ret 000017a6 : { static uint8_t count = 0; uint8_t i, ascii = 1; if (count++ == 9) 17a6: 80 91 38 20 lds r24, 0x2038 17aa: 8f 5f subi r24, 0xFF ; 255 17ac: 80 93 38 20 sts 0x2038, r24 17b0: 81 50 subi r24, 0x01 ; 1 17b2: 89 30 cpi r24, 0x09 ; 9 17b4: 51 f4 brne .+20 ; 0x17ca { count = 0; 17b6: 10 92 38 20 sts 0x2038, r1 LED_BLUE_TOGGLE; 17ba: 80 e8 ldi r24, 0x80 ; 128 17bc: e0 ea ldi r30, 0xA0 ; 160 17be: f6 e0 ldi r31, 0x06 ; 6 17c0: 87 83 std Z+7, r24 ; 0x07 PORTD.OUTTGL = (1 << PIN0); 17c2: 81 e0 ldi r24, 0x01 ; 1 17c4: e0 e6 ldi r30, 0x60 ; 96 17c6: f6 e0 ldi r31, 0x06 ; 6 17c8: 87 83 std Z+7, r24 ; 0x07 for (i = 0; i < 6; i++) Debug_ch (((char *)Debug_EulerAngle_Filter_i16_m8__deg)[i]); } else if (ascii == 1) { Debug_Str ("p,q,r: "); 17ca: 87 e2 ldi r24, 0x27 ; 39 17cc: 92 e0 ldi r25, 0x02 ; 2 17ce: 0e 94 56 0b call 0x16ac ; 0x16ac for (i = 0; i < 3; i++) Debug_i16 (Debug_BodyRate_i16_m8__deg_d_s[i] / 8); 17d2: 80 91 55 20 lds r24, 0x2055 17d6: 90 91 56 20 lds r25, 0x2056 17da: 68 e0 ldi r22, 0x08 ; 8 17dc: 70 e0 ldi r23, 0x00 ; 0 17de: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 17e2: cb 01 movw r24, r22 17e4: 0e 94 4c 0a call 0x1498 ; 0x1498 17e8: 80 91 57 20 lds r24, 0x2057 17ec: 90 91 58 20 lds r25, 0x2058 17f0: 68 e0 ldi r22, 0x08 ; 8 17f2: 70 e0 ldi r23, 0x00 ; 0 17f4: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 17f8: cb 01 movw r24, r22 17fa: 0e 94 4c 0a call 0x1498 ; 0x1498 17fe: 80 91 59 20 lds r24, 0x2059 1802: 90 91 5a 20 lds r25, 0x205A 1806: 68 e0 ldi r22, 0x08 ; 8 1808: 70 e0 ldi r23, 0x00 ; 0 180a: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 180e: cb 01 movw r24, r22 1810: 0e 94 4c 0a call 0x1498 ; 0x1498 Debug_Str (" ax,ay,az: "); 1814: 89 e1 ldi r24, 0x19 ; 25 1816: 92 e0 ldi r25, 0x02 ; 2 1818: 0e 94 56 0b call 0x16ac ; 0x16ac for (i = 0; i < 3; i++) Debug_i16 (AccelRaw_i16_m1024__1g[i]); 181c: 80 91 5f 20 lds r24, 0x205F 1820: 90 91 60 20 lds r25, 0x2060 1824: 0e 94 4c 0a call 0x1498 ; 0x1498 1828: 80 91 61 20 lds r24, 0x2061 182c: 90 91 62 20 lds r25, 0x2062 1830: 0e 94 4c 0a call 0x1498 ; 0x1498 1834: 80 91 63 20 lds r24, 0x2063 1838: 90 91 64 20 lds r25, 0x2064 183c: 0e 94 4c 0a call 0x1498 ; 0x1498 Debug_Str (" AccelAngles: "); 1840: 88 e0 ldi r24, 0x08 ; 8 1842: 92 e0 ldi r25, 0x02 ; 2 1844: 0e 94 56 0b call 0x16ac ; 0x16ac for (i = 0; i < 3; i++) Debug_i16 (Debug_EulerAngle_Accel_i16_m8__deg[i] / 8); 1848: 80 91 49 20 lds r24, 0x2049 184c: 90 91 4a 20 lds r25, 0x204A 1850: 68 e0 ldi r22, 0x08 ; 8 1852: 70 e0 ldi r23, 0x00 ; 0 1854: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1858: cb 01 movw r24, r22 185a: 0e 94 4c 0a call 0x1498 ; 0x1498 185e: 80 91 4b 20 lds r24, 0x204B 1862: 90 91 4c 20 lds r25, 0x204C 1866: 68 e0 ldi r22, 0x08 ; 8 1868: 70 e0 ldi r23, 0x00 ; 0 186a: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 186e: cb 01 movw r24, r22 1870: 0e 94 4c 0a call 0x1498 ; 0x1498 1874: 80 91 4d 20 lds r24, 0x204D 1878: 90 91 4e 20 lds r25, 0x204E 187c: 68 e0 ldi r22, 0x08 ; 8 187e: 70 e0 ldi r23, 0x00 ; 0 1880: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 1884: cb 01 movw r24, r22 1886: 0e 94 4c 0a call 0x1498 ; 0x1498 Debug_Str (" FilterAngles: "); 188a: 86 ef ldi r24, 0xF6 ; 246 188c: 91 e0 ldi r25, 0x01 ; 1 188e: 0e 94 56 0b call 0x16ac ; 0x16ac for (i = 0; i < 3; i++) Debug_i16 ((int16_t)(Debug_EulerAngle_Filter_i16_m8__deg[i] / 8)); 1892: 80 91 4f 20 lds r24, 0x204F 1896: 90 91 50 20 lds r25, 0x2050 189a: 68 e0 ldi r22, 0x08 ; 8 189c: 70 e0 ldi r23, 0x00 ; 0 189e: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 18a2: cb 01 movw r24, r22 18a4: 0e 94 4c 0a call 0x1498 ; 0x1498 18a8: 80 91 51 20 lds r24, 0x2051 18ac: 90 91 52 20 lds r25, 0x2052 18b0: 68 e0 ldi r22, 0x08 ; 8 18b2: 70 e0 ldi r23, 0x00 ; 0 18b4: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 18b8: cb 01 movw r24, r22 18ba: 0e 94 4c 0a call 0x1498 ; 0x1498 18be: 80 91 53 20 lds r24, 0x2053 18c2: 90 91 54 20 lds r25, 0x2054 18c6: 68 e0 ldi r22, 0x08 ; 8 18c8: 70 e0 ldi r23, 0x00 ; 0 18ca: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 18ce: cb 01 movw r24, r22 18d0: 0e 94 4c 0a call 0x1498 ; 0x1498 Debug_Str ("\n"); 18d4: 84 ef ldi r24, 0xF4 ; 244 18d6: 91 e0 ldi r25, 0x01 ; 1 18d8: 0e 94 56 0b call 0x16ac ; 0x16ac } } 18dc: 08 95 ret 000018de : int16_t Debug_EulerAngle_Filter_i16_m8__deg[3]; /*****************************************************************************************************************************************/ void FlightControl () { 18de: 2f 92 push r2 18e0: 3f 92 push r3 18e2: 4f 92 push r4 18e4: 5f 92 push r5 18e6: 6f 92 push r6 18e8: 7f 92 push r7 18ea: 8f 92 push r8 18ec: 9f 92 push r9 18ee: af 92 push r10 18f0: bf 92 push r11 18f2: cf 92 push r12 18f4: df 92 push r13 18f6: ef 92 push r14 18f8: ff 92 push r15 18fa: 0f 93 push r16 18fc: 1f 93 push r17 18fe: df 93 push r29 1900: cf 93 push r28 1902: cd b7 in r28, 0x3d ; 61 1904: de b7 in r29, 0x3e ; 62 1906: 6c 97 sbiw r28, 0x1c ; 28 1908: cd bf out 0x3d, r28 ; 61 190a: de bf out 0x3e, r29 ; 62 uint8_t i; // Check for new LIS values if (UpdateLISSensorValues ()) 190c: 0e 94 75 0b call 0x16ea ; 0x16ea 1910: 88 23 and r24, r24 1912: 31 f1 breq .+76 ; 0x1960 { if ( (AccelVectorLength > (629146)) && (AccelVectorLength < 1468006)) 1914: 80 91 5b 20 lds r24, 0x205B 1918: 90 91 5c 20 lds r25, 0x205C 191c: a0 91 5d 20 lds r26, 0x205D 1920: b0 91 5e 20 lds r27, 0x205E 1924: 8b 59 subi r24, 0x9B ; 155 1926: 99 49 sbci r25, 0x99 ; 153 1928: a9 40 sbci r26, 0x09 ; 9 192a: b0 40 sbci r27, 0x00 ; 0 192c: 8b 5c subi r24, 0xCB ; 203 192e: 9c 4c sbci r25, 0xCC ; 204 1930: ac 40 sbci r26, 0x0C ; 12 1932: b0 40 sbci r27, 0x00 ; 0 1934: 68 f4 brcc .+26 ; 0x1950 { FilterWeight = 41; 1936: 89 e2 ldi r24, 0x29 ; 41 1938: 90 e0 ldi r25, 0x00 ; 0 193a: a0 e0 ldi r26, 0x00 ; 0 193c: b0 e0 ldi r27, 0x00 ; 0 193e: 80 93 01 20 sts 0x2001, r24 1942: 90 93 02 20 sts 0x2002, r25 1946: a0 93 03 20 sts 0x2003, r26 194a: b0 93 04 20 sts 0x2004, r27 194e: 08 c0 rjmp .+16 ; 0x1960 } else { FilterWeight = 0; 1950: 10 92 01 20 sts 0x2001, r1 1954: 10 92 02 20 sts 0x2002, r1 1958: 10 92 03 20 sts 0x2003, r1 195c: 10 92 04 20 sts 0x2004, r1 } } // Check for new ITG values if (UpdateITGSensorValues ()) 1960: 0e 94 69 0b call 0x16d2 ; 0x16d2 1964: 88 23 and r24, r24 1966: 09 f4 brne .+2 ; 0x196a 1968: 08 c3 rjmp .+1552 ; 0x1f7a { // Convert and scale body fixed rates p, q, r // GyroRaw_i16_m14d375__deg_d_s range: +-2000 * 14.375 p = (int32_t)(((int32_t) GyroRaw_i16_m14d375__deg_d_s[2] * 8192L) / 115L); // p [deg/s] 196a: 60 91 47 20 lds r22, 0x2047 196e: 70 91 48 20 lds r23, 0x2048 1972: 88 27 eor r24, r24 1974: 77 fd sbrc r23, 7 1976: 80 95 com r24 1978: 98 2f mov r25, r24 197a: ed e0 ldi r30, 0x0D ; 13 197c: 66 0f add r22, r22 197e: 77 1f adc r23, r23 1980: 88 1f adc r24, r24 1982: 99 1f adc r25, r25 1984: ea 95 dec r30 1986: d1 f7 brne .-12 ; 0x197c 1988: 23 e7 ldi r18, 0x73 ; 115 198a: 30 e0 ldi r19, 0x00 ; 0 198c: 40 e0 ldi r20, 0x00 ; 0 198e: 50 e0 ldi r21, 0x00 ; 0 1990: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1994: 79 01 movw r14, r18 1996: 8a 01 movw r16, r20 1998: e0 92 08 20 sts 0x2008, r14 199c: f0 92 09 20 sts 0x2009, r15 19a0: 00 93 0a 20 sts 0x200A, r16 19a4: 10 93 0b 20 sts 0x200B, r17 q = (int32_t)(((int32_t)-GyroRaw_i16_m14d375__deg_d_s[0] * 8192L) / 115L); // q [deg/s] 19a8: 60 91 43 20 lds r22, 0x2043 19ac: 70 91 44 20 lds r23, 0x2044 19b0: 70 95 com r23 19b2: 61 95 neg r22 19b4: 7f 4f sbci r23, 0xFF ; 255 19b6: 88 27 eor r24, r24 19b8: 77 fd sbrc r23, 7 19ba: 80 95 com r24 19bc: 98 2f mov r25, r24 19be: 5d e0 ldi r21, 0x0D ; 13 19c0: 66 0f add r22, r22 19c2: 77 1f adc r23, r23 19c4: 88 1f adc r24, r24 19c6: 99 1f adc r25, r25 19c8: 5a 95 dec r21 19ca: d1 f7 brne .-12 ; 0x19c0 19cc: 23 e7 ldi r18, 0x73 ; 115 19ce: 30 e0 ldi r19, 0x00 ; 0 19d0: 40 e0 ldi r20, 0x00 ; 0 19d2: 50 e0 ldi r21, 0x00 ; 0 19d4: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 19d8: 29 8f std Y+25, r18 ; 0x19 19da: 3a 8f std Y+26, r19 ; 0x1a 19dc: 4b 8f std Y+27, r20 ; 0x1b 19de: 5c 8f std Y+28, r21 ; 0x1c 19e0: 20 93 0c 20 sts 0x200C, r18 19e4: 30 93 0d 20 sts 0x200D, r19 19e8: 40 93 0e 20 sts 0x200E, r20 19ec: 50 93 0f 20 sts 0x200F, r21 r = (int32_t)(((int32_t)-GyroRaw_i16_m14d375__deg_d_s[1] * 8192L) / 115L); // r [deg/s] 19f0: 60 91 45 20 lds r22, 0x2045 19f4: 70 91 46 20 lds r23, 0x2046 19f8: 70 95 com r23 19fa: 61 95 neg r22 19fc: 7f 4f sbci r23, 0xFF ; 255 19fe: 88 27 eor r24, r24 1a00: 77 fd sbrc r23, 7 1a02: 80 95 com r24 1a04: 98 2f mov r25, r24 1a06: 4d e0 ldi r20, 0x0D ; 13 1a08: 66 0f add r22, r22 1a0a: 77 1f adc r23, r23 1a0c: 88 1f adc r24, r24 1a0e: 99 1f adc r25, r25 1a10: 4a 95 dec r20 1a12: d1 f7 brne .-12 ; 0x1a08 1a14: 23 e7 ldi r18, 0x73 ; 115 1a16: 30 e0 ldi r19, 0x00 ; 0 1a18: 40 e0 ldi r20, 0x00 ; 0 1a1a: 50 e0 ldi r21, 0x00 ; 0 1a1c: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1a20: 20 93 10 20 sts 0x2010, r18 1a24: 30 93 11 20 sts 0x2011, r19 1a28: 40 93 12 20 sts 0x2012, r20 1a2c: 50 93 13 20 sts 0x2013, r21 // Calculate Euler rates from body rates (body fixed -> earth fixed) // Euler_vector_dot = Jacobi_Matrix * BodyRate_vector // Calculate Phi dot phi_dot = p + 1a30: 60 91 2c 20 lds r22, 0x202C 1a34: 70 91 2d 20 lds r23, 0x202D 1a38: 80 91 2e 20 lds r24, 0x202E 1a3c: 90 91 2f 20 lds r25, 0x202F 1a40: 0e 94 06 17 call 0x2e0c ; 0x2e0c 1a44: 3b 01 movw r6, r22 1a46: 4c 01 movw r8, r24 1a48: 60 91 30 20 lds r22, 0x2030 1a4c: 70 91 31 20 lds r23, 0x2031 1a50: 80 91 32 20 lds r24, 0x2032 1a54: 90 91 33 20 lds r25, 0x2033 1a58: 0e 94 a1 17 call 0x2f42 ; 0x2f42 1a5c: 5b 01 movw r10, r22 1a5e: 6c 01 movw r12, r24 1a60: 80 91 10 20 lds r24, 0x2010 1a64: 90 91 11 20 lds r25, 0x2011 1a68: a0 91 12 20 lds r26, 0x2012 1a6c: b0 91 13 20 lds r27, 0x2013 1a70: 8d 8b std Y+21, r24 ; 0x15 1a72: 9e 8b std Y+22, r25 ; 0x16 1a74: af 8b std Y+23, r26 ; 0x17 1a76: b8 8f std Y+24, r27 ; 0x18 1a78: 60 91 2c 20 lds r22, 0x202C 1a7c: 70 91 2d 20 lds r23, 0x202D 1a80: 80 91 2e 20 lds r24, 0x202E 1a84: 90 91 2f 20 lds r25, 0x202F 1a88: 0e 94 94 17 call 0x2f28 ; 0x2f28 1a8c: 69 8b std Y+17, r22 ; 0x11 1a8e: 7a 8b std Y+18, r23 ; 0x12 1a90: 8b 8b std Y+19, r24 ; 0x13 1a92: 9c 8b std Y+20, r25 ; 0x14 1a94: 60 91 30 20 lds r22, 0x2030 1a98: 70 91 31 20 lds r23, 0x2031 1a9c: 80 91 32 20 lds r24, 0x2032 1aa0: 90 91 33 20 lds r25, 0x2033 1aa4: 0e 94 a1 17 call 0x2f42 ; 0x2f42 1aa8: 1b 01 movw r2, r22 1aaa: 2c 01 movw r4, r24 1aac: c6 01 movw r24, r12 1aae: b5 01 movw r22, r10 1ab0: a4 01 movw r20, r8 1ab2: 93 01 movw r18, r6 1ab4: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1ab8: 20 e0 ldi r18, 0x00 ; 0 1aba: 34 e0 ldi r19, 0x04 ; 4 1abc: 40 e0 ldi r20, 0x00 ; 0 1abe: 50 e0 ldi r21, 0x00 ; 0 1ac0: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1ac4: ca 01 movw r24, r20 1ac6: b9 01 movw r22, r18 1ac8: 29 8d ldd r18, Y+25 ; 0x19 1aca: 3a 8d ldd r19, Y+26 ; 0x1a 1acc: 4b 8d ldd r20, Y+27 ; 0x1b 1ace: 5c 8d ldd r21, Y+28 ; 0x1c 1ad0: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1ad4: 20 e0 ldi r18, 0x00 ; 0 1ad6: 34 e0 ldi r19, 0x04 ; 4 1ad8: 40 e0 ldi r20, 0x00 ; 0 1ada: 50 e0 ldi r21, 0x00 ; 0 1adc: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1ae0: 59 01 movw r10, r18 1ae2: 6a 01 movw r12, r20 1ae4: c2 01 movw r24, r4 1ae6: b1 01 movw r22, r2 1ae8: 29 89 ldd r18, Y+17 ; 0x11 1aea: 3a 89 ldd r19, Y+18 ; 0x12 1aec: 4b 89 ldd r20, Y+19 ; 0x13 1aee: 5c 89 ldd r21, Y+20 ; 0x14 1af0: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1af4: 20 e0 ldi r18, 0x00 ; 0 1af6: 34 e0 ldi r19, 0x04 ; 4 1af8: 40 e0 ldi r20, 0x00 ; 0 1afa: 50 e0 ldi r21, 0x00 ; 0 1afc: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1b00: ca 01 movw r24, r20 1b02: b9 01 movw r22, r18 1b04: 2d 89 ldd r18, Y+21 ; 0x15 1b06: 3e 89 ldd r19, Y+22 ; 0x16 1b08: 4f 89 ldd r20, Y+23 ; 0x17 1b0a: 58 8d ldd r21, Y+24 ; 0x18 1b0c: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1b10: 20 e0 ldi r18, 0x00 ; 0 1b12: 34 e0 ldi r19, 0x04 ; 4 1b14: 40 e0 ldi r20, 0x00 ; 0 1b16: 50 e0 ldi r21, 0x00 ; 0 1b18: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1b1c: a2 0e add r10, r18 1b1e: b3 1e adc r11, r19 1b20: c4 1e adc r12, r20 1b22: d5 1e adc r13, r21 1b24: ea 0c add r14, r10 1b26: fb 1c adc r15, r11 1b28: 0c 1d adc r16, r12 1b2a: 1d 1d adc r17, r13 1b2c: e0 92 14 20 sts 0x2014, r14 1b30: f0 92 15 20 sts 0x2015, r15 1b34: 00 93 16 20 sts 0x2016, r16 1b38: 10 93 17 20 sts 0x2017, r17 ((q * ((sini(phi) * tani(theta)) / 1024L)) / 1024L) + ((r * ((cosi(phi) * tani(theta)) / 1024L)) / 1024L); // Calculate Theta dot theta_dot = ((q * cosi(phi)) / 1024L) - 1b3c: 60 90 0c 20 lds r6, 0x200C 1b40: 70 90 0d 20 lds r7, 0x200D 1b44: 80 90 0e 20 lds r8, 0x200E 1b48: 90 90 0f 20 lds r9, 0x200F 1b4c: 60 91 2c 20 lds r22, 0x202C 1b50: 70 91 2d 20 lds r23, 0x202D 1b54: 80 91 2e 20 lds r24, 0x202E 1b58: 90 91 2f 20 lds r25, 0x202F 1b5c: 0e 94 94 17 call 0x2f28 ; 0x2f28 1b60: 5b 01 movw r10, r22 1b62: 6c 01 movw r12, r24 1b64: e0 90 10 20 lds r14, 0x2010 1b68: f0 90 11 20 lds r15, 0x2011 1b6c: 00 91 12 20 lds r16, 0x2012 1b70: 10 91 13 20 lds r17, 0x2013 1b74: 60 91 2c 20 lds r22, 0x202C 1b78: 70 91 2d 20 lds r23, 0x202D 1b7c: 80 91 2e 20 lds r24, 0x202E 1b80: 90 91 2f 20 lds r25, 0x202F 1b84: 0e 94 06 17 call 0x2e0c ; 0x2e0c 1b88: a8 01 movw r20, r16 1b8a: 97 01 movw r18, r14 1b8c: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1b90: 20 e0 ldi r18, 0x00 ; 0 1b92: 34 e0 ldi r19, 0x04 ; 4 1b94: 40 e0 ldi r20, 0x00 ; 0 1b96: 50 e0 ldi r21, 0x00 ; 0 1b98: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1b9c: 79 01 movw r14, r18 1b9e: 8a 01 movw r16, r20 1ba0: c6 01 movw r24, r12 1ba2: b5 01 movw r22, r10 1ba4: a4 01 movw r20, r8 1ba6: 93 01 movw r18, r6 1ba8: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1bac: 20 e0 ldi r18, 0x00 ; 0 1bae: 34 e0 ldi r19, 0x04 ; 4 1bb0: 40 e0 ldi r20, 0x00 ; 0 1bb2: 50 e0 ldi r21, 0x00 ; 0 1bb4: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1bb8: 2e 19 sub r18, r14 1bba: 3f 09 sbc r19, r15 1bbc: 40 0b sbc r20, r16 1bbe: 51 0b sbc r21, r17 1bc0: 20 93 18 20 sts 0x2018, r18 1bc4: 30 93 19 20 sts 0x2019, r19 1bc8: 40 93 1a 20 sts 0x201A, r20 1bcc: 50 93 1b 20 sts 0x201B, r21 ((r * sini(phi)) / 1024L); // Calculate Psi dot psi_dot = ((q * sini(phi)) / cosi(theta) ) + 1bd0: 80 91 0c 20 lds r24, 0x200C 1bd4: 90 91 0d 20 lds r25, 0x200D 1bd8: a0 91 0e 20 lds r26, 0x200E 1bdc: b0 91 0f 20 lds r27, 0x200F 1be0: 89 83 std Y+1, r24 ; 0x01 1be2: 9a 83 std Y+2, r25 ; 0x02 1be4: ab 83 std Y+3, r26 ; 0x03 1be6: bc 83 std Y+4, r27 ; 0x04 1be8: 60 91 2c 20 lds r22, 0x202C 1bec: 70 91 2d 20 lds r23, 0x202D 1bf0: 80 91 2e 20 lds r24, 0x202E 1bf4: 90 91 2f 20 lds r25, 0x202F 1bf8: 0e 94 06 17 call 0x2e0c ; 0x2e0c 1bfc: 1b 01 movw r2, r22 1bfe: 2c 01 movw r4, r24 1c00: 60 91 30 20 lds r22, 0x2030 1c04: 70 91 31 20 lds r23, 0x2031 1c08: 80 91 32 20 lds r24, 0x2032 1c0c: 90 91 33 20 lds r25, 0x2033 1c10: 0e 94 94 17 call 0x2f28 ; 0x2f28 1c14: 6d 87 std Y+13, r22 ; 0x0d 1c16: 7e 87 std Y+14, r23 ; 0x0e 1c18: 8f 87 std Y+15, r24 ; 0x0f 1c1a: 98 8b std Y+16, r25 ; 0x10 1c1c: 60 90 10 20 lds r6, 0x2010 1c20: 70 90 11 20 lds r7, 0x2011 1c24: 80 90 12 20 lds r8, 0x2012 1c28: 90 90 13 20 lds r9, 0x2013 1c2c: 60 91 2c 20 lds r22, 0x202C 1c30: 70 91 2d 20 lds r23, 0x202D 1c34: 80 91 2e 20 lds r24, 0x202E 1c38: 90 91 2f 20 lds r25, 0x202F 1c3c: 0e 94 94 17 call 0x2f28 ; 0x2f28 1c40: 7b 01 movw r14, r22 1c42: 8c 01 movw r16, r24 1c44: 60 91 30 20 lds r22, 0x2030 1c48: 70 91 31 20 lds r23, 0x2031 1c4c: 80 91 32 20 lds r24, 0x2032 1c50: 90 91 33 20 lds r25, 0x2033 1c54: 0e 94 94 17 call 0x2f28 ; 0x2f28 1c58: 5b 01 movw r10, r22 1c5a: 6c 01 movw r12, r24 1c5c: c8 01 movw r24, r16 1c5e: b7 01 movw r22, r14 1c60: a4 01 movw r20, r8 1c62: 93 01 movw r18, r6 1c64: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1c68: a6 01 movw r20, r12 1c6a: 95 01 movw r18, r10 1c6c: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1c70: 79 01 movw r14, r18 1c72: 8a 01 movw r16, r20 1c74: c2 01 movw r24, r4 1c76: b1 01 movw r22, r2 1c78: 29 81 ldd r18, Y+1 ; 0x01 1c7a: 3a 81 ldd r19, Y+2 ; 0x02 1c7c: 4b 81 ldd r20, Y+3 ; 0x03 1c7e: 5c 81 ldd r21, Y+4 ; 0x04 1c80: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1c84: 2d 85 ldd r18, Y+13 ; 0x0d 1c86: 3e 85 ldd r19, Y+14 ; 0x0e 1c88: 4f 85 ldd r20, Y+15 ; 0x0f 1c8a: 58 89 ldd r21, Y+16 ; 0x10 1c8c: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1c90: e2 0e add r14, r18 1c92: f3 1e adc r15, r19 1c94: 04 1f adc r16, r20 1c96: 15 1f adc r17, r21 1c98: e0 92 1c 20 sts 0x201C, r14 1c9c: f0 92 1d 20 sts 0x201D, r15 1ca0: 00 93 1e 20 sts 0x201E, r16 1ca4: 10 93 1f 20 sts 0x201F, r17 ((r * cosi(phi)) / cosi(theta) ); // Calculate Euler angles from acceleration values phi_acc = atan2_i32_m1024 (-AccelRaw_i16_m1024__1g[1], AccelRaw_i16_m1024__1g[0]); 1ca8: 60 91 61 20 lds r22, 0x2061 1cac: 70 91 62 20 lds r23, 0x2062 1cb0: 70 95 com r23 1cb2: 61 95 neg r22 1cb4: 7f 4f sbci r23, 0xFF ; 255 1cb6: 88 27 eor r24, r24 1cb8: 77 fd sbrc r23, 7 1cba: 80 95 com r24 1cbc: 98 2f mov r25, r24 1cbe: 20 91 5f 20 lds r18, 0x205F 1cc2: 30 91 60 20 lds r19, 0x2060 1cc6: 44 27 eor r20, r20 1cc8: 37 fd sbrc r19, 7 1cca: 40 95 com r20 1ccc: 54 2f mov r21, r20 1cce: 0e 94 c4 15 call 0x2b88 ; 0x2b88 1cd2: 60 93 20 20 sts 0x2020, r22 1cd6: 70 93 21 20 sts 0x2021, r23 1cda: 80 93 22 20 sts 0x2022, r24 1cde: 90 93 23 20 sts 0x2023, r25 theta_acc = atan2_i32_m1024 ( AccelRaw_i16_m1024__1g[2], AccelRaw_i16_m1024__1g[0]); 1ce2: 60 91 63 20 lds r22, 0x2063 1ce6: 70 91 64 20 lds r23, 0x2064 1cea: 88 27 eor r24, r24 1cec: 77 fd sbrc r23, 7 1cee: 80 95 com r24 1cf0: 98 2f mov r25, r24 1cf2: 20 91 5f 20 lds r18, 0x205F 1cf6: 30 91 60 20 lds r19, 0x2060 1cfa: 44 27 eor r20, r20 1cfc: 37 fd sbrc r19, 7 1cfe: 40 95 com r20 1d00: 54 2f mov r21, r20 1d02: 0e 94 c4 15 call 0x2b88 ; 0x2b88 1d06: 69 87 std Y+9, r22 ; 0x09 1d08: 7a 87 std Y+10, r23 ; 0x0a 1d0a: 8b 87 std Y+11, r24 ; 0x0b 1d0c: 9c 87 std Y+12, r25 ; 0x0c 1d0e: 60 93 24 20 sts 0x2024, r22 1d12: 70 93 25 20 sts 0x2025, r23 1d16: 80 93 26 20 sts 0x2026, r24 1d1a: 90 93 27 20 sts 0x2027, r25 EulerAngle_Accel_i32_m1024__deg[2] = 0; // Value from magnetometer 1d1e: 10 92 28 20 sts 0x2028, r1 1d22: 10 92 29 20 sts 0x2029, r1 1d26: 10 92 2a 20 sts 0x202A, r1 1d2a: 10 92 2b 20 sts 0x202B, r1 int32_t RollIntegration_i32_m1024__deg = phi_dot / CONTROL_FREQUENCY_HZ; int32_t PitchIntegration_i32_m1024__deg = theta_dot / CONTROL_FREQUENCY_HZ; 1d2e: 60 91 18 20 lds r22, 0x2018 1d32: 70 91 19 20 lds r23, 0x2019 1d36: 80 91 1a 20 lds r24, 0x201A 1d3a: 90 91 1b 20 lds r25, 0x201B 1d3e: 24 e6 ldi r18, 0x64 ; 100 1d40: 30 e0 ldi r19, 0x00 ; 0 1d42: 40 e0 ldi r20, 0x00 ; 0 1d44: 50 e0 ldi r21, 0x00 ; 0 1d46: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1d4a: 59 01 movw r10, r18 1d4c: 6a 01 movw r12, r20 int32_t YawIntegration_i32_m1024__deg = psi_dot / CONTROL_FREQUENCY_HZ; 1d4e: 60 91 1c 20 lds r22, 0x201C 1d52: 70 91 1d 20 lds r23, 0x201D 1d56: 80 91 1e 20 lds r24, 0x201E 1d5a: 90 91 1f 20 lds r25, 0x201F 1d5e: 24 e6 ldi r18, 0x64 ; 100 1d60: 30 e0 ldi r19, 0x00 ; 0 1d62: 40 e0 ldi r20, 0x00 ; 0 1d64: 50 e0 ldi r21, 0x00 ; 0 1d66: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1d6a: 39 01 movw r6, r18 1d6c: 4a 01 movw r8, r20 // Complementary filter phi = ((4096L - FilterWeight) * (phi + RollIntegration_i32_m1024__deg) + 1d6e: 80 91 01 20 lds r24, 0x2001 1d72: 90 91 02 20 lds r25, 0x2002 1d76: a0 91 03 20 lds r26, 0x2003 1d7a: b0 91 04 20 lds r27, 0x2004 1d7e: 8d 83 std Y+5, r24 ; 0x05 1d80: 9e 83 std Y+6, r25 ; 0x06 1d82: af 83 std Y+7, r26 ; 0x07 1d84: b8 87 std Y+8, r27 ; 0x08 1d86: 21 2c mov r2, r1 1d88: 30 e1 ldi r19, 0x10 ; 16 1d8a: 33 2e mov r3, r19 1d8c: 41 2c mov r4, r1 1d8e: 51 2c mov r5, r1 1d90: 28 1a sub r2, r24 1d92: 39 0a sbc r3, r25 1d94: 4a 0a sbc r4, r26 1d96: 5b 0a sbc r5, r27 1d98: 60 91 14 20 lds r22, 0x2014 1d9c: 70 91 15 20 lds r23, 0x2015 1da0: 80 91 16 20 lds r24, 0x2016 1da4: 90 91 17 20 lds r25, 0x2017 1da8: 24 e6 ldi r18, 0x64 ; 100 1daa: 30 e0 ldi r19, 0x00 ; 0 1dac: 40 e0 ldi r20, 0x00 ; 0 1dae: 50 e0 ldi r21, 0x00 ; 0 1db0: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1db4: 60 91 2c 20 lds r22, 0x202C 1db8: 70 91 2d 20 lds r23, 0x202D 1dbc: 80 91 2e 20 lds r24, 0x202E 1dc0: 90 91 2f 20 lds r25, 0x202F 1dc4: 62 0f add r22, r18 1dc6: 73 1f adc r23, r19 1dc8: 84 1f adc r24, r20 1dca: 95 1f adc r25, r21 1dcc: a2 01 movw r20, r4 1dce: 91 01 movw r18, r2 1dd0: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1dd4: 7b 01 movw r14, r22 1dd6: 8c 01 movw r16, r24 1dd8: 20 91 20 20 lds r18, 0x2020 1ddc: 30 91 21 20 lds r19, 0x2021 1de0: 40 91 22 20 lds r20, 0x2022 1de4: 50 91 23 20 lds r21, 0x2023 1de8: 6d 81 ldd r22, Y+5 ; 0x05 1dea: 7e 81 ldd r23, Y+6 ; 0x06 1dec: 8f 81 ldd r24, Y+7 ; 0x07 1dee: 98 85 ldd r25, Y+8 ; 0x08 1df0: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1df4: e6 0e add r14, r22 1df6: f7 1e adc r15, r23 1df8: 08 1f adc r16, r24 1dfa: 19 1f adc r17, r25 1dfc: c8 01 movw r24, r16 1dfe: b7 01 movw r22, r14 1e00: 20 e0 ldi r18, 0x00 ; 0 1e02: 30 e1 ldi r19, 0x10 ; 16 1e04: 40 e0 ldi r20, 0x00 ; 0 1e06: 50 e0 ldi r21, 0x00 ; 0 1e08: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1e0c: 20 93 2c 20 sts 0x202C, r18 1e10: 30 93 2d 20 sts 0x202D, r19 1e14: 40 93 2e 20 sts 0x202E, r20 1e18: 50 93 2f 20 sts 0x202F, r21 (FilterWeight) * (phi_acc)) / 4096L; theta = ((4096L - FilterWeight) * (theta + PitchIntegration_i32_m1024__deg) + 1e1c: 69 85 ldd r22, Y+9 ; 0x09 1e1e: 7a 85 ldd r23, Y+10 ; 0x0a 1e20: 8b 85 ldd r24, Y+11 ; 0x0b 1e22: 9c 85 ldd r25, Y+12 ; 0x0c 1e24: 2d 81 ldd r18, Y+5 ; 0x05 1e26: 3e 81 ldd r19, Y+6 ; 0x06 1e28: 4f 81 ldd r20, Y+7 ; 0x07 1e2a: 58 85 ldd r21, Y+8 ; 0x08 1e2c: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1e30: 7b 01 movw r14, r22 1e32: 8c 01 movw r16, r24 1e34: 80 91 30 20 lds r24, 0x2030 1e38: 90 91 31 20 lds r25, 0x2031 1e3c: a0 91 32 20 lds r26, 0x2032 1e40: b0 91 33 20 lds r27, 0x2033 1e44: a8 0e add r10, r24 1e46: b9 1e adc r11, r25 1e48: ca 1e adc r12, r26 1e4a: db 1e adc r13, r27 1e4c: c6 01 movw r24, r12 1e4e: b5 01 movw r22, r10 1e50: a2 01 movw r20, r4 1e52: 91 01 movw r18, r2 1e54: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 1e58: e6 0e add r14, r22 1e5a: f7 1e adc r15, r23 1e5c: 08 1f adc r16, r24 1e5e: 19 1f adc r17, r25 1e60: c8 01 movw r24, r16 1e62: b7 01 movw r22, r14 1e64: 20 e0 ldi r18, 0x00 ; 0 1e66: 30 e1 ldi r19, 0x10 ; 16 1e68: 40 e0 ldi r20, 0x00 ; 0 1e6a: 50 e0 ldi r21, 0x00 ; 0 1e6c: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1e70: 20 93 30 20 sts 0x2030, r18 1e74: 30 93 31 20 sts 0x2031, r19 1e78: 40 93 32 20 sts 0x2032, r20 1e7c: 50 93 33 20 sts 0x2033, r21 (FilterWeight) * (theta_acc)) / 4096L; psi = ((4096L - 2048L) * (psi + YawIntegration_i32_m1024__deg) + 1e80: 60 91 34 20 lds r22, 0x2034 1e84: 70 91 35 20 lds r23, 0x2035 1e88: 80 91 36 20 lds r24, 0x2036 1e8c: 90 91 37 20 lds r25, 0x2037 1e90: 66 0f add r22, r22 1e92: 77 1f adc r23, r23 1e94: 88 1f adc r24, r24 1e96: 99 1f adc r25, r25 1e98: 66 0d add r22, r6 1e9a: 77 1d adc r23, r7 1e9c: 88 1d adc r24, r8 1e9e: 99 1d adc r25, r9 1ea0: 22 e0 ldi r18, 0x02 ; 2 1ea2: 30 e0 ldi r19, 0x00 ; 0 1ea4: 40 e0 ldi r20, 0x00 ; 0 1ea6: 50 e0 ldi r21, 0x00 ; 0 1ea8: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1eac: 20 93 34 20 sts 0x2034, r18 1eb0: 30 93 35 20 sts 0x2035, r19 1eb4: 40 93 36 20 sts 0x2036, r20 1eb8: 50 93 37 20 sts 0x2037, r21 1ebc: cc 24 eor r12, r12 1ebe: dd 24 eor r13, r13 1ec0: ee 24 eor r14, r14 1ec2: ff 24 eor r15, r15 (2048) * (psi)) / 4096L; // Debug & Logging values for (i = 0; i < 3; i++) { Debug_BodyRate_i16_m8__deg_d_s[i] = (int16_t) (BodyRate_i32_m1024__deg_d_s[i] / 128L); 1ec4: 87 01 movw r16, r14 1ec6: 0b 5a subi r16, 0xAB ; 171 1ec8: 1f 4d sbci r17, 0xDF ; 223 1eca: f6 01 movw r30, r12 1ecc: e8 5f subi r30, 0xF8 ; 248 1ece: ff 4d sbci r31, 0xDF ; 223 1ed0: 60 81 ld r22, Z 1ed2: 71 81 ldd r23, Z+1 ; 0x01 1ed4: 82 81 ldd r24, Z+2 ; 0x02 1ed6: 93 81 ldd r25, Z+3 ; 0x03 1ed8: 20 e8 ldi r18, 0x80 ; 128 1eda: 30 e0 ldi r19, 0x00 ; 0 1edc: 40 e0 ldi r20, 0x00 ; 0 1ede: 50 e0 ldi r21, 0x00 ; 0 1ee0: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1ee4: d8 01 movw r26, r16 1ee6: 2d 93 st X+, r18 1ee8: 3c 93 st X, r19 Debug_EulerRate_i16_m8__deg_d_s[i] = (int16_t) (EulerRate_i32_m1024__deg_d_s[i] / 128L); 1eea: 87 01 movw r16, r14 1eec: 03 5c subi r16, 0xC3 ; 195 1eee: 1f 4d sbci r17, 0xDF ; 223 1ef0: f6 01 movw r30, r12 1ef2: ec 5e subi r30, 0xEC ; 236 1ef4: ff 4d sbci r31, 0xDF ; 223 1ef6: 60 81 ld r22, Z 1ef8: 71 81 ldd r23, Z+1 ; 0x01 1efa: 82 81 ldd r24, Z+2 ; 0x02 1efc: 93 81 ldd r25, Z+3 ; 0x03 1efe: 20 e8 ldi r18, 0x80 ; 128 1f00: 30 e0 ldi r19, 0x00 ; 0 1f02: 40 e0 ldi r20, 0x00 ; 0 1f04: 50 e0 ldi r21, 0x00 ; 0 1f06: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1f0a: f8 01 movw r30, r16 1f0c: 20 83 st Z, r18 1f0e: 31 83 std Z+1, r19 ; 0x01 Debug_EulerAngle_Accel_i16_m8__deg[i] = (int16_t) (EulerAngle_Accel_i32_m1024__deg[i] / 128L); 1f10: 87 01 movw r16, r14 1f12: 07 5b subi r16, 0xB7 ; 183 1f14: 1f 4d sbci r17, 0xDF ; 223 1f16: f6 01 movw r30, r12 1f18: e0 5e subi r30, 0xE0 ; 224 1f1a: ff 4d sbci r31, 0xDF ; 223 1f1c: 60 81 ld r22, Z 1f1e: 71 81 ldd r23, Z+1 ; 0x01 1f20: 82 81 ldd r24, Z+2 ; 0x02 1f22: 93 81 ldd r25, Z+3 ; 0x03 1f24: 20 e8 ldi r18, 0x80 ; 128 1f26: 30 e0 ldi r19, 0x00 ; 0 1f28: 40 e0 ldi r20, 0x00 ; 0 1f2a: 50 e0 ldi r21, 0x00 ; 0 1f2c: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1f30: d8 01 movw r26, r16 1f32: 2d 93 st X+, r18 1f34: 3c 93 st X, r19 Debug_EulerAngle_Filter_i16_m8__deg[i] = (int16_t) (EulerAngle_Filtered_i32_m1024__deg[i] / 128L); 1f36: 87 01 movw r16, r14 1f38: 01 5b subi r16, 0xB1 ; 177 1f3a: 1f 4d sbci r17, 0xDF ; 223 1f3c: f6 01 movw r30, r12 1f3e: e4 5d subi r30, 0xD4 ; 212 1f40: ff 4d sbci r31, 0xDF ; 223 1f42: 60 81 ld r22, Z 1f44: 71 81 ldd r23, Z+1 ; 0x01 1f46: 82 81 ldd r24, Z+2 ; 0x02 1f48: 93 81 ldd r25, Z+3 ; 0x03 1f4a: 20 e8 ldi r18, 0x80 ; 128 1f4c: 30 e0 ldi r19, 0x00 ; 0 1f4e: 40 e0 ldi r20, 0x00 ; 0 1f50: 50 e0 ldi r21, 0x00 ; 0 1f52: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 1f56: f8 01 movw r30, r16 1f58: 20 83 st Z, r18 1f5a: 31 83 std Z+1, r19 ; 0x01 1f5c: 84 e0 ldi r24, 0x04 ; 4 1f5e: 90 e0 ldi r25, 0x00 ; 0 1f60: c8 0e add r12, r24 1f62: d9 1e adc r13, r25 1f64: a2 e0 ldi r26, 0x02 ; 2 1f66: b0 e0 ldi r27, 0x00 ; 0 1f68: ea 0e add r14, r26 1f6a: fb 1e adc r15, r27 (FilterWeight) * (theta_acc)) / 4096L; psi = ((4096L - 2048L) * (psi + YawIntegration_i32_m1024__deg) + (2048) * (psi)) / 4096L; // Debug & Logging values for (i = 0; i < 3; i++) 1f6c: bc e0 ldi r27, 0x0C ; 12 1f6e: cb 16 cp r12, r27 1f70: d1 04 cpc r13, r1 1f72: 09 f0 breq .+2 ; 0x1f76 1f74: a7 cf rjmp .-178 ; 0x1ec4 Debug_EulerRate_i16_m8__deg_d_s[i] = (int16_t) (EulerRate_i32_m1024__deg_d_s[i] / 128L); Debug_EulerAngle_Accel_i16_m8__deg[i] = (int16_t) (EulerAngle_Accel_i32_m1024__deg[i] / 128L); Debug_EulerAngle_Filter_i16_m8__deg[i] = (int16_t) (EulerAngle_Filtered_i32_m1024__deg[i] / 128L); } SensorDebug (); 1f76: 0e 94 d3 0b call 0x17a6 ; 0x17a6 } } 1f7a: 6c 96 adiw r28, 0x1c ; 28 1f7c: cd bf out 0x3d, r28 ; 61 1f7e: de bf out 0x3e, r29 ; 62 1f80: cf 91 pop r28 1f82: df 91 pop r29 1f84: 1f 91 pop r17 1f86: 0f 91 pop r16 1f88: ff 90 pop r15 1f8a: ef 90 pop r14 1f8c: df 90 pop r13 1f8e: cf 90 pop r12 1f90: bf 90 pop r11 1f92: af 90 pop r10 1f94: 9f 90 pop r9 1f96: 8f 90 pop r8 1f98: 7f 90 pop r7 1f9a: 6f 90 pop r6 1f9c: 5f 90 pop r5 1f9e: 4f 90 pop r4 1fa0: 3f 90 pop r3 1fa2: 2f 90 pop r2 1fa4: 08 95 ret 00001fa6 : void InitMainClock () { // Enable external oscillator frequency from 12 - 16 MHz and enable crystal oscillator selection (startup time 256 CLKs) OSC.XOSCCTRL = (uint8_t) OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_256CLK_gc; 1fa6: e0 e5 ldi r30, 0x50 ; 80 1fa8: f0 e0 ldi r31, 0x00 ; 0 1faa: 83 ec ldi r24, 0xC3 ; 195 1fac: 82 83 std Z+2, r24 ; 0x02 // Enable configured oscillator as PLL input source and set PLL multiplier OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | (PLL_MULTIPLIER & OSC_PLLFAC_gm); 1fae: 82 ec ldi r24, 0xC2 ; 194 1fb0: 85 83 std Z+5, r24 ; 0x05 // enable external oscillator OSC.CTRL |= OSC_XOSCEN_bm; 1fb2: 80 91 50 00 lds r24, 0x0050 1fb6: 88 60 ori r24, 0x08 ; 8 1fb8: 80 93 50 00 sts 0x0050, r24 // Wait for clock to stabilize while ( !(OSC.STATUS & OSC_XOSCRDY_bm) ) {}; 1fbc: 80 91 51 00 lds r24, 0x0051 1fc0: 83 ff sbrs r24, 3 1fc2: fc cf rjmp .-8 ; 0x1fbc // Enable the PLL OSC.CTRL |= OSC_PLLEN_bm; 1fc4: 80 91 50 00 lds r24, 0x0050 1fc8: 80 61 ori r24, 0x10 ; 16 1fca: 80 93 50 00 sts 0x0050, r24 // Wait for clock to stabilize while ( !(OSC.STATUS & OSC_PLLRDY_bm) ) {}; 1fce: 80 91 51 00 lds r24, 0x0051 1fd2: 84 ff sbrs r24, 4 1fd4: fc cf rjmp .-8 ; 0x1fce // Set PLL as main clock source CCP = CCP_IOREG_gc; 1fd6: 88 ed ldi r24, 0xD8 ; 216 1fd8: 84 bf out 0x34, r24 ; 52 CLK.CTRL = CLK_SCLKSEL_PLL_gc; 1fda: 84 e0 ldi r24, 0x04 ; 4 1fdc: 80 93 40 00 sts 0x0040, r24 } 1fe0: 08 95 ret 00001fe2 : /*****************************************************************************************************************************************/ // Send TWI start condition and device address inline void TWI_START(TWI_t * _twi, uint8_t address) { 1fe2: fc 01 movw r30, r24 Debug_Str ("TWI_START: "); Debug_u8(address); Debug_ch('\n'); #endif // Start action + write LIS_ADDRESS address _twi->MASTER.ADDR = address; 1fe4: 66 83 std Z+6, r22 ; 0x06 } 1fe6: 08 95 ret 00001fe8 : /*****************************************************************************************************************************************/ // Write TWI data inline void TWI_WRITE(TWI_t * _twi, uint8_t data) { 1fe8: fc 01 movw r30, r24 Debug_Str ("TWI_WRITE: "); Debug_u8(data); Debug_ch('\n'); #endif // Write TWI data _twi->MASTER.DATA = data; 1fea: 67 83 std Z+7, r22 ; 0x07 } 1fec: 08 95 ret 00001fee : /*****************************************************************************************************************************************/ // Send TWI ACK and continue inline void TWI_ACK(TWI_t * _twi) { 1fee: fc 01 movw r30, r24 #if (TWI_DEBUGGING == 1) Debug_Str ("TWI_ACK\n"); #endif _twi->MASTER.CTRLC = TWI_MASTER_CMD_RECVTRANS_gc; 1ff0: 82 e0 ldi r24, 0x02 ; 2 1ff2: 83 83 std Z+3, r24 ; 0x03 } 1ff4: 08 95 ret 00001ff6 : /*****************************************************************************************************************************************/ // Send TWI NACK and stop condition inline void TWI_NACK_STOP(TWI_t * _twi) { 1ff6: fc 01 movw r30, r24 #if (TWI_DEBUGGING == 1) Debug_Str ("TWI_NACK_STOP\n"); #endif _twi->MASTER.CTRLC = TWI_MASTER_ACKACT_bm | TWI_MASTER_CMD_STOP_gc; 1ff8: 87 e0 ldi r24, 0x07 ; 7 1ffa: 83 83 std Z+3, r24 ; 0x03 } 1ffc: 08 95 ret 00001ffe : /*****************************************************************************************************************************************/ // Send TWI stop condition inline void TWI_STOP(TWI_t * _twi) { 1ffe: fc 01 movw r30, r24 #if (TWI_DEBUGGING == 1) Debug_Str ("---TWI_STOP---\n"); #endif _twi->MASTER.CTRLC = TWI_MASTER_CMD_STOP_gc; 2000: 83 e0 ldi r24, 0x03 ; 3 2002: 83 83 std Z+3, r24 ; 0x03 } 2004: 08 95 ret 00002006 : #include "m_usart.h" void USART_putc (USART_t * _usart, char ch) { 2006: fc 01 movw r30, r24 // Wait until data register empty interrupt flag is set while(!(_usart->STATUS & USART_DREIF_bm)); 2008: 81 81 ldd r24, Z+1 ; 0x01 200a: 85 ff sbrs r24, 5 200c: fd cf rjmp .-6 ; 0x2008 // Write char to DATA register _usart->DATA = ch; 200e: 60 83 st Z, r22 } 2010: 08 95 ret 00002012 : void USART_puts (USART_t * _usart, char * string) { 2012: dc 01 movw r26, r24 2014: fb 01 movw r30, r22 2016: 07 c0 rjmp .+14 ; 0x2026 void USART_putc (USART_t * _usart, char ch) { // Wait until data register empty interrupt flag is set while(!(_usart->STATUS & USART_DREIF_bm)); 2018: 11 96 adiw r26, 0x01 ; 1 201a: 8c 91 ld r24, X 201c: 11 97 sbiw r26, 0x01 ; 1 201e: 85 ff sbrs r24, 5 2020: fb cf rjmp .-10 ; 0x2018 } void USART_puts (USART_t * _usart, char * string) { while (*string != 0) USART_putc(_usart, *string++); 2022: 31 96 adiw r30, 0x01 ; 1 void USART_putc (USART_t * _usart, char ch) { // Wait until data register empty interrupt flag is set while(!(_usart->STATUS & USART_DREIF_bm)); // Write char to DATA register _usart->DATA = ch; 2024: 9c 93 st X, r25 } void USART_puts (USART_t * _usart, char * string) { while (*string != 0) 2026: 90 81 ld r25, Z 2028: 99 23 and r25, r25 202a: b1 f7 brne .-20 ; 0x2018 USART_putc(_usart, *string++); } 202c: 08 95 ret 0000202e : #include "main.h" /*****************************************************************************************************************************************/ void wait(uint32_t ws) { 202e: 05 c0 rjmp .+10 ; 0x203a while (ws--) asm volatile ("nop"); 2030: 00 00 nop 2032: 61 50 subi r22, 0x01 ; 1 2034: 70 40 sbci r23, 0x00 ; 0 2036: 80 40 sbci r24, 0x00 ; 0 2038: 90 40 sbci r25, 0x00 ; 0 #include "main.h" /*****************************************************************************************************************************************/ void wait(uint32_t ws) { while (ws--) 203a: 61 15 cp r22, r1 203c: 71 05 cpc r23, r1 203e: 81 05 cpc r24, r1 2040: 91 05 cpc r25, r1 2042: b1 f7 brne .-20 ; 0x2030 asm volatile ("nop"); } 2044: 08 95 ret 00002046
: /*****************************************************************************************************************************************/ int main(void) { 2046: 0f 93 push r16 2048: 1f 93 push r17 // LA diagnosis PORTD.DIRSET = (1 << PIN0); 204a: 81 e0 ldi r24, 0x01 ; 1 204c: e0 e6 ldi r30, 0x60 ; 96 204e: f6 e0 ldi r31, 0x06 ; 6 2050: 81 83 std Z+1, r24 ; 0x01 PORTE.DIRSET = (1 << PIN3); 2052: e0 e8 ldi r30, 0x80 ; 128 2054: f6 e0 ldi r31, 0x06 ; 6 2056: 88 e0 ldi r24, 0x08 ; 8 2058: 81 83 std Z+1, r24 ; 0x01 PORTE.OUTCLR = (1 << PIN3); 205a: 86 83 std Z+6, r24 ; 0x06 PORTF.DIRSET = (1 << PIN6); 205c: e0 ea ldi r30, 0xA0 ; 160 205e: f6 e0 ldi r31, 0x06 ; 6 2060: 80 e4 ldi r24, 0x40 ; 64 2062: 81 83 std Z+1, r24 ; 0x01 PORTF.OUTCLR = (1 << PIN6); 2064: 86 83 std Z+6, r24 ; 0x06 LED_BLUE_INIT; // INIT blue LED 2066: 80 e8 ldi r24, 0x80 ; 128 2068: 81 83 std Z+1, r24 ; 0x01 InitMainClock (); // Init main clock 206a: 0e 94 d3 0f call 0x1fa6 ; 0x1fa6 206e: 80 ed ldi r24, 0xD0 ; 208 2070: 97 e0 ldi r25, 0x07 ; 7 milliseconds can be achieved. */ void _delay_loop_2(uint16_t __count) { __asm__ volatile ( 2072: 2b ed ldi r18, 0xDB ; 219 2074: 32 e0 ldi r19, 0x02 ; 2 2076: f9 01 movw r30, r18 2078: 31 97 sbiw r30, 0x01 ; 1 207a: f1 f7 brne .-4 ; 0x2078 __ticks = (uint16_t) (__ms * 10.0); while(__ticks) { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 207c: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 207e: d9 f7 brne .-10 ; 0x2076 _delay_ms (200); // Wait a bit for clock to settle and sensors to start up PMIC_EnableInterruptLevel(PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm); // Enable interrupt levels in PMIC 2080: 00 ea ldi r16, 0xA0 ; 160 2082: 10 e0 ldi r17, 0x00 ; 0 2084: 80 91 a2 00 lds r24, 0x00A2 2088: 87 60 ori r24, 0x07 ; 7 208a: f8 01 movw r30, r16 208c: 82 83 std Z+2, r24 ; 0x02 InitDebugUSART(); // Init Debug port (USART) 208e: 0e 94 3b 0a call 0x1476 ; 0x1476 Debug_Str ("*****************************************************\n"); 2092: 84 ea ldi r24, 0xA4 ; 164 2094: 92 e0 ldi r25, 0x02 ; 2 2096: 0e 94 56 0b call 0x16ac ; 0x16ac Debug_Str ("STARTING FLIGHTCONTROL\n\n"); 209a: 8b e8 ldi r24, 0x8B ; 139 209c: 92 e0 ldi r25, 0x02 ; 2 209e: 0e 94 56 0b call 0x16ac ; 0x16ac PMIC_EnableInterruptLevel(PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm); // Enable interrupt levels in PMIC 20a2: 80 91 a2 00 lds r24, 0x00A2 20a6: 87 60 ori r24, 0x07 ; 7 20a8: f8 01 movw r30, r16 20aa: 82 83 std Z+2, r24 ; 0x02 sei (); // Globally enable interrupts 20ac: 78 94 sei if (Init_ITG_Sensor () != 1) // Init ITG sensor and port 20ae: 0e 94 4e 11 call 0x229c ; 0x229c 20b2: 81 30 cpi r24, 0x01 ; 1 20b4: 19 f0 breq .+6 ; 0x20bc { Debug_Str ("ERROR initializing ITG sensor ...\n"); 20b6: 88 e6 ldi r24, 0x68 ; 104 20b8: 92 e0 ldi r25, 0x02 ; 2 20ba: 06 c0 rjmp .+12 ; 0x20c8 return 0; } if (Init_LIS_Sensor () != 1) // Init LIS sensor and port 20bc: 0e 94 43 14 call 0x2886 ; 0x2886 20c0: 81 30 cpi r24, 0x01 ; 1 20c2: 49 f0 breq .+18 ; 0x20d6 { Debug_Str ("ERROR initializing LIS sensor ...\n"); 20c4: 85 e4 ldi r24, 0x45 ; 69 20c6: 92 e0 ldi r25, 0x02 ; 2 20c8: 0e 94 56 0b call 0x16ac ; 0x16ac FlightControl (); } // Never reached return 0; } 20cc: 80 e0 ldi r24, 0x00 ; 0 20ce: 90 e0 ldi r25, 0x00 ; 0 20d0: 1f 91 pop r17 20d2: 0f 91 pop r16 20d4: 08 95 ret { Debug_Str ("ERROR initializing LIS sensor ...\n"); return 0; } Debug_Str ("start lis\n"); // Debug output 20d6: 8a e3 ldi r24, 0x3A ; 58 20d8: 92 e0 ldi r25, 0x02 ; 2 20da: 0e 94 56 0b call 0x16ac ; 0x16ac Start_LIS_Data_Query (); // Start accelerometer query 20de: 0e 94 80 13 call 0x2700 ; 0x2700 Debug_Str ("start itg\n"); // Debug output 20e2: 8f e2 ldi r24, 0x2F ; 47 20e4: 92 e0 ldi r25, 0x02 ; 2 20e6: 0e 94 56 0b call 0x16ac ; 0x16ac StartITGDataQuery(); // Start gyro query 20ea: 0e 94 a8 10 call 0x2150 ; 0x2150 // Main loop -------------------------------------------------------------------------------------------------- for (;;) { FlightControl (); 20ee: 0e 94 6f 0c call 0x18de ; 0x18de 20f2: fd cf rjmp .-6 ; 0x20ee 000020f4 : } /*************************************************************************************************************/ void Disable_ITG_TWI () { ITG_TWI_PORT.MASTER.BAUD = 0; 20f4: e0 ea ldi r30, 0xA0 ; 160 20f6: f4 e0 ldi r31, 0x04 ; 4 20f8: 15 82 std Z+5, r1 ; 0x05 ITG_TWI_PORT.MASTER.CTRLA = 0; 20fa: 11 82 std Z+1, r1 ; 0x01 ITG_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 20fc: 81 e0 ldi r24, 0x01 ; 1 20fe: 84 83 std Z+4, r24 ; 0x04 } 2100: 08 95 ret 00002102 : /*************************************************************************************************************/ void Init_ITG_TWI () { // Init ITG TWI port and baudrate ITG_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, ITG_TWI_FREQUENCY_HZ); 2102: e0 ea ldi r30, 0xA0 ; 160 2104: f4 e0 ldi r31, 0x04 ; 4 2106: 8f e1 ldi r24, 0x1F ; 31 2108: 85 83 std Z+5, r24 ; 0x05 ITG_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_HI_gc | 210a: 88 ef ldi r24, 0xF8 ; 248 210c: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; ITG_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 210e: 81 e0 ldi r24, 0x01 ; 1 2110: 84 83 std Z+4, r24 ; 0x04 } 2112: 08 95 ret 00002114 : } /*************************************************************************************************************/ inline int16_t Get_ITG_Temperature () { return Temperature_m10; 2114: 20 91 65 20 lds r18, 0x2065 2118: 30 91 66 20 lds r19, 0x2066 } 211c: c9 01 movw r24, r18 211e: 08 95 ret 00002120 : /*************************************************************************************************************/ inline void Get_ITGRawValues (int16_t * DataBuffer) { 2120: fc 01 movw r30, r24 uint8_t i; cli (); 2122: f8 94 cli for (i = 0; i < 3; i++) DataBuffer[i] = GyroRawData[i]; 2124: 80 91 71 20 lds r24, 0x2071 2128: 90 91 72 20 lds r25, 0x2072 212c: 80 83 st Z, r24 212e: 91 83 std Z+1, r25 ; 0x01 2130: 80 91 73 20 lds r24, 0x2073 2134: 90 91 74 20 lds r25, 0x2074 2138: 82 83 std Z+2, r24 ; 0x02 213a: 93 83 std Z+3, r25 ; 0x03 213c: 80 91 75 20 lds r24, 0x2075 2140: 90 91 76 20 lds r25, 0x2076 2144: 84 83 std Z+4, r24 ; 0x04 2146: 95 83 std Z+5, r25 ; 0x05 sei (); 2148: 78 94 sei LastITGActionStatus = 0; 214a: 10 92 05 20 sts 0x2005, r1 } 214e: 08 95 ret 00002150 : // Init ITG timer TC_SetPeriod (&ITG_UPDATE_TIMER, SYS_CLK / ITG_UPDATE_FREQUENCY_HZ / ITG_TIMER_PRESCALER_VALUE); TC_SetPrescaler (&ITG_UPDATE_TIMER, ITG_TIMER_PRESCALER); TC_SetOverflowInt (&ITG_UPDATE_TIMER, TC_OVFINTLVL_HI_gc); #else ITG_INT_PORT.DIRCLR = (1 << ITG_INT_PIN); // Make ITG interrupt pin an input 2150: e0 e8 ldi r30, 0x80 ; 128 2152: f6 e0 ldi r31, 0x06 ; 6 2154: 94 e0 ldi r25, 0x04 ; 4 2156: 92 83 std Z+2, r25 ; 0x02 PORTCFG.MPCMASK = (1 << ITG_INT_PIN); // Mask ITG input pin 2158: 90 93 b0 00 sts 0x00B0, r25 ITG_INT_PORT.PIN0CTRL = PORT_OPC_PULLDOWN_gc; // Enable pulldown resistor at ITG input pin 215c: 80 e1 ldi r24, 0x10 ; 16 215e: 80 8b std Z+16, r24 ; 0x10 ITG_INT_PORT.INTCTRL |= PORT_INT0LVL_MED_gc; // Enable medium level interrupt 2160: 81 85 ldd r24, Z+9 ; 0x09 2162: 82 60 ori r24, 0x02 ; 2 2164: 81 87 std Z+9, r24 ; 0x09 ITG_INT_PORT.INT0MASK = (1 << ITG_INT_PIN); // Mask ITG input pin for interrupt 0 2166: 92 87 std Z+10, r25 ; 0x0a ITG_INT_PORT.PIN2CTRL = PORT_ISC_RISING_gc; // Enable interrupt on rising edge 2168: 81 e0 ldi r24, 0x01 ; 1 216a: 82 8b std Z+18, r24 ; 0x12 #endif } 216c: 08 95 ret 0000216e : /*************************************************************************************************************/ inline uint8_t GetLastITGActionStatus () { return LastITGActionStatus; 216e: 80 91 05 20 lds r24, 0x2005 } 2172: 08 95 ret 00002174 : /*************************************************************************************************************/ inline uint8_t GetITGActionStatus () { return ITGActionStatus; 2174: 80 91 39 20 lds r24, 0x2039 } 2178: 08 95 ret 0000217a : /*************************************************************************************************************/ uint8_t StartITGAction (uint8_t ActionType, uint8_t Blocking) { 217a: 1f 93 push r17 217c: 98 2f mov r25, r24 217e: 16 2f mov r17, r22 ITG_Debug_Str ("\nITG-ACT: "); // Debug output ITG_Debug_u8 (ActionType); // Debug output ITG_Debug_Str ("\n"); // Debug output if (ActionType > ITG_ACTION_NUMBER_OF_STATES) // Check if action is valid 2180: 86 30 cpi r24, 0x06 ; 6 2182: 18 f0 brcs .+6 ; 0x218a { ITG_Debug_Str ("ITG action is unknown\n"); // Debug output ITGActionStatus = ITG_ACTION_IDLE; // Reset current action 2184: 10 92 39 20 sts 0x2039, r1 2188: 1b c0 rjmp .+54 ; 0x21c0 return ITGActionStatus; // Return from function } else if (ITGActionStatus != ITG_ACTION_IDLE) // Check if no action is pending 218a: 80 91 39 20 lds r24, 0x2039 218e: 88 23 and r24, r24 2190: b9 f4 brne .+46 ; 0x21c0 ITG_Debug_Str ("ITG ACTION PENDING\n"); // Debug output return ITGActionStatus; // Return from function } else // Start action { ITGActionStatus = ActionType; // Set current action type 2192: 90 93 39 20 sts 0x2039, r25 ITGByteCount = 0; // Reset byte count 2196: 10 92 3a 20 sts 0x203A, r1 } ITG_Debug_Str ("ITG-BUS: "); // Debug output switch (ITG_TWI_PORT.MASTER.STATUS & TWI_MASTER_BUSSTATE_gm) // Debug current bus state 219a: 80 91 a4 04 lds r24, 0x04A4 // Start condition + ITG address ITG_Debug_Str ("ADDR: "); // Debug output ITG_Debug_u8 (ITG); // Debug output ITG_Debug_Str ("\n"); // Debug output TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Start TWI 219e: 80 ea ldi r24, 0xA0 ; 160 21a0: 94 e0 ldi r25, 0x04 ; 4 21a2: 62 ed ldi r22, 0xD2 ; 210 21a4: 0e 94 f1 0f call 0x1fe2 ; 0x1fe2 if (Blocking) // Check if function shall block 21a8: 11 23 and r17, r17 21aa: 31 f4 brne .+12 ; 0x21b8 21ac: 09 c0 rjmp .+18 ; 0x21c0 { while (ITGActionStatus != ITG_ACTION_IDLE) { Debug_Str ("BLOCK"); 21ae: 8b ed ldi r24, 0xDB ; 219 21b0: 92 e0 ldi r25, 0x02 ; 2 21b2: 0e 94 56 0b call 0x16ac ; 0x16ac asm volatile ("nop"); 21b6: 00 00 nop ITG_Debug_Str ("\n"); // Debug output TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Start TWI if (Blocking) // Check if function shall block { while (ITGActionStatus != ITG_ACTION_IDLE) 21b8: 80 91 39 20 lds r24, 0x2039 21bc: 88 23 and r24, r24 21be: b9 f7 brne .-18 ; 0x21ae asm volatile ("nop"); } ITG_Debug_Str ("ITG action done ...\n\n"); // Debug output } return ITGActionStatus; 21c0: 80 91 39 20 lds r24, 0x2039 } 21c4: 1f 91 pop r17 21c6: 08 95 ret 000021c8 <__vector_43>: } } #else /*****************************************************************************************************************************************/ ISR (ITG_INT_ISR_VECTOR) { 21c8: 1f 92 push r1 21ca: 0f 92 push r0 21cc: 0f b6 in r0, 0x3f ; 63 21ce: 0f 92 push r0 21d0: 08 b6 in r0, 0x38 ; 56 21d2: 0f 92 push r0 21d4: 18 be out 0x38, r1 ; 56 21d6: 09 b6 in r0, 0x39 ; 57 21d8: 0f 92 push r0 21da: 19 be out 0x39, r1 ; 57 21dc: 0b b6 in r0, 0x3b ; 59 21de: 0f 92 push r0 21e0: 1b be out 0x3b, r1 ; 59 21e2: 11 24 eor r1, r1 21e4: 2f 93 push r18 21e6: 3f 93 push r19 21e8: 4f 93 push r20 21ea: 5f 93 push r21 21ec: 6f 93 push r22 21ee: 7f 93 push r23 21f0: 8f 93 push r24 21f2: 9f 93 push r25 21f4: af 93 push r26 21f6: bf 93 push r27 21f8: ef 93 push r30 21fa: ff 93 push r31 if (ITGActionStatus == ITG_ACTION_IDLE) 21fc: 80 91 39 20 lds r24, 0x2039 2200: 88 23 and r24, r24 2202: 21 f4 brne .+8 ; 0x220c <__vector_43+0x44> StartITGAction(ITG_ACTION_GETTEMPXYZ, FUNC_NON_BLOCKING); 2204: 84 e0 ldi r24, 0x04 ; 4 2206: 60 e0 ldi r22, 0x00 ; 0 2208: 0e 94 bd 10 call 0x217a ; 0x217a } 220c: ff 91 pop r31 220e: ef 91 pop r30 2210: bf 91 pop r27 2212: af 91 pop r26 2214: 9f 91 pop r25 2216: 8f 91 pop r24 2218: 7f 91 pop r23 221a: 6f 91 pop r22 221c: 5f 91 pop r21 221e: 4f 91 pop r20 2220: 3f 91 pop r19 2222: 2f 91 pop r18 2224: 0f 90 pop r0 2226: 0b be out 0x3b, r0 ; 59 2228: 0f 90 pop r0 222a: 09 be out 0x39, r0 ; 57 222c: 0f 90 pop r0 222e: 08 be out 0x38, r0 ; 56 2230: 0f 90 pop r0 2232: 0f be out 0x3f, r0 ; 63 2234: 0f 90 pop r0 2236: 1f 90 pop r1 2238: 18 95 reti 0000223a : Recover_ITG_TWI_from_ARBLost (); // Recover from ARB lost condition } /*************************************************************************************************************/ inline void Recover_ITG_TWI_from_ARBLost() { 223a: 0f 93 push r16 223c: 1f 93 push r17 223e: cf 93 push r28 2240: df 93 push r29 } /*************************************************************************************************************/ void Disable_ITG_TWI () { ITG_TWI_PORT.MASTER.BAUD = 0; 2242: e0 ea ldi r30, 0xA0 ; 160 2244: f4 e0 ldi r31, 0x04 ; 4 2246: 15 82 std Z+5, r1 ; 0x05 ITG_TWI_PORT.MASTER.CTRLA = 0; 2248: 11 82 std Z+1, r1 ; 0x01 ITG_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 224a: 91 e0 ldi r25, 0x01 ; 1 224c: 94 83 std Z+4, r25 ; 0x04 /*************************************************************************************************************/ inline void Recover_ITG_TWI_from_ARBLost() { Disable_ITG_TWI (); ITG_PORT.DIRSET = (1 << PIN1); 224e: e0 e8 ldi r30, 0x80 ; 128 2250: f6 e0 ldi r31, 0x06 ; 6 2252: 82 e0 ldi r24, 0x02 ; 2 2254: 81 83 std Z+1, r24 ; 0x01 ITG_PORT.DIRCLR = (1 << PIN0); 2256: 92 83 std Z+2, r25 ; 0x02 while (!(ITG_PORT.IN & (1 << PIN0))) 2258: c0 e8 ldi r28, 0x80 ; 128 225a: d6 e0 ldi r29, 0x06 ; 6 { Debug_Str ("ITG RECOVER"); ITG_PORT.OUTTGL = (1 << PIN1); 225c: 02 e0 ldi r16, 0x02 ; 2 can be achieved. */ void _delay_loop_1(uint8_t __count) { __asm__ volatile ( 225e: 11 e6 ldi r17, 0x61 ; 97 2260: 08 c0 rjmp .+16 ; 0x2272 ITG_PORT.DIRSET = (1 << PIN1); ITG_PORT.DIRCLR = (1 << PIN0); while (!(ITG_PORT.IN & (1 << PIN0))) { Debug_Str ("ITG RECOVER"); 2262: 8c ec ldi r24, 0xCC ; 204 2264: 93 e0 ldi r25, 0x03 ; 3 2266: 0e 94 56 0b call 0x16ac ; 0x16ac ITG_PORT.OUTTGL = (1 << PIN1); 226a: 0f 83 std Y+7, r16 ; 0x07 226c: 81 2f mov r24, r17 226e: 8a 95 dec r24 2270: f1 f7 brne .-4 ; 0x226e Disable_ITG_TWI (); ITG_PORT.DIRSET = (1 << PIN1); ITG_PORT.DIRCLR = (1 << PIN0); while (!(ITG_PORT.IN & (1 << PIN0))) 2272: 80 91 88 06 lds r24, 0x0688 2276: 80 ff sbrs r24, 0 2278: f4 cf rjmp .-24 ; 0x2262 /*************************************************************************************************************/ void Init_ITG_TWI () { // Init ITG TWI port and baudrate ITG_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, ITG_TWI_FREQUENCY_HZ); 227a: e0 ea ldi r30, 0xA0 ; 160 227c: f4 e0 ldi r31, 0x04 ; 4 227e: 8f e1 ldi r24, 0x1F ; 31 2280: 85 83 std Z+5, r24 ; 0x05 ITG_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_HI_gc | 2282: 88 ef ldi r24, 0xF8 ; 248 2284: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; ITG_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 2286: 81 e0 ldi r24, 0x01 ; 1 2288: 84 83 std Z+4, r24 ; 0x04 ITG_PORT.OUTTGL = (1 << PIN1); _delay_us(10); } Init_ITG_TWI(); ITG_ERROR_Debug_Str ("Recover_ITG_TWI_from_ARBLost\n"); 228a: 8e ea ldi r24, 0xAE ; 174 228c: 93 e0 ldi r25, 0x03 ; 3 228e: 0e 94 56 0b call 0x16ac ; 0x16ac } 2292: df 91 pop r29 2294: cf 91 pop r28 2296: 1f 91 pop r17 2298: 0f 91 pop r16 229a: 08 95 ret 0000229c : /*************************************************************************************************************/ void Init_ITG_TWI () { // Init ITG TWI port and baudrate ITG_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, ITG_TWI_FREQUENCY_HZ); 229c: e0 ea ldi r30, 0xA0 ; 160 229e: f4 e0 ldi r31, 0x04 ; 4 22a0: 8f e1 ldi r24, 0x1F ; 31 22a2: 85 83 std Z+5, r24 ; 0x05 ITG_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_HI_gc | 22a4: 88 ef ldi r24, 0xF8 ; 248 22a6: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; ITG_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 22a8: 81 e0 ldi r24, 0x01 ; 1 22aa: 84 83 std Z+4, r24 ; 0x04 22ac: 84 ef ldi r24, 0xF4 ; 244 22ae: 91 e0 ldi r25, 0x01 ; 1 milliseconds can be achieved. */ void _delay_loop_2(uint16_t __count) { __asm__ volatile ( 22b0: 2b ed ldi r18, 0xDB ; 219 22b2: 32 e0 ldi r19, 0x02 ; 2 22b4: f9 01 movw r30, r18 22b6: 31 97 sbiw r30, 0x01 ; 1 22b8: f1 f7 brne .-4 ; 0x22b6 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 22ba: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 22bc: d9 f7 brne .-10 ; 0x22b4 { Init_ITG_TWI (); _delay_ms (50); Debug_Str ("Checking ITG communications:\n"); 22be: 80 e9 ldi r24, 0x90 ; 144 22c0: 93 e0 ldi r25, 0x03 ; 3 22c2: 0e 94 56 0b call 0x16ac ; 0x16ac StartITGAction(ITG_ACTION_CHECK_COMM, FUNC_BLOCKING); 22c6: 82 e0 ldi r24, 0x02 ; 2 22c8: 61 e0 ldi r22, 0x01 ; 1 22ca: 0e 94 bd 10 call 0x217a ; 0x217a } /*************************************************************************************************************/ inline uint8_t GetLastITGActionStatus () { return LastITGActionStatus; 22ce: 80 91 05 20 lds r24, 0x2005 _delay_ms (50); Debug_Str ("Checking ITG communications:\n"); StartITGAction(ITG_ACTION_CHECK_COMM, FUNC_BLOCKING); if (GetLastITGActionStatus() == ITG_ACTION_CHECK_COMM) 22d2: 82 30 cpi r24, 0x02 ; 2 22d4: d1 f4 brne .+52 ; 0x230a Debug_Str (" -ITG-3200 3 axes gyro found ...\n"); 22d6: 8d e6 ldi r24, 0x6D ; 109 22d8: 93 e0 ldi r25, 0x03 ; 3 22da: 0e 94 56 0b call 0x16ac ; 0x16ac 22de: 84 ef ldi r24, 0xF4 ; 244 22e0: 91 e0 ldi r25, 0x01 ; 1 22e2: 2b ed ldi r18, 0xDB ; 219 22e4: 32 e0 ldi r19, 0x02 ; 2 22e6: f9 01 movw r30, r18 22e8: 31 97 sbiw r30, 0x01 ; 1 22ea: f1 f7 brne .-4 ; 0x22e8 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 22ec: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 22ee: d9 f7 brne .-10 ; 0x22e6 return 0; } _delay_ms (50); Debug_Str ("Configure ITG sensor:\n"); 22f0: 89 e4 ldi r24, 0x49 ; 73 22f2: 93 e0 ldi r25, 0x03 ; 3 22f4: 0e 94 56 0b call 0x16ac ; 0x16ac StartITGAction(ITG_ACTION_CONFIGURE, FUNC_BLOCKING); 22f8: 83 e0 ldi r24, 0x03 ; 3 22fa: 61 e0 ldi r22, 0x01 ; 1 22fc: 0e 94 bd 10 call 0x217a ; 0x217a } /*************************************************************************************************************/ inline uint8_t GetLastITGActionStatus () { return LastITGActionStatus; 2300: 80 91 05 20 lds r24, 0x2005 _delay_ms (50); Debug_Str ("Configure ITG sensor:\n"); StartITGAction(ITG_ACTION_CONFIGURE, FUNC_BLOCKING); if (GetLastITGActionStatus() == ITG_ACTION_CONFIGURE) 2304: 83 30 cpi r24, 0x03 ; 3 2306: 69 f5 brne .+90 ; 0x2362 2308: 06 c0 rjmp .+12 ; 0x2316 StartITGAction(ITG_ACTION_CHECK_COMM, FUNC_BLOCKING); if (GetLastITGActionStatus() == ITG_ACTION_CHECK_COMM) Debug_Str (" -ITG-3200 3 axes gyro found ...\n"); else { Debug_Str (" - FAILURE\n"); 230a: 80 e6 ldi r24, 0x60 ; 96 230c: 93 e0 ldi r25, 0x03 ; 3 230e: 0e 94 56 0b call 0x16ac ; 0x16ac 2312: 80 e0 ldi r24, 0x00 ; 0 2314: 08 95 ret Debug_Str ("Configure ITG sensor:\n"); StartITGAction(ITG_ACTION_CONFIGURE, FUNC_BLOCKING); if (GetLastITGActionStatus() == ITG_ACTION_CONFIGURE) { Debug_Str (" -ITG-3200 3 axes gyro configured ...\n"); 2316: 81 e2 ldi r24, 0x21 ; 33 2318: 93 e0 ldi r25, 0x03 ; 3 231a: 0e 94 56 0b call 0x16ac ; 0x16ac Debug_Str (" -Sample rate [Hz]: "); 231e: 8b e0 ldi r24, 0x0B ; 11 2320: 93 e0 ldi r25, 0x03 ; 3 2322: 0e 94 56 0b call 0x16ac ; 0x16ac Debug_i16_left (ITG_SAMPLE_RATE_HZ); 2326: 84 e6 ldi r24, 0x64 ; 100 2328: 90 e0 ldi r25, 0x00 ; 0 232a: 0e 94 c5 0a call 0x158a ; 0x158a Debug_Str ("\n"); 232e: 89 e0 ldi r24, 0x09 ; 9 2330: 93 e0 ldi r25, 0x03 ; 3 2332: 0e 94 56 0b call 0x16ac ; 0x16ac Debug_Str (" -Sample rate divider: "); 2336: 80 ef ldi r24, 0xF0 ; 240 2338: 92 e0 ldi r25, 0x02 ; 2 233a: 0e 94 56 0b call 0x16ac ; 0x16ac Debug_u8 (ITG_SAMPLE_RATE_DIVIDER); 233e: 8f e4 ldi r24, 0x4F ; 79 2340: 0e 94 2b 0b call 0x1656 ; 0x1656 Debug_Str ("\n\n"); 2344: 8d ee ldi r24, 0xED ; 237 2346: 92 e0 ldi r25, 0x02 ; 2 2348: 0e 94 56 0b call 0x16ac ; 0x16ac 234c: 84 ef ldi r24, 0xF4 ; 244 234e: 91 e0 ldi r25, 0x01 ; 1 2350: 2b ed ldi r18, 0xDB ; 219 2352: 32 e0 ldi r19, 0x02 ; 2 2354: f9 01 movw r30, r18 2356: 31 97 sbiw r30, 0x01 ; 1 2358: f1 f7 brne .-4 ; 0x2356 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 235a: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 235c: d9 f7 brne .-10 ; 0x2354 235e: 81 e0 ldi r24, 0x01 ; 1 2360: 08 95 ret } else { Debug_Str (" - FAILURE\n"); 2362: 81 ee ldi r24, 0xE1 ; 225 2364: 92 e0 ldi r25, 0x02 ; 2 2366: 0e 94 56 0b call 0x16ac ; 0x16ac 236a: 80 e0 ldi r24, 0x00 ; 0 } _delay_ms (50); return 1; } 236c: 08 95 ret 0000236e : } /*************************************************************************************************************/ inline void ITG_Handle_TWI_ARB_Lost () { TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 236e: 80 ea ldi r24, 0xA0 ; 160 2370: 94 e0 ldi r25, 0x04 ; 4 2372: 0e 94 ff 0f call 0x1ffe ; 0x1ffe ITG_ERROR_Debug_Str ("ITG ARBLOST\n"); // Debug output 2376: 88 ed ldi r24, 0xD8 ; 216 2378: 93 e0 ldi r25, 0x03 ; 3 237a: 0e 94 56 0b call 0x16ac ; 0x16ac ITG_TWI_PORT.MASTER.STATUS |= TWI_MASTER_ARBLOST_bm; // Reset ARB lost flag 237e: e0 ea ldi r30, 0xA0 ; 160 2380: f4 e0 ldi r31, 0x04 ; 4 2382: 84 81 ldd r24, Z+4 ; 0x04 2384: 88 60 ori r24, 0x08 ; 8 2386: 84 83 std Z+4, r24 ; 0x04 ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine 2388: 10 92 39 20 sts 0x2039, r1 ITGByteCount = 0; // Reset ITGByteCount 238c: 10 92 3a 20 sts 0x203A, r1 Recover_ITG_TWI_from_ARBLost (); // Recover from ARB lost condition 2390: 0e 94 1d 11 call 0x223a ; 0x223a } 2394: 08 95 ret 00002396 : } /*************************************************************************************************************/ inline void ITG_Handle_TWI_BUS_Error () { TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 2396: 80 ea ldi r24, 0xA0 ; 160 2398: 94 e0 ldi r25, 0x04 ; 4 239a: 0e 94 ff 0f call 0x1ffe ; 0x1ffe ITG_ERROR_Debug_Str ("ITG BUSERR\n"); // Debug output 239e: 85 ee ldi r24, 0xE5 ; 229 23a0: 93 e0 ldi r25, 0x03 ; 3 23a2: 0e 94 56 0b call 0x16ac ; 0x16ac ITG_TWI_PORT.MASTER.STATUS |= TWI_MASTER_BUSERR_bm; // Reset BUS error flag 23a6: e0 ea ldi r30, 0xA0 ; 160 23a8: f4 e0 ldi r31, 0x04 ; 4 23aa: 84 81 ldd r24, Z+4 ; 0x04 23ac: 84 60 ori r24, 0x04 ; 4 23ae: 84 83 std Z+4, r24 ; 0x04 ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine 23b0: 10 92 39 20 sts 0x2039, r1 ITGByteCount = 0; // Reset ITGByteCount 23b4: 10 92 3a 20 sts 0x203A, r1 } 23b8: 08 95 ret 000023ba : } /*************************************************************************************************************/ inline void ITG_Handle_TWI_WIF_NACK () { TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 23ba: 80 ea ldi r24, 0xA0 ; 160 23bc: 94 e0 ldi r25, 0x04 ; 4 23be: 0e 94 ff 0f call 0x1ffe ; 0x1ffe ITG_ERROR_Debug_Str ("ITG NACK\n"); // Debug output 23c2: 81 ef ldi r24, 0xF1 ; 241 23c4: 93 e0 ldi r25, 0x03 ; 3 23c6: 0e 94 56 0b call 0x16ac ; 0x16ac LastITGActionStatus = ITG_ACTION_FAILURE; // Set last action result 23ca: 81 e0 ldi r24, 0x01 ; 1 23cc: 80 93 05 20 sts 0x2005, r24 ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine 23d0: 10 92 39 20 sts 0x2039, r1 } 23d4: 08 95 ret 000023d6 : } /*************************************************************************************************************/ inline void ITG_Handle_TWI_Unexpected () { TWI_STOP(&ITG_TWI_PORT); // Send TWI stop condition 23d6: 80 ea ldi r24, 0xA0 ; 160 23d8: 94 e0 ldi r25, 0x04 ; 4 23da: 0e 94 ff 0f call 0x1ffe ; 0x1ffe ITG_ERROR_Debug_Str ("ITG unexpected condition\n"); // Debug output 23de: 8b ef ldi r24, 0xFB ; 251 23e0: 93 e0 ldi r25, 0x03 ; 3 23e2: 0e 94 56 0b call 0x16ac ; 0x16ac ITGActionStatus = ITG_ACTION_FAILURE; // Reset ITG status state machine 23e6: 81 e0 ldi r24, 0x01 ; 1 23e8: 80 93 39 20 sts 0x2039, r24 ITGByteCount = 0; // Reset ITGByteCount 23ec: 10 92 3a 20 sts 0x203A, r1 } 23f0: 08 95 ret 000023f2 : ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine } /*************************************************************************************************************/ inline void ITG_Handle_TWI_WIF_ACK () { 23f2: 1f 93 push r17 ITG_Debug_Str ("ACK "); // Debug output //ITG_Debug_u8(ITGStatus); switch (ITGActionStatus) // Switch "action" state machine status 23f4: 10 91 39 20 lds r17, 0x2039 23f8: 12 30 cpi r17, 0x02 ; 2 23fa: 81 f0 breq .+32 ; 0x241c 23fc: 13 30 cpi r17, 0x03 ; 3 23fe: 18 f4 brcc .+6 ; 0x2406 2400: 11 23 and r17, r17 2402: 39 f0 breq .+14 ; 0x2412 2404: 99 c0 rjmp .+306 ; 0x2538 2406: 13 30 cpi r17, 0x03 ; 3 2408: 91 f0 breq .+36 ; 0x242e 240a: 14 30 cpi r17, 0x04 ; 4 240c: 09 f0 breq .+2 ; 0x2410 240e: 94 c0 rjmp .+296 ; 0x2538 2410: 79 c0 rjmp .+242 ; 0x2504 { // ***************************************************************************** case ITG_ACTION_IDLE: TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 2412: 80 ea ldi r24, 0xA0 ; 160 2414: 94 e0 ldi r25, 0x04 ; 4 2416: 0e 94 ff 0f call 0x1ffe ; 0x1ffe 241a: 06 c0 rjmp .+12 ; 0x2428 ITG_Debug_Str ("ITG_CHECK_COMM SUCCESSFUL - TWI stopped\n"); // Debug output break; // ***************************************************************************** case ITG_ACTION_CHECK_COMM: // COMM CHECK +++++++++++++++++++++++++++++++++++++++++++++ TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 241c: 80 ea ldi r24, 0xA0 ; 160 241e: 94 e0 ldi r25, 0x04 ; 4 2420: 0e 94 ff 0f call 0x1ffe ; 0x1ffe LastITGActionStatus = ITG_ACTION_CHECK_COMM; // Set last action result 2424: 10 93 05 20 sts 0x2005, r17 ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine 2428: 10 92 39 20 sts 0x2039, r1 242c: 83 c0 rjmp .+262 ; 0x2534 break; // ***************************************************************************** case ITG_ACTION_CONFIGURE: // Set sample rate // ITG Sensor reset if (ITGByteCount == 0) // Reset ITG sensor 242e: 80 91 3a 20 lds r24, 0x203A 2432: 88 23 and r24, r24 2434: 09 f4 brne .+2 ; 0x2438 2436: 52 c0 rjmp .+164 ; 0x24dc { TWI_WRITE(&ITG_TWI_PORT, ITG_PWR_MGM); // Write power management register address ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_PWR_MGM\n"); } else if (ITGByteCount == 1) // Set device reset flag in power management register address 2438: 80 91 3a 20 lds r24, 0x203A 243c: 81 30 cpi r24, 0x01 ; 1 243e: 21 f4 brne .+8 ; 0x2448 { TWI_WRITE(&ITG_TWI_PORT, 0x80); // Set device reset flag 2440: 80 ea ldi r24, 0xA0 ; 160 2442: 94 e0 ldi r25, 0x04 ; 4 2444: 60 e8 ldi r22, 0x80 ; 128 2446: 65 c0 rjmp .+202 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("0x80\n"); } // Repeated start --------------------------------------------------------- // ITG Sensor set sample rate divider else if (ITGByteCount == 2) // Send repeated start condition 2448: 80 91 3a 20 lds r24, 0x203A 244c: 82 30 cpi r24, 0x02 ; 2 244e: e1 f1 breq .+120 ; 0x24c8 { TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Send repeated start condition ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("RepStart\n"); } else if (ITGByteCount == 3) // Send sample rate register address 2450: 80 91 3a 20 lds r24, 0x203A 2454: 83 30 cpi r24, 0x03 ; 3 2456: 21 f4 brne .+8 ; 0x2460 { TWI_WRITE(&ITG_TWI_PORT, ITG_SMPLRT_DIV); // Write sample rate divider register address 2458: 80 ea ldi r24, 0xA0 ; 160 245a: 94 e0 ldi r25, 0x04 ; 4 245c: 65 e1 ldi r22, 0x15 ; 21 245e: 59 c0 rjmp .+178 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_SMPLRT_DIV\n"); } else if (ITGByteCount == 4) // Send sample rate config value 2460: 80 91 3a 20 lds r24, 0x203A 2464: 84 30 cpi r24, 0x04 ; 4 2466: 21 f4 brne .+8 ; 0x2470 { TWI_WRITE(&ITG_TWI_PORT, ITG_SAMPLE_RATE_DIVIDER); // 0x13 = 19d; Desired sample rate = 400Hz (see manual) 2468: 80 ea ldi r24, 0xA0 ; 160 246a: 94 e0 ldi r25, 0x04 ; 4 246c: 6f e4 ldi r22, 0x4F ; 79 246e: 51 c0 rjmp .+162 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_SAMPLE_RATE_DIVIDER\n"); } // Repeated start --------------------------------------------------------- // ITG Sensor set digital low pass filter (8khz internal frequency, 256Hz low pass filter bandwidth) else if (ITGByteCount == 5) // Send repeated start condition 2470: 80 91 3a 20 lds r24, 0x203A 2474: 85 30 cpi r24, 0x05 ; 5 2476: 41 f1 breq .+80 ; 0x24c8 { TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Send repeated start condition ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("RepStart\n"); } else if (ITGByteCount == 6) // Send DLPF register address 2478: 80 91 3a 20 lds r24, 0x203A 247c: 86 30 cpi r24, 0x06 ; 6 247e: 21 f4 brne .+8 ; 0x2488 { TWI_WRITE (&ITG_TWI_PORT, ITG_DLPF_FS); // Write DLPF register address 2480: 80 ea ldi r24, 0xA0 ; 160 2482: 94 e0 ldi r25, 0x04 ; 4 2484: 66 e1 ldi r22, 0x16 ; 22 2486: 45 c0 rjmp .+138 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_DLPF_FS\n"); } else if (ITGByteCount == 7) // Send DLPF config value 2488: 80 91 3a 20 lds r24, 0x203A 248c: 87 30 cpi r24, 0x07 ; 7 248e: 21 f4 brne .+8 ; 0x2498 { TWI_WRITE (&ITG_TWI_PORT, 0x18); // Write DLPF config value (see manual) 2490: 80 ea ldi r24, 0xA0 ; 160 2492: 94 e0 ldi r25, 0x04 ; 4 2494: 68 e1 ldi r22, 0x18 ; 24 2496: 3d c0 rjmp .+122 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("0x18\n"); } // Repeated start --------------------------------------------------------- else if (ITGByteCount == 8) // Send repeated start condition 2498: 80 91 3a 20 lds r24, 0x203A 249c: 88 30 cpi r24, 0x08 ; 8 249e: a1 f0 breq .+40 ; 0x24c8 { TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Send repeated start condition ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("RepStart\n"); } else if (ITGByteCount == 9) // Send interrupt configuration register address 24a0: 80 91 3a 20 lds r24, 0x203A 24a4: 89 30 cpi r24, 0x09 ; 9 24a6: 21 f4 brne .+8 ; 0x24b0 { TWI_WRITE (&ITG_TWI_PORT, ITG_INT_CFG); // Write interrupt configuration register address 24a8: 80 ea ldi r24, 0xA0 ; 160 24aa: 94 e0 ldi r25, 0x04 ; 4 24ac: 67 e1 ldi r22, 0x17 ; 23 24ae: 31 c0 rjmp .+98 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_INT_CFG\n"); } else if (ITGByteCount == 10) // Send interrupt configuration register address 24b0: 80 91 3a 20 lds r24, 0x203A 24b4: 8a 30 cpi r24, 0x0A ; 10 24b6: 21 f4 brne .+8 ; 0x24c0 { TWI_WRITE (&ITG_TWI_PORT, 0x05); // Write ITG interrupt config (enable ITG_RDY_EN & RAW_RDY_EN) 24b8: 80 ea ldi r24, 0xA0 ; 160 24ba: 94 e0 ldi r25, 0x04 ; 4 24bc: 65 e0 ldi r22, 0x05 ; 5 24be: 29 c0 rjmp .+82 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("0x05\n"); } // Repeated start --------------------------------------------------------- else if (ITGByteCount == 11) // Send repeated start condition 24c0: 80 91 3a 20 lds r24, 0x203A 24c4: 8b 30 cpi r24, 0x0B ; 11 24c6: 31 f4 brne .+12 ; 0x24d4 { TWI_START (&ITG_TWI_PORT, ITG + I2C_WRITE); // Send repeated start condition 24c8: 80 ea ldi r24, 0xA0 ; 160 24ca: 94 e0 ldi r25, 0x04 ; 4 24cc: 62 ed ldi r22, 0xD2 ; 210 24ce: 0e 94 f1 0f call 0x1fe2 ; 0x1fe2 24d2: 21 c0 rjmp .+66 ; 0x2516 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("RepStart\n"); } else if (ITGByteCount == 12) // Send power management register address 24d4: 80 91 3a 20 lds r24, 0x203A 24d8: 8c 30 cpi r24, 0x0C ; 12 24da: 21 f4 brne .+8 ; 0x24e4 { TWI_WRITE (&ITG_TWI_PORT, ITG_PWR_MGM); // Write power management register address 24dc: 80 ea ldi r24, 0xA0 ; 160 24de: 94 e0 ldi r25, 0x04 ; 4 24e0: 6e e3 ldi r22, 0x3E ; 62 24e2: 17 c0 rjmp .+46 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("ITG_PWR_MGM\n"); } else if (ITGByteCount == 13) // Set clock reference to PLL with Gyro X reference 24e4: 80 91 3a 20 lds r24, 0x203A 24e8: 8d 30 cpi r24, 0x0D ; 13 24ea: 21 f4 brne .+8 ; 0x24f4 { TWI_WRITE (&ITG_TWI_PORT, 0x01); // Write clock reference to PLL with Gyro X reference (see manual) 24ec: 80 ea ldi r24, 0xA0 ; 160 24ee: 94 e0 ldi r25, 0x04 ; 4 24f0: 61 e0 ldi r22, 0x01 ; 1 24f2: 0f c0 rjmp .+30 ; 0x2512 ITGByteCount++; // Increase ITGByteCount ITG_Debug_Str ("0x01\n"); } else { TWI_STOP (&ITG_TWI_PORT); // TWI stop condition 24f4: 80 ea ldi r24, 0xA0 ; 160 24f6: 94 e0 ldi r25, 0x04 ; 4 24f8: 0e 94 ff 0f call 0x1ffe ; 0x1ffe LastITGActionStatus = ITG_ACTION_CONFIGURE; // Set last action result 24fc: 83 e0 ldi r24, 0x03 ; 3 24fe: 80 93 05 20 sts 0x2005, r24 2502: 92 cf rjmp .-220 ; 0x2428 } break; // ***************************************************************************** case ITG_ACTION_GETTEMPXYZ: // Get temperature and XYZ gyro value if (ITGByteCount == 0) // Send register address from which to read from 2504: 80 91 3a 20 lds r24, 0x203A 2508: 88 23 and r24, r24 250a: 59 f4 brne .+22 ; 0x2522 { TWI_WRITE (&ITG_TWI_PORT, ITG_TEMP_OUT_H); // Write register address of temperature msb 250c: 80 ea ldi r24, 0xA0 ; 160 250e: 94 e0 ldi r25, 0x04 ; 4 2510: 6b e1 ldi r22, 0x1B ; 27 2512: 0e 94 f4 0f call 0x1fe8 ; 0x1fe8 ITGByteCount++; // Increase ITGByteCount 2516: 80 91 3a 20 lds r24, 0x203A 251a: 8f 5f subi r24, 0xFF ; 255 251c: 80 93 3a 20 sts 0x203A, r24 2520: 0b c0 rjmp .+22 ; 0x2538 ITG_Debug_Str ("W-Reg\n"); } else if (ITGByteCount == 1) // Switch transmission to reading 2522: 80 91 3a 20 lds r24, 0x203A 2526: 81 30 cpi r24, 0x01 ; 1 2528: 39 f4 brne .+14 ; 0x2538 { TWI_START (&ITG_TWI_PORT, ITG + I2C_READ); // Send ITG read address 252a: 80 ea ldi r24, 0xA0 ; 160 252c: 94 e0 ldi r25, 0x04 ; 4 252e: 63 ed ldi r22, 0xD3 ; 211 2530: 0e 94 f1 0f call 0x1fe2 ; 0x1fe2 ITGByteCount = 0; // Reset ITGByteCount for reading 2534: 10 92 3a 20 sts 0x203A, r1 ITG_Debug_Str ("RS-Read\n"); } break; } } 2538: 1f 91 pop r17 253a: 08 95 ret 0000253c : /*************************************************************************************************************/ inline void ITG_Handle_TWI_WIF () { ITG_Debug_Str ("WIF "); // Debug output if (ITG_TWI_PORT.MASTER.STATUS & TWI_MASTER_RXACK_bm) // Check if NACK received 253c: 80 91 a4 04 lds r24, 0x04A4 2540: 84 ff sbrs r24, 4 2542: 03 c0 rjmp .+6 ; 0x254a ITG_Handle_TWI_WIF_NACK (); // Handle ITG WIF NACK 2544: 0e 94 dd 11 call 0x23ba ; 0x23ba 2548: 08 95 ret else // ACK received ITG_Handle_TWI_WIF_ACK (); // Handle ITG WIF ACK 254a: 0e 94 f9 11 call 0x23f2 ; 0x23f2 254e: 08 95 ret 00002550 : ITG_Handle_TWI_Unexpected (); // Handle it! } /*************************************************************************************************************/ void ITG_Handle_TWI_RIF () { 2550: 1f 93 push r17 ITG_Debug_Str ("RIF: "); // Debug output uint8_t data = ITG_TWI_PORT.MASTER.DATA; // Read byte from TWI data register 2552: 90 91 a7 04 lds r25, 0x04A7 ITG_Debug_u8 (data); // Debug output ITG_Debug_Str (" "); // Debug output switch (ITGActionStatus) // Switch "action" state machine status 2556: 10 91 39 20 lds r17, 0x2039 255a: 14 30 cpi r17, 0x04 ; 4 255c: 09 f0 breq .+2 ; 0x2560 255e: 5d c0 rjmp .+186 ; 0x261a { case ITG_ACTION_GETTEMPXYZ: // Read temperature and XYZ gyro values if (ITGByteCount < ITG_TXYZ_LEN) // One of eight 16er complement bytes 2560: 80 91 3a 20 lds r24, 0x203A 2564: 87 30 cpi r24, 0x07 ; 7 2566: 80 f4 brcc .+32 ; 0x2588 { GyroByteData[ITGByteCount] = data; // Write received data byte to buffer 2568: e0 91 3a 20 lds r30, 0x203A 256c: f0 e0 ldi r31, 0x00 ; 0 256e: e9 59 subi r30, 0x99 ; 153 2570: ff 4d sbci r31, 0xDF ; 223 2572: 90 83 st Z, r25 TWI_ACK (&ITG_TWI_PORT); // Issue ACK and start a new byte read 2574: 80 ea ldi r24, 0xA0 ; 160 2576: 94 e0 ldi r25, 0x04 ; 4 2578: 0e 94 f7 0f call 0x1fee ; 0x1fee ITGByteCount++; // Increment ITGByteCount 257c: 80 91 3a 20 lds r24, 0x203A 2580: 8f 5f subi r24, 0xFF ; 255 2582: 80 93 3a 20 sts 0x203A, r24 2586: 49 c0 rjmp .+146 ; 0x261a ITG_Debug_Str ("\n"); // Debug output } else // Receive last byte and stop TWI reading { GyroByteData[ITGByteCount] = data; // Write received data byte to buffer 2588: e0 91 3a 20 lds r30, 0x203A 258c: f0 e0 ldi r31, 0x00 ; 0 258e: e9 59 subi r30, 0x99 ; 153 2590: ff 4d sbci r31, 0xDF ; 223 2592: 90 83 st Z, r25 TWI_NACK_STOP (&ITG_TWI_PORT); // Issue NACK and send stop condition 2594: 80 ea ldi r24, 0xA0 ; 160 2596: 94 e0 ldi r25, 0x04 ; 4 2598: 0e 94 fb 0f call 0x1ff6 ; 0x1ff6 LastITGActionStatus = ITG_ACTION_GETTEMPXYZ; // Set last action result 259c: 10 93 05 20 sts 0x2005, r17 ITGActionStatus = ITG_ACTION_IDLE; // Reset ITG status state machine 25a0: 10 92 39 20 sts 0x2039, r1 ITGByteCount = 0; // Reset ITGByteCount 25a4: 10 92 3a 20 sts 0x203A, r1 GyroRawData[0] = (int16_t)((GyroByteData[2] << 8) + GyroByteData[3]); // Calc p * 14,375 [deg / s] 25a8: 20 91 69 20 lds r18, 0x2069 25ac: 30 91 6a 20 lds r19, 0x206A 25b0: 92 2f mov r25, r18 25b2: 80 e0 ldi r24, 0x00 ; 0 25b4: 83 0f add r24, r19 25b6: 91 1d adc r25, r1 25b8: 80 93 71 20 sts 0x2071, r24 25bc: 90 93 72 20 sts 0x2072, r25 GyroRawData[1] = (int16_t)((GyroByteData[4] << 8) + GyroByteData[5]); // Calc q * 14,375 [deg / s] 25c0: 20 91 6b 20 lds r18, 0x206B 25c4: 30 91 6c 20 lds r19, 0x206C 25c8: 92 2f mov r25, r18 25ca: 80 e0 ldi r24, 0x00 ; 0 25cc: 83 0f add r24, r19 25ce: 91 1d adc r25, r1 25d0: 80 93 73 20 sts 0x2073, r24 25d4: 90 93 74 20 sts 0x2074, r25 GyroRawData[2] = (int16_t)((GyroByteData[6] << 8) + GyroByteData[7]); // Calc r * 14,375 [deg / s] 25d8: 20 91 6d 20 lds r18, 0x206D 25dc: 30 91 6e 20 lds r19, 0x206E 25e0: 92 2f mov r25, r18 25e2: 80 e0 ldi r24, 0x00 ; 0 25e4: 83 0f add r24, r19 25e6: 91 1d adc r25, r1 25e8: 80 93 75 20 sts 0x2075, r24 25ec: 90 93 76 20 sts 0x2076, r25 Temperature_m10 = 350 + (((GyroByteData[0] << 8) + 25f0: 40 91 67 20 lds r20, 0x2067 25f4: 80 91 68 20 lds r24, 0x2068 25f8: 90 e0 ldi r25, 0x00 ; 0 25fa: 80 57 subi r24, 0x70 ; 112 25fc: 9c 4c sbci r25, 0xCC ; 204 25fe: 34 2f mov r19, r20 2600: 20 e0 ldi r18, 0x00 ; 0 2602: 82 0f add r24, r18 2604: 93 1f adc r25, r19 2606: 6c e1 ldi r22, 0x1C ; 28 2608: 70 e0 ldi r23, 0x00 ; 0 260a: 0e 94 7e 18 call 0x30fc ; 0x30fc <__divmodhi4> 260e: 62 5a subi r22, 0xA2 ; 162 2610: 7e 4f sbci r23, 0xFE ; 254 2612: 60 93 65 20 sts 0x2065, r22 2616: 70 93 66 20 sts 0x2066, r23 ITG_Debug_Str ("DATA OK\n"); // Debug output } break; } } 261a: 1f 91 pop r17 261c: 08 95 ret 0000261e <__vector_46>: #endif /*****************************************************************************************************************************************/ ISR (ITG_TWI_INT) { 261e: 1f 92 push r1 2620: 0f 92 push r0 2622: 0f b6 in r0, 0x3f ; 63 2624: 0f 92 push r0 2626: 08 b6 in r0, 0x38 ; 56 2628: 0f 92 push r0 262a: 18 be out 0x38, r1 ; 56 262c: 09 b6 in r0, 0x39 ; 57 262e: 0f 92 push r0 2630: 19 be out 0x39, r1 ; 57 2632: 0b b6 in r0, 0x3b ; 59 2634: 0f 92 push r0 2636: 1b be out 0x3b, r1 ; 59 2638: 11 24 eor r1, r1 263a: 2f 93 push r18 263c: 3f 93 push r19 263e: 4f 93 push r20 2640: 5f 93 push r21 2642: 6f 93 push r22 2644: 7f 93 push r23 2646: 8f 93 push r24 2648: 9f 93 push r25 264a: af 93 push r26 264c: bf 93 push r27 264e: ef 93 push r30 2650: ff 93 push r31 ITG_Debug_Str ("ITG-INT bc: "); // Debug output ITG_Debug_u8 (ITGByteCount); // Debug output ITG_Debug_Str (" "); // Debug output uint8_t status = ITG_TWI_PORT.MASTER.STATUS; // Get status from TWI Master 2652: 90 91 a4 04 lds r25, 0x04A4 if (status & TWI_MASTER_ARBLOST_bm) // Check if TWI ARB was lost 2656: 93 ff sbrs r25, 3 2658: 03 c0 rjmp .+6 ; 0x2660 <__vector_46+0x42> ITG_Handle_TWI_ARB_Lost (); // Recover from ARB lost 265a: 0e 94 b7 11 call 0x236e ; 0x236e 265e: 11 c0 rjmp .+34 ; 0x2682 <__vector_46+0x64> else if (status & TWI_MASTER_BUSERR_bm) // Check if TWI bus error happened 2660: 92 ff sbrs r25, 2 2662: 03 c0 rjmp .+6 ; 0x266a <__vector_46+0x4c> ITG_Handle_TWI_BUS_Error (); // Recover from BUS error 2664: 0e 94 cb 11 call 0x2396 ; 0x2396 2668: 0c c0 rjmp .+24 ; 0x2682 <__vector_46+0x64> else if (status & TWI_MASTER_WIF_bm) // Check if WIF (write interrupt flag) is set 266a: 96 ff sbrs r25, 6 266c: 03 c0 rjmp .+6 ; 0x2674 <__vector_46+0x56> ITG_Handle_TWI_WIF (); // Handle TWI WIF 266e: 0e 94 9e 12 call 0x253c ; 0x253c 2672: 07 c0 rjmp .+14 ; 0x2682 <__vector_46+0x64> else if (status & TWI_MASTER_RIF_bm) // Check if RIF (read interrupt flag) is set 2674: 97 ff sbrs r25, 7 2676: 03 c0 rjmp .+6 ; 0x267e <__vector_46+0x60> ITG_Handle_TWI_RIF (); // Handle TWI RIF 2678: 0e 94 a8 12 call 0x2550 ; 0x2550 267c: 02 c0 rjmp .+4 ; 0x2682 <__vector_46+0x64> else // Some other unexpected condition happened ITG_Handle_TWI_Unexpected (); // Handle it! 267e: 0e 94 eb 11 call 0x23d6 ; 0x23d6 } 2682: ff 91 pop r31 2684: ef 91 pop r30 2686: bf 91 pop r27 2688: af 91 pop r26 268a: 9f 91 pop r25 268c: 8f 91 pop r24 268e: 7f 91 pop r23 2690: 6f 91 pop r22 2692: 5f 91 pop r21 2694: 4f 91 pop r20 2696: 3f 91 pop r19 2698: 2f 91 pop r18 269a: 0f 90 pop r0 269c: 0b be out 0x3b, r0 ; 59 269e: 0f 90 pop r0 26a0: 09 be out 0x39, r0 ; 57 26a2: 0f 90 pop r0 26a4: 08 be out 0x38, r0 ; 56 26a6: 0f 90 pop r0 26a8: 0f be out 0x3f, r0 ; 63 26aa: 0f 90 pop r0 26ac: 1f 90 pop r1 26ae: 18 95 reti 000026b0 : } /*************************************************************************************************************/ void LIS_Disable_TWI () { LIS_TWI_PORT.MASTER.BAUD = 0; 26b0: e0 eb ldi r30, 0xB0 ; 176 26b2: f4 e0 ldi r31, 0x04 ; 4 26b4: 15 82 std Z+5, r1 ; 0x05 LIS_TWI_PORT.MASTER.CTRLA = 0; 26b6: 11 82 std Z+1, r1 ; 0x01 LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 26b8: 81 e0 ldi r24, 0x01 ; 1 26ba: 84 83 std Z+4, r24 ; 0x04 } 26bc: 08 95 ret 000026be : /*************************************************************************************************************/ void LIS_Init_TWI () { LIS_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, LIS_TWI_FREQENCY_HZ); 26be: e0 eb ldi r30, 0xB0 ; 176 26c0: f4 e0 ldi r31, 0x04 ; 4 26c2: 8f e1 ldi r24, 0x1F ; 31 26c4: 85 83 std Z+5, r24 ; 0x05 LIS_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_LO_gc | 26c6: 88 e7 ldi r24, 0x78 ; 120 26c8: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 26ca: 81 e0 ldi r24, 0x01 ; 1 26cc: 84 83 std Z+4, r24 ; 0x04 } 26ce: 08 95 ret 000026d0 : return 1; } /*************************************************************************************************************/ void Get_LISRawValues (int16_t * DataBuffer) { 26d0: fc 01 movw r30, r24 uint8_t i; cli (); 26d2: f8 94 cli for (i = 0; i < 3; i++) DataBuffer[i] = AccelRawData[i]; 26d4: 80 91 77 20 lds r24, 0x2077 26d8: 90 91 78 20 lds r25, 0x2078 26dc: 80 83 st Z, r24 26de: 91 83 std Z+1, r25 ; 0x01 26e0: 80 91 79 20 lds r24, 0x2079 26e4: 90 91 7a 20 lds r25, 0x207A 26e8: 82 83 std Z+2, r24 ; 0x02 26ea: 93 83 std Z+3, r25 ; 0x03 26ec: 80 91 7b 20 lds r24, 0x207B 26f0: 90 91 7c 20 lds r25, 0x207C 26f4: 84 83 std Z+4, r24 ; 0x04 26f6: 95 83 std Z+5, r25 ; 0x05 sei (); 26f8: 78 94 sei LastLis3LActionStatus = 0; 26fa: 10 92 06 20 sts 0x2006, r1 } 26fe: 08 95 ret 00002700 : /*************************************************************************************************************/ inline void Start_LIS_Data_Query () { // Init LIS_ADDRESS timer TC_SetPeriod (&LIS_UPDATE_TIMER, SYS_CLK / LIS_UPDATE_FREQUENCY_HZ / LIS_TIMER_PRESCALER_VALUE); 2700: e0 e0 ldi r30, 0x00 ; 0 2702: fb e0 ldi r31, 0x0B ; 11 2704: 80 e0 ldi r24, 0x00 ; 0 2706: 90 e9 ldi r25, 0x90 ; 144 2708: 86 a3 std Z+38, r24 ; 0x26 270a: 97 a3 std Z+39, r25 ; 0x27 TC_SetPrescaler (&LIS_UPDATE_TIMER, LIS_TIMER_PRESCALER); 270c: 84 e0 ldi r24, 0x04 ; 4 270e: 80 83 st Z, r24 TC_SetOverflowInt (&LIS_UPDATE_TIMER, TC_OVFINTLVL_HI_gc); 2710: 86 81 ldd r24, Z+6 ; 0x06 2712: 83 60 ori r24, 0x03 ; 3 2714: 86 83 std Z+6, r24 ; 0x06 } 2716: 08 95 ret 00002718 : /*************************************************************************************************************/ inline uint8_t Get_LIS_LastActionStatus () { return LastLis3LActionStatus; 2718: 80 91 06 20 lds r24, 0x2006 } 271c: 08 95 ret 0000271e : /*************************************************************************************************************/ inline uint8_t Get_LIS_ActionStatus () { return Lis3LStatus; 271e: 80 91 3b 20 lds r24, 0x203B } 2722: 08 95 ret 00002724 : /*************************************************************************************************************/ uint8_t Start_LIS_Action (uint8_t ActionType, uint8_t Blocking) { 2724: 1f 93 push r17 2726: 98 2f mov r25, r24 2728: 16 2f mov r17, r22 LIS_Debug_Str ("LISact: "); LIS_Debug_u8 (ActionType); LIS_Debug_Str ("\n"); if (ActionType > LIS_ACTION_NUMBER_OF_STATES) 272a: 87 30 cpi r24, 0x07 ; 7 272c: 18 f0 brcs .+6 ; 0x2734 { LIS_Debug_Str ("LIS3L action is unknown\n"); Lis3LStatus = LIS_ACTION_IDLE; 272e: 10 92 3b 20 sts 0x203B, r1 2732: 1b c0 rjmp .+54 ; 0x276a return Lis3LStatus; } else if (Lis3LStatus != LIS_ACTION_IDLE) 2734: 80 91 3b 20 lds r24, 0x203B 2738: 88 23 and r24, r24 273a: b9 f4 brne .+46 ; 0x276a LIS_Debug_Str ("-"); return Lis3LStatus; } else { Lis3LStatus = ActionType; 273c: 90 93 3b 20 sts 0x203B, r25 LISByteCount = 0; 2740: 10 92 3c 20 sts 0x203C, r1 } LIS_Debug_Str ("TWI bus status: "); switch (LIS_TWI_PORT.MASTER.STATUS & TWI_MASTER_BUSSTATE_gm) 2744: 80 91 b4 04 lds r24, 0x04B4 LIS_Debug_Str ("LIS BUS STATE BUSY\n"); break; } // Start condition + LIS_ADDRESS address TWI_START (&LIS_TWI_PORT, LIS_ADDRESS + I2C_WRITE); 2748: 80 eb ldi r24, 0xB0 ; 176 274a: 94 e0 ldi r25, 0x04 ; 4 274c: 6a e3 ldi r22, 0x3A ; 58 274e: 0e 94 f1 0f call 0x1fe2 ; 0x1fe2 // Loop until done if blocking flag is set if (Blocking) 2752: 11 23 and r17, r17 2754: 31 f4 brne .+12 ; 0x2762 2756: 09 c0 rjmp .+18 ; 0x276a { while (Lis3LStatus != LIS_ACTION_IDLE) { Debug_Str ("BLOCK"); 2758: 85 e1 ldi r24, 0x15 ; 21 275a: 94 e0 ldi r25, 0x04 ; 4 275c: 0e 94 56 0b call 0x16ac ; 0x16ac asm volatile ("nop"); 2760: 00 00 nop TWI_START (&LIS_TWI_PORT, LIS_ADDRESS + I2C_WRITE); // Loop until done if blocking flag is set if (Blocking) { while (Lis3LStatus != LIS_ACTION_IDLE) 2762: 80 91 3b 20 lds r24, 0x203B 2766: 88 23 and r24, r24 2768: b9 f7 brne .-18 ; 0x2758 Debug_Str ("BLOCK"); asm volatile ("nop"); } } return Lis3LStatus; 276a: 80 91 3b 20 lds r24, 0x203B } 276e: 1f 91 pop r17 2770: 08 95 ret 00002772 <__vector_108>: volatile int16_t AccelCalibData[3]; // Calibrated acceleration values 0=X 1=Y 2=Z volatile uint16_t AccelVectorLength; // Length of acceleration vector /*****************************************************************************************************************************************/ ISR (LIS_UPDATE_TIMER_INT) { 2772: 1f 92 push r1 2774: 0f 92 push r0 2776: 0f b6 in r0, 0x3f ; 63 2778: 0f 92 push r0 277a: 08 b6 in r0, 0x38 ; 56 277c: 0f 92 push r0 277e: 18 be out 0x38, r1 ; 56 2780: 09 b6 in r0, 0x39 ; 57 2782: 0f 92 push r0 2784: 19 be out 0x39, r1 ; 57 2786: 0b b6 in r0, 0x3b ; 59 2788: 0f 92 push r0 278a: 1b be out 0x3b, r1 ; 59 278c: 11 24 eor r1, r1 278e: 2f 93 push r18 2790: 3f 93 push r19 2792: 4f 93 push r20 2794: 5f 93 push r21 2796: 6f 93 push r22 2798: 7f 93 push r23 279a: 8f 93 push r24 279c: 9f 93 push r25 279e: af 93 push r26 27a0: bf 93 push r27 27a2: ef 93 push r30 27a4: ff 93 push r31 if (Lis3LStatus == LIS_ACTION_IDLE) 27a6: 80 91 3b 20 lds r24, 0x203B 27aa: 88 23 and r24, r24 27ac: 21 f4 brne .+8 ; 0x27b6 <__vector_108+0x44> Start_LIS_Action(LIS_ACTION_GETXYZ, FUNC_NON_BLOCKING); 27ae: 85 e0 ldi r24, 0x05 ; 5 27b0: 60 e0 ldi r22, 0x00 ; 0 27b2: 0e 94 92 13 call 0x2724 ; 0x2724 } 27b6: ff 91 pop r31 27b8: ef 91 pop r30 27ba: bf 91 pop r27 27bc: af 91 pop r26 27be: 9f 91 pop r25 27c0: 8f 91 pop r24 27c2: 7f 91 pop r23 27c4: 6f 91 pop r22 27c6: 5f 91 pop r21 27c8: 4f 91 pop r20 27ca: 3f 91 pop r19 27cc: 2f 91 pop r18 27ce: 0f 90 pop r0 27d0: 0b be out 0x3b, r0 ; 59 27d2: 0f 90 pop r0 27d4: 09 be out 0x39, r0 ; 57 27d6: 0f 90 pop r0 27d8: 08 be out 0x38, r0 ; 56 27da: 0f 90 pop r0 27dc: 0f be out 0x3f, r0 ; 63 27de: 0f 90 pop r0 27e0: 1f 90 pop r1 27e2: 18 95 reti 000027e4 : LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; } /*************************************************************************************************************/ void LIS_Recover_TWI_from_ARBLost() { 27e4: 0f 93 push r16 27e6: 1f 93 push r17 27e8: cf 93 push r28 27ea: df 93 push r29 } /*************************************************************************************************************/ void LIS_Disable_TWI () { LIS_TWI_PORT.MASTER.BAUD = 0; 27ec: e0 eb ldi r30, 0xB0 ; 176 27ee: f4 e0 ldi r31, 0x04 ; 4 27f0: 15 82 std Z+5, r1 ; 0x05 LIS_TWI_PORT.MASTER.CTRLA = 0; 27f2: 11 82 std Z+1, r1 ; 0x01 LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 27f4: 91 e0 ldi r25, 0x01 ; 1 27f6: 94 83 std Z+4, r25 ; 0x04 /*************************************************************************************************************/ void LIS_Recover_TWI_from_ARBLost() { LIS_Disable_TWI (); // Disable TWI LIS_PORT.DIRSET = (1 << PIN1); // Set SCL pin as output 27f8: e0 ea ldi r30, 0xA0 ; 160 27fa: f6 e0 ldi r31, 0x06 ; 6 27fc: 82 e0 ldi r24, 0x02 ; 2 27fe: 81 83 std Z+1, r24 ; 0x01 LIS_PORT.DIRCLR = (1 << PIN0); // Set SDA pin as input 2800: 92 83 std Z+2, r25 ; 0x02 while (!(LIS_PORT.IN & (1 << PIN0))) // Loop while SDA is low to recover from ARB lost 2802: c0 ea ldi r28, 0xA0 ; 160 2804: d6 e0 ldi r29, 0x06 ; 6 { Debug_Str ("LIS RECOVER"); LIS_PORT.OUTTGL = (1 << PIN1); // Toggle SCL 2806: 02 e0 ldi r16, 0x02 ; 2 can be achieved. */ void _delay_loop_1(uint8_t __count) { __asm__ volatile ( 2808: 11 e6 ldi r17, 0x61 ; 97 280a: 08 c0 rjmp .+16 ; 0x281c LIS_PORT.DIRSET = (1 << PIN1); // Set SCL pin as output LIS_PORT.DIRCLR = (1 << PIN0); // Set SDA pin as input while (!(LIS_PORT.IN & (1 << PIN0))) // Loop while SDA is low to recover from ARB lost { Debug_Str ("LIS RECOVER"); 280c: 8f ef ldi r24, 0xFF ; 255 280e: 94 e0 ldi r25, 0x04 ; 4 2810: 0e 94 56 0b call 0x16ac ; 0x16ac LIS_PORT.OUTTGL = (1 << PIN1); // Toggle SCL 2814: 0f 83 std Y+7, r16 ; 0x07 2816: 81 2f mov r24, r17 2818: 8a 95 dec r24 281a: f1 f7 brne .-4 ; 0x2818 LIS_Disable_TWI (); // Disable TWI LIS_PORT.DIRSET = (1 << PIN1); // Set SCL pin as output LIS_PORT.DIRCLR = (1 << PIN0); // Set SDA pin as input while (!(LIS_PORT.IN & (1 << PIN0))) // Loop while SDA is low to recover from ARB lost 281c: 80 91 a8 06 lds r24, 0x06A8 2820: 80 ff sbrs r24, 0 2822: f4 cf rjmp .-24 ; 0x280c } /*************************************************************************************************************/ void LIS_Init_TWI () { LIS_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, LIS_TWI_FREQENCY_HZ); 2824: e0 eb ldi r30, 0xB0 ; 176 2826: f4 e0 ldi r31, 0x04 ; 4 2828: 8f e1 ldi r24, 0x1F ; 31 282a: 85 83 std Z+5, r24 ; 0x05 LIS_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_LO_gc | 282c: 88 e7 ldi r24, 0x78 ; 120 282e: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 2830: 81 e0 ldi r24, 0x01 ; 1 2832: 84 83 std Z+4, r24 ; 0x04 LIS_PORT.OUTTGL = (1 << PIN1); // Toggle SCL _delay_us(10); // Wait 10µs } LIS_Init_TWI(); // Initialize TWI LIS_ERROR_Debug_Str ("Recover_LIS_TWI_from_ARBLost\n"); // Output message 2834: 81 ee ldi r24, 0xE1 ; 225 2836: 94 e0 ldi r25, 0x04 ; 4 2838: 0e 94 56 0b call 0x16ac ; 0x16ac Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 283c: 10 92 3b 20 sts 0x203B, r1 } 2840: df 91 pop r29 2842: cf 91 pop r28 2844: 1f 91 pop r17 2846: 0f 91 pop r16 2848: 08 95 ret 0000284a : } /*************************************************************************************************************/ inline void LIS_Handle_TWI_BUS_Error () { LIS_ERROR_Debug_Str ("LIS BUSerror\n"); // Debug output 284a: 8b e0 ldi r24, 0x0B ; 11 284c: 95 e0 ldi r25, 0x05 ; 5 284e: 0e 94 56 0b call 0x16ac ; 0x16ac LIS_TWI_PORT.MASTER.STATUS |= TWI_MASTER_BUSERR_bm; // Reset bus error flag 2852: e0 eb ldi r30, 0xB0 ; 176 2854: f4 e0 ldi r31, 0x04 ; 4 2856: 84 81 ldd r24, Z+4 ; 0x04 2858: 84 60 ori r24, 0x04 ; 4 285a: 84 83 std Z+4, r24 ; 0x04 Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 285c: 10 92 3b 20 sts 0x203B, r1 LISByteCount = 0; // Reset ByteCount 2860: 10 92 3c 20 sts 0x203C, r1 } 2864: 08 95 ret 00002866 : } /*************************************************************************************************************/ inline void LIS_Handle_TWI_ARB_Lost () { LIS_ERROR_Debug_Str ("LIS ARBlost\n"); // Debug output 2866: 89 e1 ldi r24, 0x19 ; 25 2868: 95 e0 ldi r25, 0x05 ; 5 286a: 0e 94 56 0b call 0x16ac ; 0x16ac LIS_TWI_PORT.MASTER.STATUS |= TWI_MASTER_ARBLOST_bm; // Reset ARB lost flag 286e: e0 eb ldi r30, 0xB0 ; 176 2870: f4 e0 ldi r31, 0x04 ; 4 2872: 84 81 ldd r24, Z+4 ; 0x04 2874: 88 60 ori r24, 0x08 ; 8 2876: 84 83 std Z+4, r24 ; 0x04 Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 2878: 10 92 3b 20 sts 0x203B, r1 LISByteCount = 0; // Reset ByteCount 287c: 10 92 3c 20 sts 0x203C, r1 LIS_Recover_TWI_from_ARBLost (); // Recover from ARB lost 2880: 0e 94 f2 13 call 0x27e4 ; 0x27e4 } 2884: 08 95 ret 00002886 : } /*************************************************************************************************************/ void LIS_Init_TWI () { LIS_TWI_PORT.MASTER.BAUD = TWI_BAUD(SYS_CLK, LIS_TWI_FREQENCY_HZ); 2886: e0 eb ldi r30, 0xB0 ; 176 2888: f4 e0 ldi r31, 0x04 ; 4 288a: 8f e1 ldi r24, 0x1F ; 31 288c: 85 83 std Z+5, r24 ; 0x05 LIS_TWI_PORT.MASTER.CTRLA = TWI_MASTER_INTLVL_LO_gc | 288e: 88 e7 ldi r24, 0x78 ; 120 2890: 81 83 std Z+1, r24 ; 0x01 TWI_MASTER_ENABLE_bm | TWI_MASTER_RIEN_bm | TWI_MASTER_WIEN_bm; LIS_TWI_PORT.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; 2892: 81 e0 ldi r24, 0x01 ; 1 2894: 84 83 std Z+4, r24 ; 0x04 /*************************************************************************************************************/ uint8_t Init_LIS_Sensor () { LIS_Init_TWI (); // Init LIS TWI Debug_Str ("Checking LIS communications:\n"); // Debug output 2896: 83 ec ldi r24, 0xC3 ; 195 2898: 94 e0 ldi r25, 0x04 ; 4 289a: 0e 94 56 0b call 0x16ac ; 0x16ac Start_LIS_Action(LIS_ACTION_CHECK_COMM, FUNC_BLOCKING); // Check if LIS is there, block this function 289e: 82 e0 ldi r24, 0x02 ; 2 28a0: 61 e0 ldi r22, 0x01 ; 1 28a2: 0e 94 92 13 call 0x2724 ; 0x2724 } /*************************************************************************************************************/ inline uint8_t Get_LIS_LastActionStatus () { return LastLis3LActionStatus; 28a6: 80 91 06 20 lds r24, 0x2006 { LIS_Init_TWI (); // Init LIS TWI Debug_Str ("Checking LIS communications:\n"); // Debug output Start_LIS_Action(LIS_ACTION_CHECK_COMM, FUNC_BLOCKING); // Check if LIS is there, block this function if (Get_LIS_LastActionStatus() == LIS_ACTION_CHECK_COMM) // LIS sensor found 28aa: 82 30 cpi r24, 0x02 ; 2 28ac: d1 f4 brne .+52 ; 0x28e2 Debug_Str (" - LIS3LV02DQ found ...\n"); // Debug output 28ae: 89 ea ldi r24, 0xA9 ; 169 28b0: 94 e0 ldi r25, 0x04 ; 4 28b2: 0e 94 56 0b call 0x16ac ; 0x16ac 28b6: 84 ef ldi r24, 0xF4 ; 244 28b8: 91 e0 ldi r25, 0x01 ; 1 milliseconds can be achieved. */ void _delay_loop_2(uint16_t __count) { __asm__ volatile ( 28ba: 2b ed ldi r18, 0xDB ; 219 28bc: 32 e0 ldi r19, 0x02 ; 2 28be: f9 01 movw r30, r18 28c0: 31 97 sbiw r30, 0x01 ; 1 28c2: f1 f7 brne .-4 ; 0x28c0 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 28c4: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 28c6: d9 f7 brne .-10 ; 0x28be return 0; // Quit program } _delay_ms (50); Debug_Str ("Configure LIS sensor:\n"); 28c8: 85 e8 ldi r24, 0x85 ; 133 28ca: 94 e0 ldi r25, 0x04 ; 4 28cc: 0e 94 56 0b call 0x16ac ; 0x16ac Start_LIS_Action(LIS_ACTION_CONFIGURE, FUNC_BLOCKING); 28d0: 84 e0 ldi r24, 0x04 ; 4 28d2: 61 e0 ldi r22, 0x01 ; 1 28d4: 0e 94 92 13 call 0x2724 ; 0x2724 } /*************************************************************************************************************/ inline uint8_t Get_LIS_LastActionStatus () { return LastLis3LActionStatus; 28d8: 80 91 06 20 lds r24, 0x2006 _delay_ms (50); Debug_Str ("Configure LIS sensor:\n"); Start_LIS_Action(LIS_ACTION_CONFIGURE, FUNC_BLOCKING); if (Get_LIS_LastActionStatus() == LIS_ACTION_CONFIGURE) 28dc: 84 30 cpi r24, 0x04 ; 4 28de: 11 f5 brne .+68 ; 0x2924 28e0: 07 c0 rjmp .+14 ; 0x28f0 } /*************************************************************************************************************/ inline uint8_t Get_LIS_LastActionStatus () { return LastLis3LActionStatus; 28e2: 80 91 06 20 lds r24, 0x2006 Start_LIS_Action(LIS_ACTION_CHECK_COMM, FUNC_BLOCKING); // Check if LIS is there, block this function if (Get_LIS_LastActionStatus() == LIS_ACTION_CHECK_COMM) // LIS sensor found Debug_Str (" - LIS3LV02DQ found ...\n"); // Debug output else // LIS sensor NOT found { Debug_u8(Get_LIS_LastActionStatus()); // Debug output 28e6: 0e 94 2b 0b call 0x1656 ; 0x1656 Debug_Str (" - FAILURE\n"); // Debug output 28ea: 8c e9 ldi r24, 0x9C ; 156 28ec: 94 e0 ldi r25, 0x04 ; 4 28ee: 1c c0 rjmp .+56 ; 0x2928 _delay_ms (50); Debug_Str ("Configure LIS sensor:\n"); Start_LIS_Action(LIS_ACTION_CONFIGURE, FUNC_BLOCKING); if (Get_LIS_LastActionStatus() == LIS_ACTION_CONFIGURE) Debug_Str (" - LIS3LV02DQ configured ...\n"); 28f0: 86 e6 ldi r24, 0x66 ; 102 28f2: 94 e0 ldi r25, 0x04 ; 4 28f4: 0e 94 56 0b call 0x16ac ; 0x16ac 28f8: 84 ef ldi r24, 0xF4 ; 244 28fa: 91 e0 ldi r25, 0x01 ; 1 28fc: 2b ed ldi r18, 0xDB ; 219 28fe: 32 e0 ldi r19, 0x02 ; 2 2900: f9 01 movw r30, r18 2902: 31 97 sbiw r30, 0x01 ; 1 2904: f1 f7 brne .-4 ; 0x2902 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 2906: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 2908: d9 f7 brne .-10 ; 0x2900 return 0; } _delay_ms (50); Debug_Str ("Enable LIS sensor:\n"); 290a: 85 e4 ldi r24, 0x45 ; 69 290c: 94 e0 ldi r25, 0x04 ; 4 290e: 0e 94 56 0b call 0x16ac ; 0x16ac Start_LIS_Action(LIS_ACTION_ENABLE, FUNC_BLOCKING); 2912: 83 e0 ldi r24, 0x03 ; 3 2914: 61 e0 ldi r22, 0x01 ; 1 2916: 0e 94 92 13 call 0x2724 ; 0x2724 } /*************************************************************************************************************/ inline uint8_t Get_LIS_LastActionStatus () { return LastLis3LActionStatus; 291a: 80 91 06 20 lds r24, 0x2006 _delay_ms (50); Debug_Str ("Enable LIS sensor:\n"); Start_LIS_Action(LIS_ACTION_ENABLE, FUNC_BLOCKING); if (Get_LIS_LastActionStatus() == LIS_ACTION_ENABLE) 291e: 83 30 cpi r24, 0x03 ; 3 2920: b1 f4 brne .+44 ; 0x294e 2922: 06 c0 rjmp .+12 ; 0x2930 Start_LIS_Action(LIS_ACTION_CONFIGURE, FUNC_BLOCKING); if (Get_LIS_LastActionStatus() == LIS_ACTION_CONFIGURE) Debug_Str (" - LIS3LV02DQ configured ...\n"); else { Debug_Str (" - FAILURE\n"); 2924: 89 e5 ldi r24, 0x59 ; 89 2926: 94 e0 ldi r25, 0x04 ; 4 2928: 0e 94 56 0b call 0x16ac ; 0x16ac 292c: 80 e0 ldi r24, 0x00 ; 0 292e: 08 95 ret _delay_ms (50); Debug_Str ("Enable LIS sensor:\n"); Start_LIS_Action(LIS_ACTION_ENABLE, FUNC_BLOCKING); if (Get_LIS_LastActionStatus() == LIS_ACTION_ENABLE) Debug_Str (" - LIS3LV02DQ enabled ...\n\n"); 2930: 88 e2 ldi r24, 0x28 ; 40 2932: 94 e0 ldi r25, 0x04 ; 4 2934: 0e 94 56 0b call 0x16ac ; 0x16ac 2938: 84 ef ldi r24, 0xF4 ; 244 293a: 91 e0 ldi r25, 0x01 ; 1 293c: 2b ed ldi r18, 0xDB ; 219 293e: 32 e0 ldi r19, 0x02 ; 2 2940: f9 01 movw r30, r18 2942: 31 97 sbiw r30, 0x01 ; 1 2944: f1 f7 brne .-4 ; 0x2942 { // wait 1/10 ms _delay_loop_2(((F_CPU) / 4e3) / 10); __ticks --; 2946: 01 97 sbiw r24, 0x01 ; 1 __ticks = 1; else if (__tmp > 65535) { // __ticks = requested delay in 1/10 ms __ticks = (uint16_t) (__ms * 10.0); while(__ticks) 2948: d9 f7 brne .-10 ; 0x2940 294a: 81 e0 ldi r24, 0x01 ; 1 294c: 08 95 ret else { Debug_Str (" - FAILURE\n"); 294e: 8b e1 ldi r24, 0x1B ; 27 2950: 94 e0 ldi r25, 0x04 ; 4 2952: 0e 94 56 0b call 0x16ac ; 0x16ac 2956: 80 e0 ldi r24, 0x00 ; 0 } _delay_ms (50); return 1; } 2958: 08 95 ret 0000295a : } } /*************************************************************************************************************/ inline void LIS_Handle_TWI_RIF () { 295a: 1f 93 push r17 LIS_Debug_Str ("RIF: "); // Debug output uint8_t data = LIS_TWI_PORT.MASTER.DATA; // Read byte from TWI data register 295c: 90 91 b7 04 lds r25, 0x04B7 LIS_Debug_u8 (data); // Debug output LIS_Debug_Str (" "); // Debug output switch (Lis3LStatus) // Switch "action" state machine status 2960: 10 91 3b 20 lds r17, 0x203B 2964: 15 30 cpi r17, 0x05 ; 5 2966: 09 f0 breq .+2 ; 0x296a 2968: 48 c0 rjmp .+144 ; 0x29fa { case LIS_ACTION_GETXYZ: // Read XYZ acceleration values if (LISByteCount < LIS_XYZ_BYTE_LEN) // One of six 16er complement bytes 296a: 80 91 3c 20 lds r24, 0x203C 296e: 85 30 cpi r24, 0x05 ; 5 2970: 80 f4 brcc .+32 ; 0x2992 { AccelByteData[LISByteCount] = data; // Write received data byte to buffer 2972: e0 91 3c 20 lds r30, 0x203C 2976: f0 e0 ldi r31, 0x00 ; 0 2978: e3 58 subi r30, 0x83 ; 131 297a: ff 4d sbci r31, 0xDF ; 223 297c: 90 83 st Z, r25 TWI_ACK(&LIS_TWI_PORT); // Issue ACK and continue reading 297e: 80 eb ldi r24, 0xB0 ; 176 2980: 94 e0 ldi r25, 0x04 ; 4 2982: 0e 94 f7 0f call 0x1fee ; 0x1fee LISByteCount++; // Increment ByteCount 2986: 80 91 3c 20 lds r24, 0x203C 298a: 8f 5f subi r24, 0xFF ; 255 298c: 80 93 3c 20 sts 0x203C, r24 2990: 34 c0 rjmp .+104 ; 0x29fa LIS_Debug_Str ("\n"); // Debug output } else { AccelByteData[LISByteCount] = data; // Write received data byte to buffer 2992: e0 91 3c 20 lds r30, 0x203C 2996: f0 e0 ldi r31, 0x00 ; 0 2998: e3 58 subi r30, 0x83 ; 131 299a: ff 4d sbci r31, 0xDF ; 223 299c: 90 83 st Z, r25 TWI_NACK_STOP(&LIS_TWI_PORT); // Issue NACK and send stop condition 299e: 80 eb ldi r24, 0xB0 ; 176 29a0: 94 e0 ldi r25, 0x04 ; 4 29a2: 0e 94 fb 0f call 0x1ff6 ; 0x1ff6 LastLis3LActionStatus = LIS_ACTION_GETXYZ; // Set last action result 29a6: 10 93 06 20 sts 0x2006, r17 Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 29aa: 10 92 3b 20 sts 0x203B, r1 LISByteCount = 0; // Reset ByteCount 29ae: 10 92 3c 20 sts 0x203C, r1 AccelRawData[0] = (AccelByteData[1] << 8) + AccelByteData[0]; // Calc X acceleration 29b2: 20 91 7e 20 lds r18, 0x207E 29b6: 30 91 7d 20 lds r19, 0x207D 29ba: 92 2f mov r25, r18 29bc: 80 e0 ldi r24, 0x00 ; 0 29be: 83 0f add r24, r19 29c0: 91 1d adc r25, r1 29c2: 80 93 77 20 sts 0x2077, r24 29c6: 90 93 78 20 sts 0x2078, r25 AccelRawData[1] = (AccelByteData[3] << 8) + AccelByteData[2]; // Calc Y acceleration 29ca: 20 91 80 20 lds r18, 0x2080 29ce: 30 91 7f 20 lds r19, 0x207F 29d2: 92 2f mov r25, r18 29d4: 80 e0 ldi r24, 0x00 ; 0 29d6: 83 0f add r24, r19 29d8: 91 1d adc r25, r1 29da: 80 93 79 20 sts 0x2079, r24 29de: 90 93 7a 20 sts 0x207A, r25 AccelRawData[2] = (AccelByteData[5] << 8) + AccelByteData[4]; // Calc Z acceleration 29e2: 20 91 82 20 lds r18, 0x2082 29e6: 30 91 81 20 lds r19, 0x2081 29ea: 92 2f mov r25, r18 29ec: 80 e0 ldi r24, 0x00 ; 0 29ee: 83 0f add r24, r19 29f0: 91 1d adc r25, r1 29f2: 80 93 7b 20 sts 0x207B, r24 29f6: 90 93 7c 20 sts 0x207C, r25 LIS_Debug_Str ("LIS_ACTION_GETXYZ OK\n"); // Debug output } break; } } 29fa: 1f 91 pop r17 29fc: 08 95 ret 000029fe : } /*************************************************************************************************************/ inline void LIS_Handle_TWI_WIF_NACK () { LIS_ERROR_Debug_Str ("LIS NACK\n"); // Debug output 29fe: 86 e2 ldi r24, 0x26 ; 38 2a00: 95 e0 ldi r25, 0x05 ; 5 2a02: 0e 94 56 0b call 0x16ac ; 0x16ac TWI_STOP(&LIS_TWI_PORT); // TWI stop condition 2a06: 80 eb ldi r24, 0xB0 ; 176 2a08: 94 e0 ldi r25, 0x04 ; 4 2a0a: 0e 94 ff 0f call 0x1ffe ; 0x1ffe LastLis3LActionStatus = LIS_ACTION_FAILURE; // Set last action result 2a0e: 81 e0 ldi r24, 0x01 ; 1 2a10: 80 93 06 20 sts 0x2006, r24 Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 2a14: 10 92 3b 20 sts 0x203B, r1 } 2a18: 08 95 ret 00002a1a : /*************************************************************************************************************/ inline void LIS_Handle_TWI_Unexpected () { LIS_Debug_Str ("LIS unexpected condition\n"); // Debug output Lis3LStatus = LIS_ACTION_FAILURE; // Reset LIS status state machine 2a1a: 81 e0 ldi r24, 0x01 ; 1 2a1c: 80 93 3b 20 sts 0x203B, r24 LISByteCount = 0; // Reset ByteCount 2a20: 10 92 3c 20 sts 0x203C, r1 TWI_STOP (&LIS_TWI_PORT); // Send TWI stop condition 2a24: 80 eb ldi r24, 0xB0 ; 176 2a26: 94 e0 ldi r25, 0x04 ; 4 2a28: 0e 94 ff 0f call 0x1ffe ; 0x1ffe } 2a2c: 08 95 ret 00002a2e : LIS_Handle_TWI_WIF_ACK(); } /*************************************************************************************************************/ inline void LIS_Handle_TWI_WIF_ACK () { 2a2e: 1f 93 push r17 LIS_Debug_Str ("ACK\n"); // Debug output switch (Lis3LStatus) // Switch "action" state machine status 2a30: 10 91 3b 20 lds r17, 0x203B 2a34: 13 30 cpi r17, 0x03 ; 3 2a36: 61 f0 breq .+24 ; 0x2a50 2a38: 14 30 cpi r17, 0x04 ; 4 2a3a: 20 f4 brcc .+8 ; 0x2a44 2a3c: 12 30 cpi r17, 0x02 ; 2 2a3e: 09 f0 breq .+2 ; 0x2a42 2a40: 4e c0 rjmp .+156 ; 0x2ade 2a42: 2a c0 rjmp .+84 ; 0x2a98 2a44: 14 30 cpi r17, 0x04 ; 4 2a46: a1 f0 breq .+40 ; 0x2a70 2a48: 15 30 cpi r17, 0x05 ; 5 2a4a: 09 f0 breq .+2 ; 0x2a4e 2a4c: 48 c0 rjmp .+144 ; 0x2ade 2a4e: 2d c0 rjmp .+90 ; 0x2aaa LISByteCount = 0; // Reset bytecount LIS_Debug_Str ("LIS_ACTION_CHECK_COMM SUCCESSFUL - TWI stopped\n"); // Debug output break; // ***************************************************************************** case LIS_ACTION_ENABLE: // Enable LIS_ADDRESS sensor if (LISByteCount == 0) // Send register address from which to read from 2a50: 80 91 3c 20 lds r24, 0x203C 2a54: 88 23 and r24, r24 2a56: 21 f4 brne .+8 ; 0x2a60 { TWI_WRITE(&LIS_TWI_PORT, LIS_REG_CTRL_REG1); // Write control register 1 address 2a58: 80 eb ldi r24, 0xB0 ; 176 2a5a: 94 e0 ldi r25, 0x04 ; 4 2a5c: 60 e2 ldi r22, 0x20 ; 32 2a5e: 2c c0 rjmp .+88 ; 0x2ab8 LISByteCount++; // Increase ByteCount } else if (LISByteCount == 1) // Write LIS_ADDRESS enable configuration bits 2a60: 80 91 3c 20 lds r24, 0x203C 2a64: 81 30 cpi r24, 0x01 ; 1 2a66: a1 f4 brne .+40 ; 0x2a90 { TWI_WRITE (&LIS_TWI_PORT, LIS_CONFIG1_BYTE); // Enable power 2a68: 80 eb ldi r24, 0xB0 ; 176 2a6a: 94 e0 ldi r25, 0x04 ; 4 2a6c: 67 ed ldi r22, 0xD7 ; 215 2a6e: 24 c0 rjmp .+72 ; 0x2ab8 LIS_Debug_Str ("LIS_ACTION_ENABLE SUCCESSFUL - TWI stopped\n"); // Debug output } break; // ***************************************************************************** case LIS_ACTION_CONFIGURE: // Configure LIS_ADDRESS sensor if (LISByteCount == 0) // Send register address from which to read from 2a70: 80 91 3c 20 lds r24, 0x203C 2a74: 88 23 and r24, r24 2a76: 21 f4 brne .+8 ; 0x2a80 { TWI_WRITE(&LIS_TWI_PORT, LIS_REG_CTRL_REG2); // Write control register 1 address 2a78: 80 eb ldi r24, 0xB0 ; 176 2a7a: 94 e0 ldi r25, 0x04 ; 4 2a7c: 61 e2 ldi r22, 0x21 ; 33 2a7e: 1c c0 rjmp .+56 ; 0x2ab8 LISByteCount++; // Increase ByteCount } else if (LISByteCount == 1) 2a80: 80 91 3c 20 lds r24, 0x203C 2a84: 81 30 cpi r24, 0x01 ; 1 2a86: 21 f4 brne .+8 ; 0x2a90 { TWI_WRITE (&LIS_TWI_PORT, LIS_CONFIG2_BYTE); // Enable 2g scale, data ready generation 2a88: 80 eb ldi r24, 0xB0 ; 176 2a8a: 94 e0 ldi r25, 0x04 ; 4 2a8c: 64 e0 ldi r22, 0x04 ; 4 2a8e: 14 c0 rjmp .+40 ; 0x2ab8 LISByteCount++; // Increase ByteCount } else if (LISByteCount == 2) 2a90: 80 91 3c 20 lds r24, 0x203C 2a94: 82 30 cpi r24, 0x02 ; 2 2a96: 19 f5 brne .+70 ; 0x2ade { TWI_STOP (&LIS_TWI_PORT); // TWI stop condition 2a98: 80 eb ldi r24, 0xB0 ; 176 2a9a: 94 e0 ldi r25, 0x04 ; 4 2a9c: 0e 94 ff 0f call 0x1ffe ; 0x1ffe LastLis3LActionStatus = LIS_ACTION_CONFIGURE; // Set last action result 2aa0: 10 93 06 20 sts 0x2006, r17 Lis3LStatus = LIS_ACTION_IDLE; // Reset LIS status state machine 2aa4: 10 92 3b 20 sts 0x203B, r1 2aa8: 18 c0 rjmp .+48 ; 0x2ada LIS_Debug_Str ("LIS_ACTION_CONFIGURE SUCCESSFUL - TWI stopped\n"); // Debug output } break; // ***************************************************************************** case LIS_ACTION_GETXYZ: // Get XYZ acceleration value if (LISByteCount == 0) // Send register address from which to read from 2aaa: 80 91 3c 20 lds r24, 0x203C 2aae: 88 23 and r24, r24 2ab0: 59 f4 brne .+22 ; 0x2ac8 { TWI_WRITE(&LIS_TWI_PORT, LIS_REG_OUTX_L | LIS_FLAG_AUTOINC); // Write register address of X low value and enable address auto increment 2ab2: 80 eb ldi r24, 0xB0 ; 176 2ab4: 94 e0 ldi r25, 0x04 ; 4 2ab6: 68 ea ldi r22, 0xA8 ; 168 2ab8: 0e 94 f4 0f call 0x1fe8 ; 0x1fe8 LISByteCount++; // Increase ByteCount 2abc: 80 91 3c 20 lds r24, 0x203C 2ac0: 8f 5f subi r24, 0xFF ; 255 2ac2: 80 93 3c 20 sts 0x203C, r24 2ac6: 0b c0 rjmp .+22 ; 0x2ade } else if (LISByteCount == 1) // Switch transmission to reading 2ac8: 80 91 3c 20 lds r24, 0x203C 2acc: 81 30 cpi r24, 0x01 ; 1 2ace: 39 f4 brne .+14 ; 0x2ade { //LIS_TWI_PORT.MASTER.CTRLC = TWI_MASTER_CMD_REPSTART_gc; // Send repeated start condition (not needed as bus is already owned) TWI_START (&LIS_TWI_PORT, LIS_ADDRESS + I2C_READ); // Send LIS_ADDRESS read address 2ad0: 80 eb ldi r24, 0xB0 ; 176 2ad2: 94 e0 ldi r25, 0x04 ; 4 2ad4: 6b e3 ldi r22, 0x3B ; 59 2ad6: 0e 94 f1 0f call 0x1fe2 ; 0x1fe2 LISByteCount = 0; // Reset ByteCount for reading 2ada: 10 92 3c 20 sts 0x203C, r1 } break; } } 2ade: 1f 91 pop r17 2ae0: 08 95 ret 00002ae2 : /*************************************************************************************************************/ inline void LIS_Handle_TWI_WIF () { LIS_Debug_Str ("WIF "); // Debug output if (LIS_TWI_PORT.MASTER.STATUS & TWI_MASTER_RXACK_bm) // Check if NACK received 2ae2: 80 91 b4 04 lds r24, 0x04B4 2ae6: 84 ff sbrs r24, 4 2ae8: 03 c0 rjmp .+6 ; 0x2af0 LIS_Handle_TWI_WIF_NACK (); 2aea: 0e 94 ff 14 call 0x29fe ; 0x29fe 2aee: 08 95 ret else // ACK received LIS_Handle_TWI_WIF_ACK(); 2af0: 0e 94 17 15 call 0x2a2e ; 0x2a2e 2af4: 08 95 ret 00002af6 <__vector_107>: Start_LIS_Action(LIS_ACTION_GETXYZ, FUNC_NON_BLOCKING); } /*****************************************************************************************************************************************/ ISR (LIS_TWI_INT) { 2af6: 1f 92 push r1 2af8: 0f 92 push r0 2afa: 0f b6 in r0, 0x3f ; 63 2afc: 0f 92 push r0 2afe: 08 b6 in r0, 0x38 ; 56 2b00: 0f 92 push r0 2b02: 18 be out 0x38, r1 ; 56 2b04: 09 b6 in r0, 0x39 ; 57 2b06: 0f 92 push r0 2b08: 19 be out 0x39, r1 ; 57 2b0a: 0b b6 in r0, 0x3b ; 59 2b0c: 0f 92 push r0 2b0e: 1b be out 0x3b, r1 ; 59 2b10: 11 24 eor r1, r1 2b12: 2f 93 push r18 2b14: 3f 93 push r19 2b16: 4f 93 push r20 2b18: 5f 93 push r21 2b1a: 6f 93 push r22 2b1c: 7f 93 push r23 2b1e: 8f 93 push r24 2b20: 9f 93 push r25 2b22: af 93 push r26 2b24: bf 93 push r27 2b26: ef 93 push r30 2b28: ff 93 push r31 LIS_Debug_Str ("LIS3int bytecount: "); // Debug output LIS_Debug_u8 (LISByteCount); // Debug output LIS_Debug_Str (" "); // Debug output uint8_t status = LIS_TWI_PORT.MASTER.STATUS; // Get status from TWI Master 2b2a: 90 91 b4 04 lds r25, 0x04B4 // Check bus state if (status & TWI_MASTER_ARBLOST_bm) // Check if ARB was lost 2b2e: 93 ff sbrs r25, 3 2b30: 03 c0 rjmp .+6 ; 0x2b38 <__vector_107+0x42> LIS_Handle_TWI_ARB_Lost(); // Recover from ARB lost 2b32: 0e 94 33 14 call 0x2866 ; 0x2866 2b36: 11 c0 rjmp .+34 ; 0x2b5a <__vector_107+0x64> else if (status & TWI_MASTER_BUSERR_bm) // Check if BUS error occured 2b38: 92 ff sbrs r25, 2 2b3a: 03 c0 rjmp .+6 ; 0x2b42 <__vector_107+0x4c> LIS_Handle_TWI_BUS_Error(); // Recover from BUS error 2b3c: 0e 94 25 14 call 0x284a ; 0x284a 2b40: 0c c0 rjmp .+24 ; 0x2b5a <__vector_107+0x64> else if (status & TWI_MASTER_WIF_bm) // Check if WIF (write interrupt flag) is set 2b42: 96 ff sbrs r25, 6 2b44: 03 c0 rjmp .+6 ; 0x2b4c <__vector_107+0x56> LIS_Handle_TWI_WIF(); // Handle TWI WIF 2b46: 0e 94 71 15 call 0x2ae2 ; 0x2ae2 2b4a: 07 c0 rjmp .+14 ; 0x2b5a <__vector_107+0x64> else if (status & TWI_MASTER_RIF_bm) // Check if RIF (read interrupt flag) is set 2b4c: 97 ff sbrs r25, 7 2b4e: 03 c0 rjmp .+6 ; 0x2b56 <__vector_107+0x60> LIS_Handle_TWI_RIF (); // Handle TWI RIF 2b50: 0e 94 ad 14 call 0x295a ; 0x295a 2b54: 02 c0 rjmp .+4 ; 0x2b5a <__vector_107+0x64> else // Unexpected condition LIS_Handle_TWI_Unexpected(); // Handle it 2b56: 0e 94 0d 15 call 0x2a1a ; 0x2a1a } 2b5a: ff 91 pop r31 2b5c: ef 91 pop r30 2b5e: bf 91 pop r27 2b60: af 91 pop r26 2b62: 9f 91 pop r25 2b64: 8f 91 pop r24 2b66: 7f 91 pop r23 2b68: 6f 91 pop r22 2b6a: 5f 91 pop r21 2b6c: 4f 91 pop r20 2b6e: 3f 91 pop r19 2b70: 2f 91 pop r18 2b72: 0f 90 pop r0 2b74: 0b be out 0x3b, r0 ; 59 2b76: 0f 90 pop r0 2b78: 09 be out 0x39, r0 ; 57 2b7a: 0f 90 pop r0 2b7c: 08 be out 0x38, r0 ; 56 2b7e: 0f 90 pop r0 2b80: 0f be out 0x3f, r0 ; 63 2b82: 0f 90 pop r0 2b84: 1f 90 pop r1 2b86: 18 95 reti 00002b88 : * * Input: x and y * Output: angle * 1024 with quadrant checking ****************************************************************************************************************************************/ int32_t atan2_i32_m1024(int32_t y, int32_t x) { 2b88: af 92 push r10 2b8a: bf 92 push r11 2b8c: cf 92 push r12 2b8e: df 92 push r13 2b90: ef 92 push r14 2b92: ff 92 push r15 2b94: 0f 93 push r16 2b96: 1f 93 push r17 2b98: 7b 01 movw r14, r22 2b9a: 8c 01 movw r16, r24 int32_t quotient = 0; int32_t angle_m512 = 0; if (x == 0) // Check if x equals 0 2b9c: 21 15 cp r18, r1 2b9e: 31 05 cpc r19, r1 2ba0: 41 05 cpc r20, r1 2ba2: 51 05 cpc r21, r1 2ba4: c9 f4 brne .+50 ; 0x2bd8 { if (y == 0) // Function normally not defined 2ba6: 61 15 cp r22, r1 2ba8: 71 05 cpc r23, r1 2baa: 81 05 cpc r24, r1 2bac: 91 05 cpc r25, r1 2bae: 29 f4 brne .+10 ; 0x2bba 2bb0: 20 e0 ldi r18, 0x00 ; 0 2bb2: 30 e0 ldi r19, 0x00 ; 0 2bb4: 40 e0 ldi r20, 0x00 ; 0 2bb6: 50 e0 ldi r21, 0x00 ; 0 2bb8: 1e c1 rjmp .+572 ; 0x2df6 return 0; else if (y > 0) // Check if y is positive then 2bba: 16 16 cp r1, r22 2bbc: 17 06 cpc r1, r23 2bbe: 18 06 cpc r1, r24 2bc0: 19 06 cpc r1, r25 2bc2: 2c f0 brlt .+10 ; 0x2bce 2bc4: 20 e0 ldi r18, 0x00 ; 0 2bc6: 3c e4 ldi r19, 0x4C ; 76 2bc8: 4f ef ldi r20, 0xFF ; 255 2bca: 5f ef ldi r21, 0xFF ; 255 2bcc: 10 c1 rjmp .+544 ; 0x2dee 2bce: 20 e0 ldi r18, 0x00 ; 0 2bd0: 34 eb ldi r19, 0xB4 ; 180 2bd2: 40 e0 ldi r20, 0x00 ; 0 2bd4: 50 e0 ldi r21, 0x00 ; 0 2bd6: 0b c1 rjmp .+534 ; 0x2dee angle_m512 = 90L * 512L; // Return PI / 2 else // Y is negative angle_m512 = -90L * 512L; // Return -PI / 2 } else if (x > 0) // X is positive 2bd8: 12 16 cp r1, r18 2bda: 13 06 cpc r1, r19 2bdc: 14 06 cpc r1, r20 2bde: 15 06 cpc r1, r21 2be0: 0c f0 brlt .+2 ; 0x2be4 2be2: 7c c0 rjmp .+248 ; 0x2cdc { if (y >= 0) // Y is positive 2be4: 97 fd sbrc r25, 7 2be6: 36 c0 rjmp .+108 ; 0x2c54 { if (x >= y) // Angle is between 0 - 45 degs 2be8: 26 17 cp r18, r22 2bea: 37 07 cpc r19, r23 2bec: 48 07 cpc r20, r24 2bee: 59 07 cpc r21, r25 2bf0: b4 f0 brlt .+44 ; 0x2c1e { quotient = (y * 512L / x); // Calculate quotient angle_m512 = (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient]); // Get (angle * 4) from lookup table 2bf2: 89 e0 ldi r24, 0x09 ; 9 2bf4: ee 0c add r14, r14 2bf6: ff 1c adc r15, r15 2bf8: 00 1f adc r16, r16 2bfa: 11 1f adc r17, r17 2bfc: 8a 95 dec r24 2bfe: d1 f7 brne .-12 ; 0x2bf4 2c00: c8 01 movw r24, r16 2c02: b7 01 movw r22, r14 2c04: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2c08: f9 01 movw r30, r18 2c0a: ee 0f add r30, r30 2c0c: ff 1f adc r31, r31 2c0e: e0 5d subi r30, 0xD0 ; 208 2c10: fa 4f sbci r31, 0xFA ; 250 2c12: 85 91 lpm r24, Z+ 2c14: 94 91 lpm r25, Z+ 2c16: 9c 01 movw r18, r24 2c18: 40 e0 ldi r20, 0x00 ; 0 2c1a: 50 e0 ldi r21, 0x00 ; 0 2c1c: e8 c0 rjmp .+464 ; 0x2dee } else // Angle is between 45 - 90 degs { quotient = (x * 512L / y); // Calculate quotient angle_m512 = (90L * 512L) - (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient]); // Get (angle * 4) from lookup table 2c1e: b9 e0 ldi r27, 0x09 ; 9 2c20: 22 0f add r18, r18 2c22: 33 1f adc r19, r19 2c24: 44 1f adc r20, r20 2c26: 55 1f adc r21, r21 2c28: ba 95 dec r27 2c2a: d1 f7 brne .-12 ; 0x2c20 2c2c: ca 01 movw r24, r20 2c2e: b9 01 movw r22, r18 2c30: a8 01 movw r20, r16 2c32: 97 01 movw r18, r14 2c34: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2c38: f9 01 movw r30, r18 2c3a: ee 0f add r30, r30 2c3c: ff 1f adc r31, r31 2c3e: e0 5d subi r30, 0xD0 ; 208 2c40: fa 4f sbci r31, 0xFA ; 250 2c42: 85 91 lpm r24, Z+ 2c44: 94 91 lpm r25, Z+ 2c46: a0 e0 ldi r26, 0x00 ; 0 2c48: b0 e0 ldi r27, 0x00 ; 0 2c4a: 20 e0 ldi r18, 0x00 ; 0 2c4c: 34 eb ldi r19, 0xB4 ; 180 2c4e: 40 e0 ldi r20, 0x00 ; 0 2c50: 50 e0 ldi r21, 0x00 ; 0 2c52: c9 c0 rjmp .+402 ; 0x2de6 } } else // Y is negative { y = -y; // Make y positive 2c54: aa 24 eor r10, r10 2c56: bb 24 eor r11, r11 2c58: 65 01 movw r12, r10 2c5a: a6 1a sub r10, r22 2c5c: b7 0a sbc r11, r23 2c5e: c8 0a sbc r12, r24 2c60: d9 0a sbc r13, r25 if (x >= y) // -Angle is between 0 - 45 degs 2c62: 2a 15 cp r18, r10 2c64: 3b 05 cpc r19, r11 2c66: 4c 05 cpc r20, r12 2c68: 5d 05 cpc r21, r13 2c6a: e4 f0 brlt .+56 ; 0x2ca4 { quotient = (y * 512L / x); // Calculate quotient angle_m512 = -(int32_t)pgm_read_word(&pgm_atanlut_m512[quotient]); // Get (angle * 4) from lookup table 2c6c: e9 e0 ldi r30, 0x09 ; 9 2c6e: aa 0c add r10, r10 2c70: bb 1c adc r11, r11 2c72: cc 1c adc r12, r12 2c74: dd 1c adc r13, r13 2c76: ea 95 dec r30 2c78: d1 f7 brne .-12 ; 0x2c6e 2c7a: c6 01 movw r24, r12 2c7c: b5 01 movw r22, r10 2c7e: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2c82: f9 01 movw r30, r18 2c84: ee 0f add r30, r30 2c86: ff 1f adc r31, r31 2c88: e0 5d subi r30, 0xD0 ; 208 2c8a: fa 4f sbci r31, 0xFA ; 250 2c8c: 85 91 lpm r24, Z+ 2c8e: 94 91 lpm r25, Z+ 2c90: a0 e0 ldi r26, 0x00 ; 0 2c92: b0 e0 ldi r27, 0x00 ; 0 2c94: 22 27 eor r18, r18 2c96: 33 27 eor r19, r19 2c98: a9 01 movw r20, r18 2c9a: 28 1b sub r18, r24 2c9c: 39 0b sbc r19, r25 2c9e: 4a 0b sbc r20, r26 2ca0: 5b 0b sbc r21, r27 2ca2: a5 c0 rjmp .+330 ; 0x2dee } else // -Angle is between 45 - 90 degs { quotient = (x * 512L / y); // Calculate quotient angle_m512 = -((90L * 512L) - (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient])); // Get (angle * 4) from lookup table 2ca4: 69 e0 ldi r22, 0x09 ; 9 2ca6: 22 0f add r18, r18 2ca8: 33 1f adc r19, r19 2caa: 44 1f adc r20, r20 2cac: 55 1f adc r21, r21 2cae: 6a 95 dec r22 2cb0: d1 f7 brne .-12 ; 0x2ca6 2cb2: ca 01 movw r24, r20 2cb4: b9 01 movw r22, r18 2cb6: a6 01 movw r20, r12 2cb8: 95 01 movw r18, r10 2cba: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2cbe: f9 01 movw r30, r18 2cc0: ee 0f add r30, r30 2cc2: ff 1f adc r31, r31 2cc4: e0 5d subi r30, 0xD0 ; 208 2cc6: fa 4f sbci r31, 0xFA ; 250 2cc8: 85 91 lpm r24, Z+ 2cca: 94 91 lpm r25, Z+ 2ccc: 9c 01 movw r18, r24 2cce: 40 e0 ldi r20, 0x00 ; 0 2cd0: 50 e0 ldi r21, 0x00 ; 0 2cd2: 20 50 subi r18, 0x00 ; 0 2cd4: 34 4b sbci r19, 0xB4 ; 180 2cd6: 40 40 sbci r20, 0x00 ; 0 2cd8: 50 40 sbci r21, 0x00 ; 0 2cda: 89 c0 rjmp .+274 ; 0x2dee } } } else // X is negative (resulting angle is bigger than 90 degs) { x = -x; // Make x positive 2cdc: aa 24 eor r10, r10 2cde: bb 24 eor r11, r11 2ce0: 65 01 movw r12, r10 2ce2: a2 1a sub r10, r18 2ce4: b3 0a sbc r11, r19 2ce6: c4 0a sbc r12, r20 2ce8: d5 0a sbc r13, r21 if (y >= 0) // Y is positive 2cea: 97 fd sbrc r25, 7 2cec: 3c c0 rjmp .+120 ; 0x2d66 { if (x >= y) // Angle is between 135 - 180 degs 2cee: a6 16 cp r10, r22 2cf0: b7 06 cpc r11, r23 2cf2: c8 06 cpc r12, r24 2cf4: d9 06 cpc r13, r25 2cf6: dc f0 brlt .+54 ; 0x2d2e { quotient = (y * 512L / x); // Calculate quotient angle_m512 = (180L * 512L) - (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient]); // Get (angle * 4) from lookup table 2cf8: 89 e0 ldi r24, 0x09 ; 9 2cfa: ee 0c add r14, r14 2cfc: ff 1c adc r15, r15 2cfe: 00 1f adc r16, r16 2d00: 11 1f adc r17, r17 2d02: 8a 95 dec r24 2d04: d1 f7 brne .-12 ; 0x2cfa 2d06: c8 01 movw r24, r16 2d08: b7 01 movw r22, r14 2d0a: a6 01 movw r20, r12 2d0c: 95 01 movw r18, r10 2d0e: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2d12: f9 01 movw r30, r18 2d14: ee 0f add r30, r30 2d16: ff 1f adc r31, r31 2d18: e0 5d subi r30, 0xD0 ; 208 2d1a: fa 4f sbci r31, 0xFA ; 250 2d1c: 85 91 lpm r24, Z+ 2d1e: 94 91 lpm r25, Z+ 2d20: a0 e0 ldi r26, 0x00 ; 0 2d22: b0 e0 ldi r27, 0x00 ; 0 2d24: 20 e0 ldi r18, 0x00 ; 0 2d26: 38 e6 ldi r19, 0x68 ; 104 2d28: 41 e0 ldi r20, 0x01 ; 1 2d2a: 50 e0 ldi r21, 0x00 ; 0 2d2c: 5c c0 rjmp .+184 ; 0x2de6 } else // Angle is between 90 - 135 degs { quotient = (x * 512L / y); // Calculate quotient angle_m512 = (90L * 512L) + (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient]); // Get (angle * 4) from lookup table 2d2e: b9 e0 ldi r27, 0x09 ; 9 2d30: aa 0c add r10, r10 2d32: bb 1c adc r11, r11 2d34: cc 1c adc r12, r12 2d36: dd 1c adc r13, r13 2d38: ba 95 dec r27 2d3a: d1 f7 brne .-12 ; 0x2d30 2d3c: c6 01 movw r24, r12 2d3e: b5 01 movw r22, r10 2d40: a8 01 movw r20, r16 2d42: 97 01 movw r18, r14 2d44: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2d48: f9 01 movw r30, r18 2d4a: ee 0f add r30, r30 2d4c: ff 1f adc r31, r31 2d4e: e0 5d subi r30, 0xD0 ; 208 2d50: fa 4f sbci r31, 0xFA ; 250 2d52: 85 91 lpm r24, Z+ 2d54: 94 91 lpm r25, Z+ 2d56: 9c 01 movw r18, r24 2d58: 40 e0 ldi r20, 0x00 ; 0 2d5a: 50 e0 ldi r21, 0x00 ; 0 2d5c: 20 50 subi r18, 0x00 ; 0 2d5e: 3c 44 sbci r19, 0x4C ; 76 2d60: 4f 4f sbci r20, 0xFF ; 255 2d62: 5f 4f sbci r21, 0xFF ; 255 2d64: 44 c0 rjmp .+136 ; 0x2dee } } else // Y is negative { y = -y; // Make y positive 2d66: 22 27 eor r18, r18 2d68: 33 27 eor r19, r19 2d6a: a9 01 movw r20, r18 2d6c: 26 1b sub r18, r22 2d6e: 37 0b sbc r19, r23 2d70: 48 0b sbc r20, r24 2d72: 59 0b sbc r21, r25 if (x >= y) // -Angle is between 135 - 180 degs 2d74: a2 16 cp r10, r18 2d76: b3 06 cpc r11, r19 2d78: c4 06 cpc r12, r20 2d7a: d5 06 cpc r13, r21 2d7c: e4 f0 brlt .+56 ; 0x2db6 { quotient = (y * 512L / x); // Calculate quotient angle_m512 = -((180L * 512L) - (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient])); // Get (angle * 4) from lookup table 2d7e: e9 e0 ldi r30, 0x09 ; 9 2d80: 22 0f add r18, r18 2d82: 33 1f adc r19, r19 2d84: 44 1f adc r20, r20 2d86: 55 1f adc r21, r21 2d88: ea 95 dec r30 2d8a: d1 f7 brne .-12 ; 0x2d80 2d8c: ca 01 movw r24, r20 2d8e: b9 01 movw r22, r18 2d90: a6 01 movw r20, r12 2d92: 95 01 movw r18, r10 2d94: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2d98: f9 01 movw r30, r18 2d9a: ee 0f add r30, r30 2d9c: ff 1f adc r31, r31 2d9e: e0 5d subi r30, 0xD0 ; 208 2da0: fa 4f sbci r31, 0xFA ; 250 2da2: 85 91 lpm r24, Z+ 2da4: 94 91 lpm r25, Z+ 2da6: 9c 01 movw r18, r24 2da8: 40 e0 ldi r20, 0x00 ; 0 2daa: 50 e0 ldi r21, 0x00 ; 0 2dac: 20 50 subi r18, 0x00 ; 0 2dae: 38 46 sbci r19, 0x68 ; 104 2db0: 41 40 sbci r20, 0x01 ; 1 2db2: 50 40 sbci r21, 0x00 ; 0 2db4: 1c c0 rjmp .+56 ; 0x2dee } else // -Angle is between 90 - 135 degs { quotient = (x * 512L / y); // Calculate quotient angle_m512 = -((90L * 512L) + (int32_t)pgm_read_word(&pgm_atanlut_m512[quotient])); // Get (angle * 4) from lookup table 2db6: 69 e0 ldi r22, 0x09 ; 9 2db8: aa 0c add r10, r10 2dba: bb 1c adc r11, r11 2dbc: cc 1c adc r12, r12 2dbe: dd 1c adc r13, r13 2dc0: 6a 95 dec r22 2dc2: d1 f7 brne .-12 ; 0x2db8 2dc4: c6 01 movw r24, r12 2dc6: b5 01 movw r22, r10 2dc8: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2dcc: f9 01 movw r30, r18 2dce: ee 0f add r30, r30 2dd0: ff 1f adc r31, r31 2dd2: e0 5d subi r30, 0xD0 ; 208 2dd4: fa 4f sbci r31, 0xFA ; 250 2dd6: 85 91 lpm r24, Z+ 2dd8: 94 91 lpm r25, Z+ 2dda: a0 e0 ldi r26, 0x00 ; 0 2ddc: b0 e0 ldi r27, 0x00 ; 0 2dde: 20 e0 ldi r18, 0x00 ; 0 2de0: 3c e4 ldi r19, 0x4C ; 76 2de2: 4f ef ldi r20, 0xFF ; 255 2de4: 5f ef ldi r21, 0xFF ; 255 2de6: 28 1b sub r18, r24 2de8: 39 0b sbc r19, r25 2dea: 4a 0b sbc r20, r26 2dec: 5b 0b sbc r21, r27 } } } return ((int32_t)angle_m512 * 2); 2dee: 22 0f add r18, r18 2df0: 33 1f adc r19, r19 2df2: 44 1f adc r20, r20 2df4: 55 1f adc r21, r21 } 2df6: b9 01 movw r22, r18 2df8: ca 01 movw r24, r20 2dfa: 1f 91 pop r17 2dfc: 0f 91 pop r16 2dfe: ff 90 pop r15 2e00: ef 90 pop r14 2e02: df 90 pop r13 2e04: cf 90 pop r12 2e06: bf 90 pop r11 2e08: af 90 pop r10 2e0a: 08 95 ret 00002e0c : /*****************************************************************************************************************************************/ // Input: Angle * 1024 // Output: sin (angle) * 1024 /*****************************************************************************************************************************************/ int32_t sin_i32_m1024(int32_t angle_m1024) { 2e0c: af 92 push r10 2e0e: bf 92 push r11 2e10: cf 92 push r12 2e12: df 92 push r13 2e14: ef 92 push r14 2e16: ff 92 push r15 2e18: 0f 93 push r16 2e1a: 1f 93 push r17 int32_t angle_sign = 1; // Angle sign; 1 = positive; -1 = negative int32_t quadrant_factor; // Quadrant int32_t sinus; // Sinus return value int32_t angle_m8 = angle_m1024 / 128L; 2e1c: 20 e8 ldi r18, 0x80 ; 128 2e1e: 30 e0 ldi r19, 0x00 ; 0 2e20: 40 e0 ldi r20, 0x00 ; 0 2e22: 50 e0 ldi r21, 0x00 ; 0 2e24: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2e28: c9 01 movw r24, r18 2e2a: da 01 movw r26, r20 2e2c: bc 01 movw r22, r24 2e2e: cd 01 movw r24, r26 if (angle_m8 < 0) // Check if angle is negative 2e30: 97 fd sbrc r25, 7 2e32: 06 c0 rjmp .+12 ; 0x2e40 2e34: 31 e0 ldi r19, 0x01 ; 1 2e36: a3 2e mov r10, r19 2e38: b1 2c mov r11, r1 2e3a: c1 2c mov r12, r1 2e3c: d1 2c mov r13, r1 2e3e: 0b c0 rjmp .+22 ; 0x2e56 { angle_sign = -1; // Set angle_sign to negative angle_m8 = -angle_m8; // Make angle positive 2e40: 90 95 com r25 2e42: 80 95 com r24 2e44: 70 95 com r23 2e46: 61 95 neg r22 2e48: 7f 4f sbci r23, 0xFF ; 255 2e4a: 8f 4f sbci r24, 0xFF ; 255 2e4c: 9f 4f sbci r25, 0xFF ; 255 2e4e: aa 24 eor r10, r10 2e50: aa 94 dec r10 2e52: ba 2c mov r11, r10 2e54: 65 01 movw r12, r10 } angle_m8 %= (8L * 360L); // bring angle into interval from 0 to (8 * 360) 2e56: 20 e4 ldi r18, 0x40 ; 64 2e58: 3b e0 ldi r19, 0x0B ; 11 2e5a: 40 e0 ldi r20, 0x00 ; 0 2e5c: 50 e0 ldi r21, 0x00 ; 0 2e5e: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2e62: ac 01 movw r20, r24 2e64: cb 01 movw r24, r22 2e66: da 01 movw r26, r20 2e68: 9c 01 movw r18, r24 2e6a: ad 01 movw r20, r26 if (angle_m8 <= (90 * 8)) // check if angle is in 1. quadrant 2e6c: 21 3d cpi r18, 0xD1 ; 209 2e6e: 02 e0 ldi r16, 0x02 ; 2 2e70: 30 07 cpc r19, r16 2e72: 00 e0 ldi r16, 0x00 ; 0 2e74: 40 07 cpc r20, r16 2e76: 00 e0 ldi r16, 0x00 ; 0 2e78: 50 07 cpc r21, r16 2e7a: bc f0 brlt .+46 ; 0x2eaa quadrant_factor = 1; else if ((angle_m8 > (90 * 8)) && (angle_m8 <= (180 * 8))) // check if angle is in 2. quadrant 2e7c: da 01 movw r26, r20 2e7e: c9 01 movw r24, r18 2e80: 81 5d subi r24, 0xD1 ; 209 2e82: 92 40 sbci r25, 0x02 ; 2 2e84: a0 40 sbci r26, 0x00 ; 0 2e86: b0 40 sbci r27, 0x00 ; 0 2e88: 80 5d subi r24, 0xD0 ; 208 2e8a: 92 40 sbci r25, 0x02 ; 2 2e8c: a0 40 sbci r26, 0x00 ; 0 2e8e: b0 40 sbci r27, 0x00 ; 0 2e90: 88 f4 brcc .+34 ; 0x2eb4 { angle_m8 = (180 * 8) - angle_m8; // Mirror angle at 90 deg (mult. 8 due to 0.125 deg resolution) 2e92: 80 ea ldi r24, 0xA0 ; 160 2e94: 95 e0 ldi r25, 0x05 ; 5 2e96: a0 e0 ldi r26, 0x00 ; 0 2e98: b0 e0 ldi r27, 0x00 ; 0 2e9a: 7c 01 movw r14, r24 2e9c: 8d 01 movw r16, r26 2e9e: e2 1a sub r14, r18 2ea0: f3 0a sbc r15, r19 2ea2: 04 0b sbc r16, r20 2ea4: 15 0b sbc r17, r21 2ea6: a8 01 movw r20, r16 2ea8: 97 01 movw r18, r14 2eaa: 61 e0 ldi r22, 0x01 ; 1 2eac: 70 e0 ldi r23, 0x00 ; 0 2eae: 80 e0 ldi r24, 0x00 ; 0 2eb0: 90 e0 ldi r25, 0x00 ; 0 2eb2: 20 c0 rjmp .+64 ; 0x2ef4 quadrant_factor = 1; // Set quadrant factor } else if ((angle_m8 > (180 * 8)) && (angle_m8 <= (270 * 8))) // Check if angle is in 3. quadrant 2eb4: da 01 movw r26, r20 2eb6: c9 01 movw r24, r18 2eb8: 81 5a subi r24, 0xA1 ; 161 2eba: 95 40 sbci r25, 0x05 ; 5 2ebc: a0 40 sbci r26, 0x00 ; 0 2ebe: b0 40 sbci r27, 0x00 ; 0 2ec0: 80 5d subi r24, 0xD0 ; 208 2ec2: 92 40 sbci r25, 0x02 ; 2 2ec4: a0 40 sbci r26, 0x00 ; 0 2ec6: b0 40 sbci r27, 0x00 ; 0 2ec8: 28 f4 brcc .+10 ; 0x2ed4 { angle_m8 = angle_m8 - (180 * 8); // Decrement angle by 180 degs and (mult. 8 due to 0.125 deg resolution) 2eca: 20 5a subi r18, 0xA0 ; 160 2ecc: 35 40 sbci r19, 0x05 ; 5 2ece: 40 40 sbci r20, 0x00 ; 0 2ed0: 50 40 sbci r21, 0x00 ; 0 2ed2: 0c c0 rjmp .+24 ; 0x2eec quadrant_factor = -1; // set quadrant factor negative } else // Check if angle is in 4. quadrant { angle_m8 = (360 * 8) - angle_m8; // Decrement angle by 360 degs and (mult. 8 due to 0.125 deg resolution) 2ed4: 80 e4 ldi r24, 0x40 ; 64 2ed6: 9b e0 ldi r25, 0x0B ; 11 2ed8: a0 e0 ldi r26, 0x00 ; 0 2eda: b0 e0 ldi r27, 0x00 ; 0 2edc: 7c 01 movw r14, r24 2ede: 8d 01 movw r16, r26 2ee0: e2 1a sub r14, r18 2ee2: f3 0a sbc r15, r19 2ee4: 04 0b sbc r16, r20 2ee6: 15 0b sbc r17, r21 2ee8: a8 01 movw r20, r16 2eea: 97 01 movw r18, r14 2eec: 6f ef ldi r22, 0xFF ; 255 2eee: 7f ef ldi r23, 0xFF ; 255 2ef0: 8f ef ldi r24, 0xFF ; 255 2ef2: 9f ef ldi r25, 0xFF ; 255 quadrant_factor = -1; // set quadrant factor negative } sinus = (int32_t)pgm_read_word(&pgm_sinlut_m1024[angle_m8]); // Read value from look up table 2ef4: f9 01 movw r30, r18 2ef6: ee 0f add r30, r30 2ef8: ff 1f adc r31, r31 2efa: ee 5c subi r30, 0xCE ; 206 2efc: f6 4f sbci r31, 0xF6 ; 246 2efe: e5 90 lpm r14, Z+ 2f00: f4 90 lpm r15, Z+ 2f02: a6 01 movw r20, r12 2f04: 95 01 movw r18, r10 2f06: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 2f0a: 00 e0 ldi r16, 0x00 ; 0 2f0c: 10 e0 ldi r17, 0x00 ; 0 2f0e: a8 01 movw r20, r16 2f10: 97 01 movw r18, r14 2f12: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> return (sinus * angle_sign * quadrant_factor); // calculate sinus value } 2f16: 1f 91 pop r17 2f18: 0f 91 pop r16 2f1a: ff 90 pop r15 2f1c: ef 90 pop r14 2f1e: df 90 pop r13 2f20: cf 90 pop r12 2f22: bf 90 pop r11 2f24: af 90 pop r10 2f26: 08 95 ret 00002f28 : /*****************************************************************************************************************************************/ int32_t cos_i32_m1024(int32_t angle_m1024) { 2f28: 9b 01 movw r18, r22 2f2a: ac 01 movw r20, r24 return ( sin_i32_m1024(90L * 1024L - angle_m1024) ); 2f2c: 60 e0 ldi r22, 0x00 ; 0 2f2e: 78 e6 ldi r23, 0x68 ; 104 2f30: 81 e0 ldi r24, 0x01 ; 1 2f32: 90 e0 ldi r25, 0x00 ; 0 2f34: 62 1b sub r22, r18 2f36: 73 0b sbc r23, r19 2f38: 84 0b sbc r24, r20 2f3a: 95 0b sbc r25, r21 2f3c: 0e 94 06 17 call 0x2e0c ; 0x2e0c } 2f40: 08 95 ret 00002f42 : /*****************************************************************************************************************************************/ // Input: Angle * 1024 // Output: tan (angle) * 1024 /*****************************************************************************************************************************************/ int32_t tan_i32_m1024 (int32_t angle_m1024) { 2f42: af 92 push r10 2f44: bf 92 push r11 2f46: cf 92 push r12 2f48: df 92 push r13 2f4a: ef 92 push r14 2f4c: ff 92 push r15 2f4e: 0f 93 push r16 2f50: 1f 93 push r17 int32_t angle_sign = 1; // Angle sign; 1 = positive; -1 = negative int32_t quadrant_factor; // Quadrant int32_t tangens; // Tangens return value int32_t angle_m8 = angle_m1024 / 128L; 2f52: 20 e8 ldi r18, 0x80 ; 128 2f54: 30 e0 ldi r19, 0x00 ; 0 2f56: 40 e0 ldi r20, 0x00 ; 0 2f58: 50 e0 ldi r21, 0x00 ; 0 2f5a: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2f5e: c9 01 movw r24, r18 2f60: da 01 movw r26, r20 2f62: bc 01 movw r22, r24 2f64: cd 01 movw r24, r26 if (angle_m8 < 0) // Check if angle is negative 2f66: 97 fd sbrc r25, 7 2f68: 06 c0 rjmp .+12 ; 0x2f76 2f6a: 31 e0 ldi r19, 0x01 ; 1 2f6c: a3 2e mov r10, r19 2f6e: b1 2c mov r11, r1 2f70: c1 2c mov r12, r1 2f72: d1 2c mov r13, r1 2f74: 0b c0 rjmp .+22 ; 0x2f8c { angle_sign = -1; // Set angle_sign to negative angle_m8 = -angle_m8; // Make angle positive 2f76: 90 95 com r25 2f78: 80 95 com r24 2f7a: 70 95 com r23 2f7c: 61 95 neg r22 2f7e: 7f 4f sbci r23, 0xFF ; 255 2f80: 8f 4f sbci r24, 0xFF ; 255 2f82: 9f 4f sbci r25, 0xFF ; 255 2f84: aa 24 eor r10, r10 2f86: aa 94 dec r10 2f88: ba 2c mov r11, r10 2f8a: 65 01 movw r12, r10 } angle_m8 %= (8L * 360L); // bring angle into interval from 0 to (8 * 360) 2f8c: 20 e4 ldi r18, 0x40 ; 64 2f8e: 3b e0 ldi r19, 0x0B ; 11 2f90: 40 e0 ldi r20, 0x00 ; 0 2f92: 50 e0 ldi r21, 0x00 ; 0 2f94: 0e 94 91 18 call 0x3122 ; 0x3122 <__divmodsi4> 2f98: 9b 01 movw r18, r22 2f9a: ac 01 movw r20, r24 2f9c: 79 01 movw r14, r18 2f9e: 8a 01 movw r16, r20 // 1. Quadrant ------------------------------------------------- if (angle_m8 <= (90 * 8)) // check if angle is in 1. quadrant 2fa0: 61 ed ldi r22, 0xD1 ; 209 2fa2: e6 16 cp r14, r22 2fa4: 62 e0 ldi r22, 0x02 ; 2 2fa6: f6 06 cpc r15, r22 2fa8: 60 e0 ldi r22, 0x00 ; 0 2faa: 06 07 cpc r16, r22 2fac: 60 e0 ldi r22, 0x00 ; 0 2fae: 16 07 cpc r17, r22 2fb0: 5c f4 brge .+22 ; 0x2fc8 { if (angle_m8 > (85 * 8)) // check if angle is greater than 85 degs 2fb2: 89 ea ldi r24, 0xA9 ; 169 2fb4: e8 16 cp r14, r24 2fb6: 82 e0 ldi r24, 0x02 ; 2 2fb8: f8 06 cpc r15, r24 2fba: 80 e0 ldi r24, 0x00 ; 0 2fbc: 08 07 cpc r16, r24 2fbe: 80 e0 ldi r24, 0x00 ; 0 2fc0: 18 07 cpc r17, r24 2fc2: 0c f4 brge .+2 ; 0x2fc6 2fc4: 35 c0 rjmp .+106 ; 0x3030 2fc6: 25 c0 rjmp .+74 ; 0x3012 else quadrant_factor = 1; } // 2. Quadrant ------------------------------------------------- else if ((angle_m8 > (90 * 8)) && (angle_m8 <= (180 * 8))) // check if angle is in 2. quadrant 2fc8: da 01 movw r26, r20 2fca: c9 01 movw r24, r18 2fcc: 81 5d subi r24, 0xD1 ; 209 2fce: 92 40 sbci r25, 0x02 ; 2 2fd0: a0 40 sbci r26, 0x00 ; 0 2fd2: b0 40 sbci r27, 0x00 ; 0 2fd4: 80 5d subi r24, 0xD0 ; 208 2fd6: 92 40 sbci r25, 0x02 ; 2 2fd8: a0 40 sbci r26, 0x00 ; 0 2fda: b0 40 sbci r27, 0x00 ; 0 2fdc: 50 f4 brcc .+20 ; 0x2ff2 { if (angle_m8 < (95 * 8)) 2fde: 28 5f subi r18, 0xF8 ; 248 2fe0: 32 40 sbci r19, 0x02 ; 2 2fe2: 40 40 sbci r20, 0x00 ; 0 2fe4: 50 40 sbci r21, 0x00 ; 0 2fe6: 74 f1 brlt .+92 ; 0x3044 return (-15000L * angle_sign); angle_m8 = (180 * 8) - angle_m8; // Mirror angle at 90 deg (mult. 8 due to 0.125 deg resolution) 2fe8: 80 ea ldi r24, 0xA0 ; 160 2fea: 95 e0 ldi r25, 0x05 ; 5 2fec: a0 e0 ldi r26, 0x00 ; 0 2fee: b0 e0 ldi r27, 0x00 ; 0 2ff0: 34 c0 rjmp .+104 ; 0x305a quadrant_factor = -1; // Set quadrant factor } // 3. Quadrant ------------------------------------------------- else if ((angle_m8 > (180 * 8)) && (angle_m8 <= (270 * 8))) // Check if angle is in 3. quadrant 2ff2: da 01 movw r26, r20 2ff4: c9 01 movw r24, r18 2ff6: 81 5a subi r24, 0xA1 ; 161 2ff8: 95 40 sbci r25, 0x05 ; 5 2ffa: a0 40 sbci r26, 0x00 ; 0 2ffc: b0 40 sbci r27, 0x00 ; 0 2ffe: 80 5d subi r24, 0xD0 ; 208 3000: 92 40 sbci r25, 0x02 ; 2 3002: a0 40 sbci r26, 0x00 ; 0 3004: b0 40 sbci r27, 0x00 ; 0 3006: c8 f4 brcc .+50 ; 0x303a { if (angle_m8 > (265 * 8)) 3008: 29 54 subi r18, 0x49 ; 73 300a: 38 40 sbci r19, 0x08 ; 8 300c: 40 40 sbci r20, 0x00 ; 0 300e: 50 40 sbci r21, 0x00 ; 0 3010: 3c f0 brlt .+14 ; 0x3020 return (15000L * angle_sign); 3012: c6 01 movw r24, r12 3014: b5 01 movw r22, r10 3016: 28 e9 ldi r18, 0x98 ; 152 3018: 3a e3 ldi r19, 0x3A ; 58 301a: 40 e0 ldi r20, 0x00 ; 0 301c: 50 e0 ldi r21, 0x00 ; 0 301e: 38 c0 rjmp .+112 ; 0x3090 angle_m8 = angle_m8 - (180 * 8); // Decrement angle by 180 degs and (mult. 8 due to 0.125 deg resolution) 3020: 20 e6 ldi r18, 0x60 ; 96 3022: 3a ef ldi r19, 0xFA ; 250 3024: 4f ef ldi r20, 0xFF ; 255 3026: 5f ef ldi r21, 0xFF ; 255 3028: e2 0e add r14, r18 302a: f3 1e adc r15, r19 302c: 04 1f adc r16, r20 302e: 15 1f adc r17, r21 3030: 61 e0 ldi r22, 0x01 ; 1 3032: 70 e0 ldi r23, 0x00 ; 0 3034: 80 e0 ldi r24, 0x00 ; 0 3036: 90 e0 ldi r25, 0x00 ; 0 3038: 1c c0 rjmp .+56 ; 0x3072 } // 4. Quadrant ------------------------------------------------- else // Check if angle is in 4. quadrant { if (angle_m8 < (275 * 8)) 303a: 28 59 subi r18, 0x98 ; 152 303c: 38 40 sbci r19, 0x08 ; 8 303e: 40 40 sbci r20, 0x00 ; 0 3040: 50 40 sbci r21, 0x00 ; 0 3042: 3c f4 brge .+14 ; 0x3052 return (-15000L * angle_sign); 3044: c6 01 movw r24, r12 3046: b5 01 movw r22, r10 3048: 28 e6 ldi r18, 0x68 ; 104 304a: 35 ec ldi r19, 0xC5 ; 197 304c: 4f ef ldi r20, 0xFF ; 255 304e: 5f ef ldi r21, 0xFF ; 255 3050: 1f c0 rjmp .+62 ; 0x3090 angle_m8 = (360 * 8) - angle_m8; // Decrement angle by 360 degs and (mult. 8 due to 0.125 deg resolution) 3052: 80 e4 ldi r24, 0x40 ; 64 3054: 9b e0 ldi r25, 0x0B ; 11 3056: a0 e0 ldi r26, 0x00 ; 0 3058: b0 e0 ldi r27, 0x00 ; 0 305a: 9c 01 movw r18, r24 305c: ad 01 movw r20, r26 305e: 2e 19 sub r18, r14 3060: 3f 09 sbc r19, r15 3062: 40 0b sbc r20, r16 3064: 51 0b sbc r21, r17 3066: 79 01 movw r14, r18 3068: 8a 01 movw r16, r20 306a: 6f ef ldi r22, 0xFF ; 255 306c: 7f ef ldi r23, 0xFF ; 255 306e: 8f ef ldi r24, 0xFF ; 255 3070: 9f ef ldi r25, 0xFF ; 255 quadrant_factor = -1; // set quadrant factor negative } tangens = (int32_t)pgm_read_word(&pgm_tanlut_m1024[angle_m8]); // Read value from look up table 3072: f7 01 movw r30, r14 3074: ee 0f add r30, r30 3076: ff 1f adc r31, r31 3078: ec 52 subi r30, 0x2C ; 44 307a: f1 4f sbci r31, 0xF1 ; 241 307c: e5 90 lpm r14, Z+ 307e: f4 90 lpm r15, Z+ return (tangens * angle_sign * quadrant_factor); // calculate tangens value 3080: a6 01 movw r20, r12 3082: 95 01 movw r18, r10 3084: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> 3088: 00 e0 ldi r16, 0x00 ; 0 308a: 10 e0 ldi r17, 0x00 ; 0 308c: a8 01 movw r20, r16 308e: 97 01 movw r18, r14 3090: 0e 94 53 18 call 0x30a6 ; 0x30a6 <__mulsi3> } 3094: 1f 91 pop r17 3096: 0f 91 pop r16 3098: ff 90 pop r15 309a: ef 90 pop r14 309c: df 90 pop r13 309e: cf 90 pop r12 30a0: bf 90 pop r11 30a2: af 90 pop r10 30a4: 08 95 ret 000030a6 <__mulsi3>: 30a6: 62 9f mul r22, r18 30a8: d0 01 movw r26, r0 30aa: 73 9f mul r23, r19 30ac: f0 01 movw r30, r0 30ae: 82 9f mul r24, r18 30b0: e0 0d add r30, r0 30b2: f1 1d adc r31, r1 30b4: 64 9f mul r22, r20 30b6: e0 0d add r30, r0 30b8: f1 1d adc r31, r1 30ba: 92 9f mul r25, r18 30bc: f0 0d add r31, r0 30be: 83 9f mul r24, r19 30c0: f0 0d add r31, r0 30c2: 74 9f mul r23, r20 30c4: f0 0d add r31, r0 30c6: 65 9f mul r22, r21 30c8: f0 0d add r31, r0 30ca: 99 27 eor r25, r25 30cc: 72 9f mul r23, r18 30ce: b0 0d add r27, r0 30d0: e1 1d adc r30, r1 30d2: f9 1f adc r31, r25 30d4: 63 9f mul r22, r19 30d6: b0 0d add r27, r0 30d8: e1 1d adc r30, r1 30da: f9 1f adc r31, r25 30dc: bd 01 movw r22, r26 30de: cf 01 movw r24, r30 30e0: 11 24 eor r1, r1 30e2: 08 95 ret 000030e4 <__udivmodqi4>: 30e4: 99 1b sub r25, r25 30e6: 79 e0 ldi r23, 0x09 ; 9 30e8: 04 c0 rjmp .+8 ; 0x30f2 <__udivmodqi4_ep> 000030ea <__udivmodqi4_loop>: 30ea: 99 1f adc r25, r25 30ec: 96 17 cp r25, r22 30ee: 08 f0 brcs .+2 ; 0x30f2 <__udivmodqi4_ep> 30f0: 96 1b sub r25, r22 000030f2 <__udivmodqi4_ep>: 30f2: 88 1f adc r24, r24 30f4: 7a 95 dec r23 30f6: c9 f7 brne .-14 ; 0x30ea <__udivmodqi4_loop> 30f8: 80 95 com r24 30fa: 08 95 ret 000030fc <__divmodhi4>: 30fc: 97 fb bst r25, 7 30fe: 09 2e mov r0, r25 3100: 07 26 eor r0, r23 3102: 0a d0 rcall .+20 ; 0x3118 <__divmodhi4_neg1> 3104: 77 fd sbrc r23, 7 3106: 04 d0 rcall .+8 ; 0x3110 <__divmodhi4_neg2> 3108: 27 d0 rcall .+78 ; 0x3158 <__udivmodhi4> 310a: 06 d0 rcall .+12 ; 0x3118 <__divmodhi4_neg1> 310c: 00 20 and r0, r0 310e: 1a f4 brpl .+6 ; 0x3116 <__divmodhi4_exit> 00003110 <__divmodhi4_neg2>: 3110: 70 95 com r23 3112: 61 95 neg r22 3114: 7f 4f sbci r23, 0xFF ; 255 00003116 <__divmodhi4_exit>: 3116: 08 95 ret 00003118 <__divmodhi4_neg1>: 3118: f6 f7 brtc .-4 ; 0x3116 <__divmodhi4_exit> 311a: 90 95 com r25 311c: 81 95 neg r24 311e: 9f 4f sbci r25, 0xFF ; 255 3120: 08 95 ret 00003122 <__divmodsi4>: 3122: 97 fb bst r25, 7 3124: 09 2e mov r0, r25 3126: 05 26 eor r0, r21 3128: 0e d0 rcall .+28 ; 0x3146 <__divmodsi4_neg1> 312a: 57 fd sbrc r21, 7 312c: 04 d0 rcall .+8 ; 0x3136 <__divmodsi4_neg2> 312e: 28 d0 rcall .+80 ; 0x3180 <__udivmodsi4> 3130: 0a d0 rcall .+20 ; 0x3146 <__divmodsi4_neg1> 3132: 00 1c adc r0, r0 3134: 38 f4 brcc .+14 ; 0x3144 <__divmodsi4_exit> 00003136 <__divmodsi4_neg2>: 3136: 50 95 com r21 3138: 40 95 com r20 313a: 30 95 com r19 313c: 21 95 neg r18 313e: 3f 4f sbci r19, 0xFF ; 255 3140: 4f 4f sbci r20, 0xFF ; 255 3142: 5f 4f sbci r21, 0xFF ; 255 00003144 <__divmodsi4_exit>: 3144: 08 95 ret 00003146 <__divmodsi4_neg1>: 3146: f6 f7 brtc .-4 ; 0x3144 <__divmodsi4_exit> 3148: 90 95 com r25 314a: 80 95 com r24 314c: 70 95 com r23 314e: 61 95 neg r22 3150: 7f 4f sbci r23, 0xFF ; 255 3152: 8f 4f sbci r24, 0xFF ; 255 3154: 9f 4f sbci r25, 0xFF ; 255 3156: 08 95 ret 00003158 <__udivmodhi4>: 3158: aa 1b sub r26, r26 315a: bb 1b sub r27, r27 315c: 51 e1 ldi r21, 0x11 ; 17 315e: 07 c0 rjmp .+14 ; 0x316e <__udivmodhi4_ep> 00003160 <__udivmodhi4_loop>: 3160: aa 1f adc r26, r26 3162: bb 1f adc r27, r27 3164: a6 17 cp r26, r22 3166: b7 07 cpc r27, r23 3168: 10 f0 brcs .+4 ; 0x316e <__udivmodhi4_ep> 316a: a6 1b sub r26, r22 316c: b7 0b sbc r27, r23 0000316e <__udivmodhi4_ep>: 316e: 88 1f adc r24, r24 3170: 99 1f adc r25, r25 3172: 5a 95 dec r21 3174: a9 f7 brne .-22 ; 0x3160 <__udivmodhi4_loop> 3176: 80 95 com r24 3178: 90 95 com r25 317a: bc 01 movw r22, r24 317c: cd 01 movw r24, r26 317e: 08 95 ret 00003180 <__udivmodsi4>: 3180: a1 e2 ldi r26, 0x21 ; 33 3182: 1a 2e mov r1, r26 3184: aa 1b sub r26, r26 3186: bb 1b sub r27, r27 3188: fd 01 movw r30, r26 318a: 0d c0 rjmp .+26 ; 0x31a6 <__udivmodsi4_ep> 0000318c <__udivmodsi4_loop>: 318c: aa 1f adc r26, r26 318e: bb 1f adc r27, r27 3190: ee 1f adc r30, r30 3192: ff 1f adc r31, r31 3194: a2 17 cp r26, r18 3196: b3 07 cpc r27, r19 3198: e4 07 cpc r30, r20 319a: f5 07 cpc r31, r21 319c: 20 f0 brcs .+8 ; 0x31a6 <__udivmodsi4_ep> 319e: a2 1b sub r26, r18 31a0: b3 0b sbc r27, r19 31a2: e4 0b sbc r30, r20 31a4: f5 0b sbc r31, r21 000031a6 <__udivmodsi4_ep>: 31a6: 66 1f adc r22, r22 31a8: 77 1f adc r23, r23 31aa: 88 1f adc r24, r24 31ac: 99 1f adc r25, r25 31ae: 1a 94 dec r1 31b0: 69 f7 brne .-38 ; 0x318c <__udivmodsi4_loop> 31b2: 60 95 com r22 31b4: 70 95 com r23 31b6: 80 95 com r24 31b8: 90 95 com r25 31ba: 9b 01 movw r18, r22 31bc: ac 01 movw r20, r24 31be: bd 01 movw r22, r26 31c0: cf 01 movw r24, r30 31c2: 08 95 ret 000031c4 <_exit>: 31c4: f8 94 cli 000031c6 <__stop_program>: 31c6: ff cf rjmp .-2 ; 0x31c6 <__stop_program>