CORDIC Arctg

Алгоритмы определения функции arctg различными методами



#include <vcl.h>
#pragma hdrstop


#include "Unit1.h"
#include "math.h"
#include "fixed_math.hpp"
 //---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma resource "*.dfm"
TForm1 *Form1;


Q15 cordic(Q15* x0, Q15* y0, Q15* z0, Q15 vecmode);
short zzz[16];
const int MAXBITS=15;
//---------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner)
        : TForm(Owner)
{
}
//---------------------------------------------------------------------------
static float atanTable[MAXBITS];

void initCordic()
{

    float t = 1.0;
    int i;
    for (i = 0; i < MAXBITS; ++i)
    {
        atanTable[i] = atan(t);
        t /= 2;                
    }

}


/* CORDIC m=1, y-->0 */


Q15 cordic(Q15* x0, Q15* y0, Q15* z0, Q15 vecmode)
{
    AnsiString buf;
    short t;
    Q15 x, y, z;
    int i;

    t = 1.0;
    x = *x0; y = *y0; z = *z0;

    Q15 x1;

    for (i = 0; i < MAXBITS; ++i)
    {

           if (vecmode >= 0.0f && y < vecmode || vecmode<0.0f  && z >= 0.0f)
        {
       

            x1 = x - y>>t;
             y = y + x>>t;
             z = z - Q15(atanTable[i]);
                    }
        else

        {
            x1 = x + y>>t;
              y = y - x>>t;
              z = z + Q15(atanTable[i]);
         
        }

        x = x1;

    }

    *x0 = x;
    *y0 = y;
    *z0 = z;

    return z;        

}

Q15 atanCordic(Q15 a)
{

    Q15 x = 1.0f;
    Q15 z = 0.0f;
    return cordic(&x, &a, &z, 0.0f);

}

void __fastcall TForm1::Button1Click(TObject *Sender)
{



    AnsiString buf;
    float x;
    float y;


    initCordic();

    for (x=0.0;x<=1.0;x=x+0.1)
    {
 
     Q15 v = atanCordic(Q15(x));
         y = atan(x);

        Memo1->Lines->Add(buf.sprintf("%5.2f", x));
        Memo2->Lines->Add(buf.sprintf("%f",Q15ToShort(v)/32768.0f));
        Memo3->Lines->Add(buf.sprintf("%10.6f",Q15ToShort(y)/32768.0f));
     

    }
 
}


//---------------------------------------------------------------------------


#ifndef _CORDIC_32_
#define _CORDIC_32_

//Constants
#define cordic_1K 0x26DD3B6A    // 1/k = 0.6072529350088812561694
#define half_pi 0x6487ED51      // pi / 2
#define MUL 1073741824.000000   // 1.0 = 1073741824
#define CORDIC_NTAB 32
int cordic_tab [] = {
    0x3243F6A8, 0x1DAC6705, 0x0FADBAFC, 0x07F56EA6, 0x03FEAB76, 0x01FFD55B, 0x00FFFAAA, 0x007FFF55,
    0x003FFFEA, 0x001FFFFD, 0x000FFFFF, 0x0007FFFF, 0x0003FFFF, 0x0001FFFF, 0x0000FFFF, 0x00007FFF,
    0x00003FFF, 0x00001FFF, 0x00000FFF, 0x000007FF, 0x000003FF, 0x000001FF, 0x000000FF, 0x0000007F,
    0x0000003F, 0x0000001F, 0x0000000F, 0x00000008, 0x00000004, 0x00000002, 0x00000001, 0x00000000
};

void cordic32_sin_cos(int alpha, int *s, int *c, int n)
{
    int k, d, tx;
    int z = alpha;
    *c = cordic_1K, *s = 0;
    n = (n > CORDIC_NTAB) ? CORDIC_NTAB : n;
    if(n<3) n = 3;
    for (k=0; k<n; ++k)
    {
        d = z >> 31;    //d = z>=0 ? 0 : -1;
        tx = *c;
        *c -= (((*s>>k) ^ d) - d);
        *s += (((tx>>k) ^ d) - d);
        z -= ((cordic_tab[k] ^ d) - d);
    }
}

// y = s, x = c
int cordic32_atan2(int s, int c, int n)
{
    int k, d, angle = 0;
    int re = c, im = s, tx;
    n = (n > CORDIC_NTAB) ? CORDIC_NTAB : n;
    if(n<3) n = 3;
    for (k=0; k<n; ++k)
    {
        d = im >> 31;
        angle += ((cordic_tab[k] ^ d) - d);
        tx = re;
        re += (((im>>k) ^ d) - d);
        im -= (((tx>>k) ^ d) - d);
    }
    return angle;
}

#endif