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