T536_DTU/app_public/fuxi_public/fuxi_bsp/source/calculate.c

/******************************************************************************
版权所有：
文件名称：	calculate.c
文件版本：	01.01
创建作者:	sunxi
创建日期：	2008-08-25
功能说明：	算术计算公式。
其它说明：	Q16定点数定义----如果一个32bits数,高16代表整数部分,低16位代表小数部分,
            我们就将它称为Q16定点数(Qxx代表xx位小数).
修改记录：
*/

/*------------------------------- 头文件 --------------------------------------
 */
#ifndef WIN32 // VC6.0 中定义的宏
#include "bspconfig.h"
#include "calculate.h"
#define __int64 long long
#endif

#include "rt.h"

/*------------------------------- 宏定义 --------------------------------------
 */
#if CFG_BSP_DEBUG

#define _CAL_DEBUG

#endif

/*------------------------------ 全局变量 -------------------------------------
 */
const long asin_table[257] =
    {
        0x00000000,
        0x0000394B,
        0x00007297,
        0x0000ABE4,
        0x0000E531,
        0x00011E7F,
        0x000157CE,
        0x0001911E,
        0x0001CA70,
        0x000203C4,
        0x00023D1A,
        0x00027672,
        0x0002AFCD,
        0x0002E92A,
        0x0003228A,
        0x00035BED,
        0x00039554,
        0x0003CEBE,
        0x0004082C,
        0x0004419F,
        0x00047B15,
        0x0004B490,
        0x0004EE10,
        0x00052795,
        0x0005611E,
        0x00059AAE,
        0x0005D443,
        0x00060DDE,
        0x0006477F,
        0x00068126,
        0x0006BAD5,
        0x0006F489,
        0x00072E46,
        0x00076809,
        0x0007A1D4,
        0x0007DBA7,
        0x00081581,
        0x00084F65,
        0x00088950,
        0x0008C345,
        0x0008FD42,
        0x00093749,
        0x00097159,
        0x0009AB74,
        0x0009E598,
        0x000A1FC6,
        0x000A59FF,
        0x000A9443,
        0x000ACE92,
        0x000B08EC,
        0x000B4352,
        0x000B7DC4,
        0x000BB842,
        0x000BF2CC,
        0x000C2D63,
        0x000C6807,
        0x000CA2B8,
        0x000CDD76,
        0x000D1843,
        0x000D531D,
        0x000D8E06,
        0x000DC8FD,
        0x000E0403,
        0x000E3F19,
        0x000E7A3E,
        0x000EB572,
        0x000EF0B7,
        0x000F2C0C,
        0x000F6772,
        0x000FA2E9,
        0x000FDE71,
        0x00101A0B,
        0x001055B6,
        0x00109174,
        0x0010CD45,
        0x00110928,
        0x0011451F,
        0x00118129,
        0x0011BD46,
        0x0011F979,
        0x001235BF,
        0x0012721B,
        0x0012AE8C,
        0x0012EB12,
        0x001327AE,
        0x00136461,
        0x0013A12A,
        0x0013DE0A,
        0x00141B02,
        0x00145812,
        0x00149539,
        0x0014D27A,
        0x00150FD3,
        0x00154D45,
        0x00158AD2,
        0x0015C878,
        0x00160639,
        0x00164415,
        0x0016820C,
        0x0016C01F,
        0x0016FE4F,
        0x00173C9B,
        0x00177B04,
        0x0017B98B,
        0x0017F830,
        0x001836F3,
        0x001875D5,
        0x0018B4D7,
        0x0018F3F9,
        0x0019333B,
        0x0019729E,
        0x0019B222,
        0x0019F1C9,
        0x001A3192,
        0x001A717E,
        0x001AB18D,
        0x001AF1C1,
        0x001B3219,
        0x001B7297,
        0x001BB33A,
        0x001BF404,
        0x001C34F4,
        0x001C760C,
        0x001CB74D,
        0x001CF8B6,
        0x001D3A48,
        0x001D7C05,
        0x001DBDED,
        0x001E0000,
        0x001E423F,
        0x001E84AA,
        0x001EC744,
        0x001F0A0B,
        0x001F4D02,
        0x001F9028,
        0x001FD37F,
        0x00201707,
        0x00205AC1,
        0x00209EAD,
        0x0020E2CE,
        0x00212723,
        0x00216BAE,
        0x0021B06F,
        0x0021F566,
        0x00223A97,
        0x00228000,
        0x0022C5A3,
        0x00230B81,
        0x0023519B,
        0x002397F2,
        0x0023DE87,
        0x0024255B,
        0x00246C6F,
        0x0024B3C4,
        0x0024FB5C,
        0x00254338,
        0x00258B58,
        0x0025D3BF,
        0x00261C6D,
        0x00266563,
        0x0026AEA3,
        0x0026F82F,
        0x00274207,
        0x00278C2E,
        0x0027D6A4,
        0x0028216B,
        0x00286C84,
        0x0028B7F2,
        0x002903B6,
        0x00294FD1,
        0x00299C46,
        0x0029E915,
        0x002A3641,
        0x002A83CD,
        0x002AD1B8,
        0x002B2007,
        0x002B6EBA,
        0x002BBDD4,
        0x002C0D58,
        0x002C5D46,
        0x002CADA2,
        0x002CFE6E,
        0x002D4FAD,
        0x002DA161,
        0x002DF38D,
        0x002E4633,
        0x002E9957,
        0x002EECFB,
        0x002F4122,
        0x002F95D0,
        0x002FEB08,
        0x003040CD,
        0x00309723,
        0x0030EE0D,
        0x00314590,
        0x00319DAF,
        0x0031F66E,
        0x00324FD3,
        0x0032A9E0,
        0x0033049C,
        0x0033600B,
        0x0033BC31,
        0x00341915,
        0x003476BC,
        0x0034D52C,
        0x0035346B,
        0x00359480,
        0x0035F571,
        0x00365746,
        0x0036BA05,
        0x00371DB8,
        0x00378265,
        0x0037E817,
        0x00384ED6,
        0x0038B6AD,
        0x00391FA5,
        0x003989CB,
        0x0039F529,
        0x003A61CC,
        0x003ACFC2,
        0x003B3F19,
        0x003BAFE0,
        0x003C2228,
        0x003C9603,
        0x003D0B83,
        0x003D82BD,
        0x003DFBC7,
        0x003E76BA,
        0x003EF3AF,
        0x003F72C2,
        0x003FF414,
        0x004077C6,
        0x0040FDFE,
        0x004186E6,
        0x004212AC,
        0x0042A183,
        0x004333A6,
        0x0043C955,
        0x004462DB,
        0x0045008B,
        0x0045A2C8,
        0x00464A00,
        0x0046F6B9,
        0x0047A98D,
        0x00486337,
        0x00492498,
        0x0049EEC8,
        0x004AC325,
        0x004BA374,
        0x004C9209,
        0x004D921C,
        0x004EA84C,
        0x004FDBB3,
        0x00513846,
        0x0052D556,
        0x0054EF1F,
        0x005A0000,
};

/*------------------------------ 函数声明 -------------------------------------
 */
long _cal_asin(long x);

/*------------------------------ 外部函数 -------------------------------------
 */
/******************************************************************************
函数名称：	cal_complex_magnitude
函数版本：	01.01
创建作者:	sunxi
创建日期：	2008-08-26
函数说明：	得到复数的幅值(sqrt(real*real + imag*imag))。算法如下:
            l=max{|real|,|imag|},s=min{|real|,|imag|}
            m = l + 5*s*s/(9*l+3*s);
            此公式的实数算法精度(最大相对误差值)为0.1735%.
参数说明：
    real:	复数的实部
    imag:	复数的虚部
返回值：  	复数的幅值.
修改记录：
*/
unsigned long cal_complex_magnitude(long real, long imag)
{
    unsigned long m = 0, l, t, s;

    if (real == 0 && imag == 0)
    {

        return 0;
    }

    if (real < 0)
    {
        real = -real;
    }
    if (imag < 0)
    {
        imag = -imag;
    }

    if (real < imag)
    {
        l = (unsigned long)imag;
        s = (unsigned long)real;
    }
    else
    {
        l = (unsigned long)real;
        s = (unsigned long)imag;
    }

    // 计算结果,并进行四舍五入.
    t = 9 * l + 3 * s;
    if (t)
    {
        m = (unsigned long)(l + (5 * (__int64)s * s + (t >> 1)) / t);
    }

    return m;
}

/******************************************************************************
函数名称：	cal_complex_phase_cos
函数版本：	01.01
创建作者:	sunxi
创建日期：	2008-08-26
函数说明：	得到复数极坐标表示法的相位角的余弦
参数说明：
    real:	复数的实部
    imag:	复数的虚部
返回值：  	复数相位角的余弦值(Q16定点数)
修改记录：
*/
long cal_complex_phase_cos(long real, long imag)
{
    unsigned long m;

    m = cal_complex_magnitude(real, imag);
    if (m == 0)
    {
        return 0;
    }

    return (long)((((__int64)real << 16) + (m >> 1)) / m);
}

/******************************************************************************
函数名称：	cal_complex_phase
函数版本：	01.01
创建作者:	   sunxi
创建日期：	2008-08-26
函数说明：	得到复数的相位角.
参数说明：
    real:	复数的实部
    imag:	复数的虚部
返回值：  	复数的相位角(-180～180,Q16定点数)
修改记录：
*/
#define ANGLE_90 (90 << 16)
#define ANGLE_180 (180 << 16)
long cal_complex_phase(long real, long imag)
{
    long real_abs, imag_abs;
    long angle;

    // 得到绝对值
    real_abs = real >= 0 ? real : -real;
    imag_abs = imag >= 0 ? imag : -imag;

    // 对于sin函数来说，0～45度的线性要好于45～90度的线性。
    // 所以我们只使用0～45度。
    if (real_abs >= imag_abs)
    {
        //<=45度,用sin去算
        angle = _cal_asin(cal_complex_phase_cos(imag_abs, real_abs));
    }
    else
    {
        //>45度，用cos去算
        angle = _cal_asin(cal_complex_phase_cos(real_abs, imag_abs));
        angle = ANGLE_90 - angle;
    }

    // 象限处理
    if (real >= 0)
    {
        if (imag >= 0)
        {
            return angle; // 第一象限
        }
        else
        {

            return (0 - angle); // 第四象限
        }
    }
    else
    {
        if (imag >= 0)
        {

            return ANGLE_180 - angle; // 第二象限
        }
        else
        {

            return (-ANGLE_180 + angle); // 第三象限
        }
    }
}

/******************************************************************************
函数名称：	cal_q16_multi
函数版本：	01.01
创建作者:	   sunxi
创建日期：	2008-08-26
函数说明：	两个Q16定点数相乘,得到一个Q16数.计算公式为:
            (a*b)>>16
参数说明：
    a:		Q16定点数1
    b:		Q16定点数2
返回值：  	乘积(Q16定点数).注:相乘后的结果应不超过48位,否则出现溢出错误
修改记录：
*/
long cal_q16_multi(long a, long b)
{
    __int64 ll;

    ll = (__int64)a * b;

    return (long)(ll >> 16);
}

/******************************************************************************
函数名称：	cal_multi_divide
函数版本：	01.01
创建作者:	   sunxi
创建日期：	2008-08-26
函数说明：	使用64位算法计算a*b/c.有符号算法.注意:结果仍有可能溢出.
参数说明：
    a:		被乘数
    b:		乘数
    c:		除数
返回值：  	结果
修改记录：
*/
long cal_multi_divide(long a, long b, long c)
{
    __int64 ll;

#ifdef __KERNEL__
    ll = a * b;
    div_s64(ll, c);
#else
    ll = (__int64)a * b / c;
#endif

    return (long)ll;
}

/******************************************************************************
函数名称：	cal_multi_divide_unsigned
函数版本：	01.01
创建作者:	   sunxi
创建日期：	2008-08-26
函数说明：	使用64位算法计算a*b/c.无符号算法.注意:结果仍有可能溢出.
参数说明：
    a:		被乘数
    b:		乘数
    c:		除数
返回值：  	结果
修改记录：
*/
unsigned long cal_multi_divide_unsigned(unsigned long a, unsigned long b, unsigned long c)
{
    unsigned __int64 ll;

    ll = (unsigned __int64)a * b / c;

    return (unsigned long)ll;
}

/*------------------------------ 内部函数 -------------------------------------
 */
/******************************************************************************
函数名称：	_cal_asin
函数版本：	01.01
创建作者:	   sunxi
创建日期：	2008-08-26
函数说明：	反sin函数
参数说明：
    x:		反sin函数输入值，Q16定点数，0<=x<=1。
返回值：  	结果y，Q16定点数，0<=y<=90.
修改记录：
*/
long _cal_asin(long x)
{
    int high, low;

    // x>=1,返回90度。
    if (x >= 0x10000)
    {

        return asin_table[256];
    }

    high = (x >> 8) & 0xff;
    low = x & 0xff;

    return ((asin_table[high + 1] - asin_table[high]) * low + (256 >> 1)) / 256 + asin_table[high];
}

/*------------------------------ 测试函数 -------------------------------------
 */
#ifndef WIN32 // VC6.0 中定义的宏

#ifdef _CAL_DEBUG

#include "rt.h"

int cal_test(void)
{
    long angle, a, b, ans;
    //	int i1,i2;

    rt_printf("cal_test begin!\r\n");

    a = 0x77777;
    b = 0x66666;
    ans = cal_q16_multi(a, b);
    rt_printf("cal_q16_multi:%d*%d=%d\r\n", a, b, ans);

    a = 234;
    b = 567;
    angle = cal_complex_phase(a, b);
    rt_printf("angle=%d.%04d(real=%d,imag=%d)\r\n", angle >> 16, (angle & 0xffff) * 10000 / 0x10000, a, b);
    rt_printf("cal_test end!\r\n\r\n");

    return 0;
}

#endif

#else

#include "stdio.h"
#include "math.h"

int cal_test_vc(void)
{
    long i, j, angle;
    double angle_db1, angle_db2, diff, diff_max;

    angle = cal_complex_phase(136, -8010);

    diff_max = 0;
    for (i = 100; i < 65536; i++)
    {

        for (j = 100; j < 65536; j++)
        {
            angle = cal_complex_phase(i, j);
            angle_db1 = (double)angle / 65536;
            angle_db2 = atan((double)j / i);
            angle_db2 = angle_db2 * 180 / 3.1416;

            diff = angle_db2 - angle_db1;
            if (diff < 0)
            {

                diff = -diff;
            }
            if (diff > diff_max)
            {
                diff_max = diff;
                rt_printf("cal_test_vc:i=%d,j=%d,diff_max=%f\n", i, j, diff);
            }
        }
    }

    return 0;
}
#endif