Root/lib/cordic.c

1/*
2 * Copyright (c) 2011 Broadcom Corporation
3 *
4 * Permission to use, copy, modify, and/or distribute this software for any
5 * purpose with or without fee is hereby granted, provided that the above
6 * copyright notice and this permission notice appear in all copies.
7 *
8 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
9 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
10 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
11 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
12 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
13 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
14 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
15 */
16#include <linux/module.h>
17#include <linux/cordic.h>
18
19#define CORDIC_ANGLE_GEN 39797
20#define CORDIC_PRECISION_SHIFT 16
21#define CORDIC_NUM_ITER (CORDIC_PRECISION_SHIFT + 2)
22
23#define FIXED(X) ((s32)((X) << CORDIC_PRECISION_SHIFT))
24#define FLOAT(X) (((X) >= 0) \
25        ? ((((X) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1) \
26        : -((((-(X)) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1))
27
28static const s32 arctan_table[] = {
29    2949120,
30    1740967,
31    919879,
32    466945,
33    234379,
34    117304,
35    58666,
36    29335,
37    14668,
38    7334,
39    3667,
40    1833,
41    917,
42    458,
43    229,
44    115,
45    57,
46    29
47};
48
49/*
50 * cordic_calc_iq() - calculates the i/q coordinate for given angle
51 *
52 * theta: angle in degrees for which i/q coordinate is to be calculated
53 * coord: function output parameter holding the i/q coordinate
54 */
55struct cordic_iq cordic_calc_iq(s32 theta)
56{
57    struct cordic_iq coord;
58    s32 angle, valtmp;
59    unsigned iter;
60    int signx = 1;
61    int signtheta;
62
63    coord.i = CORDIC_ANGLE_GEN;
64    coord.q = 0;
65    angle = 0;
66
67    theta = FIXED(theta);
68    signtheta = (theta < 0) ? -1 : 1;
69    theta = ((theta + FIXED(180) * signtheta) % FIXED(360)) -
70        FIXED(180) * signtheta;
71
72    if (FLOAT(theta) > 90) {
73        theta -= FIXED(180);
74        signx = -1;
75    } else if (FLOAT(theta) < -90) {
76        theta += FIXED(180);
77        signx = -1;
78    }
79
80    for (iter = 0; iter < CORDIC_NUM_ITER; iter++) {
81        if (theta > angle) {
82            valtmp = coord.i - (coord.q >> iter);
83            coord.q += (coord.i >> iter);
84            angle += arctan_table[iter];
85        } else {
86            valtmp = coord.i + (coord.q >> iter);
87            coord.q -= (coord.i >> iter);
88            angle -= arctan_table[iter];
89        }
90        coord.i = valtmp;
91    }
92
93    coord.i *= signx;
94    coord.q *= signx;
95    return coord;
96}
97EXPORT_SYMBOL(cordic_calc_iq);
98
99MODULE_DESCRIPTION("CORDIC algorithm");
100MODULE_AUTHOR("Broadcom Corporation");
101MODULE_LICENSE("Dual BSD/GPL");
102

Archive Download this file



interactive