| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511 |
- /*************************************************************************
- * 版权所有:
- * 文件版本: V1.00
- * 文件名称:Gen.c
- * 生成日期: 2008年9月1日
- * 作 者: xxxxxx
- * 功 能: 通用计算函数文件,本文件包含了系统使用的部分计算函数
- * 更新信息:
- * 更新日志1:
- * 日期:
- * 修改者:
- * 修改内容:
- * 修改原因:
- * 更新日志2:
- **************************************************************************/
- #include "head.h"
- DWORD _AbsL(long a)
- {
- if(a<0)
- return (DWORD)(-a);
- return (DWORD)a;
- }
- WORD _AbsS(short a)
- {
- if(a<0)
- return (WORD)(-a);
- return (WORD)a;
- }
- /**************************************************************************
- 函数名称:lAbs
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:32位长整数的绝对值
-
- 输入参数:long la 32位长整数
- 输出参数:
- 返回值: long 32位长整数的绝对值
- **************************************************************************/
- long lAbs(long la)
- {
- if(la<0)
- {
- la=-la;
- }
- return(la);
- }
- /*两个DWORD数相乘,结果缩小65536倍*/
- //v*f/65536
- DWORD _MulFac_U(DWORD v,DWORD f)
- {
- #if 1
- DWORD ah,am; /*此算法1.2us*/
- DWORD al;
- ah=(v>>16)*(f>>16);
- am=(v>>16)*(f&0x0000ffff)+(f>>16)*(v&0x0000ffff);
- al=(f&0x0000ffff)*(v&0x0000ffff);
- return((ah<<16)+am+(al>>16));
- #else
- DWORD ret; /*此算法4.8us*/
- DWORD long ll;
- ll=v;
- ll*=f;
- ret=(ll/65536);
- return ret;
- #endif
- }
- /*两个long数相乘,结果缩小65536倍*/
- //v*f/65536
- long _MulFac_S(long v,long f)
- {
- #if 1
- int sign=1;
- long ret;
- if((v>0&&f<0) || (v<0&&f>0))
- {
- sign=-1;
- }
- if(v<0)v=-v;
- if(f<0)f=-f;
-
- ret=_MulFac_U(v,f);
- return (ret*sign);
- #else
- long ret;
- long long ll;
- ll=v;
- ll*=f;
- ret=(ll/65536);
- return ret;
- #endif
- }
- //v*f/ch
- DWORD _Mul_Div_U(DWORD v,DWORD f,DWORD ch)
- {
- DWORD ret;
- unsigned long long ll;
- ll=v;
- ll*=f;
-
- #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
- ret = div_u64(ll,ch);
- #else
- ret=ll/ch;
- #endif
- return ret;
-
- }
- long _Mul_Div_S(long v,long f,long ch)
- {
- long ret;
- long long ll;
- ll=v;
- ll*=f;
-
- #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
- ret = div_s64(ll,ch);
- #else
- ret=ll/ch;
- #endif
-
- return ret;
- }
- /**************************************************************************
- 函数名称:_AmXY
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:计算幅值|a+bj|=(L^2+S^2)^0.5=L+5*S*S/(9*L+3*S),16位数
- L为大值,S为小值,|Large|<2^14
- 输入参数:short nRe,short nIm 矢量实部,虚部
- 输出参数:
- 返回值:矢量的模WORD,
- **************************************************************************/
- DWORD _AmXY(long nRe,long nIm)
- {
- DWORD dLarge,dSmall;
- DWORD wOut;
- DWORD long dA,dC;
- if(nRe>=0)
- {
- dLarge=nRe;
- }
- else
- {
- dLarge=-nRe;
-
- }
- if(nIm>=0)
- {
- dSmall=nIm;
- }
- else
- {
- dSmall=-nIm;
- }
- if(dLarge==0)
- {
- return((DWORD)dSmall);
- }
- if(dSmall==0)
- {
- return((DWORD)dLarge);
- }
- if(dLarge<dSmall)
- {
- dA=dLarge;
- dLarge=dSmall;
- dSmall=dA;
- }
- dC=dSmall*dSmall;
- dC=dC*5;
- dA=dLarge*9+dSmall*3;//程序会自动优化成移位和加法
-
- #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
- //wOut=dLarge+dC/dA;
- do_div(dC,dA);
- wOut=dLarge + dC;
- #else
- wOut=dLarge+dC/dA;
- #endif
- return (wOut);
- }
- static DWORD _tblul(const DWORD *pTable,long step)
- {
- long l1,l2;
- DWORD ret;
- l1=(step/256);
- l2=(step%256);
- ret=((pTable[l1+1]-pTable[l1])*l2)/256+pTable[l1];
- return ret;
- }
- //正弦0-90度的值,放大65536倍
- //0-90,89,共92个元素
- const DWORD SIN0_90DU[92]={
- 0, 1143, 2287, 3429, 4571, 5711, 6850, 7986, 9120,10252,
- 11380,12504,13625,14742,15854,16961,18064,19160,20251,21336,
- 22414,23486,24550,25606,26655,27696,28729,29752,30767,31772,
- 32768,33753,34728,35693,36647,37589,38521,39440,40347,41243,
- 42125,42995,43852,44695,45525,46340,47142,47929,48702,49460,
- 50203,50931,51643,52339,53019,53683,54331,54963,55577,56175,
- 56755,57319,57864,58393,58903,59395,59870,60326,60763,61183,
- 61583,61965,62328,62672,62997,63302,63589,63856,64103,64331,
- 64540,64729,64898,65047,65176,65286,65376,65446,65496,65526,
- 65536,65526
- };
- const DWORD dASinTable[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,
- };
- //
- #define A90DU (90<<8)
- #define A180DU (180<<8)
- #define A270DU (270<<8)
- #define A360DU (360<<8)
- /**************************************************************************
- 函数名称:mSin0_90
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:0-90度正弦函数
-
- 输入参数:long lAngle 角度*256 单位为1/256度
- 输出参数:
- 返回值:long放大65536倍正弦值
- **************************************************************************/
- static long mSin0_90(long lAngle)
- {
- long la;
- la=_tblul(&SIN0_90DU[0],lAngle);
- return(la);
- }
- /**************************************************************************
- 函数名称:mSin
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:正弦函数
-
- 输入参数:long lAngle 角度*256 单位为1/256度
- 输出参数:
- 返回值:long放大65536倍正弦值
- **************************************************************************/
- long mSin(long lAngle)
- {
- long la;
-
- lAngle%=A360DU;//规格化(-360-360)
- if(lAngle<0)//-x度<<8
- {
- lAngle+=A360DU;
- }
- if((lAngle>=0)&&(lAngle<=A90DU)) //(0-90度)<<8
- {
- la=mSin0_90(lAngle);
- }
- else if(lAngle<=A180DU) //&&(lAngle>A90DU) (90-180度)<<8
- {
- la=mSin0_90(A180DU-lAngle);
- }
- else if(lAngle<=A270DU) //&&(lAngle>A180DU)(180-270度)<<8
- {
- la=-mSin0_90(lAngle-A180DU);
- }
- else if(lAngle<A360DU) //&&(lAngle>A270DU)(270-370度)<<8
- {
- la=-mSin0_90(A360DU-lAngle);
- }
- else
- {
- la=0x7fffffff;
- }
- return(la);
- }
- /**************************************************************************
- 函数名称:mCos
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:余弦函数
-
- 输入参数:long lAngle 角度*256 单位为1/256度
- 输出参数:
- 返回值: long 放大65536倍余弦值
- **************************************************************************/
- long mCos(long lAngle)
- {
- long la;
- la=mSin(A90DU-lAngle);
- return(la);
- }
- /**************************************************************************
- 函数名称:mTan
- 函数版本:1.00
- 作者:
- 创建日期:2005.4.11
- 函数功能说明:正切函数
-
- 输入参数:long lAngle 角度*256 单位为1/256度
- 输出参数:
- 返回值: long 放大65536倍正切值
- **************************************************************************/
- #if 0
- static long mTan(long lAngle)
- {
- long la;
- la=mCos(lAngle);
- if(la==0)
- {
- la=0x1f0000;//(2^6-1)*2^16,Tan89=57,令Tan90=63 tan88.1=31
- }
- else
- {
- la=((mSin(lAngle)<<14)/la)<<2;
- }
- return(la);
- }
- #endif
- //
- long _CosPQ(long p,long q)
- {
- long r,ret=0;
- r=_AmXY(p,q); // 高16位为整数,低16位小数
- if(r>0)
- {
- ret=_Mul_Div_S(p,65536,r);
- }
- return ret;
- }
- // 返回值:0~180,-0~-180.
- long _AngPQ(long P,long Q) // 角度误差,以10000为幅值,计数角度误差,最大1.28
- {
- long p,q;
- long angel;
- p=P;
- q=Q;
- if(p<0)p=-P;
- if(q<0)q=-Q;
- if(p<100&&q<100) // add by sunxi 2008-11-11 若参数太小,人为扩大32倍
- {
- p<<=5;
- q<<=5;
- }
-
- if(p>=q)
- {
- angel=_tblul(dASinTable,_CosPQ(p,q));
- }
- else
- {
- angel=_tblul(dASinTable,_CosPQ(q,p));
- angel=0x05A0000-angel;
- }
- angel=0x05A0000-angel; // 调整角度
-
- if(P>=0&&Q>=0) // angel
- {
- return angel;
- }
- if(P>=0&&Q<0) // -angel
- {
- return (-angel);
- }
- if(P<0&&Q>=0) // 180-angel
- {
- return (0xB40000-angel); // 180-angel
- }
- return (angel-0xB40000); // angel-180
- }
- // modify for am335x xxxxxx
- void Swap_16_memcpy(BYTE *dat1,BYTE *dat2)
- {
- memcpy(dat1,dat2,2);
- #ifdef MODE_LITTLE_ENDIAN
- swap16(dat1);
- #endif
- }
- // modify for am335x xxxxxx
- void Swap_32(void *p)
- {
- #ifdef MODE_LITTLE_ENDIAN
- swap32(p);
- #endif
- }
- /*******************************************************************
- 函数名称:CopySwap
- 版 本:01.01
- 作 者:xxxxxx
- 日 期:2022/06/02
- 函数说明:内存拷贝交换处理
- 形 参:*lval:左值
- *rval:右值
- bytes:字节数
- rval_swap:交换右值(否则交换左值)
- 返 回 值:内存拷贝成功的字节数
- ********************************************************************/
- int CopySwap(BYTE *lval,BYTE *rval,int bytes,bool rval_swap)
- {
- int i;
- int len = 0;
- if(lval==NULL || rval==NULL)
- return len;
- len = bytes;
- #ifdef MODE_LITTLE_ENDIAN
- for(i=0;i<len;i++)
- {
- lval[i] = rval[i];
- }
- #else
- {
- int j=len;
-
- if(rval_swap)
- {
- for(i=0;i<len;i++)
- {
- j--;
- lval[i] = rval[j];
- }
- }
- else
- {
- for(i=0;i<len;i++)
- {
- j--;
- lval[j] = rval[i];
- }
- }
- }
- #endif
- return len;
- }
|