Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1564 | LPDunwell | 1 | 1 .file "timer2.c" |
| 2 | 2 __SREG__ = 0x3f |
||
| 3 | 3 __SP_H__ = 0x3e |
||
| 4 | 4 __SP_L__ = 0x3d |
||
| 5 | 5 __CCP__ = 0x34 |
||
| 6 | 6 __tmp_reg__ = 0 |
||
| 7 | 7 __zero_reg__ = 1 |
||
| 8 | 8 .text |
||
| 9 | 9 .global TIMER2_Init |
||
| 10 | 11 TIMER2_Init: |
||
| 11 | 12 /* prologue: function */ |
||
| 12 | 13 /* frame size = 0 */ |
||
| 13 | 14 0000 9FB7 in r25,__SREG__ |
||
| 14 | 15 /* #APP */ |
||
| 15 | 16 ; 78 "timer2.c" 1 |
||
| 16 | 17 0002 F894 cli |
||
| 17 | 18 ; 0 "" 2 |
||
| 18 | 19 /* #NOAPP */ |
||
| 19 | 20 0004 5798 cbi 42-32,7 |
||
| 20 | 21 0006 5F98 cbi 43-32,7 |
||
| 21 | 22 0008 3E9A sbi 39-32,6 |
||
| 22 | 23 000a 469A sbi 40-32,6 |
||
| 23 | 24 000c A0EB ldi r26,lo8(176) |
||
| 24 | 25 000e B0E0 ldi r27,hi8(176) |
||
| 25 | 26 0010 8C91 ld r24,X |
||
| 26 | 27 0012 8F70 andi r24,lo8(15) |
||
| 27 | 28 0014 8C93 st X,r24 |
||
| 28 | 29 0016 8C91 ld r24,X |
||
| 29 | 30 0018 8360 ori r24,lo8(3) |
||
| 30 | 31 001a 8C93 st X,r24 |
||
| 31 | 32 001c E1EB ldi r30,lo8(177) |
||
| 32 | 33 001e F0E0 ldi r31,hi8(177) |
||
| 33 | 34 0020 8081 ld r24,Z |
||
| 34 | 35 0022 8B73 andi r24,lo8(59) |
||
| 35 | 36 0024 8083 st Z,r24 |
||
| 36 | 37 0026 8081 ld r24,Z |
||
| 37 | 38 0028 8B60 ori r24,lo8(11) |
||
| 38 | 39 002a 8083 st Z,r24 |
||
| 39 | 40 002c 1092 B200 sts 178,__zero_reg__ |
||
| 40 | 41 0030 8FEF ldi r24,lo8(-1) |
||
| 41 | 42 0032 8093 B300 sts 179,r24 |
||
| 42 | 43 0036 8C91 ld r24,X |
||
| 43 | 44 0038 8068 ori r24,lo8(-128) |
||
| 44 | 45 003a 8C93 st X,r24 |
||
| 45 | 46 003c E0E7 ldi r30,lo8(112) |
||
| 46 | 47 003e F0E0 ldi r31,hi8(112) |
||
| 47 | 48 0040 8081 ld r24,Z |
||
| 48 | 49 0042 8A7F andi r24,lo8(-6) |
||
| 49 | 50 0044 8083 st Z,r24 |
||
| 50 | 51 0046 8081 ld r24,Z |
||
| 51 | 52 0048 8260 ori r24,lo8(2) |
||
| 52 | 53 004a 8083 st Z,r24 |
||
| 53 | 54 004c 9FBF out __SREG__,r25 |
||
| 54 | 55 /* epilogue start */ |
||
| 55 | 56 004e 0895 ret |
||
| 56 | 58 .global Servo_On |
||
| 57 | 60 Servo_On: |
||
| 58 | 61 /* prologue: function */ |
||
| 59 | 62 /* frame size = 0 */ |
||
| 60 | 63 0050 81E0 ldi r24,lo8(1) |
||
| 61 | 64 0052 8093 0000 sts ServoActive,r24 |
||
| 62 | 65 0056 579A sbi 42-32,7 |
||
| 63 | 66 /* epilogue start */ |
||
| 64 | 67 0058 0895 ret |
||
| 65 | 69 .global Servo_Off |
||
| 66 | 71 Servo_Off: |
||
| 67 | 72 /* prologue: function */ |
||
| 68 | 73 /* frame size = 0 */ |
||
| 69 | 74 005a 1092 0000 sts ServoActive,__zero_reg__ |
||
| 70 | 75 005e 5798 cbi 42-32,7 |
||
| 71 | 76 0060 469A sbi 40-32,6 |
||
| 72 | 77 /* epilogue start */ |
||
| 73 | 78 0062 0895 ret |
||
| 74 | 80 .global __vector_9 |
||
| 75 | 82 __vector_9: |
||
| 76 | 83 0064 1F92 push __zero_reg__ |
||
| 77 | 84 0066 0F92 push r0 |
||
| 78 | 85 0068 0FB6 in r0,__SREG__ |
||
| 79 | 86 006a 0F92 push r0 |
||
| 80 | 87 006c 1124 clr __zero_reg__ |
||
| 81 | 88 006e CF92 push r12 |
||
| 82 | 89 0070 DF92 push r13 |
||
| 83 | 90 0072 EF92 push r14 |
||
| 84 | 91 0074 FF92 push r15 |
||
| 85 | 92 0076 0F93 push r16 |
||
| 86 | 93 0078 1F93 push r17 |
||
| 87 | 94 007a 2F93 push r18 |
||
| 88 | 95 007c 3F93 push r19 |
||
| 89 | 96 007e 4F93 push r20 |
||
| 90 | 97 0080 5F93 push r21 |
||
| 91 | 98 0082 6F93 push r22 |
||
| 92 | 99 0084 7F93 push r23 |
||
| 93 | 100 0086 8F93 push r24 |
||
| 94 | 101 0088 9F93 push r25 |
||
| 95 | 102 008a AF93 push r26 |
||
| 96 | 103 008c BF93 push r27 |
||
| 97 | 104 008e EF93 push r30 |
||
| 98 | 105 0090 FF93 push r31 |
||
| 99 | 106 /* prologue: Signal */ |
||
| 100 | 107 /* frame size = 0 */ |
||
| 101 | 108 0092 8091 0000 lds r24,BoardRelease |
||
| 102 | 109 0096 8431 cpi r24,lo8(20) |
||
| 103 | 110 0098 00F4 brsh .L8 |
||
| 104 | 111 009a 8091 0000 lds r24,PulseOutput.1543 |
||
| 105 | 112 009e 8823 tst r24 |
||
| 106 | 113 00a0 01F4 brne .+2 |
||
| 107 | 114 00a2 00C0 rjmp .L51 |
||
| 108 | 115 .L59: |
||
| 109 | 116 00a4 4091 0000 lds r20,RemainingPulse.1544 |
||
| 110 | 117 00a8 5091 0000 lds r21,(RemainingPulse.1544)+1 |
||
| 111 | 118 00ac 81E0 ldi r24,hi8(383) |
||
| 112 | 119 00ae 4F37 cpi r20,lo8(383) |
||
| 113 | 120 00b0 5807 cpc r21,r24 |
||
| 114 | 121 00b2 00F0 brlo .+2 |
||
| 115 | 122 00b4 00C0 rjmp .L61 |
||
| 116 | 123 .L43: |
||
| 117 | 124 00b6 4F3F cpi r20,255 |
||
| 118 | 125 00b8 5105 cpc r21,__zero_reg__ |
||
| 119 | 126 00ba 01F0 breq .+4 |
||
| 120 | 127 00bc 00F0 brlo .+2 |
||
| 121 | 128 00be 00C0 rjmp .L62 |
||
| 122 | 129 00c0 4093 B300 sts 179,r20 |
||
| 123 | 130 00c4 1092 0000 sts (RemainingPulse.1544)+1,__zero_reg__ |
||
| 124 | 131 00c8 1092 0000 sts RemainingPulse.1544,__zero_reg__ |
||
| 125 | 132 00cc 1092 0000 sts PulseOutput.1543,__zero_reg__ |
||
| 126 | 133 .L47: |
||
| 127 | 134 /* epilogue start */ |
||
| 128 | 135 00d0 FF91 pop r31 |
||
| 129 | 136 00d2 EF91 pop r30 |
||
| 130 | 137 00d4 BF91 pop r27 |
||
| 131 | 138 00d6 AF91 pop r26 |
||
| 132 | 139 00d8 9F91 pop r25 |
||
| 133 | 140 00da 8F91 pop r24 |
||
| 134 | 141 00dc 7F91 pop r23 |
||
| 135 | 142 00de 6F91 pop r22 |
||
| 136 | 143 00e0 5F91 pop r21 |
||
| 137 | 144 00e2 4F91 pop r20 |
||
| 138 | 145 00e4 3F91 pop r19 |
||
| 139 | 146 00e6 2F91 pop r18 |
||
| 140 | 147 00e8 1F91 pop r17 |
||
| 141 | 148 00ea 0F91 pop r16 |
||
| 142 | 149 00ec FF90 pop r15 |
||
| 143 | 150 00ee EF90 pop r14 |
||
| 144 | 151 00f0 DF90 pop r13 |
||
| 145 | 152 00f2 CF90 pop r12 |
||
| 146 | 153 00f4 0F90 pop r0 |
||
| 147 | 154 00f6 0FBE out __SREG__,r0 |
||
| 148 | 155 00f8 0F90 pop r0 |
||
| 149 | 156 00fa 1F90 pop __zero_reg__ |
||
| 150 | 157 00fc 1895 reti |
||
| 151 | 158 .L8: |
||
| 152 | 159 00fe 8091 0000 lds r24,PulseOutput.1543 |
||
| 153 | 160 0102 8823 tst r24 |
||
| 154 | 161 0104 01F4 brne .L59 |
||
| 155 | 162 0106 8091 B000 lds r24,176 |
||
| 156 | 163 010a 86FF sbrs r24,6 |
||
| 157 | 164 010c 00C0 rjmp .L20 |
||
| 158 | 165 010e 8091 B000 lds r24,176 |
||
| 159 | 166 0112 8F7B andi r24,lo8(-65) |
||
| 160 | 167 0114 8093 B000 sts 176,r24 |
||
| 161 | 168 0118 E091 0000 lds r30,ServoIndex.1546 |
||
| 162 | 169 011c EE23 tst r30 |
||
| 163 | 170 011e 01F0 breq .+2 |
||
| 164 | 171 0120 00C0 rjmp .L21 |
||
| 165 | 172 0122 8091 0000 lds r24,ParamSet+47 |
||
| 166 | 173 0126 90E0 ldi r25,lo8(0) |
||
| 167 | 174 0128 2DED ldi r18,lo8(1757) |
||
| 168 | 175 012a 36E0 ldi r19,hi8(1757) |
||
| 169 | 176 012c A901 movw r20,r18 |
||
| 170 | 177 012e 849F mul r24,r20 |
||
| 171 | 178 0130 9001 movw r18,r0 |
||
| 172 | 179 0132 859F mul r24,r21 |
||
| 173 | 180 0134 300D add r19,r0 |
||
| 174 | 181 0136 949F mul r25,r20 |
||
| 175 | 182 0138 300D add r19,r0 |
||
| 176 | 183 013a 1124 clr r1 |
||
| 177 | 184 013c 8091 0000 lds r24,ServoFrameTime.1545 |
||
| 178 | 185 0140 9091 0000 lds r25,(ServoFrameTime.1545)+1 |
||
| 179 | 186 0144 281B sub r18,r24 |
||
| 180 | 187 0146 390B sbc r19,r25 |
||
| 181 | 188 0148 3093 0000 sts (RemainingPulse.1544)+1,r19 |
||
| 182 | 189 014c 2093 0000 sts RemainingPulse.1544,r18 |
||
| 183 | 190 0150 1092 0000 sts (ServoFrameTime.1545)+1,__zero_reg__ |
||
| 184 | 191 0154 1092 0000 sts ServoFrameTime.1545,__zero_reg__ |
||
| 185 | 192 0158 469A sbi 40-32,6 |
||
| 186 | 193 015a A901 movw r20,r18 |
||
| 187 | 194 .L22: |
||
| 188 | 195 015c 81E0 ldi r24,lo8(1) |
||
| 189 | 196 015e 8093 0000 sts PulseOutput.1543,r24 |
||
| 190 | 197 0162 81E0 ldi r24,hi8(383) |
||
| 191 | 198 0164 4F37 cpi r20,lo8(383) |
||
| 192 | 199 0166 5807 cpc r21,r24 |
||
| 193 | 200 0168 00F4 brsh .+2 |
||
| 194 | 201 016a 00C0 rjmp .L43 |
||
| 195 | 202 .L61: |
||
| 196 | 203 016c 8FEF ldi r24,lo8(-1) |
||
| 197 | 204 016e 8093 B300 sts 179,r24 |
||
| 198 | 205 0172 4F5F subi r20,lo8(-(-255)) |
||
| 199 | 206 0174 5040 sbci r21,hi8(-(-255)) |
||
| 200 | 207 0176 5093 0000 sts (RemainingPulse.1544)+1,r21 |
||
| 201 | 208 017a 4093 0000 sts RemainingPulse.1544,r20 |
||
| 202 | 209 017e 00C0 rjmp .L47 |
||
| 203 | 210 .L62: |
||
| 204 | 211 0180 9A01 movw r18,r20 |
||
| 205 | 212 0182 2F5F subi r18,lo8(-(-255)) |
||
| 206 | 213 0184 3040 sbci r19,hi8(-(-255)) |
||
| 207 | 214 0186 2F37 cpi r18,127 |
||
| 208 | 215 0188 3105 cpc r19,__zero_reg__ |
||
| 209 | 216 018a 00F0 brlo .+2 |
||
| 210 | 217 018c 00C0 rjmp .L46 |
||
| 211 | 218 018e 80E8 ldi r24,lo8(-128) |
||
| 212 | 219 0190 8093 B300 sts 179,r24 |
||
| 213 | 220 0194 4058 subi r20,lo8(-(-128)) |
||
| 214 | 221 0196 5040 sbci r21,hi8(-(-128)) |
||
| 215 | 222 0198 5093 0000 sts (RemainingPulse.1544)+1,r21 |
||
| 216 | 223 019c 4093 0000 sts RemainingPulse.1544,r20 |
||
| 217 | 224 01a0 00C0 rjmp .L47 |
||
| 218 | 225 .L51: |
||
| 219 | 226 01a2 8091 B000 lds r24,176 |
||
| 220 | 227 01a6 86FF sbrs r24,6 |
||
| 221 | 228 01a8 00C0 rjmp .L10 |
||
| 222 | 229 01aa 8091 B000 lds r24,176 |
||
| 223 | 230 01ae 8F7B andi r24,lo8(-65) |
||
| 224 | 231 01b0 8093 B000 sts 176,r24 |
||
| 225 | 232 01b4 8091 0000 lds r24,FCParam+21 |
||
| 226 | 233 01b8 482F mov r20,r24 |
||
| 227 | 234 01ba 50E0 ldi r21,lo8(0) |
||
| 228 | 235 01bc 440F lsl r20 |
||
| 229 | 236 01be 551F rol r21 |
||
| 230 | 237 01c0 440F lsl r20 |
||
| 231 | 238 01c2 551F rol r21 |
||
| 232 | 239 01c4 2091 0000 lds r18,ServoNickOffset.1547 |
||
| 233 | 240 01c8 3091 0000 lds r19,(ServoNickOffset.1547)+1 |
||
| 234 | 241 01cc C901 movw r24,r18 |
||
| 235 | 242 01ce 880F lsl r24 |
||
| 236 | 243 01d0 991F rol r25 |
||
| 237 | 244 01d2 820F add r24,r18 |
||
| 238 | 245 01d4 931F adc r25,r19 |
||
| 239 | 246 01d6 480F add r20,r24 |
||
| 240 | 247 01d8 591F adc r21,r25 |
||
| 241 | 248 01da 57FD sbrc r21,7 |
||
| 242 | 249 01dc 00C0 rjmp .L63 |
||
| 243 | 250 .L11: |
||
| 244 | 251 01de CA01 movw r24,r20 |
||
| 245 | 252 01e0 9595 asr r25 |
||
| 246 | 253 01e2 8795 ror r24 |
||
| 247 | 254 01e4 9595 asr r25 |
||
| 248 | 255 01e6 8795 ror r24 |
||
| 249 | 256 01e8 9093 0000 sts (ServoNickOffset.1547)+1,r25 |
||
| 250 | 257 01ec 8093 0000 sts ServoNickOffset.1547,r24 |
||
| 251 | 258 01f0 9093 0000 sts (ServoNickValue)+1,r25 |
||
| 252 | 259 01f4 8093 0000 sts ServoNickValue,r24 |
||
| 253 | 260 01f8 8091 0000 lds r24,ParamSet+87 |
||
| 254 | 261 01fc 80FF sbrs r24,0 |
||
| 255 | 262 01fe 00C0 rjmp .L12 |
||
| 256 | 263 0200 C090 0000 lds r12,ServoNickValue |
||
| 257 | 264 0204 D090 0000 lds r13,(ServoNickValue)+1 |
||
| 258 | 265 0208 E090 0000 lds r14,ParamSet+40 |
||
| 259 | 266 020c FF24 clr r15 |
||
| 260 | 267 020e 00E0 ldi r16,lo8(0) |
||
| 261 | 268 0210 10E0 ldi r17,hi8(0) |
||
| 262 | 269 0212 6091 0000 lds r22,IntegralGyroNick |
||
| 263 | 270 0216 7091 0000 lds r23,(IntegralGyroNick)+1 |
||
| 264 | 271 021a 8091 0000 lds r24,(IntegralGyroNick)+2 |
||
| 265 | 272 021e 9091 0000 lds r25,(IntegralGyroNick)+3 |
||
| 266 | 273 0222 20E8 ldi r18,lo8(128) |
||
| 267 | 274 0224 30E0 ldi r19,hi8(128) |
||
| 268 | 275 0226 40E0 ldi r20,hlo8(128) |
||
| 269 | 276 0228 50E0 ldi r21,hhi8(128) |
||
| 270 | 277 022a 0E94 0000 call __divmodsi4 |
||
| 271 | 278 022e C801 movw r24,r16 |
||
| 272 | 279 0230 B701 movw r22,r14 |
||
| 273 | 280 0232 0E94 0000 call __mulsi3 |
||
| 274 | 281 0236 660F lsl r22 |
||
| 275 | 282 0238 771F rol r23 |
||
| 276 | 283 023a 881F rol r24 |
||
| 277 | 284 023c 991F rol r25 |
||
| 278 | 285 023e 660F lsl r22 |
||
| 279 | 286 0240 771F rol r23 |
||
| 280 | 287 0242 881F rol r24 |
||
| 281 | 288 0244 991F rol r25 |
||
| 282 | 289 0246 20E0 ldi r18,lo8(256) |
||
| 283 | 290 0248 31E0 ldi r19,hi8(256) |
||
| 284 | 291 024a 40E0 ldi r20,hlo8(256) |
||
| 285 | 292 024c 50E0 ldi r21,hhi8(256) |
||
| 286 | 293 024e 0E94 0000 call __divmodsi4 |
||
| 287 | 294 0252 C20E add r12,r18 |
||
| 288 | 295 0254 D31E adc r13,r19 |
||
| 289 | 296 0256 D092 0000 sts (ServoNickValue)+1,r13 |
||
| 290 | 297 025a C092 0000 sts ServoNickValue,r12 |
||
| 291 | 298 .L13: |
||
| 292 | 299 025e 8091 0000 lds r24,ParamSet+41 |
||
| 293 | 300 0262 282F mov r18,r24 |
||
| 294 | 301 0264 30E0 ldi r19,lo8(0) |
||
| 295 | 302 0266 220F lsl r18 |
||
| 296 | 303 0268 331F rol r19 |
||
| 297 | 304 026a 220F lsl r18 |
||
| 298 | 305 026c 331F rol r19 |
||
| 299 | 306 026e 8091 0000 lds r24,ServoNickValue |
||
| 300 | 307 0272 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 301 | 308 0276 8217 cp r24,r18 |
||
| 302 | 309 0278 9307 cpc r25,r19 |
||
| 303 | 310 027a 04F4 brge .+2 |
||
| 304 | 311 027c 00C0 rjmp .L55 |
||
| 305 | 312 027e 8091 0000 lds r24,ParamSet+42 |
||
| 306 | 313 0282 282F mov r18,r24 |
||
| 307 | 314 0284 30E0 ldi r19,lo8(0) |
||
| 308 | 315 0286 220F lsl r18 |
||
| 309 | 316 0288 331F rol r19 |
||
| 310 | 317 028a 220F lsl r18 |
||
| 311 | 318 028c 331F rol r19 |
||
| 312 | 319 028e 8091 0000 lds r24,ServoNickValue |
||
| 313 | 320 0292 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 314 | 321 0296 2817 cp r18,r24 |
||
| 315 | 322 0298 3907 cpc r19,r25 |
||
| 316 | 323 029a 04F4 brge .+2 |
||
| 317 | 324 029c 00C0 rjmp .L55 |
||
| 318 | 325 .L15: |
||
| 319 | 326 029e 2091 0000 lds r18,ServoNickValue |
||
| 320 | 327 02a2 3091 0000 lds r19,(ServoNickValue)+1 |
||
| 321 | 328 02a6 2755 subi r18,lo8(-(425)) |
||
| 322 | 329 02a8 3E4F sbci r19,hi8(-(425)) |
||
| 323 | 330 02aa 3093 0000 sts (RemainingPulse.1544)+1,r19 |
||
| 324 | 331 02ae 2093 0000 sts RemainingPulse.1544,r18 |
||
| 325 | 332 02b2 8091 0000 lds r24,ServoNickValue |
||
| 326 | 333 02b6 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 327 | 334 02ba 97FD sbrc r25,7 |
||
| 328 | 335 02bc 00C0 rjmp .L64 |
||
| 329 | 336 .L16: |
||
| 330 | 337 02be 9595 asr r25 |
||
| 331 | 338 02c0 8795 ror r24 |
||
| 332 | 339 02c2 9595 asr r25 |
||
| 333 | 340 02c4 8795 ror r24 |
||
| 334 | 341 02c6 9093 0000 sts (ServoNickValue)+1,r25 |
||
| 335 | 342 02ca 8093 0000 sts ServoNickValue,r24 |
||
| 336 | 343 02ce 45E0 ldi r20,hi8(1501) |
||
| 337 | 344 02d0 2D3D cpi r18,lo8(1501) |
||
| 338 | 345 02d2 3407 cpc r19,r20 |
||
| 339 | 346 02d4 00F4 brsh .+2 |
||
| 340 | 347 02d6 00C0 rjmp .L17 |
||
| 341 | 348 02d8 8CED ldi r24,lo8(1500) |
||
| 342 | 349 02da 95E0 ldi r25,hi8(1500) |
||
| 343 | 350 02dc 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 344 | 351 02e0 8093 0000 sts RemainingPulse.1544,r24 |
||
| 345 | 352 .L18: |
||
| 346 | 353 02e4 4091 0000 lds r20,RemainingPulse.1544 |
||
| 347 | 354 02e8 5091 0000 lds r21,(RemainingPulse.1544)+1 |
||
| 348 | 355 02ec 5093 0000 sts (ServoFrameTime.1545)+1,r21 |
||
| 349 | 356 02f0 4093 0000 sts ServoFrameTime.1545,r20 |
||
| 350 | 357 02f4 00C0 rjmp .L22 |
||
| 351 | 358 .L46: |
||
| 352 | 359 02f6 8FEF ldi r24,lo8(-1) |
||
| 353 | 360 02f8 8093 B300 sts 179,r24 |
||
| 354 | 361 02fc 3093 0000 sts (RemainingPulse.1544)+1,r19 |
||
| 355 | 362 0300 2093 0000 sts RemainingPulse.1544,r18 |
||
| 356 | 363 0304 00C0 rjmp .L47 |
||
| 357 | 364 .L20: |
||
| 358 | 365 0306 8091 B000 lds r24,176 |
||
| 359 | 366 030a 8064 ori r24,lo8(64) |
||
| 360 | 367 030c 8093 B000 sts 176,r24 |
||
| 361 | 368 0310 8CEB ldi r24,lo8(188) |
||
| 362 | 369 0312 90E0 ldi r25,hi8(188) |
||
| 363 | 370 0314 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 364 | 371 0318 8093 0000 sts RemainingPulse.1544,r24 |
||
| 365 | 372 031c 8091 0000 lds r24,ServoFrameTime.1545 |
||
| 366 | 373 0320 9091 0000 lds r25,(ServoFrameTime.1545)+1 |
||
| 367 | 374 0324 8454 subi r24,lo8(-(188)) |
||
| 368 | 375 0326 9F4F sbci r25,hi8(-(188)) |
||
| 369 | 376 0328 9093 0000 sts (ServoFrameTime.1545)+1,r25 |
||
| 370 | 377 032c 8093 0000 sts ServoFrameTime.1545,r24 |
||
| 371 | 378 0330 8091 0000 lds r24,ServoActive |
||
| 372 | 379 0334 8823 tst r24 |
||
| 373 | 380 0336 01F0 breq .L41 |
||
| 374 | 381 0338 8091 0000 lds r24,RC_Quality |
||
| 375 | 382 033c 853B cpi r24,lo8(-75) |
||
| 376 | 383 033e 00F0 brlo .+2 |
||
| 377 | 384 0340 00C0 rjmp .L65 |
||
| 378 | 385 .L41: |
||
| 379 | 386 0342 469A sbi 40-32,6 |
||
| 380 | 387 .L42: |
||
| 381 | 388 0344 8091 0000 lds r24,ServoIndex.1546 |
||
| 382 | 389 0348 8F5F subi r24,lo8(-(1)) |
||
| 383 | 390 034a 8093 0000 sts ServoIndex.1546,r24 |
||
| 384 | 391 034e 9091 0000 lds r25,ParamSet+47 |
||
| 385 | 392 0352 9817 cp r25,r24 |
||
| 386 | 393 0354 00F4 brsh .L58 |
||
| 387 | 394 0356 1092 0000 sts ServoIndex.1546,__zero_reg__ |
||
| 388 | 395 .L58: |
||
| 389 | 396 035a 4091 0000 lds r20,RemainingPulse.1544 |
||
| 390 | 397 035e 5091 0000 lds r21,(RemainingPulse.1544)+1 |
||
| 391 | 398 0362 00C0 rjmp .L22 |
||
| 392 | 399 .L10: |
||
| 393 | 400 0364 8091 B000 lds r24,176 |
||
| 394 | 401 0368 8064 ori r24,lo8(64) |
||
| 395 | 402 036a 8093 B000 sts 176,r24 |
||
| 396 | 403 036e 8091 0000 lds r24,ParamSet+47 |
||
| 397 | 404 0372 90E0 ldi r25,lo8(0) |
||
| 398 | 405 0374 2DED ldi r18,lo8(1757) |
||
| 399 | 406 0376 36E0 ldi r19,hi8(1757) |
||
| 400 | 407 0378 829F mul r24,r18 |
||
| 401 | 408 037a A001 movw r20,r0 |
||
| 402 | 409 037c 839F mul r24,r19 |
||
| 403 | 410 037e 500D add r21,r0 |
||
| 404 | 411 0380 929F mul r25,r18 |
||
| 405 | 412 0382 500D add r21,r0 |
||
| 406 | 413 0384 1124 clr r1 |
||
| 407 | 414 0386 8091 0000 lds r24,ServoFrameTime.1545 |
||
| 408 | 415 038a 9091 0000 lds r25,(ServoFrameTime.1545)+1 |
||
| 409 | 416 038e 481B sub r20,r24 |
||
| 410 | 417 0390 590B sbc r21,r25 |
||
| 411 | 418 0392 00C0 rjmp .L22 |
||
| 412 | 419 .L55: |
||
| 413 | 420 0394 3093 0000 sts (ServoNickValue)+1,r19 |
||
| 414 | 421 0398 2093 0000 sts ServoNickValue,r18 |
||
| 415 | 422 039c 00C0 rjmp .L15 |
||
| 416 | 423 .L21: |
||
| 417 | 424 039e 89EA ldi r24,lo8(937) |
||
| 418 | 425 03a0 93E0 ldi r25,hi8(937) |
||
| 419 | 426 03a2 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 420 | 427 03a6 8093 0000 sts RemainingPulse.1544,r24 |
||
| 421 | 428 03aa E130 cpi r30,lo8(1) |
||
| 422 | 429 03ac 01F4 brne .+2 |
||
| 423 | 430 03ae 00C0 rjmp .L24 |
||
| 424 | 431 03b0 E230 cpi r30,lo8(2) |
||
| 425 | 432 03b2 01F4 brne .+2 |
||
| 426 | 433 03b4 00C0 rjmp .L66 |
||
| 427 | 434 03b6 F0E0 ldi r31,lo8(0) |
||
| 428 | 435 03b8 EE0F lsl r30 |
||
| 429 | 436 03ba FF1F rol r31 |
||
| 430 | 437 03bc E050 subi r30,lo8(-(PPM_in)) |
||
| 431 | 438 03be F040 sbci r31,hi8(-(PPM_in)) |
||
| 432 | 439 03c0 8081 ld r24,Z |
||
| 433 | 440 03c2 9181 ldd r25,Z+1 |
||
| 434 | 441 03c4 880F lsl r24 |
||
| 435 | 442 03c6 991F rol r25 |
||
| 436 | 443 03c8 8755 subi r24,lo8(-(937)) |
||
| 437 | 444 03ca 9C4F sbci r25,hi8(-(937)) |
||
| 438 | 445 03cc 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 439 | 446 03d0 8093 0000 sts RemainingPulse.1544,r24 |
||
| 440 | 447 .L32: |
||
| 441 | 448 03d4 4091 0000 lds r20,RemainingPulse.1544 |
||
| 442 | 449 03d8 5091 0000 lds r21,(RemainingPulse.1544)+1 |
||
| 443 | 450 03dc 85E0 ldi r24,hi8(1501) |
||
| 444 | 451 03de 4D3D cpi r20,lo8(1501) |
||
| 445 | 452 03e0 5807 cpc r21,r24 |
||
| 446 | 453 03e2 00F4 brsh .+2 |
||
| 447 | 454 03e4 00C0 rjmp .L39 |
||
| 448 | 455 03e6 4CED ldi r20,lo8(1500) |
||
| 449 | 456 03e8 55E0 ldi r21,hi8(1500) |
||
| 450 | 457 .L40: |
||
| 451 | 458 03ea 4C5B subi r20,lo8(-(-188)) |
||
| 452 | 459 03ec 5040 sbci r21,hi8(-(-188)) |
||
| 453 | 460 03ee 8091 0000 lds r24,ServoFrameTime.1545 |
||
| 454 | 461 03f2 9091 0000 lds r25,(ServoFrameTime.1545)+1 |
||
| 455 | 462 03f6 840F add r24,r20 |
||
| 456 | 463 03f8 951F adc r25,r21 |
||
| 457 | 464 03fa 9093 0000 sts (ServoFrameTime.1545)+1,r25 |
||
| 458 | 465 03fe 8093 0000 sts ServoFrameTime.1545,r24 |
||
| 459 | 466 0402 00C0 rjmp .L22 |
||
| 460 | 467 .L12: |
||
| 461 | 468 0404 C090 0000 lds r12,ServoNickValue |
||
| 462 | 469 0408 D090 0000 lds r13,(ServoNickValue)+1 |
||
| 463 | 470 040c E090 0000 lds r14,ParamSet+40 |
||
| 464 | 471 0410 FF24 clr r15 |
||
| 465 | 472 0412 00E0 ldi r16,lo8(0) |
||
| 466 | 473 0414 10E0 ldi r17,hi8(0) |
||
| 467 | 474 0416 6091 0000 lds r22,IntegralGyroNick |
||
| 468 | 475 041a 7091 0000 lds r23,(IntegralGyroNick)+1 |
||
| 469 | 476 041e 8091 0000 lds r24,(IntegralGyroNick)+2 |
||
| 470 | 477 0422 9091 0000 lds r25,(IntegralGyroNick)+3 |
||
| 471 | 478 0426 20E8 ldi r18,lo8(128) |
||
| 472 | 479 0428 30E0 ldi r19,hi8(128) |
||
| 473 | 480 042a 40E0 ldi r20,hlo8(128) |
||
| 474 | 481 042c 50E0 ldi r21,hhi8(128) |
||
| 475 | 482 042e 0E94 0000 call __divmodsi4 |
||
| 476 | 483 0432 C801 movw r24,r16 |
||
| 477 | 484 0434 B701 movw r22,r14 |
||
| 478 | 485 0436 0E94 0000 call __mulsi3 |
||
| 479 | 486 043a 660F lsl r22 |
||
| 480 | 487 043c 771F rol r23 |
||
| 481 | 488 043e 881F rol r24 |
||
| 482 | 489 0440 991F rol r25 |
||
| 483 | 490 0442 660F lsl r22 |
||
| 484 | 491 0444 771F rol r23 |
||
| 485 | 492 0446 881F rol r24 |
||
| 486 | 493 0448 991F rol r25 |
||
| 487 | 494 044a 20E0 ldi r18,lo8(256) |
||
| 488 | 495 044c 31E0 ldi r19,hi8(256) |
||
| 489 | 496 044e 40E0 ldi r20,hlo8(256) |
||
| 490 | 497 0450 50E0 ldi r21,hhi8(256) |
||
| 491 | 498 0452 0E94 0000 call __divmodsi4 |
||
| 492 | 499 0456 C21A sub r12,r18 |
||
| 493 | 500 0458 D30A sbc r13,r19 |
||
| 494 | 501 045a D092 0000 sts (ServoNickValue)+1,r13 |
||
| 495 | 502 045e C092 0000 sts ServoNickValue,r12 |
||
| 496 | 503 0462 00C0 rjmp .L13 |
||
| 497 | 504 .L17: |
||
| 498 | 505 0464 2757 subi r18,lo8(375) |
||
| 499 | 506 0466 3140 sbci r19,hi8(375) |
||
| 500 | 507 0468 00F0 brlo .+2 |
||
| 501 | 508 046a 00C0 rjmp .L18 |
||
| 502 | 509 046c 87E7 ldi r24,lo8(375) |
||
| 503 | 510 046e 91E0 ldi r25,hi8(375) |
||
| 504 | 511 0470 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 505 | 512 0474 8093 0000 sts RemainingPulse.1544,r24 |
||
| 506 | 513 0478 00C0 rjmp .L18 |
||
| 507 | 514 .L65: |
||
| 508 | 515 047a 4698 cbi 40-32,6 |
||
| 509 | 516 047c 00C0 rjmp .L42 |
||
| 510 | 517 .L24: |
||
| 511 | 518 047e 8091 0000 lds r24,FCParam+21 |
||
| 512 | 519 0482 482F mov r20,r24 |
||
| 513 | 520 0484 50E0 ldi r21,lo8(0) |
||
| 514 | 521 0486 440F lsl r20 |
||
| 515 | 522 0488 551F rol r21 |
||
| 516 | 523 048a 440F lsl r20 |
||
| 517 | 524 048c 551F rol r21 |
||
| 518 | 525 048e 2091 0000 lds r18,ServoNickOffset.1547 |
||
| 519 | 526 0492 3091 0000 lds r19,(ServoNickOffset.1547)+1 |
||
| 520 | 527 0496 C901 movw r24,r18 |
||
| 521 | 528 0498 880F lsl r24 |
||
| 522 | 529 049a 991F rol r25 |
||
| 523 | 530 049c 820F add r24,r18 |
||
| 524 | 531 049e 931F adc r25,r19 |
||
| 525 | 532 04a0 480F add r20,r24 |
||
| 526 | 533 04a2 591F adc r21,r25 |
||
| 527 | 534 04a4 57FD sbrc r21,7 |
||
| 528 | 535 04a6 00C0 rjmp .L67 |
||
| 529 | 536 .L26: |
||
| 530 | 537 04a8 CA01 movw r24,r20 |
||
| 531 | 538 04aa 9595 asr r25 |
||
| 532 | 539 04ac 8795 ror r24 |
||
| 533 | 540 04ae 9595 asr r25 |
||
| 534 | 541 04b0 8795 ror r24 |
||
| 535 | 542 04b2 9093 0000 sts (ServoNickOffset.1547)+1,r25 |
||
| 536 | 543 04b6 8093 0000 sts ServoNickOffset.1547,r24 |
||
| 537 | 544 04ba 9093 0000 sts (ServoNickValue)+1,r25 |
||
| 538 | 545 04be 8093 0000 sts ServoNickValue,r24 |
||
| 539 | 546 04c2 8091 0000 lds r24,ParamSet+87 |
||
| 540 | 547 04c6 80FF sbrs r24,0 |
||
| 541 | 548 04c8 00C0 rjmp .L27 |
||
| 542 | 549 04ca C090 0000 lds r12,ServoNickValue |
||
| 543 | 550 04ce D090 0000 lds r13,(ServoNickValue)+1 |
||
| 544 | 551 04d2 E090 0000 lds r14,ParamSet+40 |
||
| 545 | 552 04d6 FF24 clr r15 |
||
| 546 | 553 04d8 00E0 ldi r16,lo8(0) |
||
| 547 | 554 04da 10E0 ldi r17,hi8(0) |
||
| 548 | 555 04dc 6091 0000 lds r22,IntegralGyroNick |
||
| 549 | 556 04e0 7091 0000 lds r23,(IntegralGyroNick)+1 |
||
| 550 | 557 04e4 8091 0000 lds r24,(IntegralGyroNick)+2 |
||
| 551 | 558 04e8 9091 0000 lds r25,(IntegralGyroNick)+3 |
||
| 552 | 559 04ec 20E8 ldi r18,lo8(128) |
||
| 553 | 560 04ee 30E0 ldi r19,hi8(128) |
||
| 554 | 561 04f0 40E0 ldi r20,hlo8(128) |
||
| 555 | 562 04f2 50E0 ldi r21,hhi8(128) |
||
| 556 | 563 04f4 0E94 0000 call __divmodsi4 |
||
| 557 | 564 04f8 C801 movw r24,r16 |
||
| 558 | 565 04fa B701 movw r22,r14 |
||
| 559 | 566 04fc 0E94 0000 call __mulsi3 |
||
| 560 | 567 0500 660F lsl r22 |
||
| 561 | 568 0502 771F rol r23 |
||
| 562 | 569 0504 881F rol r24 |
||
| 563 | 570 0506 991F rol r25 |
||
| 564 | 571 0508 660F lsl r22 |
||
| 565 | 572 050a 771F rol r23 |
||
| 566 | 573 050c 881F rol r24 |
||
| 567 | 574 050e 991F rol r25 |
||
| 568 | 575 0510 20E0 ldi r18,lo8(256) |
||
| 569 | 576 0512 31E0 ldi r19,hi8(256) |
||
| 570 | 577 0514 40E0 ldi r20,hlo8(256) |
||
| 571 | 578 0516 50E0 ldi r21,hhi8(256) |
||
| 572 | 579 0518 0E94 0000 call __divmodsi4 |
||
| 573 | 580 051c C20E add r12,r18 |
||
| 574 | 581 051e D31E adc r13,r19 |
||
| 575 | 582 0520 D092 0000 sts (ServoNickValue)+1,r13 |
||
| 576 | 583 0524 C092 0000 sts ServoNickValue,r12 |
||
| 577 | 584 .L28: |
||
| 578 | 585 0528 8091 0000 lds r24,ParamSet+41 |
||
| 579 | 586 052c 282F mov r18,r24 |
||
| 580 | 587 052e 30E0 ldi r19,lo8(0) |
||
| 581 | 588 0530 220F lsl r18 |
||
| 582 | 589 0532 331F rol r19 |
||
| 583 | 590 0534 220F lsl r18 |
||
| 584 | 591 0536 331F rol r19 |
||
| 585 | 592 0538 8091 0000 lds r24,ServoNickValue |
||
| 586 | 593 053c 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 587 | 594 0540 8217 cp r24,r18 |
||
| 588 | 595 0542 9307 cpc r25,r19 |
||
| 589 | 596 0544 04F0 brlt .L56 |
||
| 590 | 597 0546 8091 0000 lds r24,ParamSet+42 |
||
| 591 | 598 054a 282F mov r18,r24 |
||
| 592 | 599 054c 30E0 ldi r19,lo8(0) |
||
| 593 | 600 054e 220F lsl r18 |
||
| 594 | 601 0550 331F rol r19 |
||
| 595 | 602 0552 220F lsl r18 |
||
| 596 | 603 0554 331F rol r19 |
||
| 597 | 604 0556 8091 0000 lds r24,ServoNickValue |
||
| 598 | 605 055a 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 599 | 606 055e 2817 cp r18,r24 |
||
| 600 | 607 0560 3907 cpc r19,r25 |
||
| 601 | 608 0562 04F4 brge .L30 |
||
| 602 | 609 .L56: |
||
| 603 | 610 0564 3093 0000 sts (ServoNickValue)+1,r19 |
||
| 604 | 611 0568 2093 0000 sts ServoNickValue,r18 |
||
| 605 | 612 .L30: |
||
| 606 | 613 056c 8091 0000 lds r24,ServoNickValue |
||
| 607 | 614 0570 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 608 | 615 0574 8755 subi r24,lo8(-(425)) |
||
| 609 | 616 0576 9E4F sbci r25,hi8(-(425)) |
||
| 610 | 617 0578 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 611 | 618 057c 8093 0000 sts RemainingPulse.1544,r24 |
||
| 612 | 619 0580 8091 0000 lds r24,ServoNickValue |
||
| 613 | 620 0584 9091 0000 lds r25,(ServoNickValue)+1 |
||
| 614 | 621 0588 97FD sbrc r25,7 |
||
| 615 | 622 058a 00C0 rjmp .L68 |
||
| 616 | 623 .L31: |
||
| 617 | 624 058c 9595 asr r25 |
||
| 618 | 625 058e 8795 ror r24 |
||
| 619 | 626 0590 9595 asr r25 |
||
| 620 | 627 0592 8795 ror r24 |
||
| 621 | 628 0594 9093 0000 sts (ServoNickValue)+1,r25 |
||
| 622 | 629 0598 8093 0000 sts ServoNickValue,r24 |
||
| 623 | 630 059c 00C0 rjmp .L32 |
||
| 624 | 631 .L39: |
||
| 625 | 632 059e 81E0 ldi r24,hi8(375) |
||
| 626 | 633 05a0 4737 cpi r20,lo8(375) |
||
| 627 | 634 05a2 5807 cpc r21,r24 |
||
| 628 | 635 05a4 00F0 brlo .+2 |
||
| 629 | 636 05a6 00C0 rjmp .L40 |
||
| 630 | 637 05a8 47E7 ldi r20,lo8(375) |
||
| 631 | 638 05aa 51E0 ldi r21,hi8(375) |
||
| 632 | 639 05ac 00C0 rjmp .L40 |
||
| 633 | 640 .L64: |
||
| 634 | 641 05ae 0396 adiw r24,3 |
||
| 635 | 642 05b0 00C0 rjmp .L16 |
||
| 636 | 643 .L63: |
||
| 637 | 644 05b2 4D5F subi r20,lo8(-(3)) |
||
| 638 | 645 05b4 5F4F sbci r21,hi8(-(3)) |
||
| 639 | 646 05b6 00C0 rjmp .L11 |
||
| 640 | 647 .L66: |
||
| 641 | 648 05b8 8091 0000 lds r24,FCParam+22 |
||
| 642 | 649 05bc 482F mov r20,r24 |
||
| 643 | 650 05be 50E0 ldi r21,lo8(0) |
||
| 644 | 651 05c0 440F lsl r20 |
||
| 645 | 652 05c2 551F rol r21 |
||
| 646 | 653 05c4 440F lsl r20 |
||
| 647 | 654 05c6 551F rol r21 |
||
| 648 | 655 05c8 2091 0000 lds r18,ServoRollOffset.1548 |
||
| 649 | 656 05cc 3091 0000 lds r19,(ServoRollOffset.1548)+1 |
||
| 650 | 657 05d0 C901 movw r24,r18 |
||
| 651 | 658 05d2 880F lsl r24 |
||
| 652 | 659 05d4 991F rol r25 |
||
| 653 | 660 05d6 820F add r24,r18 |
||
| 654 | 661 05d8 931F adc r25,r19 |
||
| 655 | 662 05da 480F add r20,r24 |
||
| 656 | 663 05dc 591F adc r21,r25 |
||
| 657 | 664 05de 57FD sbrc r21,7 |
||
| 658 | 665 05e0 00C0 rjmp .L69 |
||
| 659 | 666 .L33: |
||
| 660 | 667 05e2 CA01 movw r24,r20 |
||
| 661 | 668 05e4 9595 asr r25 |
||
| 662 | 669 05e6 8795 ror r24 |
||
| 663 | 670 05e8 9595 asr r25 |
||
| 664 | 671 05ea 8795 ror r24 |
||
| 665 | 672 05ec 9093 0000 sts (ServoRollOffset.1548)+1,r25 |
||
| 666 | 673 05f0 8093 0000 sts ServoRollOffset.1548,r24 |
||
| 667 | 674 05f4 9093 0000 sts (ServoRollValue)+1,r25 |
||
| 668 | 675 05f8 8093 0000 sts ServoRollValue,r24 |
||
| 669 | 676 05fc 8091 0000 lds r24,ParamSet+87 |
||
| 670 | 677 0600 81FF sbrs r24,1 |
||
| 671 | 678 0602 00C0 rjmp .L34 |
||
| 672 | 679 0604 C090 0000 lds r12,ServoRollValue |
||
| 673 | 680 0608 D090 0000 lds r13,(ServoRollValue)+1 |
||
| 674 | 681 060c E090 0000 lds r14,ParamSet+44 |
||
| 675 | 682 0610 FF24 clr r15 |
||
| 676 | 683 0612 00E0 ldi r16,lo8(0) |
||
| 677 | 684 0614 10E0 ldi r17,hi8(0) |
||
| 678 | 685 0616 6091 0000 lds r22,IntegralGyroRoll |
||
| 679 | 686 061a 7091 0000 lds r23,(IntegralGyroRoll)+1 |
||
| 680 | 687 061e 8091 0000 lds r24,(IntegralGyroRoll)+2 |
||
| 681 | 688 0622 9091 0000 lds r25,(IntegralGyroRoll)+3 |
||
| 682 | 689 0626 20E8 ldi r18,lo8(128) |
||
| 683 | 690 0628 30E0 ldi r19,hi8(128) |
||
| 684 | 691 062a 40E0 ldi r20,hlo8(128) |
||
| 685 | 692 062c 50E0 ldi r21,hhi8(128) |
||
| 686 | 693 062e 0E94 0000 call __divmodsi4 |
||
| 687 | 694 0632 C801 movw r24,r16 |
||
| 688 | 695 0634 B701 movw r22,r14 |
||
| 689 | 696 0636 0E94 0000 call __mulsi3 |
||
| 690 | 697 063a 660F lsl r22 |
||
| 691 | 698 063c 771F rol r23 |
||
| 692 | 699 063e 881F rol r24 |
||
| 693 | 700 0640 991F rol r25 |
||
| 694 | 701 0642 660F lsl r22 |
||
| 695 | 702 0644 771F rol r23 |
||
| 696 | 703 0646 881F rol r24 |
||
| 697 | 704 0648 991F rol r25 |
||
| 698 | 705 064a 20E0 ldi r18,lo8(256) |
||
| 699 | 706 064c 31E0 ldi r19,hi8(256) |
||
| 700 | 707 064e 40E0 ldi r20,hlo8(256) |
||
| 701 | 708 0650 50E0 ldi r21,hhi8(256) |
||
| 702 | 709 0652 0E94 0000 call __divmodsi4 |
||
| 703 | 710 0656 C20E add r12,r18 |
||
| 704 | 711 0658 D31E adc r13,r19 |
||
| 705 | 712 065a D092 0000 sts (ServoRollValue)+1,r13 |
||
| 706 | 713 065e C092 0000 sts ServoRollValue,r12 |
||
| 707 | 714 .L35: |
||
| 708 | 715 0662 8091 0000 lds r24,ParamSet+45 |
||
| 709 | 716 0666 282F mov r18,r24 |
||
| 710 | 717 0668 30E0 ldi r19,lo8(0) |
||
| 711 | 718 066a 220F lsl r18 |
||
| 712 | 719 066c 331F rol r19 |
||
| 713 | 720 066e 220F lsl r18 |
||
| 714 | 721 0670 331F rol r19 |
||
| 715 | 722 0672 8091 0000 lds r24,ServoRollValue |
||
| 716 | 723 0676 9091 0000 lds r25,(ServoRollValue)+1 |
||
| 717 | 724 067a 8217 cp r24,r18 |
||
| 718 | 725 067c 9307 cpc r25,r19 |
||
| 719 | 726 067e 04F0 brlt .L57 |
||
| 720 | 727 0680 8091 0000 lds r24,ParamSet+46 |
||
| 721 | 728 0684 282F mov r18,r24 |
||
| 722 | 729 0686 30E0 ldi r19,lo8(0) |
||
| 723 | 730 0688 220F lsl r18 |
||
| 724 | 731 068a 331F rol r19 |
||
| 725 | 732 068c 220F lsl r18 |
||
| 726 | 733 068e 331F rol r19 |
||
| 727 | 734 0690 8091 0000 lds r24,ServoRollValue |
||
| 728 | 735 0694 9091 0000 lds r25,(ServoRollValue)+1 |
||
| 729 | 736 0698 2817 cp r18,r24 |
||
| 730 | 737 069a 3907 cpc r19,r25 |
||
| 731 | 738 069c 04F4 brge .L37 |
||
| 732 | 739 .L57: |
||
| 733 | 740 069e 3093 0000 sts (ServoRollValue)+1,r19 |
||
| 734 | 741 06a2 2093 0000 sts ServoRollValue,r18 |
||
| 735 | 742 .L37: |
||
| 736 | 743 06a6 8091 0000 lds r24,ServoRollValue |
||
| 737 | 744 06aa 9091 0000 lds r25,(ServoRollValue)+1 |
||
| 738 | 745 06ae 8755 subi r24,lo8(-(425)) |
||
| 739 | 746 06b0 9E4F sbci r25,hi8(-(425)) |
||
| 740 | 747 06b2 9093 0000 sts (RemainingPulse.1544)+1,r25 |
||
| 741 | 748 06b6 8093 0000 sts RemainingPulse.1544,r24 |
||
| 742 | 749 06ba 8091 0000 lds r24,ServoRollValue |
||
| 743 | 750 06be 9091 0000 lds r25,(ServoRollValue)+1 |
||
| 744 | 751 06c2 97FD sbrc r25,7 |
||
| 745 | 752 06c4 00C0 rjmp .L70 |
||
| 746 | 753 .L38: |
||
| 747 | 754 06c6 9595 asr r25 |
||
| 748 | 755 06c8 8795 ror r24 |
||
| 749 | 756 06ca 9595 asr r25 |
||
| 750 | 757 06cc 8795 ror r24 |
||
| 751 | 758 06ce 9093 0000 sts (ServoRollValue)+1,r25 |
||
| 752 | 759 06d2 8093 0000 sts ServoRollValue,r24 |
||
| 753 | 760 06d6 00C0 rjmp .L32 |
||
| 754 | 761 .L34: |
||
| 755 | 762 06d8 C090 0000 lds r12,ServoRollValue |
||
| 756 | 763 06dc D090 0000 lds r13,(ServoRollValue)+1 |
||
| 757 | 764 06e0 E090 0000 lds r14,ParamSet+44 |
||
| 758 | 765 06e4 FF24 clr r15 |
||
| 759 | 766 06e6 00E0 ldi r16,lo8(0) |
||
| 760 | 767 06e8 10E0 ldi r17,hi8(0) |
||
| 761 | 768 06ea 6091 0000 lds r22,IntegralGyroRoll |
||
| 762 | 769 06ee 7091 0000 lds r23,(IntegralGyroRoll)+1 |
||
| 763 | 770 06f2 8091 0000 lds r24,(IntegralGyroRoll)+2 |
||
| 764 | 771 06f6 9091 0000 lds r25,(IntegralGyroRoll)+3 |
||
| 765 | 772 06fa 20E8 ldi r18,lo8(128) |
||
| 766 | 773 06fc 30E0 ldi r19,hi8(128) |
||
| 767 | 774 06fe 40E0 ldi r20,hlo8(128) |
||
| 768 | 775 0700 50E0 ldi r21,hhi8(128) |
||
| 769 | 776 0702 0E94 0000 call __divmodsi4 |
||
| 770 | 777 0706 C801 movw r24,r16 |
||
| 771 | 778 0708 B701 movw r22,r14 |
||
| 772 | 779 070a 0E94 0000 call __mulsi3 |
||
| 773 | 780 070e 660F lsl r22 |
||
| 774 | 781 0710 771F rol r23 |
||
| 775 | 782 0712 881F rol r24 |
||
| 776 | 783 0714 991F rol r25 |
||
| 777 | 784 0716 660F lsl r22 |
||
| 778 | 785 0718 771F rol r23 |
||
| 779 | 786 071a 881F rol r24 |
||
| 780 | 787 071c 991F rol r25 |
||
| 781 | 788 071e 20E0 ldi r18,lo8(256) |
||
| 782 | 789 0720 31E0 ldi r19,hi8(256) |
||
| 783 | 790 0722 40E0 ldi r20,hlo8(256) |
||
| 784 | 791 0724 50E0 ldi r21,hhi8(256) |
||
| 785 | 792 0726 0E94 0000 call __divmodsi4 |
||
| 786 | 793 072a C21A sub r12,r18 |
||
| 787 | 794 072c D30A sbc r13,r19 |
||
| 788 | 795 072e D092 0000 sts (ServoRollValue)+1,r13 |
||
| 789 | 796 0732 C092 0000 sts ServoRollValue,r12 |
||
| 790 | 797 0736 00C0 rjmp .L35 |
||
| 791 | 798 .L27: |
||
| 792 | 799 0738 C090 0000 lds r12,ServoNickValue |
||
| 793 | 800 073c D090 0000 lds r13,(ServoNickValue)+1 |
||
| 794 | 801 0740 E090 0000 lds r14,ParamSet+40 |
||
| 795 | 802 0744 FF24 clr r15 |
||
| 796 | 803 0746 00E0 ldi r16,lo8(0) |
||
| 797 | 804 0748 10E0 ldi r17,hi8(0) |
||
| 798 | 805 074a 6091 0000 lds r22,IntegralGyroNick |
||
| 799 | 806 074e 7091 0000 lds r23,(IntegralGyroNick)+1 |
||
| 800 | 807 0752 8091 0000 lds r24,(IntegralGyroNick)+2 |
||
| 801 | 808 0756 9091 0000 lds r25,(IntegralGyroNick)+3 |
||
| 802 | 809 075a 20E8 ldi r18,lo8(128) |
||
| 803 | 810 075c 30E0 ldi r19,hi8(128) |
||
| 804 | 811 075e 40E0 ldi r20,hlo8(128) |
||
| 805 | 812 0760 50E0 ldi r21,hhi8(128) |
||
| 806 | 813 0762 0E94 0000 call __divmodsi4 |
||
| 807 | 814 0766 C801 movw r24,r16 |
||
| 808 | 815 0768 B701 movw r22,r14 |
||
| 809 | 816 076a 0E94 0000 call __mulsi3 |
||
| 810 | 817 076e 660F lsl r22 |
||
| 811 | 818 0770 771F rol r23 |
||
| 812 | 819 0772 881F rol r24 |
||
| 813 | 820 0774 991F rol r25 |
||
| 814 | 821 0776 660F lsl r22 |
||
| 815 | 822 0778 771F rol r23 |
||
| 816 | 823 077a 881F rol r24 |
||
| 817 | 824 077c 991F rol r25 |
||
| 818 | 825 077e 20E0 ldi r18,lo8(256) |
||
| 819 | 826 0780 31E0 ldi r19,hi8(256) |
||
| 820 | 827 0782 40E0 ldi r20,hlo8(256) |
||
| 821 | 828 0784 50E0 ldi r21,hhi8(256) |
||
| 822 | 829 0786 0E94 0000 call __divmodsi4 |
||
| 823 | 830 078a C21A sub r12,r18 |
||
| 824 | 831 078c D30A sbc r13,r19 |
||
| 825 | 832 078e D092 0000 sts (ServoNickValue)+1,r13 |
||
| 826 | 833 0792 C092 0000 sts ServoNickValue,r12 |
||
| 827 | 834 0796 00C0 rjmp .L28 |
||
| 828 | 835 .L68: |
||
| 829 | 836 0798 0396 adiw r24,3 |
||
| 830 | 837 079a 00C0 rjmp .L31 |
||
| 831 | 838 .L67: |
||
| 832 | 839 079c 4D5F subi r20,lo8(-(3)) |
||
| 833 | 840 079e 5F4F sbci r21,hi8(-(3)) |
||
| 834 | 841 07a0 00C0 rjmp .L26 |
||
| 835 | 842 .L69: |
||
| 836 | 843 07a2 4D5F subi r20,lo8(-(3)) |
||
| 837 | 844 07a4 5F4F sbci r21,hi8(-(3)) |
||
| 838 | 845 07a6 00C0 rjmp .L33 |
||
| 839 | 846 .L70: |
||
| 840 | 847 07a8 0396 adiw r24,3 |
||
| 841 | 848 07aa 00C0 rjmp .L38 |
||
| 842 | 850 .global ServoNickValue |
||
| 843 | 851 .global ServoNickValue |
||
| 844 | 852 .section .bss |
||
| 845 | 855 ServoNickValue: |
||
| 846 | 856 0000 0000 .skip 2,0 |
||
| 847 | 857 .global ServoRollValue |
||
| 848 | 858 .global ServoRollValue |
||
| 849 | 861 ServoRollValue: |
||
| 850 | 862 0002 0000 .skip 2,0 |
||
| 851 | 863 .global ServoActive |
||
| 852 | 864 .global ServoActive |
||
| 853 | 867 ServoActive: |
||
| 854 | 868 0004 00 .skip 1,0 |
||
| 855 | 869 .data |
||
| 856 | 872 ServoRollOffset.1548: |
||
| 857 | 873 0000 FC01 .word 508 |
||
| 858 | 876 ServoNickOffset.1547: |
||
| 859 | 877 0002 FC01 .word 508 |
||
| 860 | 878 .lcomm ServoIndex.1546,1 |
||
| 861 | 879 .lcomm ServoFrameTime.1545,2 |
||
| 862 | 880 .lcomm RemainingPulse.1544,2 |
||
| 863 | 881 .lcomm PulseOutput.1543,1 |
||
| 864 | 882 .global __do_copy_data |
||
| 865 | 883 .global __do_clear_bss |
||
| 866 | DEFINED SYMBOLS |
||
| 867 | *ABS*:00000000 timer2.c |
||
| 868 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:2 *ABS*:0000003f __SREG__ |
||
| 869 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:3 *ABS*:0000003e __SP_H__ |
||
| 870 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:4 *ABS*:0000003d __SP_L__ |
||
| 871 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:5 *ABS*:00000034 __CCP__ |
||
| 872 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:6 *ABS*:00000000 __tmp_reg__ |
||
| 873 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:7 *ABS*:00000001 __zero_reg__ |
||
| 874 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:11 .text:00000000 TIMER2_Init |
||
| 875 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:60 .text:00000050 Servo_On |
||
| 876 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:867 .bss:00000004 ServoActive |
||
| 877 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:71 .text:0000005a Servo_Off |
||
| 878 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:82 .text:00000064 __vector_9 |
||
| 879 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:880 .bss:0000000a PulseOutput.1543 |
||
| 880 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:879 .bss:00000008 RemainingPulse.1544 |
||
| 881 | .bss:00000005 ServoIndex.1546 |
||
| 882 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:878 .bss:00000006 ServoFrameTime.1545 |
||
| 883 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:876 .data:00000002 ServoNickOffset.1547 |
||
| 884 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:855 .bss:00000000 ServoNickValue |
||
| 885 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:872 .data:00000000 ServoRollOffset.1548 |
||
| 886 | C:\DOKUME~1\LUKEPA~1\LOKALE~1\Temp/ccgdfsVT.s:861 .bss:00000002 ServoRollValue |
||
| 887 | |||
| 888 | UNDEFINED SYMBOLS |
||
| 889 | BoardRelease |
||
| 890 | ParamSet |
||
| 891 | FCParam |
||
| 892 | IntegralGyroNick |
||
| 893 | __divmodsi4 |
||
| 894 | __mulsi3 |
||
| 895 | RC_Quality |
||
| 896 | PPM_in |
||
| 897 | IntegralGyroRoll |
||
| 898 | __do_copy_data |
||
| 899 | __do_clear_bss |