apps/plugins/lib/fixedpoint.c

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