飞思卡尔智能车电磁组例程

#include "common.h" #include "math.h" /* function name: triangle_c parameters: a,b author: Xian Lee date: 2012-6-27 description: c = sqrt(a^2 + b^2), excute time:12us, accuracy:1 */ int32 triangle_c(int32 a,int32 b) { int32 c; int32 min,max; int64 squ,tsqu; squ = a*a + b*b; min = MAX(a,b); max = a + b; while(max - min > 1) { c = (max + min)>>1; tsqu = (int64)c*(int64)c; if(tsqu > squ) max = c; else if(tsqu < squ) min = c; else break; } return c; } /* function name: sqrt32 parameters: x author: Xian Lee date: 2012-6-29 description: extract an uint32 value, excute time:24-29us, accuracy:1 */ uint16 sqrt32(uint32 x) { uint32 squ_v[] = {0x1000000,0x4000000,0x9000000,0x10000000,0x19000000,0x24000000,0x31000000,0x40000000,0x51000000,0x64000000,0x79000000,0x90000000,0xa9000000,0xc4000000,0xe1000000}; uint16 root[] = {0x1000,0x2000,0x3000,0x4000,0x5000,0x6000,0x7000,0x8000,0x9000,0xa000,0xb000,0xc000,0xd000,0xe000,0xf000}; uint32 Aa,Bb,Cc,K,R; uint16 a,b,c,p,i; p = 0x800; R = 0x400000; for(i = 0;i < 15;i++) { if(squ_v[i] > x) break; } if(i == 0) { Aa = 0; a = 0; Cc = squ_v[0]; c = root[0]; } else if(i == 15) { Aa = squ_v[14]; a = root[14]; Cc = 0; c = 0; } else { Aa = squ_v[i-1]; a = root[i-1]; Cc = squ_v[i]; c = root[i]; } do { b = a + p; Bb = Aa; Bb >>= 1; if(Cc == 0) { K = 0x80000000; } else { K = Cc; K >>= 1; } Bb += K; Bb -= R; p >>= 1; R >>= 2; if(p>0) { if(x > Bb) { a = b; Aa = Bb; } else { c = b; Cc = Bb; } } } while(p>=1); K = (2*Cc + 2*Bb - 1)/4; Aa = (2*Aa + 2*Bb - 1)/4; Cc = K; if(x >= Cc) b = c; else if(x < Aa) b = a; else b = a + 1; return b; } /* function name: sqrt16 parameters: x author: Xian Lee date: 2012-6-29 description: extract an uint16 value, excute time:18-22us, accuracy:1 */ uint8 sqrt16(uint16 x) { uint16 squ_v[] = {0x400,0x1000,0x2400,0x4000,0x6400,0x9000,0xc400}; uint8 root[] = {0x20,0x40,0x60,0x80,0xa0,0xc0,0xe0}; uint16 Aa,Bb,Cc,K,R; uint8 a,b,c,p,i; p = 0x10; R = 0x100; for(i = 0;i < 7;i++) { if(squ_v[i] > x) break; } if(i == 0) { Aa = 0; a = 0; Cc = squ_v[0]; c = root[0]; } else if(i == 7) { Aa = squ_v[6]; a = root[6]; Cc = 0; c = 0; } else { Aa = squ_v[i-1]; a = root[i-1]; Cc = squ_v[i]; c = root[i]; } do { b = a + p; Bb = Aa; Bb >>= 1; if(Cc == 0) { K = 0x8000; } else { K = Cc; K >>= 1; } Bb += K; Bb -= R; p >>= 1; R >>= 2; if(p>0) { if(x > Bb) { a = b; Aa = Bb; } else { c = b; Cc = Bb; } } } while(p>=1); K = (2*Cc + 2*Bb - 1)/4; Aa = (2*Aa + 2*Bb - 1)/4; Cc = K; if(x >= Cc) b = c; else if(x < Aa) b = a; else b = a + 1; return b; } /* function name: arctan parameters: x author: Xian Lee date: 2012-7-1 description: y = arctan(x), excute time:44-55us, accuracy:0.1 degree */ float Tan[] = { 0,0.0174550649282176,0.0349207694917477,0.0524077792830412,0.0699268119435104,0.0874886635259240,0.105104235265676,0.122784560902905,0.140540834702391,0.158384440324536, 0.176326980708465,0.194380309137718,0.212556561670022,0.230868191125563,0.249328002843181,0.267949192431123,0.286745385758808,0.305730681458660,0.324919696232906,0.344327613289665, 0.363970234266202,0.383864035035416,0.404026225835157,0.424474816209605,0.445228685308536,0.466307658154999,0.487732588565861,0.509525449494429,0.531709431661479,0.554309051452769, 0.577350269189626,0.600860619027560,0.624869351909328,0.649407593197511,0.674508516842427,0.700207538209710,0.726542528005361,0.753554050102794,0.781285626506717,0.809784033195007, 0.839099631177280,0.869286737816227,0.900404044297840,0.932515086137662,0.965688774807074,1.00000000000000,1.03553031379057,1.07236871002468,1.11061251482919,1.15036840722101, 1.19175359259421,1.23489715653505,1.27994163219308,1.32704482162041,1.37638192047117,1.42814800674211,1.48256096851274,1.53986496381458,1.60033452904105,1.66427948235052, 1.73205080756888,1.80404775527142,1.88072646534633,1.96261050550515,2.05030384157930,2.14450692050956,2.24603677390422,2.35585236582375,2.47508685341630,2.60508906469380, 2.74747741945462,2.90421087767582,3.07768353717526,3.27085261848414,3.48741444384091,3.73205080756888,4.01078093353585,4.33147587428416,4.70463010947846,5.14455401597031, 5.67128181961772,6.31375151467505,7.11536972238422,8.14434642797461,9.51436445422261,11.4300523027613,14.3006662567119,19.0811366877282,28.6362532829157,57.2899616307599 }; float arctan(float x) { float nx,r; int16 down,up,mid; if(x >= Tan[89]) return 89; else if(x <= -Tan[89]) return -89; nx = ABS(x); down = 0; mid = 45; up = 89; while(up >= down) { if(nx > Tan[mid]) { down = mid + 1; } else { up = mid - 1; } mid = (up + down)>>1; } r = (nx - Tan[up])/(Tan[down] - Tan[up]) + up; if(x < 0) r = -r; return r; } float tan(float x) { int16 nx,d=1; int16 down,up; float result=0; if(x >= 89) x=89; else if(x <= -89) x= -89; d=x>0?1:-1; x=ABS(x); nx = (int16)x; if(nx<x) { down=nx; up=nx+1; result= (Tan[up]-Tan[down])*(x-nx)+Tan[down]; } else if(nx>x) { down=nx-1; up=nx; result= (Tan[up]-Tan[down])*(nx-x)+Tan[down]; } else { result= Tan[nx]; } return result*d; } /* function name: sin parameters: x author: Xian Lee date: 2012-7-1 description: y = sin(x), excute time:29-34us, accuracy:0.01 */ float Sin[] = { 0,0.0174524064372835,0.0348994967025010,0.0523359562429438,0.0697564737441253,0.0871557427476582,0.104528463267653,0.121869343405147,0.139173100960065,0.156434465040231,0.173648177666930, 0.190808995376545,0.207911690817759,0.224951054343865,0.241921895599668,0.258819045102521,0.275637355816999,0.292371704722737,0.309016994374947,0.325568154457157,0.342020143325669, 0.358367949545300,0.374606593415912,0.390731128489274,0.406736643075800,0.422618261740699,0.438371146789077,0.453990499739547,0.469471562785891,0.484809620246337,0.500000000000000, 0.515038074910054,0.529919264233205,0.544639035015027,0.559192903470747,0.573576436351046,0.587785252292473,0.601815023152048,0.615661475325658,0.629320391049837,0.642787609686539, 0.656059028990507,0.669130606358858,0.681998360062499,0.694658370458997,0.707106781186548,0.719339800338651,0.731353701619171,0.743144825477394,0.754709580222772,0.766044443118978, 0.777145961456971,0.788010753606722,0.798635510047293,0.809016994374948,0.819152044288992,0.829037572555042,0.838670567945424,0.848048096156426,0.857167300702112,0.866025403784439, 0.874619707139396,0.882947592858927,0.891006524188368,0.898794046299167,0.906307787036650,0.913545457642601,0.920504853452440,0.927183854566787,0.933580426497202,0.939692620785908, 0.945518575599317,0.951056516295154,0.956304755963035,0.961261695938319,0.965925826289068,0.970295726275997,0.974370064785235,0.978147600733806,0.981627183447664,0.984807753012208, 0.987688340595138,0.990268068741570,0.992546151641322,0.994521895368273,0.996194698091746,0.997564050259824,0.998629534754574,0.