← Back to branch summary

~ubuntu-branches/ubuntu/breezy/avr-libc/breezy

« back to all changes in this revision

Viewing changes to libm/fplib/atan2.S

  • Committer: Bazaar Package Importer
  • Author(s): Hakan Ardo
  • Date: 2002-04-15 14:53:38 UTC
  • Revision ID: james.westby@ubuntu.com-20020415145338-c8hi0tn5bx74w7o3
Tags: upstream-20020203
Import upstream version 20020203

Show diffs side-by-side

added added

removed removed

Lines of Context:
libm/fplib/atan2.S
 
1
/* ---------------------------------------------------------------------------------------
 
2
   atan2.S
 
3
 
 
4
   Contributors:
 
5
     Created by Reiner Patommel
 
6
 
 
7
   THIS SOFTWARE IS NOT COPYRIGHTED
 
8
 
 
9
   This source code is offered for use in the public domain.  You may
 
10
   use, modify or distribute it freely.
 
11
 
 
12
   This code is distributed in the hope that it will be useful, but
 
13
   WITHOUT ANY WARRANTY.  ALL WARRANTIES, EXPRESS OR IMPLIED ARE HEREBY
 
14
   DISCLAIMED.  This includes but is not limited to warranties of
 
15
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 
16
 
 
17
 
 
18
 -*- Mode: Asm -*-
 
19
 
 
20
*----------------------------------------------------------------------------------------
 
21
* A = atan2(float A, float B)
 
22
*---------------------------------------------------------------------------------------*/
 
23
 
 
24
#include "gasava.inc"
 
25
#include "fplib.inc"
 
26
#include "macros.inc"
 
27
#define Y_hi    r25
 
28
#define Y_lo    r24
 
29
#define X_hi    r23
 
30
#define X_lo    r22
 
31
#define Z_hi    r21
 
32
#define Z_lo    r20
 
33
#define tmp_hi  r19
 
34
#define tmp_lo  r18
 
35
#define sign    r26
 
36
#define lcount  r26
 
37
 
 
38
    TEXT_SEG(fplib, atan2)
 
39
    FUNCTION(atan2)
 
40
 
 
41
GLOBAL(atan2)
 
42
    CP      rB3, __zero_reg__       ; if (y != 0)
 
43
    BRNE    _atan2_normalize        ;   calculate
 
44
    CP      rA3, __zero_reg__       ; if (y == 0 && x == 0)
 
45
    BREQ    _atan2_NAN              ;   return Argument Error
 
46
    SBRC    rA3, 7                  ; if (y == 0 && x < 0)
 
47
    RJMP    _atan2_PI               ;   return PI
 
48
    XJMP    _U(__fp_zero)           ; if (x == 0 && y == 0) return 0
 
49
 
 
50
_atan2_PI:
 
51
    LDI     rA3, 0x40
 
52
    LDI     rA2, 0x49
 
53
    LDI     rA1, 0x0F
 
54
    LDI     rA0, 0xDB               ; load PI
 
55
    RET
 
56
_atan2_NAN:
 
57
    XJMP    _U(__fp_nanERANGE)
 
58
 
 
59
_atan2_normalize:                   ; normalize x and y to be LESS or EQUAL 1
 
60
    CLR     sign
 
61
    SBRC    rA3, 7
 
62
    ORI     sign, 1                 ; remember sign of x (bit 0 set: x < 0)
 
63
    SBRC    rB3, 7
 
64
    ORI     sign, 2                 ; remember sign of y (bit 1 set: y < 0)
 
65
    PUSH    sign
 
66
    ANDI    rA3, 0x7F               ; make |x|
 
67
    ANDI    rB3, 0x7F               ; make |y|
 
68
    CP      rA0, rB0
 
69
    CPC     rA1, rB1
 
70
    CPC     rA2, rB2
 
71
    CPC     rA3, rB3                ; |x| >= |y| ?
 
72
    BRGE    _atan2_X_GT
 
73
_atan2_Y_GT:
 
74
    RCALL   _atan2_scale            ; rA3:rA2 = int(28140 * x / y)
 
75
    MOV     X_hi, rA3
 
76
    MOV     X_lo, rA2               ; X = int(28140 * x / y)
 
77
    LDI     Y_hi, 0x6D
 
78
    LDI     Y_lo, 0xEC              ; Y = 28140
 
79
    RJMP    _atan2_cordic_loop_init
 
80
_atan2_X_GT:
 
81
    EOR     rA3, rB3
 
82
    EOR     rB3, rA3
 
83
    EOR     rA3, rB3
 
84
    EOR     rA2, rB2
 
85
    EOR     rB2, rA2
 
86
    EOR     rA2, rB2
 
87
    EOR     rA1, rB1
 
88
    EOR     rB1, rA1
 
89
    EOR     rA1, rB1
 
90
    EOR     rA0, rB0
 
91
    EOR     rB0, rA0
 
92
    EOR     rA0, rB0                ; swap A and B
 
93
    RCALL   _atan2_scale            ; Y = int(28140 * y / x)
 
94
    LDI     X_hi, 0x6D
 
95
    LDI     X_lo, 0xEC              ; X = 28140
 
96
 
 
97
_atan2_cordic_loop_init:            ; run cordic in vectoring mode
 
98
    CLR     Z_hi                    ;  i.e. drive Y to zero and accumulate Z
 
99
    CLR     Z_lo                    ; Z = 0
 
100
    CLR     lcount                  ; lcount = 0
 
101
    LDI     ZL, LOW(table_atan2)
 
102
    LDI     ZH, HIGH(table_atan2)   ; set pointer to table
 
103
_atan2_cordic_loop:
 
104
    CPI     lcount, 15              ; lcount >= 15 ?
 
105
    BRLO    _atan2_cordic_loop_2
 
106
    RJMP    _atan2_finish           ; ok done with cordic
 
107
_atan2_cordic_loop_2:
 
108
    PUSH    Y_hi
 
109
    PUSH    Y_lo                    ; save Y
 
110
    MOV     rA3, X_hi
 
111
    MOV     rA2, X_lo
 
112
    CLT                             ; following shift must be "logical shift"
 
113
    RCALL   _atan2_shift            ; rA3:rA2 = X >> lcount
 
114
    MOV     tmp_hi, rA3
 
115
    MOV     tmp_lo, rA2             ; tmp = X >> lcount
 
116
    POP     Y_lo
 
117
    POP     Y_hi                    ; restore Y
 
118
    PUSH    Y_hi
 
119
    PUSH    Y_lo                    ; save Y
 
120
    BST     rA3, 7                  ; following shift must be "arithmetic shift"
 
121
    RCALL   _atan2_shift            ; rA3:rA2 = Y >> lcount
 
122
    CP      Y_lo, __zero_reg__
 
123
    CPC     Y_hi, __zero_reg__      ; Y < 0 ?
 
124
    BRLT    _atan2_Y_neg
 
125
_atan2_Y_pos:
 
126
    ADD     X_lo, rA2
 
127
    ADC     X_hi, rA3               ; X = X + Y >> lcount
 
128
    POP     Y_lo
 
129
    POP     Y_hi                    ; restore Y
 
130
    SUB     Y_lo, tmp_lo
 
131
    SBC     Y_hi, tmp_hi            ; Y = Y - tmp >> lcount
 
132
    LPMRdZpp(tmp_lo)                ; tmp = Rad[lcount]
 
133
    LPMRdZpp(tmp_hi)
 
134
    ADD     Z_lo, tmp_lo
 
135
    ADC     Z_hi, tmp_hi            ; Z = Z + Rad[lcount]
 
136
    INC     lcount                  ; lcount++
 
137
    RJMP    _atan2_cordic_loop
 
138
_atan2_Y_neg:
 
139
    SUB     X_lo, rA2
 
140
    SBC     X_hi, rA3               ; X = X - Y >> lcount
 
141
    POP     Y_lo
 
142
    POP     Y_hi                    ; restore Y
 
143
    ADD     Y_lo, tmp_lo
 
144
    ADC     Y_hi, tmp_hi            ; Y = Y + tmp >> lcount
 
145
    LPMRdZpp(tmp_lo)
 
146
    LPMRdZpp(tmp_hi)                ; tmp = Rad[lcount]
 
147
    SUB     Z_lo, tmp_lo
 
148
    SBC     Z_hi, tmp_hi            ; Z = Z - Rad[lcount]
 
149
    INC     lcount                  ; lcount++
 
150
    RJMP    _atan2_cordic_loop
 
151
 
 
152
_atan2_shift:                       ; A = A >> lcount
 
153
    MOV     __tmp_reg__, lcount     ; get current loop count
 
154
_atan2_shift_loop:
 
155
    TST     __tmp_reg__
 
156
    BREQ    _atan2_shift_done       ; we are done
 
157
    LSR     rA3
 
158
    ROR     rA2
 
159
    BLD     rA3, 7                  ;  LSR for "unsigned" int. ASR for "signed" int
 
160
    DEC     __tmp_reg__
 
161
    RJMP    _atan2_shift_loop
 
162
_atan2_shift_done:
 
163
    RET
 
164
 
 
165
_atan2_scale:                       ; A = int(28140 * A / B)
 
166
    XCALL   _U(__divsf3)            ; A = A / B
 
167
    RCALL   _atan2_scale_load       ; B = 28140
 
168
    XCALL   _U(__mulsf3)            ; A = 28140 * A / B
 
169
    XCALL   _U(__fixsfsi)           ; A = (long int)(28140 * A / B)
 
170
    MOV     Y_hi, X_hi              ;  but, we have a short int
 
171
    MOV     Y_lo, X_lo
 
172
    RET
 
173
 
 
174
_atan2_scale_load:                  ; B = 28140.0
 
175
    LDI     rB3, 0x46
 
176
    LDI     rB2, 0xDB
 
177
    LDI     rB1, 0xD8
 
178
    CLR     rB0
 
179
    RET
 
180
 
 
181
_atan2_finish:                      ; convert to float and clean-up
 
182
    MOV     rA1, Z_hi
 
183
    MOV     rA0, Z_lo
 
184
    CLR     rA3
 
185
    CLR     rA2
 
186
    XCALL   _U(__floatsisf)         ; A = (float)A
 
187
    POP     sign
 
188
    PUSH    sign
 
189
    SBRC    sign, 0                 ; x < 0 ?
 
190
    RJMP    _atan2_xneg
 
191
_atan2_xpos:                        ; here: x > 0
 
192
    RCALL   _atan2_scale_load       ; B = 28140.0
 
193
    XCALL   _U(__divsf3)            ; A = A / B
 
194
    POP     sign
 
195
    SBRC    sign, 1                 ; y < 0 ?
 
196
    RJMP    _atan2_yneg
 
197
    RET                             ; return (A / B)
 
198
_atan2_xneg:                        ; here: x < 0
 
199
    LDI     rB3, 0x47
 
200
    LDI     rB2, 0xAC
 
201
    LDI     rB1, 0xAA
 
202
    LDI     rB0, 0x35               ; load M_PI * 28140.0
 
203
    ORI     rA3, 0x80               ; negate A
 
204
    XCALL   _U(__addsf3)            ; A = -A + B = B - A
 
205
    RCALL   _atan2_scale_load       ; B = 28140.0
 
206
    XCALL   _U(__divsf3)            ; A = A / B
 
207
    POP sign
 
208
    SBRC    sign, 1                 ; y < 0 ?
 
209
    RJMP    _atan2_yneg
 
210
    RET                             ; return (A / B)
 
211
_atan2_yneg:                        ; here: y < 0
 
212
    ORI     rA3, 0x80               ; negate A
 
213
    RET                             ; return -(A / B)
 
214
          ENDFUNC
 
215
 
 
216
; --------------------------------------------------------------------------------------
 
217
table_atan2:
 
218
    .word 22101, 13047, 6893, 3499, 1756, 879, 439, 219, 109, 54, 27, 13, 6, 3, 1
 
219
; --------------------------------------------------------------------------------------
 
220