/****************************************************************************** 版权所有： 文件名称： 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