Subversion Repositories Projects

Rev

Rev 1199 | Details | Compare with Previous | Last modification | View Log | RSS feed

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