Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1538 killagreg 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 (
88 
;     uint16_t n
89 
;   );
90 
;
91 
; Return Value:
92 
;   Squareroot of n.
93 
;
94 
; Size  = 33 words
95 
; Clock = 181..189 cycles
96 
; Stack = 0 byte
97 
 
98 
.global isqrt16
99 
.func isqrt16
100 
 
101 
isqrt16:
102 
	clr	r18
103 
	clr	r19
104 
	ldi	r20, 1
105 
	clr	r21
106 
	ldi	r22, 8
107 
1:	lsl	r24
108 
	rol	r25
109 
	rol	r18
110 
	rol	r19
111 
	lsl	r24
112 
	rol	r25
113 
	rol	r18
114 
	rol	r19
115 
	brpl	2f
116 
	add	r18, r20
117 
	adc	r19, r21
118 
	rjmp	3f
119 
2:	sub	r18, r20
120 
	sbc	r19, r21
121 
3:	lsl	r20
122 
	rol	r21
123 
	andi	r20, 0b11111000
124 
	ori	r20, 0b00000101
125 
	sbrc	r19, 7
126 
	subi	r20, 2
127 
	dec	r22
128 
	brne	1b
129 
	lsr	r21
130 
	ror	r20
131 
	lsr	r21
132 
	ror	r20
133 
	mov	r24, r20
134 
	ret
135 
.endfunc
136 
 
137 
 
138 
 
139 
;-----------------------------------------------------------------------------:
140 
; 16bit integer hypot (megaAVR is required)
141 
;-----------------------------------------------------------------------------;
142 
;   uint16_t ihypot (
143 
;     int16_t x,
144 
;     int16_t y
145 
;   );
146 
;
147 
; Return Value:
148 
;   Squareroot of (x*x + y*y)
149 
;
150 
; Size  = 42 words
151 
; Clock = 581..597 cycles
152 
; Stack = 0 byte
153 
 
154 
.global ihypot
155 
.func ihypot
156 
 
157 
ihypot:
158 
	clr	r26
159 
	sbrs	r25, 7
160 
	rjmp	1f
161 
	com	r24
162 
	com	r25
163 
	adc	r24, r26
164 
	adc	r25, r26
165 
1:	sbrs	r23, 7
166 
	rjmp	2f
167 
	com	r22
168 
	com	r23
169 
	adc	r22, r26
170 
	adc	r23, r26
171 
2:	mul	r22, r22
172 
	movw	r18, r0
173 
	mul	r23, r23
174 
	movw	r20, r0
175 
	mul	r22, r23
176 
	add	r19, r0
177 
	adc	r20, r1
178 
	adc	r21, r26
179 
	add	r19, r0
180 
	adc	r20, r1
181 
	adc	r21, r26
182 
	mul	r24, r24
183 
	movw	r30, r0
184 
	mul	r25, r25
185 
	add	r18, r30
186 
	adc	r19, r31
187 
	adc	r20, r0
188 
	adc	r21, r1
189 
	mul	r24, r25
190 
	add	r19, r0
191 
	adc	r20, r1
192 
	adc	r21, r26
193 
	add	r19, r0
194 
	adc	r20, r1
195 
	adc	r21, r26
196 
	movw	r24, r20
197 
	movw	r22, r18
198 
	clr	r1
199 
	rjmp	isqrt32
200 
.endfunc
201 
 
202 
 
203