Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | 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