Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1131 krheinwald 1 
   1               		.file	"pitch_neutral.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               		.global __do_copy_data
9 
   9               		.global __do_clear_bss
10 
  10               		.text
11 
  11               	.global	pitch_neutral_value
12 
  13               	pitch_neutral_value:
13 
  14 0000 CF92      		push r12
14 
  15 0002 DF92      		push r13
15 
  16 0004 EF92      		push r14
16 
  17 0006 FF92      		push r15
17 
  18 0008 0F93      		push r16
18 
  19 000a 1F93      		push r17
19 
  20 000c CF93      		push r28
20 
  21 000e DF93      		push r29
21 
  22               	/* prologue: function */
22 
  23               	/* frame size = 0 */
23 
  24 0010 8091 0000 		lds r24,MotorenEin
24 
  25 0014 8823      		tst r24
25 
  26 0016 01F4      		brne .+2
26 
  27 0018 00C0      		rjmp .L2
27 
  28 001a C090 0000 		lds r12,stickValue
28 
  29 001e D090 0000 		lds r13,(stickValue)+1
29 
  30 0022 D092 0000 		sts (lastStickValue)+1,r13
30 
  31 0026 C092 0000 		sts lastStickValue,r12
31 
  32 002a 8091 0000 		lds r24,EE_Parameter+2
32 
  33 002e 082F      		mov r16,r24
33 
  34 0030 10E0      		ldi r17,lo8(0)
34 
  35 0032 F801      		movw r30,r16
35 
  36 0034 EE0F      		lsl r30
36 
  37 0036 FF1F      		rol r31
37 
  38 0038 E050      		subi r30,lo8(-(PPM_in))
38 
  39 003a F040      		sbci r31,hi8(-(PPM_in))
39 
  40 003c 0190      		ld __tmp_reg__,Z+
40 
  41 003e F081      		ld r31,Z
41 
  42 0040 E02D      		mov r30,__tmp_reg__
42 
  43 0042 EE9F      		mul r30,r30
43 
  44 0044 C001      		movw r24,r0
44 
  45 0046 EF9F      		mul r30,r31
45 
  46 0048 900D      		add r25,r0
46 
  47 004a FE9F      		mul r31,r30
47 
  48 004c 900D      		add r25,r0
48 
  49 004e 1124      		clr r1
49 
  50 0050 1E16      		cp __zero_reg__,r30
50 
  51 0052 1F06      		cpc __zero_reg__,r31
51 
  52 0054 04F4      		brge .L3
52 
  53 0056 66E9      		ldi r22,lo8(150)
53 
  54 0058 70E0      		ldi r23,hi8(150)
54 
  55 005a 00C0      		rjmp .L25
55 
  56               	.L3:
56 
  57 005c 6AE6      		ldi r22,lo8(-150)
57 
  58 005e 7FEF      		ldi r23,hi8(-150)
58 
  59               	.L25:
59 
  60 0060 0E94 0000 		call __divmodhi4
60 
  61 0064 E60F      		add r30,r22
61 
  62 0066 F71F      		adc r31,r23
62 
  63 0068 F093 0000 		sts (temp)+1,r31
63 
  64 006c E093 0000 		sts temp,r30
64 
  65 0070 C601      		movw r24,r12
65 
  66 0072 880F      		lsl r24
66 
  67 0074 991F      		rol r25
67 
  68 0076 2091 0000 		lds r18,temp
68 
  69 007a 3091 0000 		lds r19,(temp)+1
69 
  70 007e 820F      		add r24,r18
70 
  71 0080 931F      		adc r25,r19
71 
  72 0082 63E0      		ldi r22,lo8(3)
72 
  73 0084 70E0      		ldi r23,hi8(3)
73 
  74 0086 0E94 0000 		call __divmodhi4
74 
  75 008a 9B01      		movw r18,r22
75 
  76 008c 7093 0000 		sts (stickValue)+1,r23
76 
  77 0090 6093 0000 		sts stickValue,r22
77 
  78 0094 C091 0000 		lds r28,pitchOffset
78 
  79 0098 D091 0000 		lds r29,(pitchOffset)+1
79 
  80 009c 7B01      		movw r14,r22
80 
  81 009e EC0E      		add r14,r28
81 
  82 00a0 FD1E      		adc r15,r29
82 
  83 00a2 8091 0000 		lds r24,state
83 
  84 00a6 8530      		cpi r24,lo8(5)
84 
  85 00a8 01F4      		brne .+2
85 
  86 00aa 00C0      		rjmp .L9
86 
  87 00ac 8630      		cpi r24,lo8(6)
87 
  88 00ae 00F4      		brsh .L13
88 
  89 00b0 8330      		cpi r24,lo8(3)
89 
  90 00b2 01F0      		breq .L7
90 
  91 00b4 8430      		cpi r24,lo8(4)
91 
  92 00b6 00F0      		brlo .+2
92 
  93 00b8 00C0      		rjmp .L8
93 
  94 00ba 8230      		cpi r24,lo8(2)
94 
  95 00bc 01F0      		breq .+2
95 
  96 00be 00C0      		rjmp .L5
96 
  97 00c0 00C0      		rjmp .L28
97 
  98               	.L13:
98 
  99 00c2 8730      		cpi r24,lo8(7)
99 
 100 00c4 01F4      		brne .+2
100 
 101 00c6 00C0      		rjmp .L11
101 
 102 00c8 8730      		cpi r24,lo8(7)
102 
 103 00ca 00F4      		brsh .+2
103 
 104 00cc 00C0      		rjmp .L10
104 
 105 00ce 8830      		cpi r24,lo8(8)
105 
 106 00d0 01F0      		breq .+2
106 
 107 00d2 00C0      		rjmp .L5
107 
 108 00d4 00C0      		rjmp .L29
108 
 109               	.L28:
109 
 110 00d6 46E0      		ldi r20,lo8(6)
110 
 111 00d8 C416      		cp r12,r20
111 
 112 00da D104      		cpc r13,__zero_reg__
112 
 113 00dc 04F4      		brge .+2
113 
 114 00de 00C0      		rjmp .L5
114 
 115 00e0 C601      		movw r24,r12
115 
 116 00e2 861B      		sub r24,r22
116 
 117 00e4 970B      		sbc r25,r23
117 
 118 00e6 9C01      		movw r18,r24
118 
 119 00e8 8091 0000 		lds r24,EE_Parameter+29
119 
 120 00ec 90E0      		ldi r25,lo8(0)
120 
 121 00ee 2817      		cp r18,r24
121 
 122 00f0 3907      		cpc r19,r25
122 
 123 00f2 04F4      		brge .+2
123 
 124 00f4 00C0      		rjmp .L5
124 
 125 00f6 D092 0000 		sts (pitchOffset)+1,r13
125 
 126 00fa C092 0000 		sts pitchOffset,r12
126 
 127 00fe 83E0      		ldi r24,lo8(3)
127 
 128 0100 8093 0000 		sts state,r24
128 
 129 0104 89E1      		ldi r24,lo8(25)
129 
 130 0106 90E0      		ldi r25,hi8(25)
130 
 131 0108 9093 0000 		sts (pitchNeutralTimer)+1,r25
131 
 132 010c 8093 0000 		sts pitchNeutralTimer,r24
132 
 133 0110 00C0      		rjmp .L5
133 
 134               	.L7:
134 
 135 0112 8091 0000 		lds r24,pitchNeutralTimer
135 
 136 0116 9091 0000 		lds r25,(pitchNeutralTimer)+1
136 
 137 011a 0197      		sbiw r24,1
137 
 138 011c 9093 0000 		sts (pitchNeutralTimer)+1,r25
138 
 139 0120 8093 0000 		sts pitchNeutralTimer,r24
139 
 140 0124 892B      		or r24,r25
140 
 141 0126 01F4      		brne .L15
141 
 142 0128 1092 0000 		sts (pitchOffset)+1,__zero_reg__
142 
 143 012c 1092 0000 		sts pitchOffset,__zero_reg__
143 
 144 0130 82E0      		ldi r24,lo8(2)
144 
 145 0132 8093 0000 		sts state,r24
145 
 146               	.L15:
146 
 147 0136 2B5F      		subi r18,lo8(-(5))
147 
 148 0138 3F4F      		sbci r19,hi8(-(5))
148 
 149 013a 2B30      		cpi r18,11
149 
 150 013c 3105      		cpc r19,__zero_reg__
150 
 151 013e 00F0      		brlo .+2
151 
 152 0140 00C0      		rjmp .L14
152 
 153 0142 8091 0000 		lds r24,EE_Parameter+8
153 
 154 0146 80FF      		sbrs r24,0
154 
 155 0148 00C0      		rjmp .L16
155 
 156 014a 87E0      		ldi r24,lo8(7)
156 
 157 014c 00C0      		rjmp .L26
157 
 158               	.L16:
158 
 159 014e 84E0      		ldi r24,lo8(4)
159 
 160 0150 00C0      		rjmp .L26
160 
 161               	.L8:
161 
 162 0152 8091 0000 		lds r24,EE_Parameter+30
162 
 163 0156 C82F      		mov r28,r24
163 
 164 0158 D0E0      		ldi r29,lo8(0)
164 
 165 015a EC16      		cp r14,r28
165 
 166 015c FD06      		cpc r15,r29
166 
 167 015e 04F4      		brge .+2
167 
 168 0160 00C0      		rjmp .L14
168 
 169 0162 00C0      		rjmp .L5
169 
 170               	.L9:
170 
 171 0164 8091 0000 		lds r24,EE_Parameter+51
171 
 172 0168 84FF      		sbrs r24,4
172 
 173 016a 00C0      		rjmp .L17
173 
 174 016c 000F      		lsl r16
174 
 175 016e 111F      		rol r17
175 
 176 0170 0050      		subi r16,lo8(-(PPM_in))
176 
 177 0172 1040      		sbci r17,hi8(-(PPM_in))
177 
 178 0174 F801      		movw r30,r16
178 
 179 0176 8081      		ld r24,Z
179 
 180 0178 9181      		ldd r25,Z+1
180 
 181 017a 8C5A      		subi r24,lo8(-84)
181 
 182 017c 9F4F      		sbci r25,hi8(-84)
182 
 183 017e 04F0      		brlt .L18
183 
 184 0180 84E6      		ldi r24,lo8(100)
184 
 185 0182 90E0      		ldi r25,hi8(100)
185 
 186 0184 9093 0000 		sts (pitchNeutralTimer)+1,r25
186 
 187 0188 8093 0000 		sts pitchNeutralTimer,r24
187 
 188 018c 00C0      		rjmp .L17
188 
 189               	.L18:
189 
 190 018e 8091 0000 		lds r24,pitchNeutralTimer
190 
 191 0192 9091 0000 		lds r25,(pitchNeutralTimer)+1
191 
 192 0196 0197      		sbiw r24,1
192 
 193 0198 9093 0000 		sts (pitchNeutralTimer)+1,r25
193 
 194 019c 8093 0000 		sts pitchNeutralTimer,r24
194 
 195 01a0 892B      		or r24,r25
195 
 196 01a2 01F4      		brne .L17
196 
 197 01a4 82E0      		ldi r24,lo8(2)
197 
 198 01a6 8093 0000 		sts state,r24
198 
 199 01aa 1092 0000 		sts (pitchOffset)+1,__zero_reg__
199 
 200 01ae 1092 0000 		sts pitchOffset,__zero_reg__
200 
 201 01b2 84EF      		ldi r24,lo8(500)
201 
 202 01b4 91E0      		ldi r25,hi8(500)
202 
 203 01b6 9093 0000 		sts (beeptime)+1,r25
203 
 204 01ba 8093 0000 		sts beeptime,r24
204 
 205               	.L17:
205 
 206 01be 8091 0000 		lds r24,EE_Parameter+30
206 
 207 01c2 90E0      		ldi r25,lo8(0)
207 
 208 01c4 E701      		movw r28,r14
208 
 209 01c6 E816      		cp r14,r24
209 
 210 01c8 F906      		cpc r15,r25
210 
 211 01ca 04F4      		brge .L19
211 
 212 01cc EC01      		movw r28,r24
212 
 213               	.L19:
213 
 214 01ce 2C5F      		subi r18,lo8(-(4))
214 
 215 01d0 3F4F      		sbci r19,hi8(-(4))
215 
 216 01d2 2930      		cpi r18,9
216 
 217 01d4 3105      		cpc r19,__zero_reg__
217 
 218 01d6 00F0      		brlo .+2
218 
 219 01d8 00C0      		rjmp .L14
219 
 220 01da 89E1      		ldi r24,lo8(25)
220 
 221 01dc 90E0      		ldi r25,hi8(25)
221 
 222 01de 9093 0000 		sts (pitchNeutralTimer)+1,r25
222 
 223 01e2 8093 0000 		sts pitchNeutralTimer,r24
223 
 224 01e6 86E0      		ldi r24,lo8(6)
224 
 225 01e8 00C0      		rjmp .L26
225 
 226               	.L10:
226 
 227 01ea CB01      		movw r24,r22
227 
 228 01ec 0496      		adiw r24,4
228 
 229 01ee 0997      		sbiw r24,9
229 
 230 01f0 00F4      		brsh .L20
230 
 231 01f2 C616      		cp r12,r22
231 
 232 01f4 D706      		cpc r13,r23
232 
 233 01f6 01F4      		brne .L20
233 
 234 01f8 8091 0000 		lds r24,pitchNeutralTimer
234 
 235 01fc 9091 0000 		lds r25,(pitchNeutralTimer)+1
235 
 236 0200 0197      		sbiw r24,1
236 
 237 0202 9093 0000 		sts (pitchNeutralTimer)+1,r25
237 
 238 0206 8093 0000 		sts pitchNeutralTimer,r24
238 
 239 020a 892B      		or r24,r25
239 
 240 020c 01F4      		brne .L5
240 
 241 020e 87E0      		ldi r24,lo8(7)
241 
 242 0210 00C0      		rjmp .L27
242 
 243               	.L20:
243 
 244 0212 85E0      		ldi r24,lo8(5)
244 
 245               	.L27:
245 
 246 0214 8093 0000 		sts state,r24
246 
 247 0218 00C0      		rjmp .L5
247 
 248               	.L11:
248 
 249 021a 0E94 0000 		call altcon_start
249 
 250 021e 88E0      		ldi r24,lo8(8)
250 
 251 0220 00C0      		rjmp .L27
251 
 252               	.L29:
252 
 253 0222 CB01      		movw r24,r22
253 
 254 0224 77FF      		sbrs r23,7
254 
 255 0226 00C0      		rjmp .L21
255 
 256 0228 8827      		clr r24
256 
 257 022a 9927      		clr r25
257 
 258 022c 861B      		sub r24,r22
258 
 259 022e 970B      		sbc r25,r23
259 
 260               	.L21:
260 
 261 0230 0697      		sbiw r24,6
261 
 262 0232 04F0      		brlt .L5
262 
 263 0234 8091 0000 		lds r24,averageN
263 
 264 0238 9091 0000 		lds r25,(averageN)+1
264 
 265 023c 64E0      		ldi r22,lo8(4)
265 
 266 023e 70E0      		ldi r23,hi8(4)
266 
 267 0240 0E94 0000 		call __divmodhi4
267 
 268 0244 CE01      		movw r24,r28
268 
 269 0246 861B      		sub r24,r22
269 
 270 0248 970B      		sbc r25,r23
270 
 271 024a 9093 0000 		sts (pitchOffset)+1,r25
271 
 272 024e 8093 0000 		sts pitchOffset,r24
272 
 273 0252 E901      		movw r28,r18
273 
 274 0254 C80F      		add r28,r24
274 
 275 0256 D91F      		adc r29,r25
275 
 276 0258 0E94 0000 		call altcon_stop
276 
 277 025c 85E0      		ldi r24,lo8(5)
277 
 278               	.L26:
278 
 279 025e 8093 0000 		sts state,r24
279 
 280 0262 00C0      		rjmp .L14
280 
 281               	.L2:
281 
 282 0264 1092 0000 		sts (pitchOffset)+1,__zero_reg__
282 
 283 0268 1092 0000 		sts pitchOffset,__zero_reg__
283 
 284 026c 1092 0000 		sts (stickValue)+1,__zero_reg__
284 
 285 0270 1092 0000 		sts stickValue,__zero_reg__
285 
 286 0274 82E0      		ldi r24,lo8(2)
286 
 287 0276 8093 0000 		sts state,r24
287 
 288 027a C0E0      		ldi r28,lo8(0)
288 
 289 027c D0E0      		ldi r29,hi8(0)
289 
 290 027e 00C0      		rjmp .L14
290 
 291               	.L5:
291 
 292 0280 E701      		movw r28,r14
292 
 293               	.L14:
293 
 294 0282 8091 0000 		lds r24,pitchOffset
294 
 295 0286 9091 0000 		lds r25,(pitchOffset)+1
295 
 296 028a 97FF      		sbrs r25,7
296 
 297 028c 00C0      		rjmp .L22
297 
 298 028e 1092 0000 		sts (pitchOffset)+1,__zero_reg__
298 
 299 0292 1092 0000 		sts pitchOffset,__zero_reg__
299 
 300               	.L22:
300 
 301 0296 9E01      		movw r18,r28
301 
 302 0298 D7FF      		sbrs r29,7
302 
 303 029a 00C0      		rjmp .L23
303 
 304 029c 20E0      		ldi r18,lo8(0)
304 
 305 029e 30E0      		ldi r19,hi8(0)
305 
 306               	.L23:
306 
 307 02a0 8091 0000 		lds r24,stickValue
307 
 308 02a4 9091 0000 		lds r25,(stickValue)+1
308 
 309 02a8 9093 0000 		sts (DebugOut+54)+1,r25
309 
 310 02ac 8093 0000 		sts DebugOut+54,r24
310 
 311 02b0 3093 0000 		sts (DebugOut+58)+1,r19
311 
 312 02b4 2093 0000 		sts DebugOut+58,r18
312 
 313 02b8 8091 0000 		lds r24,pitchOffset
313 
 314 02bc 9091 0000 		lds r25,(pitchOffset)+1
314 
 315 02c0 9093 0000 		sts (DebugOut+60)+1,r25
315 
 316 02c4 8093 0000 		sts DebugOut+60,r24
316 
 317 02c8 C901      		movw r24,r18
317 
 318               	/* epilogue start */
318 
 319 02ca DF91      		pop r29
319 
 320 02cc CF91      		pop r28
320 
 321 02ce 1F91      		pop r17
321 
 322 02d0 0F91      		pop r16
322 
 323 02d2 FF90      		pop r15
323 
 324 02d4 EF90      		pop r14
324 
 325 02d6 DF90      		pop r13
325 
 326 02d8 CF90      		pop r12
326 
 327 02da 0895      		ret
327 
 329               		.lcomm stickValue,2
328 
 330               		.lcomm lastStickValue,2
329 
 331               		.lcomm pitchOffset,2
330 
 332               		.lcomm state,1
331 
 333               		.data
332 
 336               	pitchNeutralTimer:
333 
 337 0000 1900      		.word	25
334 
 338               		.lcomm temp,2
335 
DEFINED SYMBOLS
336 
                            *ABS*:00000000 pitch_neutral.c
337 
  c:\TEMP/ccEXHUa7.s:2      *ABS*:0000003f __SREG__
338 
  c:\TEMP/ccEXHUa7.s:3      *ABS*:0000003e __SP_H__
339 
  c:\TEMP/ccEXHUa7.s:4      *ABS*:0000003d __SP_L__
340 
  c:\TEMP/ccEXHUa7.s:5      *ABS*:00000034 __CCP__
341 
  c:\TEMP/ccEXHUa7.s:6      *ABS*:00000000 __tmp_reg__
342 
  c:\TEMP/ccEXHUa7.s:7      *ABS*:00000001 __zero_reg__
343 
  c:\TEMP/ccEXHUa7.s:13     .text:00000000 pitch_neutral_value
344 
                             .bss:00000000 stickValue
345 
  c:\TEMP/ccEXHUa7.s:329    .bss:00000002 lastStickValue
346 
  c:\TEMP/ccEXHUa7.s:332    .bss:00000007 temp
347 
  c:\TEMP/ccEXHUa7.s:330    .bss:00000004 pitchOffset
348 
  c:\TEMP/ccEXHUa7.s:331    .bss:00000006 state
349 
  c:\TEMP/ccEXHUa7.s:336    .data:00000000 pitchNeutralTimer
350 
 
351 
UNDEFINED SYMBOLS
352 
__do_copy_data
353 
__do_clear_bss
354 
MotorenEin
355 
EE_Parameter
356 
PPM_in
357 
__divmodhi4
358 
beeptime
359 
altcon_start
360 
averageN
361 
altcon_stop
362 
DebugOut