gen.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511
  1. /*************************************************************************
  2. * 版权所有:
  3. * 文件版本: V1.00
  4. * 文件名称:Gen.c
  5. * 生成日期: 2008年9月1日
  6. * 作 者: xxxxxx
  7. * 功 能: 通用计算函数文件,本文件包含了系统使用的部分计算函数
  8. * 更新信息:
  9. * 更新日志1:
  10. * 日期:
  11. * 修改者:
  12. * 修改内容:
  13. * 修改原因:
  14. * 更新日志2:
  15. **************************************************************************/
  16. #include "head.h"
  17. DWORD _AbsL(long a)
  18. {
  19. if(a<0)
  20. return (DWORD)(-a);
  21. return (DWORD)a;
  22. }
  23. WORD _AbsS(short a)
  24. {
  25. if(a<0)
  26. return (WORD)(-a);
  27. return (WORD)a;
  28. }
  29. /**************************************************************************
  30. 函数名称:lAbs
  31. 函数版本:1.00
  32. 作者:
  33. 创建日期:2005.4.11
  34. 函数功能说明:32位长整数的绝对值
  35. 输入参数:long la 32位长整数
  36. 输出参数:
  37. 返回值: long 32位长整数的绝对值
  38. **************************************************************************/
  39. long lAbs(long la)
  40. {
  41. if(la<0)
  42. {
  43. la=-la;
  44. }
  45. return(la);
  46. }
  47. /*两个DWORD数相乘,结果缩小65536倍*/
  48. //v*f/65536
  49. DWORD _MulFac_U(DWORD v,DWORD f)
  50. {
  51. #if 1
  52. DWORD ah,am; /*此算法1.2us*/
  53. DWORD al;
  54. ah=(v>>16)*(f>>16);
  55. am=(v>>16)*(f&0x0000ffff)+(f>>16)*(v&0x0000ffff);
  56. al=(f&0x0000ffff)*(v&0x0000ffff);
  57. return((ah<<16)+am+(al>>16));
  58. #else
  59. DWORD ret; /*此算法4.8us*/
  60. DWORD long ll;
  61. ll=v;
  62. ll*=f;
  63. ret=(ll/65536);
  64. return ret;
  65. #endif
  66. }
  67. /*两个long数相乘,结果缩小65536倍*/
  68. //v*f/65536
  69. long _MulFac_S(long v,long f)
  70. {
  71. #if 1
  72. int sign=1;
  73. long ret;
  74. if((v>0&&f<0) || (v<0&&f>0))
  75. {
  76. sign=-1;
  77. }
  78. if(v<0)v=-v;
  79. if(f<0)f=-f;
  80. ret=_MulFac_U(v,f);
  81. return (ret*sign);
  82. #else
  83. long ret;
  84. long long ll;
  85. ll=v;
  86. ll*=f;
  87. ret=(ll/65536);
  88. return ret;
  89. #endif
  90. }
  91. //v*f/ch
  92. DWORD _Mul_Div_U(DWORD v,DWORD f,DWORD ch)
  93. {
  94. DWORD ret;
  95. unsigned long long ll;
  96. ll=v;
  97. ll*=f;
  98. #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
  99. ret = div_u64(ll,ch);
  100. #else
  101. ret=ll/ch;
  102. #endif
  103. return ret;
  104. }
  105. long _Mul_Div_S(long v,long f,long ch)
  106. {
  107. long ret;
  108. long long ll;
  109. ll=v;
  110. ll*=f;
  111. #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
  112. ret = div_s64(ll,ch);
  113. #else
  114. ret=ll/ch;
  115. #endif
  116. return ret;
  117. }
  118. /**************************************************************************
  119. 函数名称:_AmXY
  120. 函数版本:1.00
  121. 作者:
  122. 创建日期:2005.4.11
  123. 函数功能说明:计算幅值|a+bj|=(L^2+S^2)^0.5=L+5*S*S/(9*L+3*S),16位数
  124. L为大值,S为小值,|Large|<2^14
  125. 输入参数:short nRe,short nIm 矢量实部,虚部
  126. 输出参数:
  127. 返回值:矢量的模WORD,
  128. **************************************************************************/
  129. DWORD _AmXY(long nRe,long nIm)
  130. {
  131. DWORD dLarge,dSmall;
  132. DWORD wOut;
  133. DWORD long dA,dC;
  134. if(nRe>=0)
  135. {
  136. dLarge=nRe;
  137. }
  138. else
  139. {
  140. dLarge=-nRe;
  141. }
  142. if(nIm>=0)
  143. {
  144. dSmall=nIm;
  145. }
  146. else
  147. {
  148. dSmall=-nIm;
  149. }
  150. if(dLarge==0)
  151. {
  152. return((DWORD)dSmall);
  153. }
  154. if(dSmall==0)
  155. {
  156. return((DWORD)dLarge);
  157. }
  158. if(dLarge<dSmall)
  159. {
  160. dA=dLarge;
  161. dLarge=dSmall;
  162. dSmall=dA;
  163. }
  164. dC=dSmall*dSmall;
  165. dC=dC*5;
  166. dA=dLarge*9+dSmall*3;//程序会自动优化成移位和加法
  167. #ifdef __KERNEL__ //Linux 内核暂时不支持64位直接除法。
  168. //wOut=dLarge+dC/dA;
  169. do_div(dC,dA);
  170. wOut=dLarge + dC;
  171. #else
  172. wOut=dLarge+dC/dA;
  173. #endif
  174. return (wOut);
  175. }
  176. static DWORD _tblul(const DWORD *pTable,long step)
  177. {
  178. long l1,l2;
  179. DWORD ret;
  180. l1=(step/256);
  181. l2=(step%256);
  182. ret=((pTable[l1+1]-pTable[l1])*l2)/256+pTable[l1];
  183. return ret;
  184. }
  185. //正弦0-90度的值,放大65536倍
  186. //0-90,89,共92个元素
  187. const DWORD SIN0_90DU[92]={
  188. 0, 1143, 2287, 3429, 4571, 5711, 6850, 7986, 9120,10252,
  189. 11380,12504,13625,14742,15854,16961,18064,19160,20251,21336,
  190. 22414,23486,24550,25606,26655,27696,28729,29752,30767,31772,
  191. 32768,33753,34728,35693,36647,37589,38521,39440,40347,41243,
  192. 42125,42995,43852,44695,45525,46340,47142,47929,48702,49460,
  193. 50203,50931,51643,52339,53019,53683,54331,54963,55577,56175,
  194. 56755,57319,57864,58393,58903,59395,59870,60326,60763,61183,
  195. 61583,61965,62328,62672,62997,63302,63589,63856,64103,64331,
  196. 64540,64729,64898,65047,65176,65286,65376,65446,65496,65526,
  197. 65536,65526
  198. };
  199. const DWORD dASinTable[257]=
  200. {
  201. 0x00000000,0x0000394B,0x00007297,0x0000ABE4,0x0000E531,
  202. 0x00011E7F,0x000157CE,0x0001911E,0x0001CA70,0x000203C4,
  203. 0x00023D1A,0x00027672,0x0002AFCD,0x0002E92A,0x0003228A,
  204. 0x00035BED,0x00039554,0x0003CEBE,0x0004082C,0x0004419F,
  205. 0x00047B15,0x0004B490,0x0004EE10,0x00052795,0x0005611E,
  206. 0x00059AAE,0x0005D443,0x00060DDE,0x0006477F,0x00068126,
  207. 0x0006BAD5,0x0006F489,0x00072E46,0x00076809,0x0007A1D4,
  208. 0x0007DBA7,0x00081581,0x00084F65,0x00088950,0x0008C345,
  209. 0x0008FD42,0x00093749,0x00097159,0x0009AB74,0x0009E598,
  210. 0x000A1FC6,0x000A59FF,0x000A9443,0x000ACE92,0x000B08EC,
  211. 0x000B4352,0x000B7DC4,0x000BB842,0x000BF2CC,0x000C2D63,
  212. 0x000C6807,0x000CA2B8,0x000CDD76,0x000D1843,0x000D531D,
  213. 0x000D8E06,0x000DC8FD,0x000E0403,0x000E3F19,0x000E7A3E,
  214. 0x000EB572,0x000EF0B7,0x000F2C0C,0x000F6772,0x000FA2E9,
  215. 0x000FDE71,0x00101A0B,0x001055B6,0x00109174,0x0010CD45,
  216. 0x00110928,0x0011451F,0x00118129,0x0011BD46,0x0011F979,
  217. 0x001235BF,0x0012721B,0x0012AE8C,0x0012EB12,0x001327AE,
  218. 0x00136461,0x0013A12A,0x0013DE0A,0x00141B02,0x00145812,
  219. 0x00149539,0x0014D27A,0x00150FD3,0x00154D45,0x00158AD2,
  220. 0x0015C878,0x00160639,0x00164415,0x0016820C,0x0016C01F,
  221. 0x0016FE4F,0x00173C9B,0x00177B04,0x0017B98B,0x0017F830,
  222. 0x001836F3,0x001875D5,0x0018B4D7,0x0018F3F9,0x0019333B,
  223. 0x0019729E,0x0019B222,0x0019F1C9,0x001A3192,0x001A717E,
  224. 0x001AB18D,0x001AF1C1,0x001B3219,0x001B7297,0x001BB33A,
  225. 0x001BF404,0x001C34F4,0x001C760C,0x001CB74D,0x001CF8B6,
  226. 0x001D3A48,0x001D7C05,0x001DBDED,0x001E0000,0x001E423F,
  227. 0x001E84AA,0x001EC744,0x001F0A0B,0x001F4D02,0x001F9028,
  228. 0x001FD37F,0x00201707,0x00205AC1,0x00209EAD,0x0020E2CE,
  229. 0x00212723,0x00216BAE,0x0021B06F,0x0021F566,0x00223A97,
  230. 0x00228000,0x0022C5A3,0x00230B81,0x0023519B,0x002397F2,
  231. 0x0023DE87,0x0024255B,0x00246C6F,0x0024B3C4,0x0024FB5C,
  232. 0x00254338,0x00258B58,0x0025D3BF,0x00261C6D,0x00266563,
  233. 0x0026AEA3,0x0026F82F,0x00274207,0x00278C2E,0x0027D6A4,
  234. 0x0028216B,0x00286C84,0x0028B7F2,0x002903B6,0x00294FD1,
  235. 0x00299C46,0x0029E915,0x002A3641,0x002A83CD,0x002AD1B8,
  236. 0x002B2007,0x002B6EBA,0x002BBDD4,0x002C0D58,0x002C5D46,
  237. 0x002CADA2,0x002CFE6E,0x002D4FAD,0x002DA161,0x002DF38D,
  238. 0x002E4633,0x002E9957,0x002EECFB,0x002F4122,0x002F95D0,
  239. 0x002FEB08,0x003040CD,0x00309723,0x0030EE0D,0x00314590,
  240. 0x00319DAF,0x0031F66E,0x00324FD3,0x0032A9E0,0x0033049C,
  241. 0x0033600B,0x0033BC31,0x00341915,0x003476BC,0x0034D52C,
  242. 0x0035346B,0x00359480,0x0035F571,0x00365746,0x0036BA05,
  243. 0x00371DB8,0x00378265,0x0037E817,0x00384ED6,0x0038B6AD,
  244. 0x00391FA5,0x003989CB,0x0039F529,0x003A61CC,0x003ACFC2,
  245. 0x003B3F19,0x003BAFE0,0x003C2228,0x003C9603,0x003D0B83,
  246. 0x003D82BD,0x003DFBC7,0x003E76BA,0x003EF3AF,0x003F72C2,
  247. 0x003FF414,0x004077C6,0x0040FDFE,0x004186E6,0x004212AC,
  248. 0x0042A183,0x004333A6,0x0043C955,0x004462DB,0x0045008B,
  249. 0x0045A2C8,0x00464A00,0x0046F6B9,0x0047A98D,0x00486337,
  250. 0x00492498,0x0049EEC8,0x004AC325,0x004BA374,0x004C9209,
  251. 0x004D921C,0x004EA84C,0x004FDBB3,0x00513846,0x0052D556,
  252. 0x0054EF1F,0x005A0000,
  253. };
  254. //
  255. #define A90DU (90<<8)
  256. #define A180DU (180<<8)
  257. #define A270DU (270<<8)
  258. #define A360DU (360<<8)
  259. /**************************************************************************
  260. 函数名称:mSin0_90
  261. 函数版本:1.00
  262. 作者:
  263. 创建日期:2005.4.11
  264. 函数功能说明:0-90度正弦函数
  265. 输入参数:long lAngle 角度*256 单位为1/256度
  266. 输出参数:
  267. 返回值:long放大65536倍正弦值
  268. **************************************************************************/
  269. static long mSin0_90(long lAngle)
  270. {
  271. long la;
  272. la=_tblul(&SIN0_90DU[0],lAngle);
  273. return(la);
  274. }
  275. /**************************************************************************
  276. 函数名称:mSin
  277. 函数版本:1.00
  278. 作者:
  279. 创建日期:2005.4.11
  280. 函数功能说明:正弦函数
  281. 输入参数:long lAngle 角度*256 单位为1/256度
  282. 输出参数:
  283. 返回值:long放大65536倍正弦值
  284. **************************************************************************/
  285. long mSin(long lAngle)
  286. {
  287. long la;
  288. lAngle%=A360DU;//规格化(-360-360)
  289. if(lAngle<0)//-x度<<8
  290. {
  291. lAngle+=A360DU;
  292. }
  293. if((lAngle>=0)&&(lAngle<=A90DU)) //(0-90度)<<8
  294. {
  295. la=mSin0_90(lAngle);
  296. }
  297. else if(lAngle<=A180DU) //&&(lAngle>A90DU) (90-180度)<<8
  298. {
  299. la=mSin0_90(A180DU-lAngle);
  300. }
  301. else if(lAngle<=A270DU) //&&(lAngle>A180DU)(180-270度)<<8
  302. {
  303. la=-mSin0_90(lAngle-A180DU);
  304. }
  305. else if(lAngle<A360DU) //&&(lAngle>A270DU)(270-370度)<<8
  306. {
  307. la=-mSin0_90(A360DU-lAngle);
  308. }
  309. else
  310. {
  311. la=0x7fffffff;
  312. }
  313. return(la);
  314. }
  315. /**************************************************************************
  316. 函数名称:mCos
  317. 函数版本:1.00
  318. 作者:
  319. 创建日期:2005.4.11
  320. 函数功能说明:余弦函数
  321. 输入参数:long lAngle 角度*256 单位为1/256度
  322. 输出参数:
  323. 返回值: long 放大65536倍余弦值
  324. **************************************************************************/
  325. long mCos(long lAngle)
  326. {
  327. long la;
  328. la=mSin(A90DU-lAngle);
  329. return(la);
  330. }
  331. /**************************************************************************
  332. 函数名称:mTan
  333. 函数版本:1.00
  334. 作者:
  335. 创建日期:2005.4.11
  336. 函数功能说明:正切函数
  337. 输入参数:long lAngle 角度*256 单位为1/256度
  338. 输出参数:
  339. 返回值: long 放大65536倍正切值
  340. **************************************************************************/
  341. #if 0
  342. static long mTan(long lAngle)
  343. {
  344. long la;
  345. la=mCos(lAngle);
  346. if(la==0)
  347. {
  348. la=0x1f0000;//(2^6-1)*2^16,Tan89=57,令Tan90=63 tan88.1=31
  349. }
  350. else
  351. {
  352. la=((mSin(lAngle)<<14)/la)<<2;
  353. }
  354. return(la);
  355. }
  356. #endif
  357. //
  358. long _CosPQ(long p,long q)
  359. {
  360. long r,ret=0;
  361. r=_AmXY(p,q); // 高16位为整数,低16位小数
  362. if(r>0)
  363. {
  364. ret=_Mul_Div_S(p,65536,r);
  365. }
  366. return ret;
  367. }
  368. // 返回值:0~180,-0~-180.
  369. long _AngPQ(long P,long Q) // 角度误差,以10000为幅值,计数角度误差,最大1.28
  370. {
  371. long p,q;
  372. long angel;
  373. p=P;
  374. q=Q;
  375. if(p<0)p=-P;
  376. if(q<0)q=-Q;
  377. if(p<100&&q<100) // add by sunxi 2008-11-11 若参数太小,人为扩大32倍
  378. {
  379. p<<=5;
  380. q<<=5;
  381. }
  382. if(p>=q)
  383. {
  384. angel=_tblul(dASinTable,_CosPQ(p,q));
  385. }
  386. else
  387. {
  388. angel=_tblul(dASinTable,_CosPQ(q,p));
  389. angel=0x05A0000-angel;
  390. }
  391. angel=0x05A0000-angel; // 调整角度
  392. if(P>=0&&Q>=0) // angel
  393. {
  394. return angel;
  395. }
  396. if(P>=0&&Q<0) // -angel
  397. {
  398. return (-angel);
  399. }
  400. if(P<0&&Q>=0) // 180-angel
  401. {
  402. return (0xB40000-angel); // 180-angel
  403. }
  404. return (angel-0xB40000); // angel-180
  405. }
  406. // modify for am335x xxxxxx
  407. void Swap_16_memcpy(BYTE *dat1,BYTE *dat2)
  408. {
  409. memcpy(dat1,dat2,2);
  410. #ifdef MODE_LITTLE_ENDIAN
  411. swap16(dat1);
  412. #endif
  413. }
  414. // modify for am335x xxxxxx
  415. void Swap_32(void *p)
  416. {
  417. #ifdef MODE_LITTLE_ENDIAN
  418. swap32(p);
  419. #endif
  420. }
  421. /*******************************************************************
  422. 函数名称:CopySwap
  423. 版 本:01.01
  424. 作 者:xxxxxx
  425. 日 期:2022/06/02
  426. 函数说明:内存拷贝交换处理
  427. 形 参:*lval:左值
  428. *rval:右值
  429. bytes:字节数
  430. rval_swap:交换右值(否则交换左值)
  431. 返 回 值:内存拷贝成功的字节数
  432. ********************************************************************/
  433. int CopySwap(BYTE *lval,BYTE *rval,int bytes,bool rval_swap)
  434. {
  435. int i;
  436. int len = 0;
  437. if(lval==NULL || rval==NULL)
  438. return len;
  439. len = bytes;
  440. #ifdef MODE_LITTLE_ENDIAN
  441. for(i=0;i<len;i++)
  442. {
  443. lval[i] = rval[i];
  444. }
  445. #else
  446. {
  447. int j=len;
  448. if(rval_swap)
  449. {
  450. for(i=0;i<len;i++)
  451. {
  452. j--;
  453. lval[i] = rval[j];
  454. }
  455. }
  456. else
  457. {
  458. for(i=0;i<len;i++)
  459. {
  460. j--;
  461. lval[j] = rval[i];
  462. }
  463. }
  464. }
  465. #endif
  466. return len;
  467. }