一些基本数学公式

绝对值/最大值/最小值

  1. #define Abs(A)    ((A>=0)?A:-A)  // 绝对值函数
  2. #define Min(A,B)  ((A<=B)?A:B)   // 求最小函数
  3. #define Max(A,B)  ((A>=B)?A:B)   // 求最大函数

固定点格式IQ15

  1. #define   _IQ15(A)       (int32_t)((A)<<15)  //左移15位 32768
  2. #define   _IQ(A)          _IQ15(A)           //定义IQ格式 32768
  3. #define   _IQmpy(A,B)    (int32_t)((A*B)>>15)  //IQ格式相乘 
  4. #define   _IQ10mpy(A,B)   (int32_t)((A*B)>>10) //IQ10格式相乘 
  5. #define   _IQdiv2(A)     (int32_t)((A)>>1)     //除2
  6. #define   _IQmpy2(A)     (int32_t)(A<<1)       //乘2
  7. #define   _IQdiv(A,B)    (int32_t)((A<<15)/B)  //IQ格式相除

正弦与余弦函数查找表

查找表（lookup table）用于存储正弦和余弦函数的值.
预先计算好的正弦或余弦函数在一定范围内的离散采样值

  1. const int16_t IQSin_Cos_Table[256]={\
  2. 0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,\
  3. 0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,\
  4. 0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,\
  5. 0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,\
  6. 0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,\
  7. 0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,\
  8. 0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,\
  9. 0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,\
  10. 0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,\
  11. 0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,\
  12. 0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,\
  13. 0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,\
  14. 0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,\
  15. 0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,\
  16. 0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,\
  17. 0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,\
  18. 0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,\
  19. 0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,\
  20. 0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,\
  21. 0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,\
  22. 0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,\
  23. 0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,\
  24. 0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,\
  25. 0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,\
  26. 0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,\
  27. 0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,\
  28. 0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,\
  29. 0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,\
  30. 0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,\
  31. 0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,\
  32. 0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,\
  33. 0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};

反正切函数查找表

  1. const int32_t IQAtan2_Table[256]={\
  2. 0x00000000,0x000000C9,0x00000192,0x0000025B,0x00000324,0x000003ED,0x000004B6,0x00000580,\
  3. 0x00000649,0x00000712,0x000008A7,0x00000971,0x00000A3B,0x00000B05,0x00000BD0,0x00000C9B,\
  4. 0x00000D66,0x00000E31,0x00000EFD,0x00000FC9,0x00001095,0x00001162,0x00001218,0x0000122F,\
  5. 0x000012FC,0x000013CA,0x00001498,0x00001566,0x00001635,0x00001705,0x000017D4,0x000018A5,\
  6. 0x00001975,0x00001A47,0x00001B19,0x00001BEB,0x00001CBE,0x00001D91,0x00001E65,0x00001F3A,\
  7. 0x0000200F,0x000020E5,0x000021BC,0x00002294,0x0000236C,0x00002444,0x0000251E,0x000025F8,\
  8. 0x000026D4,0x000027B0,0x0000288C,0x0000296A,0x00002A49,0x00002B28,0x00002C08,0x00002CEA,\
  9. 0x00002DCC,0x00002EAF,0x00002F94,0x00003079,0x00003160,0x00003247,0x00003330,0x00003419,\
  10. 0x00003509,0x00003571,0x000036DE,0x000037CD,0x000038BD,0x000039AE,0x00003AA0,0x00003B94,\
  11. 0x00003C8A,0x00003D80,0x00003E79,0x00003F72,0x0000406E,0x0000416A,0x00004269,0x00004369,\
  12. 0x0000446A,0x0000456E,0x00004673,0x0000477A,0x00004882,0x0000498D,0x00004A99,0x00004BA8,\
  13. 0x00004CB8,0x00004DCA,0x00004EDF,0x00004FF5,0x0000510E,0x00005228,0x00005345,0x00005465,\
  14. 0x00005586,0x000056AA,0x000057D1,0x000058FA,0x00005A25,0x00005B53,0x00005C84,0x00005DB8,\
  15. 0x00005EEE,0x00006027,0x00006163,0x000062A2,0x000063E4,0x00006529,0x00006671,0x000067BD,\
  16. 0x0000690C,0x00006A5E,0x00006BB3,0x00006D0D,0x00006E69,0x00006FCA,0x0000712E,0x00007296,\
  17. 0x00007403,0x00007573,0x000076E7,0x00007860,0x000079DD,0x00007B5F,0x00007CE5,0x00007E70,\
  18. 0x00008000,0x00008194,0x0000832E,0x000084CD,0x00008671,0x0000881A,0x000089CA,0x00008B7F,\
  19. 0x00008D39,0x00008EFA,0x000090C1,0x0000928F,0x00009463,0x0000963D,0x0000981F,0x00009A08,\
  20. 0x00009BF7,0x00009DEF,0x00009FEE,0x0000A1F5,0x0000A404,0x0000A61B,0x0000A83B,0x0000AA64,\
  21. 0x0000AC96,0x0000AED1,0x0000B116,0x0000B365,0x0000B5BE,0x0000B822,0x0000BA91,0x0000BD0B,\
  22. 0x0000BF90,0x0000C222,0x0000C4C0,0x0000C76A,0x0000CA22,0x0000CCE7,0x0000CF88,0x0000D29D,\
  23. 0x0000D58E,0x0000D88E,0x0000DB9F,0x0000DEC0,0x0000E1F3,0x0000E538,0x0000E88F,0x0000EBFA,\
  24. 0x0000EF78,0x0000F30B,0x0000F6B4,0x0000FA74,0x0000FE4A,0x00010239,0x00010641,0x00010A64,\
  25. 0x00010EA2,0x000112FC,0x00011774,0x00011C0B,0x00012C03,0x0001259C,0x00012A99,0x00012FBC,\
  26. 0x00013504,0x00013A76,0x00014012,0x000145DB,0x00014BD3,0x000151FD,0x0001585A,0x00015EEE,\
  27. 0x000165BC,0x00016CC6,0x00017411,0x00017B9F,0x00018376,0x00018B98,0x0001940A,0x00019CD2,\
  28. 0x0001A5F5,0x0001AF78,0x0001B963,0x0001C3BB,0x0001CE88,0x0001D9D2,0x0001E5A3,0x0001F205,\
  29. 0x0001FF01,0x00020CA4,0x00021AFB,0x00022A15,0x00023A02,0x00024AD4,0x00025C9F,0x00026F7B,\
  30. 0x0002837F,0x000298CA,0x0002AF7C,0x0002C7BA,0x0002E1AE,0x0002FD8A,0x00031B84,0x00033BDF,\
  31. 0x00035EE7,0x000384F5,0x0003AE73,0x0003DBDD,0x00040DCB,0x000444F4,0x00048236,0x0004C6A6,\
  32. 0x0005139B,0x00056AC9,0x0005CE63,0x00064144,0x0006C740,0x0007658D,0x00082374,0x00090B81,\
  33. 0x000A2D7F,0x000BA246,0x000D9338,0x00104AD7,0x00145E24,0x001B28CC,0x0028BDDA,0x00517C7E };

计算正余弦

实现了根据输入角度值计算正弦和余弦值的功能

  1. void  IQSin_Cos_Cale(p_IQSin_Cos  pV)  
  2. {
  3.   uint16_t  hindex;
  4.   hindex = (uint16_t) pV->IQAngle; //-32768--- 32767 +32768===0--65535
  5.   hindex >>=6;      //65536/64  ===1024/4=90度=256 0x01-0xFF  0X100  0X1FF  0X200 0X2FF    0X300  0X3FF
  7.   switch (hindex & SIN_RAD)  //  0X300  &   0x0000 -ff    0x0100  1ff   0X200    2ff   0x0300   3ff  
  8.   {
  9.     case U0_90:                        
  10.        pV->IQSin = IQSin_Cos_Table[(uint8_t)(hindex)];  // 0---255  ==0---32766
  11.        pV->IQCos = IQSin_Cos_Table[(uint8_t)(0xFF-(u8)(hindex))];
  12.     break;
  14.     case U90_180:  
  15.        pV->IQSin = IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; // 255---0 == 0---32766 
  16.        pV->IQCos = -IQSin_Cos_Table[(u8)(hindex)];
  17.     break;
  19.     case U180_270:
  20.        pV->IQSin = -IQSin_Cos_Table[(u8)(hindex)];
  21.        pV->IQCos = -IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
  22.     break;
  24.     case U270_360:
  25.      pV->IQSin=  -IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
  26.      pV->IQCos =  IQSin_Cos_Table[(u8)(hindex)]; 
  27.     break;
  28.    default:
  29.     break;
  30.   } 
  31. }

首先，将输入角度值pV->IQAngle转换为无符号16位整数hindex。然后，对hindex进行右移6位，相当于将取值范围从0-65535缩小到0-1023，每64个值对应90度（256个值）。
接下来，通过对hindex与SIN_RAD进行按位与操作，将角度范围分为四个区间进行处理。
image.png

计算反正切值

  1. void  IQAtan_Cale(p_IQAtan pV)  
  2. {
  3.   int16_t   i=0; 
  4.     if( pV->Alpha == 0) 
  5.     { 
  6.         if (pV->Beta==0) 
  7.                     i =0; 
  8.         else       
  9.                     i =255; 
  10.     } 
  11.     else 
  12.     {  
  13.       pV->IQTan = _IQdiv(Abs(pV->Beta),Abs(pV->Alpha)); 
  15.             if (IQAtan2_Table[i + 128] <= pV->IQTan) i += 128; 
  16.             if (IQAtan2_Table[i + 64] <= pV->IQTan) i += 64; 
  17.             if (IQAtan2_Table[i + 32] <= pV->IQTan) i += 32; 
  18.             if (IQAtan2_Table[i + 16] <= pV->IQTan) i += 16; 
  19.             if (IQAtan2_Table[i + 8] <= pV->IQTan) i += 8; 
  20.             if (IQAtan2_Table[i + 4] <= pV->IQTan) i += 4; 
  21.             if (IQAtan2_Table[i + 2] <= pV->IQTan) i += 2; 
  22.             if (IQAtan2_Table[i + 1] <= pV->IQTan) i += 1;    
  24.         }
  26.     if ( pV->Beta> 0) 
  27.     { 
  28.         if (pV->Alpha >0) 
  29.            pV->IQAngle =i ;      
  30.         else      
  31.            pV->IQAngle= 512 -i;     
  32.     } 
  33.     else 
  34.     {  
  35.         if ( pV->Alpha >0)
  36.            pV->IQAngle= 1024-i;  
  37.                 else      
  38.            pV->IQAngle =i+512;     
  39.     } 
  40.      if( pV->IQAngle<0)
  41.              pV->IQAngle+=1024;
  42.    pV->IQAngle= pV->IQAngle<<6;
  44. }

用于计算输入值的平方根并返回结果

一种近似算法

  1. uint32_t IQSqrt(uint32_t  M)
  2. {
  3.     uint32_t   N, i ,tmp  ,ttp;    
  4.   if ( M==0 )
  5.       return 0;
  6.     N=0;
  8.   tmp=(M>>30);    
  10.   M<<=2;
  11.     if( tmp>1 )
  12.     {
  13.       N++;
  14.         tmp-=N;
  15.     }    
  16.     for (i=15;i>0;i--  )
  17.     {
  18.         N<<=1;
  20.         tmp<<=2;
  21.         tmp+=(M>>30);
  23.         ttp=N;    
  24.         ttp=(ttp<<1)+1;    
  26.         M<<=2;
  27.         if( tmp>= ttp )
  28.         {
  29.              tmp-=ttp;
  30.              N++;
  31.         }
  32.     }    
  33.      return N;            
  34. }

代码

IQ_math.c

  1. #include "IQ_math.h"
  4. const int16_t IQSin_Cos_Table[256]={\
  5. 0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,\
  6. 0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,\
  7. 0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,\
  8. 0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,\
  9. 0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,\
  10. 0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,\
  11. 0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,\
  12. 0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,\
  13. 0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,\
  14. 0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,\
  15. 0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,\
  16. 0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,\
  17. 0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,\
  18. 0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,\
  19. 0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,\
  20. 0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,\
  21. 0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,\
  22. 0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,\
  23. 0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,\
  24. 0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,\
  25. 0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,\
  26. 0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,\
  27. 0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,\
  28. 0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,\
  29. 0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,\
  30. 0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,\
  31. 0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,\
  32. 0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,\
  33. 0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,\
  34. 0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,\
  35. 0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,\
  36. 0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE}; 
  38. const int32_t IQAtan2_Table[256]={\
  39. 0x00000000,0x000000C9,0x00000192,0x0000025B,0x00000324,0x000003ED,0x000004B6,0x00000580,\
  40. 0x00000649,0x00000712,0x000008A7,0x00000971,0x00000A3B,0x00000B05,0x00000BD0,0x00000C9B,\
  41. 0x00000D66,0x00000E31,0x00000EFD,0x00000FC9,0x00001095,0x00001162,0x00001218,0x0000122F,\
  42. 0x000012FC,0x000013CA,0x00001498,0x00001566,0x00001635,0x00001705,0x000017D4,0x000018A5,\
  43. 0x00001975,0x00001A47,0x00001B19,0x00001BEB,0x00001CBE,0x00001D91,0x00001E65,0x00001F3A,\
  44. 0x0000200F,0x000020E5,0x000021BC,0x00002294,0x0000236C,0x00002444,0x0000251E,0x000025F8,\
  45. 0x000026D4,0x000027B0,0x0000288C,0x0000296A,0x00002A49,0x00002B28,0x00002C08,0x00002CEA,\
  46. 0x00002DCC,0x00002EAF,0x00002F94,0x00003079,0x00003160,0x00003247,0x00003330,0x00003419,\
  47. 0x00003509,0x00003571,0x000036DE,0x000037CD,0x000038BD,0x000039AE,0x00003AA0,0x00003B94,\
  48. 0x00003C8A,0x00003D80,0x00003E79,0x00003F72,0x0000406E,0x0000416A,0x00004269,0x00004369,\
  49. 0x0000446A,0x0000456E,0x00004673,0x0000477A,0x00004882,0x0000498D,0x00004A99,0x00004BA8,\
  50. 0x00004CB8,0x00004DCA,0x00004EDF,0x00004FF5,0x0000510E,0x00005228,0x00005345,0x00005465,\
  51. 0x00005586,0x000056AA,0x000057D1,0x000058FA,0x00005A25,0x00005B53,0x00005C84,0x00005DB8,\
  52. 0x00005EEE,0x00006027,0x00006163,0x000062A2,0x000063E4,0x00006529,0x00006671,0x000067BD,\
  53. 0x0000690C,0x00006A5E,0x00006BB3,0x00006D0D,0x00006E69,0x00006FCA,0x0000712E,0x00007296,\
  54. 0x00007403,0x00007573,0x000076E7,0x00007860,0x000079DD,0x00007B5F,0x00007CE5,0x00007E70,\
  55. 0x00008000,0x00008194,0x0000832E,0x000084CD,0x00008671,0x0000881A,0x000089CA,0x00008B7F,\
  56. 0x00008D39,0x00008EFA,0x000090C1,0x0000928F,0x00009463,0x0000963D,0x0000981F,0x00009A08,\
  57. 0x00009BF7,0x00009DEF,0x00009FEE,0x0000A1F5,0x0000A404,0x0000A61B,0x0000A83B,0x0000AA64,\
  58. 0x0000AC96,0x0000AED1,0x0000B116,0x0000B365,0x0000B5BE,0x0000B822,0x0000BA91,0x0000BD0B,\
  59. 0x0000BF90,0x0000C222,0x0000C4C0,0x0000C76A,0x0000CA22,0x0000CCE7,0x0000CF88,0x0000D29D,\
  60. 0x0000D58E,0x0000D88E,0x0000DB9F,0x0000DEC0,0x0000E1F3,0x0000E538,0x0000E88F,0x0000EBFA,\
  61. 0x0000EF78,0x0000F30B,0x0000F6B4,0x0000FA74,0x0000FE4A,0x00010239,0x00010641,0x00010A64,\
  62. 0x00010EA2,0x000112FC,0x00011774,0x00011C0B,0x00012C03,0x0001259C,0x00012A99,0x00012FBC,\
  63. 0x00013504,0x00013A76,0x00014012,0x000145DB,0x00014BD3,0x000151FD,0x0001585A,0x00015EEE,\
  64. 0x000165BC,0x00016CC6,0x00017411,0x00017B9F,0x00018376,0x00018B98,0x0001940A,0x00019CD2,\
  65. 0x0001A5F5,0x0001AF78,0x0001B963,0x0001C3BB,0x0001CE88,0x0001D9D2,0x0001E5A3,0x0001F205,\
  66. 0x0001FF01,0x00020CA4,0x00021AFB,0x00022A15,0x00023A02,0x00024AD4,0x00025C9F,0x00026F7B,\
  67. 0x0002837F,0x000298CA,0x0002AF7C,0x0002C7BA,0x0002E1AE,0x0002FD8A,0x00031B84,0x00033BDF,\
  68. 0x00035EE7,0x000384F5,0x0003AE73,0x0003DBDD,0x00040DCB,0x000444F4,0x00048236,0x0004C6A6,\
  69. 0x0005139B,0x00056AC9,0x0005CE63,0x00064144,0x0006C740,0x0007658D,0x00082374,0x00090B81,\
  70. 0x000A2D7F,0x000BA246,0x000D9338,0x00104AD7,0x00145E24,0x001B28CC,0x0028BDDA,0x00517C7E };
  73. void  IQSin_Cos_Cale(p_IQSin_Cos  pV)  
  74. {
  75.   uint16_t  hindex;
  76.   hindex = (uint16_t) pV->IQAngle; //-32768--- 32767 +32768===0--65535
  77.   hindex >>=6;      //65536/64  ===1024/4=90度=256 0x01-0xFF  0X100  0X1FF  0X200 0X2FF    0X300  0X3FF
  79.   switch (hindex & SIN_RAD)  //  0X300  &   0x0000 -ff    0x0100  1ff   0X200    2ff   0x0300   3ff  
  80.   {
  81.     case U0_90:                        
  82.        pV->IQSin = IQSin_Cos_Table[(uint8_t)(hindex)];  // 0---255  ==0---32766
  83.        pV->IQCos = IQSin_Cos_Table[(uint8_t)(0xFF-(u8)(hindex))];
  84.     break;
  86.     case U90_180:  
  87.        pV->IQSin = IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))]; // 255---0 == 0---32766 
  88.        pV->IQCos = -IQSin_Cos_Table[(u8)(hindex)];
  89.     break;
  91.     case U180_270:
  92.        pV->IQSin = -IQSin_Cos_Table[(u8)(hindex)];
  93.        pV->IQCos = -IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
  94.     break;
  96.     case U270_360:
  97.      pV->IQSin=  -IQSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
  98.      pV->IQCos =  IQSin_Cos_Table[(u8)(hindex)]; 
  99.     break;
  100.    default:
  101.     break;
  102.   } 
  103. } 
  104. //该函数的作用是将输入值限制在指定的范围内，如果超出范围则将其限制在范围边界值上
  105. int32_t  IQsat( int32_t Uint,int32_t  U_max, int32_t U_min) 
  106. {
  107.      int32_t Uout; 
  108.      if(Uint<= U_min)
  109.         Uout= U_min;
  110.     else if( Uint>=U_max)
  111.         Uout=U_max;
  112.     else
  113.       Uout= Uint;   
  115.   return  Uout; 
  116. }
  117. //计算反正切值
  118. void  IQAtan_Cale(p_IQAtan pV)  
  119. {
  120.   int16_t   i=0; 
  121.     if( pV->Alpha == 0) 
  122.     { 
  123.         if (pV->Beta==0) 
  124.                     i =0; 
  125.         else       
  126.                     i =255; 
  127.     } 
  128.     else 
  129.     {  
  130.       pV->IQTan = _IQdiv(Abs(pV->Beta),Abs(pV->Alpha)); 
  132.             if (IQAtan2_Table[i + 128] <= pV->IQTan) i += 128; 
  133.             if (IQAtan2_Table[i + 64] <= pV->IQTan) i += 64; 
  134.             if (IQAtan2_Table[i + 32] <= pV->IQTan) i += 32; 
  135.             if (IQAtan2_Table[i + 16] <= pV->IQTan) i += 16; 
  136.             if (IQAtan2_Table[i + 8] <= pV->IQTan) i += 8; 
  137.             if (IQAtan2_Table[i + 4] <= pV->IQTan) i += 4; 
  138.             if (IQAtan2_Table[i + 2] <= pV->IQTan) i += 2; 
  139.             if (IQAtan2_Table[i + 1] <= pV->IQTan) i += 1;    
  141.         }
  143.     if ( pV->Beta> 0) 
  144.     { 
  145.         if (pV->Alpha >0) 
  146.            pV->IQAngle =i ;      
  147.         else      
  148.            pV->IQAngle= 512 -i;     
  149.     } 
  150.     else 
  151.     {  
  152.         if ( pV->Alpha >0)
  153.            pV->IQAngle= 1024-i;  
  154.                 else      
  155.            pV->IQAngle =i+512;     
  156.     } 
  157.      if( pV->IQAngle<0)
  158.              pV->IQAngle+=1024;
  159.    pV->IQAngle= pV->IQAngle<<6;
  161. }
  163. //用于计算输入值的平方根并返回结果
  164. uint32_t IQSqrt(uint32_t  M)
  165. {
  166.     uint32_t   N, i ,tmp  ,ttp;    
  167.   if ( M==0 )
  168.       return 0;
  169.     N=0;
  171.   tmp=(M>>30);    
  173.   M<<=2;
  174.     if( tmp>1 )
  175.     {
  176.       N++;
  177.         tmp-=N;
  178.     }    
  179.     for (i=15;i>0;i--  )
  180.     {
  181.         N<<=1;
  183.         tmp<<=2;
  184.         tmp+=(M>>30);
  186.         ttp=N;    
  187.         ttp=(ttp<<1)+1;    
  189.         M<<=2;
  190.         if( tmp>= ttp )
  191.         {
  192.              tmp-=ttp;
  193.              N++;
  194.         }
  195.     }    
  196.      return N;            
  197. }

IQ_math.h

  1. #ifndef _IQ_math_H
  2. #define _IQ_math_H 
  4. #include "stm32f10x.h"
  6. //共256个数
  8. #define Abs(A)    ((A>=0)?A:-A)  // 绝对值函数
  9. #define Min(A,B)  ((A<=B)?A:B)   // 求最小函数
  10. #define Max(A,B)  ((A>=B)?A:B)   // 求最大函数
  12. #define   _IQ15(A)       (int32_t)((A)<<15)  //左移15位 32768
  13. #define   _IQ(A)          _IQ15(A)           //定义IQ格式 32768
  14. #define   _IQmpy(A,B)    (int32_t)((A*B)>>15)  //IQ格式相乘 
  15. #define   _IQ10mpy(A,B)   (int32_t)((A*B)>>10) //IQ10格式相乘 
  16. #define   _IQdiv2(A)     (int32_t)((A)>>1)     //除2
  17. #define   _IQmpy2(A)     (int32_t)(A<<1)       //乘2
  18. #define   _IQdiv(A,B)    (int32_t)((A<<15)/B)  //IQ格式相除
  21. #define SIN_RAD     0x0300
  22. #define U0_90       0x0000 
  23. #define U90_180     0x0100
  24. #define U180_270    0x0200
  25. #define U270_360    0x0300
  28. typedef struct     { 
  29.             int32_t  IQAngle;  // 电机磁极位置角度0---65536即是0---360度         
  30.                   int32_t  IQSin;         // IQ格式正弦参数，-32768---32767  -1到1 
  31.                   int32_t  IQCos;         // IQ格式余弦参数，-32768---32767  -1到1     
  32.                 } IQSin_Cos , *p_IQSin_Cos;
  35. #define IQSin_Cos_DEFAULTS  { 0,0,0} // 初始化参数
  37. typedef struct     { 
  38.             int32_t  Alpha;     //二相静止坐标系 Alpha 轴     
  39.                   int32_t  Beta;        //二相静止坐标系 Beta 轴          
  40.                   int32_t  IQTan;        //IQ格式正切 45度正切是1，IQ的格式是 
  41.                   int32_t  IQAngle;    //IQ格式角度值 0---65536 == 0---360度 
  42.             int32_t  JZIQAngle; //矫正IQ格式角度值
  43.          } IQAtan , *p_IQAtan;
  46. #define IQAtan_DEFAULTS  {0,0,0,0,0}  // 初始化参数
  48. uint32_t IQSqrt(uint32_t  M); // 开方函数
  49. void  IQSin_Cos_Cale(p_IQSin_Cos pV); //求取正余弦函数 
  50. void  IQAtan_Cale(p_IQAtan  pV) ;  //求取求反正弦函数
  51. int32_t IQsat( int32_t Uint,int32_t  U_max, int32_t U_min); //限制赋值函数
  52. #endif /* __IQ_math_H */