Subversion Repositories Projects

Rev

Rev 1199 | Only display areas with differences | Regard 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