Rev 1199 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1199 | Rev 1437 | ||
---|---|---|---|
1 | /**************************************************************************** |
1 | /**************************************************************************** |
2 | * Copyright (C) 2011 by Claas Anders "CaScAdE" Rathje * |
2 | * Copyright (C) 2011-2012 by Claas Anders "CaScAdE" Rathje * |
3 | * admiralcascade@gmail.com * |
3 | * admiralcascade@gmail.com * |
4 | * Project-URL: http://www.mylifesucks.de/oss/c-epilepsy/ * |
4 | * Project-URL: http://www.mylifesucks.de/oss/c-epilepsy/ * |
5 | * * |
5 | * * |
6 | * This program is free software; you can redistribute it and/or modify * |
6 | * This program is free software; you can redistribute it and/or modify * |
7 | * it under the terms of the GNU General Public License as published by * |
7 | * it under the terms of the GNU General Public License as published by * |
8 | * the Free Software Foundation; either version 2 of the License. * |
8 | * the Free Software Foundation; either version 2 of the License. * |
9 | * * |
9 | * * |
10 | * This program is distributed in the hope that it will be useful, * |
10 | * This program is distributed in the hope that it will be useful, * |
11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
13 | * GNU General Public License for more details. * |
13 | * GNU General Public License for more details. * |
14 | * * |
14 | * * |
15 | * You should have received a copy of the GNU General Public License * |
15 | * You should have received a copy of the GNU General Public License * |
16 | * along with this program; if not, write to the * |
16 | * along with this program; if not, write to the * |
17 | * Free Software Foundation, Inc., * |
17 | * Free Software Foundation, Inc., * |
18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
19 | ****************************************************************************/ |
19 | ****************************************************************************/ |
20 | 20 | ||
21 | #include "integer_math.h" |
21 | #include "integer_math.h" |
22 | 22 | ||
23 | /* |
23 | /* |
24 | CORDIC atan2 generated by http://vivara.net/cgi-bin/cordic.cgi?range=3600&n=10&size=int16_t |
24 | CORDIC atan2 generated by http://vivara.net/cgi-bin/cordic.cgi?range=3600&n=10&size=int16_t |
25 | 25 | ||
26 | send bugs to Mark Rages <markrages@gmail.com> |
26 | send bugs to Mark Rages <markrages@gmail.com> |
27 | 27 | ||
28 | result is one revolution per 3600 counts. |
28 | result is one revolution per 3600 counts. |
29 | 29 | ||
30 | Performance test (error as percent of revolution): |
30 | Performance test (error as percent of revolution): |
31 | Maximum: +0.088% |
31 | Maximum: +0.088% |
32 | Mean: 0.006% |
32 | Mean: 0.006% |
33 | Minimum: -0.085% |
33 | Minimum: -0.085% |
34 | mean(abs): 0.029% |
34 | mean(abs): 0.029% |
35 | rms: 0.001% |
35 | rms: 0.001% |
36 | */ |
36 | */ |
37 | 37 | ||
38 | int16_t atan2_fp(int16_t y, int16_t x) { |
38 | int16_t atan2_fp(int16_t y, int16_t x) { |
39 | 39 | ||
40 | const int16_t cordic[] = {450, 266, 140, 71, 36, 18, 9, 4, 2, 1}; |
40 | const int16_t cordic[] = {450, 266, 140, 71, 36, 18, 9, 4, 2, 1}; |
41 | int16_t theta; |
41 | int16_t theta; |
42 | int16_t i; |
42 | int16_t i; |
43 | 43 | ||
44 | if (x < 0) { |
44 | if (x < 0) { |
45 | theta = 1800; |
45 | theta = 1800; |
46 | x = -x; |
46 | x = -x; |
47 | y = -y; |
47 | y = -y; |
48 | } else { |
48 | } else { |
49 | theta = 0; |
49 | theta = 0; |
50 | } |
50 | } |
51 | 51 | ||
52 | for (i = 0; i < 10; i++) { |
52 | for (i = 0; i < 10; i++) { |
53 | int16_t last_x; |
53 | int16_t last_x; |
54 | 54 | ||
55 | last_x = x; |
55 | last_x = x; |
56 | 56 | ||
57 | if (y < 0) { // sign=1 |
57 | if (y < 0) { // sign=1 |
58 | x -= y >> i; |
58 | x -= y >> i; |
59 | y += last_x >> i; |
59 | y += last_x >> i; |
60 | theta -= cordic[i]; |
60 | theta -= cordic[i]; |
61 | } else { |
61 | } else { |
62 | x += y >> i; |
62 | x += y >> i; |
63 | y -= last_x >> i; |
63 | y -= last_x >> i; |
64 | theta += cordic[i]; |
64 | theta += cordic[i]; |
65 | } |
65 | } |
66 | } |
66 | } |
67 | 67 | ||
68 | #ifdef CORDIC_DEBUG |
68 | #ifdef CORDIC_DEBUG |
69 | printf("%d", theta); |
69 | printf("%d", theta); |
70 | printf(" %lld\n", ((long long int)x) * 155L / 256); |
70 | printf(" %lld\n", ((long long int)x) * 155L / 256); |
71 | #endif |
71 | #endif |
72 | 72 | ||
73 | return theta % 3600; |
73 | return theta % 3600; |
74 | } |
74 | } |
75 | 75 |