calc.c

lpc2136芯片的硬件全部功能示例程序

/*************************************************************
		WPD800 计算程序
		修改历史    2007-9-25  zaken create version 1.0


*************************************************************/

#include "config.h"
#include "target.h"
#include "main.h"
#include "sample.h"
#include "wpd800.h"
#include "calc.h"

static WORD wFtCntRcd=0;			//保存当前傅氏滤波指针以不被采样中断破坏

T_ACVAL  tACVal;
void  Calc(void)
{
	int32 *pFtB0;		//当前傅氏滤波缓冲指针
	int32 *pFtB10;		//半周前傅氏滤波缓冲指针
	
	static int32 arlFt0[SAM_AI_NUM<<1];
	static int32 arlFt10[SAM_AI_NUM<<1];
	
	wFtCntRcd=wFtCnt;	//保存当前傅氏滤波指针以不被采样中断破坏
	{
		int16 i;
		int16 j=(wFtCntRcd-(SAM_CYCLE_NUM>>1))&SAM_BUF_MOD;
		
		for( i=0; i<SAM_AI_NUM*2; i++ )
		{
			arlFt0[i]=lFt[wFtCntRcd][i];
			arlFt10[i]=lFt[j][i];
		}
	}

	pFtB0=arlFt0;		
	pFtB10=arlFt10;

	tACVal.lIaR= (pFtB0[FT_IARE]+ pFtB10[FT_IARE])>>1;
	tACVal.lIaI= (pFtB0[FT_IAIM]+ pFtB10[FT_IAIM])>>1;
	tACVal.lIbR= (pFtB0[FT_IARE]+ pFtB10[FT_IARE])>>1;
	tACVal.lIbI= (pFtB0[FT_IAIM]+ pFtB10[FT_IAIM])>>1;
	tACVal.lIcR= (pFtB0[FT_ICRE]+ pFtB10[FT_ICRE])>>1;
	tACVal.lIcI= (pFtB0[FT_ICIM]+ pFtB10[FT_ICIM])>>1;	
	tACVal.lI0R= (pFtB0[FT_I0RE]+ pFtB10[FT_I0RE])>>1;
	tACVal.lI0I= (pFtB0[FT_I0IM]+ pFtB10[FT_I0IM])>>1;
	
	tACVal.lUaR= (pFtB0[FT_UARE]+ pFtB10[FT_UARE])>>1;
	tACVal.lUaI= (pFtB0[FT_UAIM]+ pFtB10[FT_UAIM])>>1;
	tACVal.lUbR= (pFtB0[FT_UBRE]+ pFtB10[FT_UBRE])>>1;
	tACVal.lUbI= (pFtB0[FT_UBIM]+ pFtB10[FT_UBIM])>>1;
	tACVal.lUcR= (pFtB0[FT_UCRE]+ pFtB10[FT_UCRE])>>1;
	tACVal.lUcI= (pFtB0[FT_UCIM]+ pFtB10[FT_UCIM])>>1;
	tACVal.lU0R= (pFtB0[FT_U0RE]+ pFtB10[FT_U0RE])>>1;
	tACVal.lU0I= (pFtB0[FT_U0IM]+ pFtB10[FT_U0IM])>>1;
	tACVal.lUxR= (pFtB0[FT_UXRE]+ pFtB10[FT_UXRE])>>1;
	tACVal.lUxI= (pFtB0[FT_UXIM]+ pFtB10[FT_UXIM])>>1;
		
	tACVal.dIa= Calc_Amp_Value(tACVal.lIaR,tACVal.lIaI);
	tACVal.dIb= Calc_Amp_Value(tACVal.lIbR,tACVal.lIbI);
	tACVal.dIc= Calc_Amp_Value(tACVal.lIcR,tACVal.lIcI);
	tACVal.dUa= Calc_Amp_Value(tACVal.lUaR,tACVal.lUaI);
	tACVal.dUb= Calc_Amp_Value(tACVal.lUbR,tACVal.lUbI);
	tACVal.dUc= Calc_Amp_Value(tACVal.lUcR,tACVal.lUbI);
	tACVal.dU0= Calc_Amp_Value(tACVal.lU0R,tACVal.lU0I);
	tACVal.dUx= Calc_Amp_Value(tACVal.lUxR,tACVal.lUxI);
	
}
DWORD  Calc_Amp_Value(int32 Re,int32 Im)
{
	uint32 R,I,k,l;
	if(Re<0)
		R=-Re;
	else
		R=Re;
	if(Im<0)
		I=-Im;
	else
		I=Im;
	
	if(R==0)
		return I;
	if(I==0)
		return R;
	if(R<I)//R I互换
	{
		k=R;
		R=I;
		I=k;
	}
	if(R>=0x10000000)//压缩
	{
		l=1;
		R=R>>4;
		I=I>>4;
	}
	if(I>10000)
	{
		k=R+(5*I*I)/(9*R+3*I);
	}
	else
	{
		k=R+(5*I*I+((9*R+3*I)/2))/(9*R+3*I);
	}
	if(l)
		k=k<<4;
	return k;		
}
//计算功率因数 返回值为因数* 0x10000
//虚部/幅值
int32  Calc_Cos(int32 Re,int32 Im)
{
	uint32 R,I;
	R=Calc_Amp_Value(Re,Im);//计算幅值
	if(R==0)
		return 0;
	I=(Im<<16);//0x10000;(Im*0x10000)//放大
	return (I/R);
}
const uint32 sintable[]={
	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,
};
//计算角度 返回值为角度 * 0x10000
int32  Calc_Ang(int32 Re,int32 Im)
{
	int32 Cos,Sin;
	uint8 inter,floater;
	uint32 Amp;
	uint32 R,I;
	if(Re<0)
		R=-Re;
	else
		R=Re;
	if(Im<0)
		I=-Im;
	else
		I=Im;
	Cos=Calc_Cos(Re,I);
	if(Cos>0xffff)//不可能出现
		return 0;
	inter=(uint8)(Cos>>8);
	floater=(uint8)(Cos);
	Cos=sintable[inter];
	Sin=sintable[inter+1];
	Amp=Cos+((Sin-Cos)*floater)/256;
	if(R<I)
		Amp=0xA50000-Amp;
	if(Re>=0 && Im>=0)
		return Amp;
	else if(Re>=0 && Im<0)
		return -Amp;
	else if(Re<0 && Im>=0)
		return 0xB40000-Amp;
	else if(Re<0 && Im<0)
		return  Amp-0xB40000;
	return Amp;		
}