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