summaryrefslogtreecommitdiff
path: root/apps/plugins/lib
diff options
context:
space:
mode:
Diffstat (limited to 'apps/plugins/lib')
-rw-r--r--apps/plugins/lib/fixedpoint.c239
-rw-r--r--apps/plugins/lib/fixedpoint.h45
2 files changed, 41 insertions, 243 deletions
diff --git a/apps/plugins/lib/fixedpoint.c b/apps/plugins/lib/fixedpoint.c
index 0ae2cded69..352e246673 100644
--- a/apps/plugins/lib/fixedpoint.c
+++ b/apps/plugins/lib/fixedpoint.c
@@ -1,238 +1 @@
1/*************************************************************************** #include "../../fixedpoint.c"
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2006 Jens Arnold
11 *
12 * Fixed point library for plugins
13 *
14 * This program is free software; you can redistribute it and/or
15 * modify it under the terms of the GNU General Public License
16 * as published by the Free Software Foundation; either version 2
17 * of the License, or (at your option) any later version.
18 *
19 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
20 * KIND, either express or implied.
21 *
22 ****************************************************************************/
23
24#include <inttypes.h>
25#include "plugin.h"
26#include "fixedpoint.h"
27
28/* Inverse gain of circular cordic rotation in s0.31 format. */
29static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */
30
31/* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */
32static const unsigned long atan_table[] = {
33 0x1fffffff, /* +0.785398163 (or pi/4) */
34 0x12e4051d, /* +0.463647609 */
35 0x09fb385b, /* +0.244978663 */
36 0x051111d4, /* +0.124354995 */
37 0x028b0d43, /* +0.062418810 */
38 0x0145d7e1, /* +0.031239833 */
39 0x00a2f61e, /* +0.015623729 */
40 0x00517c55, /* +0.007812341 */
41 0x0028be53, /* +0.003906230 */
42 0x00145f2e, /* +0.001953123 */
43 0x000a2f98, /* +0.000976562 */
44 0x000517cc, /* +0.000488281 */
45 0x00028be6, /* +0.000244141 */
46 0x000145f3, /* +0.000122070 */
47 0x0000a2f9, /* +0.000061035 */
48 0x0000517c, /* +0.000030518 */
49 0x000028be, /* +0.000015259 */
50 0x0000145f, /* +0.000007629 */
51 0x00000a2f, /* +0.000003815 */
52 0x00000517, /* +0.000001907 */
53 0x0000028b, /* +0.000000954 */
54 0x00000145, /* +0.000000477 */
55 0x000000a2, /* +0.000000238 */
56 0x00000051, /* +0.000000119 */
57 0x00000028, /* +0.000000060 */
58 0x00000014, /* +0.000000030 */
59 0x0000000a, /* +0.000000015 */
60 0x00000005, /* +0.000000007 */
61 0x00000002, /* +0.000000004 */
62 0x00000001, /* +0.000000002 */
63 0x00000000, /* +0.000000001 */
64 0x00000000, /* +0.000000000 */
65};
66
67/* Precalculated sine and cosine * 16384 (2^14) (fixed point 18.14) */
68static const short sin_table[91] =
69{
70 0, 285, 571, 857, 1142, 1427, 1712, 1996, 2280, 2563,
71 2845, 3126, 3406, 3685, 3963, 4240, 4516, 4790, 5062, 5334,
72 5603, 5871, 6137, 6401, 6663, 6924, 7182, 7438, 7691, 7943,
73 8191, 8438, 8682, 8923, 9161, 9397, 9630, 9860, 10086, 10310,
74 10531, 10748, 10963, 11173, 11381, 11585, 11785, 11982, 12175, 12365,
75 12550, 12732, 12910, 13084, 13254, 13420, 13582, 13740, 13894, 14043,
76 14188, 14329, 14466, 14598, 14725, 14848, 14967, 15081, 15190, 15295,
77 15395, 15491, 15582, 15668, 15749, 15825, 15897, 15964, 16025, 16082,
78 16135, 16182, 16224, 16261, 16294, 16321, 16344, 16361, 16374, 16381,
79 16384
80};
81
82/**
83 * Implements sin and cos using CORDIC rotation.
84 *
85 * @param phase has range from 0 to 0xffffffff, representing 0 and
86 * 2*pi respectively.
87 * @param cos return address for cos
88 * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX,
89 * representing -1 and 1 respectively.
90 */
91long fsincos(unsigned long phase, long *cos)
92{
93 int32_t x, x1, y, y1;
94 unsigned long z, z1;
95 int i;
96
97 /* Setup initial vector */
98 x = cordic_circular_gain;
99 y = 0;
100 z = phase;
101
102 /* The phase has to be somewhere between 0..pi for this to work right */
103 if (z < 0xffffffff / 4) {
104 /* z in first quadrant, z += pi/2 to correct */
105 x = -x;
106 z += 0xffffffff / 4;
107 } else if (z < 3 * (0xffffffff / 4)) {
108 /* z in third quadrant, z -= pi/2 to correct */
109 z -= 0xffffffff / 4;
110 } else {
111 /* z in fourth quadrant, z -= 3pi/2 to correct */
112 x = -x;
113 z -= 3 * (0xffffffff / 4);
114 }
115
116 /* Each iteration adds roughly 1-bit of extra precision */
117 for (i = 0; i < 31; i++) {
118 x1 = x >> i;
119 y1 = y >> i;
120 z1 = atan_table[i];
121
122 /* Decided which direction to rotate vector. Pivot point is pi/2 */
123 if (z >= 0xffffffff / 4) {
124 x -= y1;
125 y += x1;
126 z -= z1;
127 } else {
128 x += y1;
129 y -= x1;
130 z += z1;
131 }
132 }
133
134 if (cos)
135 *cos = x;
136
137 return y;
138}
139
140/**
141 * Fixed point square root via Newton-Raphson.
142 * @param a square root argument.
143 * @param fracbits specifies number of fractional bits in argument.
144 * @return Square root of argument in same fixed point format as input.
145 */
146long fsqrt(long a, unsigned int fracbits)
147{
148 long b = a/2 + BIT_N(fracbits); /* initial approximation */
149 unsigned n;
150 const unsigned iterations = 4;
151
152 for (n = 0; n < iterations; ++n)
153 b = (b + (long)(((long long)(a) << fracbits)/b))/2;
154
155 return b;
156}
157
158/**
159 * Fixed point sinus using a lookup table
160 * don't forget to divide the result by 16384 to get the actual sinus value
161 * @param val sinus argument in degree
162 * @return sin(val)*16384
163 */
164long sin_int(int val)
165{
166 val = (val+360)%360;
167 if (val < 181)
168 {
169 if (val < 91)/* phase 0-90 degree */
170 return (long)sin_table[val];
171 else/* phase 91-180 degree */
172 return (long)sin_table[180-val];
173 }
174 else
175 {
176 if (val < 271)/* phase 181-270 degree */
177 return -(long)sin_table[val-180];
178 else/* phase 270-359 degree */
179 return -(long)sin_table[360-val];
180 }
181 return 0;
182}
183
184/**
185 * Fixed point cosinus using a lookup table
186 * don't forget to divide the result by 16384 to get the actual cosinus value
187 * @param val sinus argument in degree
188 * @return cos(val)*16384
189 */
190long cos_int(int val)
191{
192 val = (val+360)%360;
193 if (val < 181)
194 {
195 if (val < 91)/* phase 0-90 degree */
196 return (long)sin_table[90-val];
197 else/* phase 91-180 degree */
198 return -(long)sin_table[val-90];
199 }
200 else
201 {
202 if (val < 271)/* phase 181-270 degree */
203 return -(long)sin_table[270-val];
204 else/* phase 270-359 degree */
205 return (long)sin_table[val-270];
206 }
207 return 0;
208}
209
210/**
211 * Fixed-point natural log
212 * taken from http://www.quinapalus.com/efunc.html
213 * "The code assumes integers are at least 32 bits long. The (positive)
214 * argument and the result of the function are both expressed as fixed-point
215 * values with 16 fractional bits, although intermediates are kept with 28
216 * bits of precision to avoid loss of accuracy during shifts."
217 */
218
219long flog(int x) {
220 long t,y;
221
222 y=0xa65af;
223 if(x<0x00008000) x<<=16, y-=0xb1721;
224 if(x<0x00800000) x<<= 8, y-=0x58b91;
225 if(x<0x08000000) x<<= 4, y-=0x2c5c8;
226 if(x<0x20000000) x<<= 2, y-=0x162e4;
227 if(x<0x40000000) x<<= 1, y-=0x0b172;
228 t=x+(x>>1); if((t&0x80000000)==0) x=t,y-=0x067cd;
229 t=x+(x>>2); if((t&0x80000000)==0) x=t,y-=0x03920;
230 t=x+(x>>3); if((t&0x80000000)==0) x=t,y-=0x01e27;
231 t=x+(x>>4); if((t&0x80000000)==0) x=t,y-=0x00f85;
232 t=x+(x>>5); if((t&0x80000000)==0) x=t,y-=0x007e1;
233 t=x+(x>>6); if((t&0x80000000)==0) x=t,y-=0x003f8;
234 t=x+(x>>7); if((t&0x80000000)==0) x=t,y-=0x001fe;
235 x=0x80000000-x;
236 y-=x>>15;
237 return y;
238}
diff --git a/apps/plugins/lib/fixedpoint.h b/apps/plugins/lib/fixedpoint.h
index dfabbad8dc..ef50dd0085 100644
--- a/apps/plugins/lib/fixedpoint.h
+++ b/apps/plugins/lib/fixedpoint.h
@@ -21,11 +21,44 @@
21 * 21 *
22 ****************************************************************************/ 22 ****************************************************************************/
23 23
24long fsincos(unsigned long phase, long *cos); 24/** PLUGINS - FIXED POINT MATH ROUTINES - USAGE
25long fsqrt(long a, unsigned int fracbits); 25 *
26long cos_int(int val); 26 * - x and y arguments are fixed point integers
27long sin_int(int val); 27 * - fracbits is the number of fractional bits in the argument(s)
28long flog(int x); 28 * - functions return long fixed point integers with the specified number
29 * of fractional bits unless otherwise specified
30 *
31 * Calculate sin and cos of an angle:
32 * fp_sincos(phase, *cos)
33 * where phase is a 32 bit unsigned integer with 0 representing 0
34 * and 0xFFFFFFFF representing 2*pi, and *cos is the address to
35 * a long signed integer. Value returned is a long signed integer
36 * from LONG_MIN to LONG_MAX, representing -1 to 1 respectively.
37 * That is, value is a fixed point integer with 31 fractional bits.
38 *
39 * Take square root of a fixed point number:
40 * fp_sqrt(x, fracbits)
41 *
42 * Calculate sin or cos of an angle (very fast, from a table):
43 * fp14_sin(angle)
44 * fp14_cos(angle)
45 * where angle is a non-fixed point integer in degrees. Value
46 * returned is a fixed point integer with 14 fractional bits.
47 *
48 * Calculate the natural log of a positive fixed point integer
49 * fp16_log(x)
50 * where x and the value returned are fixed point integers
51 * with 16 fractional bits.
52 */
53
54#ifndef _FIXEDPOINT_H_PLUGINS
55#define _FIXEDPOINT_H_PLUGINS
56
57long fp_sincos(unsigned long phase, long *cos);
58long fp_sqrt(long a, unsigned int fracbits);
59long fp14_cos(int val);
60long fp14_sin(int val);
61long fp16_log(int x);
29 62
30/* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit, 63/* fast unsigned multiplication (16x16bit->32bit or 32x32bit->32bit,
31 * whichever is faster for the architecture) */ 64 * whichever is faster for the architecture) */
@@ -34,3 +67,5 @@ long flog(int x);
34#else /* SH1, coldfire */ 67#else /* SH1, coldfire */
35#define FMULU(a, b) ((uint32_t) (((uint16_t) (a)) * ((uint16_t) (b)))) 68#define FMULU(a, b) ((uint32_t) (((uint16_t) (a)) * ((uint16_t) (b))))
36#endif 69#endif
70
71#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-05 13:23:55 +0000