Subversion Repositories FlightCtrl

Rev

Rev 2248 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2248 - 1 
;-----------------------------------------------------------------------------;
2 
; Fast integer squareroot routines for avr-gcc project          (C)ChaN, 2008
3 
; http://elm-chan.org/docs/avrlib/sqrt32.S
4 
;-----------------------------------------------------------------------------;
5 
; uint16_t isqrt32 (uint32_t n);
6 
; uint8_t isqrt16 (uint16_t n);
7 
; uint16_t ihypot (int16_t x, int16_t y);
8 
 
9 
;-----------------------------------------------------------------------------:
10 
; 32bit integer squareroot
11 
;-----------------------------------------------------------------------------;
12 
;   uint16_t isqrt32 (
13 
;     uint32_t n
14 
;   );
15 
;
16 
; Return Value:
17 
;   Squareroot of n.
18 
;
19 
; Size  = 53 words
20 
; Clock = 532..548 cycles
21 
; Stack = 0 byte
22 
 
23 
.global isqrt32
24 
.func isqrt32
25 
 
26 
isqrt32:
27 
	clr	r0
28 
	clr	r18
29 
	clr	r19
30 
	clr	r20
31 
	ldi	r21, 1
32 
	clr	r27
33 
	clr	r30
34 
	clr	r31
35 
	ldi	r26, 16
36 
1:	lsl	r22
37 
	rol	r23
38 
	rol	r24
39 
	rol	r25
40 
	rol	r0
41 
	rol	r18
42 
	rol	r19
43 
	rol	r20
44 
	lsl	r22
45 
	rol	r23
46 
	rol	r24
47 
	rol	r25
48 
	rol	r0
49 
	rol	r18
50 
	rol	r19
51 
	rol	r20
52 
	brpl	2f
53 
	add	r0, r21
54 
	adc	r18, r27
55 
	adc	r19, r30
56 
	adc	r20, r31
57 
	rjmp	3f
58 
2:	sub	r0, r21
59 
	sbc	r18, r27
60 
	sbc	r19, r30
61 
	sbc	r20, r31
62 
3:	lsl	r21
63 
	rol	r27
64 
	rol	r30
65 
	andi	r21, 0b11111000
66 
	ori	r21, 0b00000101
67 
	sbrc	r20, 7
68 
	subi	r21, 2
69 
	dec	r26
70 
	brne	1b
71 
	lsr	r30
72 
	ror	r27
73 
	ror	r21
74 
	lsr	r30
75 
	ror	r27
76 
	ror	r21
77 
	mov	r24, r21
78 
	mov	r25, r27
79 
	ret
80 
.endfunc
81 
 
82 
 
83 
 
84 
;-----------------------------------------------------------------------------:
85 
; 16bit integer squareroot
86 
;-----------------------------------------------------------------------------;
87 
;   uint8_t isqrt16 ( uint16_t n );
88 
;
89 
; Return Value:
90 
;   Squareroot of n.
91 
;
92 
; Size  = 33 words
93 
; Clock = 181..189 cycles
94 
; Stack = 0 byte
95 
 
96 
.global isqrt16
97 
.func isqrt16
98 
 
99 
isqrt16:
100 
	clr	r18
101 
	clr	r19
102 
	ldi	r20, 1
103 
	clr	r21
104 
	ldi	r22, 8
105 
1:	lsl	r24
106 
	rol	r25
107 
	rol	r18
108 
	rol	r19
109 
	lsl	r24
110 
	rol	r25
111 
	rol	r18
112 
	rol	r19
113 
	brpl	2f
114 
	add	r18, r20
115 
	adc	r19, r21
116 
	rjmp	3f
117 
2:	sub	r18, r20
118 
	sbc	r19, r21
119 
3:	lsl	r20
120 
	rol	r21
121 
	andi	r20, 0b11111000
122 
	ori	r20, 0b00000101
123 
	sbrc	r19, 7
124 
	subi	r20, 2
125 
	dec	r22
126 
	brne	1b
127 
	lsr	r21
128 
	ror	r20
129 
	lsr	r21
130 
	ror	r20
131 
	mov	r24, r20
132 
	ret
133 
.endfunc
134 
 
135 
 
136 
 
137 
;-----------------------------------------------------------------------------:
138 
; 16bit integer hypot (megaAVR is required)
139 
;-----------------------------------------------------------------------------;
140 
;   uint16_t ihypot (
141 
;     int16_t x,
142 
;     int16_t y
143 
;   );
144 
;
145 
; Return Value:
146 
;   Squareroot of (x*x + y*y)
147 
;
148 
; Size  = 42 words
149 
; Clock = 581..597 cycles
150 
; Stack = 0 byte
151 
 
152 
.global ihypot
153 
.func ihypot
154 
 
155 
ihypot:
156 
	clr	r26
157 
	sbrs	r25, 7
158 
	rjmp	1f
159 
	com	r24
160 
	com	r25
161 
	adc	r24, r26
162 
	adc	r25, r26
163 
1:	sbrs	r23, 7
164 
	rjmp	2f
165 
	com	r22
166 
	com	r23
167 
	adc	r22, r26
168 
	adc	r23, r26
169 
2:	mul	r22, r22
170 
	movw	r18, r0
171 
	mul	r23, r23
172 
	movw	r20, r0
173 
	mul	r22, r23
174 
	add	r19, r0
175 
	adc	r20, r1
176 
	adc	r21, r26
177 
	add	r19, r0
178 
	adc	r20, r1
179 
	adc	r21, r26
180 
	mul	r24, r24
181 
	movw	r30, r0
182 
	mul	r25, r25
183 
	add	r18, r30
184 
	adc	r19, r31
185 
	adc	r20, r0
186 
	adc	r21, r1
187 
	mul	r24, r25
188 
	add	r19, r0
189 
	adc	r20, r1
190 
	adc	r21, r26
191 
	add	r19, r0
192 
	adc	r20, r1
193 
	adc	r21, r26
194 
	movw	r24, r20
195 
	movw	r22, r18
196 
	clr	r1
197 
	rjmp	isqrt32
198 
.endfunc
199 
 
200 
 
201