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  * 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. */
  29 static 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 */
  32 static 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) */
  68 static 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  */
  91 long 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  */
 146 long 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  */
 164 long 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  */
 190 long 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
 219 long 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 }