yal kaiyo
130110_1 experiment ver
Fork of
130110_YNU_MPU_1
by 
yal kaiyo
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "SDFileSystem.h" 00003 00004 #define pi 3.1416 00005 #define data 6 00006 00007 #define zer 0.0 00008 #define hal 0.5 00009 #define one 1.0 00010 #define dtr 0.01745 00011 #define rtd 57.3 00012 //parameter// 00013 #define dt1 0.2 // 1/N 00014 #define hdt 0.1 // 1/2N 00015 #define md 6 //kizami N 00016 #define mt 7 //md+1 00017 #define mx 4 00018 #define mv 8 00019 #define mh 56 //mt*mv 00020 00021 Serial pc(USBTX, USBRX); 00022 DigitalOut led1(LED1); 00023 DigitalOut led2(LED2); 00024 DigitalOut led3(LED3); 00025 DigitalOut led4(LED4); 00026 00027 SDFileSystem sd(p5, p6, p7, p8, "sd"); 00028 Serial mavc(p9, p10); 00029 DigitalIn stop_sd(p11); 00030 00031 int main() { 00032 00033 // ******************** TORATANI ******************** 00034 pc.baud(921600); 00035 mavc.baud(115200); 00036 00037 int flag_lc = 0; 00038 int flag_cm = 0; 00039 int loop = 100; // for 2 minuts -> loop = 1200 00040 00041 char buf_char; 00042 unsigned int buf_hex_rx[30] = {0}; 00043 unsigned int buf_dec_rx[10] = {0}; 00044 unsigned int buf_dec_tx[3] = {0}; 00045 unsigned int buf_hex_tx[3][4] = {0}; 00046 00047 // rx 00048 double acc[3] = {0}; 00049 double gyro[3] = {0}; 00050 double azi; 00051 double alt; 00052 double GPS[2] = {0}; 00053 00054 // tx 00055 float com[3] = {0}; 00056 unsigned int H; 00057 unsigned int D[data] = {0}; 00058 H = 0x02; // header of dateset2 00059 D[0] = 0x7f; D[1] = 0xff; // p_com 00060 D[2] = 0x7f; D[3] = 0xff; // q_com 00061 D[4] = 0x7f; D[5] = 0xff; // r_com 00062 /* 00063 D[0] = 0x7; D[1] = 0xf; D[2] = 0xf; D[3] = 0xf; // p_com 00064 D[4] = 0x7; D[5] = 0xf; D[6] = 0xf; D[7] = 0xf; // q_com 00065 D[8] = 0x7; D[9] = 0xf; D[10] = 0xf; D[11] = 0xf; // r_com 00066 */ 00067 // ******************** TORATANI ******************** 00068 00069 // ******************** HOSHINO ********************* 00070 float time = 0; //time 00071 float xis = 0; 00072 float ets = 0; 00073 float psis = 0; 00074 float x0 = 0; 00075 float y0 = 0; 00076 float drf = 100; //syuutandaunrenji 00077 float ht0 = 50; 00078 float htf = 5; 00079 float gm0 = 20*pi/180; 00080 float gmf = 5*pi/180; 00081 float b0 = ht0; 00082 float b1 = tan(gm0); 00083 float b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf); 00084 float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf); 00085 float htc = 0; 00086 float x = 0; //down range 00087 float ua = 10; //initial horizontal velosity 00088 float xis1 = 0; 00089 float ets1 = 0; 00090 float xir = 0; 00091 float etr = 0; 00092 float drc = 0; 00093 float drp = 0; 00094 float dt = 0.1; 00095 float psr = 0; 00096 float htr = 0; 00097 float avp = 0; 00098 float avq = 0; 00099 float avr = 0; 00100 float s = 0; 00101 float c=0; 00102 float c0=0; 00103 float c1=0; 00104 float dr[mt]={0}; 00105 float time1=0; 00106 float kp=0; 00107 float kq=0; 00108 float kr=0; 00109 00110 //homotopy 00111 float rps0 = 90.0; 00112 float rxi0 = -600.0; //terminal xi 00113 float ret0 = -200.0; //terminal et 00114 float ps0 = rps0*dtr; 00115 float xi0 = rxi0/drf; 00116 float et0 = ret0/drf; 00117 float eps = 0.00000001; 00118 float ueps = 0.000001; 00119 float ier = 0.0; 00120 float sum = 0.0; 00121 float ph0[mh] = {0}; 00122 float phi[mh] = {0}; 00123 float fn[mh] = {0}; 00124 float wk[mh] = {0}; 00125 int iwk[mh] = {0}; 00126 float dph1[mh] = {0}; 00127 float dph2[mh] = {0}; 00128 float dph3[mh] = {0}; 00129 float dph4[mh] = {0}; 00130 float fph[mh][mh] = {0}; 00131 float fpi[mh][mh] = {0}; 00132 float x1[4][mt] = {0}; 00133 float rl[4][mt] = {0}; 00134 float qmax=0.0; 00135 float rdet =1.0; 00136 float idet =0.0; 00137 float t = 0.0; 00138 float phf[mh]={0}; 00139 float ap[mt]={0.0}; 00140 float ps[mt]={0.0}; 00141 float xi[mt]={0.0}; 00142 float et[mt]={0.0}; 00143 float xi1[mt]={0.0}; 00144 float et1[mt]={0.0}; 00145 float psc; 00146 float etc; 00147 float xic; 00148 //********************* HOSHINO ********************* 00149 00150 //********************* OHTSU *********************** 00151 //teigi 00152 float grv = 9.807; 00153 float swg = 0.04695; 00154 float wgt = 0.76; 00155 float rho = 1.2255; 00156 00157 float tt1 = 0.8; 00158 float omg1 = 1.0; 00159 float zet1 = 0.7; 00160 float tt2 = 0.37; 00161 float omg2 = 2.6; 00162 float zet2 = 0.7; 00163 00164 float gmr = 20; 00165 float vmr = 0; 00166 float pse = 0; 00167 float ete = 0; 00168 float hte = 0; 00169 00170 float hte00 = 0; 00171 float dhdt = 0; 00172 00173 float v2 = 0; 00174 00175 float gkp = 0; 00176 float gkd = 0; 00177 00178 float avpc = 0; 00179 float avqc = 0; 00180 float avrc = 0; 00181 //********************* OHTSU *********************** 00182 00183 FILE *fp = fopen("/sd/sdtest.txt", "w"); 00184 if(fp == NULL) { 00185 error("Could not open file for write\n"); 00186 } 00187 00188 // **************** waiting @LC or @FB **************** 00189 // * @LC:0x40 0x4C 0x43 0x0D, @FB:0x40 0x46 0x42 0x0D * 00190 // ///////////////////////////////////////////////////// 00191 // ////////////////////// NOTICE! ////////////////////// 00192 // // flag_lc==1 is debug mode. skip this while loop. // 00193 // ////////// for flight mode, flag_lc is 0! /////////// 00194 // ///////////////////////////////////////////////////// 00195 while(flag_lc==1){ 00196 buf_char = mavc.getc(); 00197 buf_hex_rx[0] = (unsigned int)buf_char; 00198 for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; } 00199 if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){ 00200 // ******************** waiting @LC ******************** 00201 00202 // ******************** homotopy ******************** 00203 00204 ////SYOKIKATEIKAI//// 00205 for(int it=0;it<mt;it++){ 00206 double tm=dt1*it; 00207 int i0=mv*it; 00208 ph0[i0]=zer; 00209 if(fabs(ps0)<=0.0000000001){ 00210 ph0[i0+1]=(ps0+1.0)*tm; 00211 } 00212 else{ 00213 ph0[i0+1] = ps0*tm; 00214 } 00215 ph0[i0+2]= xi0*tm; 00216 ph0[i0+3]= et0*tm; 00217 ph0[i0+4]=-hal; 00218 ph0[i0+5]=-hal; 00219 ph0[i0+6]=-hal; 00220 ph0[i0+7]=-hal; 00221 } 00222 for(int it=0;it<mh;it++){ 00223 phi[it]=ph0[it]; 00224 } 00225 for(int it=0;it<mh;it++){ 00226 phf[it]=ph0[it]; 00227 } 00228 00229 //rungekutta 00230 for(int ir=0;ir<mt;ir++){ 00231 //k1 00232 00233 ////SABUNSHIKI////// 00234 for(int it=0;it<mt;it++){ 00235 int i0=mv*it; 00236 x1[0][it]=phi[i0+0]; 00237 x1[1][it]=phi[i0+1]; 00238 x1[2][it]=phi[i0+2]; 00239 x1[3][it]=phi[i0+3]; 00240 rl[0][it]=phi[i0+4]; 00241 rl[1][it]=phi[i0+5]; 00242 rl[2][it]=phi[i0+6]; 00243 rl[3][it]=phi[i0+7]; 00244 } 00245 00246 fn[0]=x1[0][0]; 00247 fn[1]=x1[1][0]; 00248 fn[2]=x1[2][0]; 00249 fn[3]=x1[3][0]; 00250 for(int it=0;it<md;it++){ 00251 int i0=mx+mv*it; 00252 int it1=it+1; 00253 double bkp = hal*(x1[0][it]+x1[0][it1]); 00254 double bps = hal*(x1[1][it]+x1[1][it1]); 00255 double br1 = hal*(rl[0][it]+rl[0][it1]); 00256 double br2 = hal*(rl[1][it]+rl[1][it1]); 00257 double br3 = hal*(rl[2][it]+rl[2][it1]); 00258 double br4 = hal*(rl[3][it]+rl[3][it1]); 00259 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; 00260 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; 00261 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); 00262 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); 00263 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; 00264 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); 00265 fn[i0+6] = rl[2][it1]-rl[2][it]; 00266 fn[i0+7] = rl[3][it1]-rl[3][it]; 00267 } 00268 fn[mh-4]=x1[0][mt-1]; 00269 fn[mh-3]=x1[1][mt-1]-ps0; 00270 fn[mh-2]=x1[2][mt-1]-xi0; 00271 fn[mh-1]=x1[3][mt-1]-et0; 00272 00273 00274 ////HENBIBUN//////////// 00275 //// I 0 ////// 00276 //// A B 0 //// 00277 //// 0 A B 0 // 00278 /////////////// 00279 ////////////I 0 00280 //I 00281 fph[0][0]=one; 00282 fph[1][1]=one; 00283 fph[2][2]=one; 00284 fph[3][3]=one; 00285 //A 00286 for(int iu=0;iu<md;iu++){ 00287 int i0=mx+mv*iu; 00288 int j0=mv*iu; 00289 int j1=j0+mv; 00290 double bps=hal*(phi[j0+1]+phi[j1+1]); 00291 double br3=hal*(phi[j0+6]+phi[j1+6]); 00292 double br4=hal*(phi[j0+7]+phi[j1+7]); 00293 double cps=cos(bps); 00294 double sps=sin(bps); 00295 for(int iv=0;iv<mv;iv++){ 00296 fph[i0+iv][j0+iv]=-one; 00297 } 00298 fph[i0+1][j0] = hdt; 00299 fph[i0+2][j0+1] =-hdt*sps; 00300 fph[i0+3][j0+1] = hdt*cps; 00301 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); 00302 fph[i0][j0+4] = hdt; 00303 fph[i0+4][j0+5] =-hdt; 00304 fph[i0+5][j0+6] = hdt*sps; 00305 fph[i0+5][j0+7] =-hdt*cps; 00306 } 00307 //B 00308 //j0=j0+mv; 00309 for(int iu=0;iu<md;iu++){ 00310 int i0=mx+mv*iu; 00311 int j0=mv*(iu+1); 00312 int j1=j0+mv; 00313 double bps=hal*(phi[j0+1]+phi[j1+1]); 00314 double br3=hal*(phi[j0+6]+phi[j1+6]); 00315 double br4=hal*(phi[j0+7]+phi[j1+7]); 00316 double cps=cos(bps); 00317 double sps=sin(bps); 00318 for(int iv=0;iv<mv;iv++){ 00319 fph[i0+iv][j0+iv]=one; 00320 } 00321 fph[i0+1][j0]=hdt; 00322 fph[i0+2][j0+1]=-hdt*sps; 00323 fph[i0+3][j0+1]=hdt*cps; 00324 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); 00325 fph[i0][j0+4]=hdt; 00326 fph[i0+4][j0+5]=-hdt; 00327 fph[i0+5][j0+6]=hdt*sps; 00328 fph[i0+5][j0+7]=-hdt*cps; 00329 } 00330 //I 00331 fph[mh-4][mh-8]=one; 00332 fph[mh-3][mh-7]=one; 00333 fph[mh-2][mh-6]=one; 00334 fph[mh-1][mh-5]=one; 00335 00336 //GYAKUGYOURETSU 00337 for(int i=0;i<mh;i++){ 00338 for(int j=0;j<mh;j++){ 00339 fpi[i][j]=fph[i][j]; 00340 } 00341 } 00342 for(int j=0;j<mh;j++) { 00343 for(int i=0;i<mh;i++) { 00344 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } 00345 } 00346 } 00347 00348 for(int i=0;i<mh;i++) { 00349 double q=wk[i]; 00350 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} 00351 wk[i]=1.0/q; 00352 if(q > qmax){qmax=q;} 00353 } 00354 00355 if(eps < 0){eps=qmax*mh*ueps;} 00356 rdet =1.0; 00357 idet =0.0; 00358 for(int k=0;k<mh;k++) { 00359 double amax = 0.0; 00360 int kmax = k; 00361 for(int i=k;i<mh;i++) { 00362 if(fabs(fpi[i][k])*wk[i] <= amax){} 00363 else{ 00364 amax=fabs(fpi[i][k])*wk[i]; 00365 kmax=i; 00366 } 00367 } 00368 //if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} 00369 rdet=rdet*fpi[kmax][k]; 00370 if(k != kmax){rdet=-rdet;} 00371 double adet =fabs(rdet); 00372 while(adet > 1.0){ 00373 adet = adet*0.0625; 00374 idet = idet + 4; 00375 } 00376 if(adet >= 0.0625){} 00377 else{ 00378 adet=adet*16.0; 00379 idet=idet-4; 00380 } 00381 if(rdet < 0.0){adet=-adet;} 00382 rdet=adet; 00383 iwk[k]=kmax; 00384 amax=-1.0/fpi[kmax][k]; 00385 t=fpi[kmax][k]; 00386 fpi[kmax][k]=fpi[k][k]; 00387 fpi[k][k]=t; 00388 for(int j=0;j<mh;j++) { 00389 if(j == k){} 00390 else{ 00391 t=fpi[kmax][j]*amax; 00392 fpi[kmax][j]=fpi[k][j]; 00393 if(fabs(t) <= 0){} 00394 else{ 00395 for(int i=0;i<mh;i++) { 00396 fpi[i][j]=fpi[i][j]+t*fpi[i][k]; 00397 } 00398 } 00399 fpi[k][j]=-t; 00400 } 00401 } 00402 for(int i=0;i<mh;i++) { 00403 fpi[i][k]=fpi[i][k]*amax; 00404 } 00405 fpi[k][k]=-amax; 00406 } 00407 00408 for(int l=0;l<mh;l++) { 00409 int k=mh-l-1; 00410 if(k == iwk[k]){} 00411 else{ 00412 int kmax=iwk[k]; 00413 for(int i=0;i<mh;i++) { 00414 t=fpi[i][k]; 00415 fpi[i][k]=fpi[i][kmax]; 00416 fpi[i][kmax]=t; 00417 } 00418 } 00419 } 00420 00421 //////function 00422 for(int jh=0;jh<mh;jh++){ 00423 sum=zer; 00424 for(int ih=0;ih<mh;ih++){ 00425 sum=sum-fpi[jh][ih]*fn[ih]; 00426 } 00427 dph1[jh]=sum; 00428 } 00429 00430 for(int it=0;it<mh;it++){ 00431 phi[it]= phf[it]+hal*dt1*dph1[it]; 00432 } 00433 00434 00435 //k2 00436 ////SABUNSHIKI////// 00437 for(int it=0;it<mt;it++){ 00438 int i0=mv*it; 00439 x1[0][it]=phi[i0+0]; 00440 x1[1][it]=phi[i0+1]; 00441 x1[2][it]=phi[i0+2]; 00442 x1[3][it]=phi[i0+3]; 00443 rl[0][it]=phi[i0+4]; 00444 rl[1][it]=phi[i0+5]; 00445 rl[2][it]=phi[i0+6]; 00446 rl[3][it]=phi[i0+7]; 00447 } 00448 00449 fn[0]=x1[0][0]; 00450 fn[1]=x1[1][0]; 00451 fn[2]=x1[2][0]; 00452 fn[3]=x1[3][0]; 00453 for(int it=0;it<md;it++){ 00454 int i0=mx+mv*it; 00455 int it1=it+1; 00456 double bkp = hal*(x1[0][it]+x1[0][it1]); 00457 double bps = hal*(x1[1][it]+x1[1][it1]); 00458 double br1 = hal*(rl[0][it]+rl[0][it1]); 00459 double br2 = hal*(rl[1][it]+rl[1][it1]); 00460 double br3 = hal*(rl[2][it]+rl[2][it1]); 00461 double br4 = hal*(rl[3][it]+rl[3][it1]); 00462 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; 00463 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; 00464 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); 00465 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); 00466 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; 00467 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); 00468 fn[i0+6] = rl[2][it1]-rl[2][it]; 00469 fn[i0+7] = rl[3][it1]-rl[3][it]; 00470 } 00471 fn[mh-4]=x1[0][mt-1]; 00472 fn[mh-3]=x1[1][mt-1]-ps0; 00473 fn[mh-2]=x1[2][mt-1]-xi0; 00474 fn[mh-1]=x1[3][mt-1]-et0; 00475 00476 00477 ////HENBIBUN//////////// 00478 //// I 0 ////// 00479 //// A B 0 //// 00480 //// 0 A B 0 // 00481 /////////////// 00482 ////////////I 0 00483 //I 00484 fph[0][0]=one; 00485 fph[1][1]=one; 00486 fph[2][2]=one; 00487 fph[3][3]=one; 00488 //A 00489 for(int iu=0;iu<md;iu++){ 00490 int i0=mx+mv*iu; 00491 int j0=mv*iu; 00492 int j1=j0+mv; 00493 double bps=hal*(phi[j0+1]+phi[j1+1]); 00494 double br3=hal*(phi[j0+6]+phi[j1+6]); 00495 double br4=hal*(phi[j0+7]+phi[j1+7]); 00496 double cps=cos(bps); 00497 double sps=sin(bps); 00498 for(int iv=0;iv<mv;iv++){ 00499 fph[i0+iv][j0+iv]=-one; 00500 } 00501 fph[i0+1][j0] = hdt; 00502 fph[i0+2][j0+1] =-hdt*sps; 00503 fph[i0+3][j0+1] = hdt*cps; 00504 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); 00505 fph[i0][j0+4] = hdt; 00506 fph[i0+4][j0+5] =-hdt; 00507 fph[i0+5][j0+6] = hdt*sps; 00508 fph[i0+5][j0+7] =-hdt*cps; 00509 } 00510 //B 00511 //j0=j0+mv; 00512 for(int iu=0;iu<md;iu++){ 00513 int i0=mx+mv*iu; 00514 int j0=mv*(iu+1); 00515 int j1=j0+mv; 00516 double bps=hal*(phi[j0+1]+phi[j1+1]); 00517 double br3=hal*(phi[j0+6]+phi[j1+6]); 00518 double br4=hal*(phi[j0+7]+phi[j1+7]); 00519 double cps=cos(bps); 00520 double sps=sin(bps); 00521 for(int iv=0;iv<mv;iv++){ 00522 fph[i0+iv][j0+iv]=one; 00523 } 00524 fph[i0+1][j0]=hdt; 00525 fph[i0+2][j0+1]=-hdt*sps; 00526 fph[i0+3][j0+1]=hdt*cps; 00527 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); 00528 fph[i0][j0+4]=hdt; 00529 fph[i0+4][j0+5]=-hdt; 00530 fph[i0+5][j0+6]=hdt*sps; 00531 fph[i0+5][j0+7]=-hdt*cps; 00532 } 00533 //I 00534 fph[mh-4][mh-8]=one; 00535 fph[mh-3][mh-7]=one; 00536 fph[mh-2][mh-6]=one; 00537 fph[mh-1][mh-5]=one; 00538 00539 //GYAKUGYOURETSU 00540 for(int i=0;i<mh;i++){ 00541 for(int j=0;j<mh;j++){ 00542 fpi[i][j]=fph[i][j]; 00543 } 00544 } 00545 for(int j=0;j<mh;j++) { 00546 for(int i=0;i<mh;i++) { 00547 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } 00548 } 00549 } 00550 00551 for(int i=0;i<mh;i++) { 00552 double q=wk[i]; 00553 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} 00554 wk[i]=1.0/q; 00555 if(q > qmax){qmax=q;} 00556 } 00557 00558 if(eps < 0){eps=qmax*mh*ueps;} 00559 rdet =1.0; 00560 idet =0.0; 00561 for(int k=0;k<mh;k++) { 00562 double amax = 0.0; 00563 int kmax = k; 00564 for(int i=k;i<mh;i++) { 00565 if(fabs(fpi[i][k])*wk[i] <= amax){} 00566 else{ 00567 amax=fabs(fpi[i][k])*wk[i]; 00568 kmax=i; 00569 } 00570 } 00571 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} 00572 rdet=rdet*fpi[kmax][k]; 00573 if(k != kmax){rdet=-rdet;} 00574 double adet =fabs(rdet); 00575 while(adet > 1.0){ 00576 adet = adet*0.0625; 00577 idet = idet + 4; 00578 } 00579 if(adet >= 0.0625){} 00580 else{ 00581 adet=adet*16.0; 00582 idet=idet-4; 00583 } 00584 if(rdet < 0.0){adet=-adet;} 00585 rdet=adet; 00586 iwk[k]=kmax; 00587 amax=-1.0/fpi[kmax][k]; 00588 t=fpi[kmax][k]; 00589 fpi[kmax][k]=fpi[k][k]; 00590 fpi[k][k]=t; 00591 for(int j=0;j<mh;j++) { 00592 if(j == k){} 00593 else{ 00594 t=fpi[kmax][j]*amax; 00595 fpi[kmax][j]=fpi[k][j]; 00596 if(fabs(t) <= 0){} 00597 else{ 00598 for(int i=0;i<mh;i++) { 00599 fpi[i][j]=fpi[i][j]+t*fpi[i][k]; 00600 } 00601 } 00602 fpi[k][j]=-t; 00603 } 00604 } 00605 for(int i=0;i<mh;i++) { 00606 fpi[i][k]=fpi[i][k]*amax; 00607 } 00608 fpi[k][k]=-amax; 00609 } 00610 00611 for(int l=0;l<mh;l++) { 00612 int k=mh-l-1; 00613 if(k == iwk[k]){} 00614 else{ 00615 int kmax=iwk[k]; 00616 for(int i=0;i<mh;i++) { 00617 t=fpi[i][k]; 00618 fpi[i][k]=fpi[i][kmax]; 00619 fpi[i][kmax]=t; 00620 } 00621 } 00622 } 00623 00624 //////function 00625 for(int jh=0;jh<mh;jh++){ 00626 sum=zer; 00627 for(int ih=0;ih<mh;ih++){ 00628 sum=sum-fpi[jh][ih]*fn[ih]; 00629 } 00630 dph2[jh]=sum; 00631 } 00632 00633 //k3 00634 for(int it=0;it<mh;it++){ 00635 phi[it]= phf[it]+hal*dt1*dph2[it]; 00636 } 00637 ////SABUNSHIKI////// 00638 for(int it=0;it<mt;it++){ 00639 int i0=mv*it; 00640 x1[0][it]=phi[i0+0]; 00641 x1[1][it]=phi[i0+1]; 00642 x1[2][it]=phi[i0+2]; 00643 x1[3][it]=phi[i0+3]; 00644 rl[0][it]=phi[i0+4]; 00645 rl[1][it]=phi[i0+5]; 00646 rl[2][it]=phi[i0+6]; 00647 rl[3][it]=phi[i0+7]; 00648 } 00649 00650 fn[0]=x1[0][0]; 00651 fn[1]=x1[1][0]; 00652 fn[2]=x1[2][0]; 00653 fn[3]=x1[3][0]; 00654 for(int it=0;it<md;it++){ 00655 int i0=mx+mv*it; 00656 int it1=it+1; 00657 double bkp = hal*(x1[0][it]+x1[0][it1]); 00658 double bps = hal*(x1[1][it]+x1[1][it1]); 00659 double br1 = hal*(rl[0][it]+rl[0][it1]); 00660 double br2 = hal*(rl[1][it]+rl[1][it1]); 00661 double br3 = hal*(rl[2][it]+rl[2][it1]); 00662 double br4 = hal*(rl[3][it]+rl[3][it1]); 00663 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; 00664 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; 00665 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); 00666 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); 00667 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; 00668 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); 00669 fn[i0+6] = rl[2][it1]-rl[2][it]; 00670 fn[i0+7] = rl[3][it1]-rl[3][it]; 00671 } 00672 fn[mh-4]=x1[0][mt-1]; 00673 fn[mh-3]=x1[1][mt-1]-ps0; 00674 fn[mh-2]=x1[2][mt-1]-xi0; 00675 fn[mh-1]=x1[3][mt-1]-et0; 00676 00677 00678 ////HENBIBUN//////////// 00679 //// I 0 ////// 00680 //// A B 0 //// 00681 //// 0 A B 0 // 00682 /////////////// 00683 ////////////I 0 00684 //I 00685 fph[0][0]=one; 00686 fph[1][1]=one; 00687 fph[2][2]=one; 00688 fph[3][3]=one; 00689 //A 00690 for(int iu=0;iu<md;iu++){ 00691 int i0=mx+mv*iu; 00692 int j0=mv*iu; 00693 int j1=j0+mv; 00694 double bps=hal*(phi[j0+1]+phi[j1+1]); 00695 double br3=hal*(phi[j0+6]+phi[j1+6]); 00696 double br4=hal*(phi[j0+7]+phi[j1+7]); 00697 double cps=cos(bps); 00698 double sps=sin(bps); 00699 for(int iv=0;iv<mv;iv++){ 00700 fph[i0+iv][j0+iv]=-one; 00701 } 00702 fph[i0+1][j0] = hdt; 00703 fph[i0+2][j0+1] =-hdt*sps; 00704 fph[i0+3][j0+1] = hdt*cps; 00705 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); 00706 fph[i0][j0+4] = hdt; 00707 fph[i0+4][j0+5] =-hdt; 00708 fph[i0+5][j0+6] = hdt*sps; 00709 fph[i0+5][j0+7] =-hdt*cps; 00710 } 00711 //B 00712 //j0=j0+mv; 00713 for(int iu=0;iu<md;iu++){ 00714 int i0=mx+mv*iu; 00715 int j0=mv*(iu+1); 00716 int j1=j0+mv; 00717 double bps=hal*(phi[j0+1]+phi[j1+1]); 00718 double br3=hal*(phi[j0+6]+phi[j1+6]); 00719 double br4=hal*(phi[j0+7]+phi[j1+7]); 00720 double cps=cos(bps); 00721 double sps=sin(bps); 00722 for(int iv=0;iv<mv;iv++){ 00723 fph[i0+iv][j0+iv]=one; 00724 } 00725 fph[i0+1][j0]=hdt; 00726 fph[i0+2][j0+1]=-hdt*sps; 00727 fph[i0+3][j0+1]=hdt*cps; 00728 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); 00729 fph[i0][j0+4]=hdt; 00730 fph[i0+4][j0+5]=-hdt; 00731 fph[i0+5][j0+6]=hdt*sps; 00732 fph[i0+5][j0+7]=-hdt*cps; 00733 } 00734 //I 00735 fph[mh-4][mh-8]=one; 00736 fph[mh-3][mh-7]=one; 00737 fph[mh-2][mh-6]=one; 00738 fph[mh-1][mh-5]=one; 00739 00740 //GYAKUGYOURETSU 00741 for(int i=0;i<mh;i++){ 00742 for(int j=0;j<mh;j++){ 00743 fpi[i][j]=fph[i][j]; 00744 } 00745 } 00746 for(int j=0;j<mh;j++) { 00747 for(int i=0;i<mh;i++) { 00748 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } 00749 } 00750 } 00751 00752 for(int i=0;i<mh;i++) { 00753 double q=wk[i]; 00754 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} 00755 wk[i]=1.0/q; 00756 if(q > qmax){qmax=q;} 00757 } 00758 00759 if(eps < 0){eps=qmax*mh*ueps;} 00760 rdet =1.0; 00761 idet =0.0; 00762 for(int k=0;k<mh;k++) { 00763 double amax = 0.0; 00764 int kmax = k; 00765 for(int i=k;i<mh;i++) { 00766 if(fabs(fpi[i][k])*wk[i] <= amax){} 00767 else{ 00768 amax=fabs(fpi[i][k])*wk[i]; 00769 kmax=i; 00770 } 00771 } 00772 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} 00773 rdet=rdet*fpi[kmax][k]; 00774 if(k != kmax){rdet=-rdet;} 00775 double adet =fabs(rdet); 00776 while(adet > 1.0){ 00777 adet = adet*0.0625; 00778 idet = idet + 4; 00779 } 00780 if(adet >= 0.0625){} 00781 else{ 00782 adet=adet*16.0; 00783 idet=idet-4; 00784 } 00785 if(rdet < 0.0){adet=-adet;} 00786 rdet=adet; 00787 iwk[k]=kmax; 00788 amax=-1.0/fpi[kmax][k]; 00789 t=fpi[kmax][k]; 00790 fpi[kmax][k]=fpi[k][k]; 00791 fpi[k][k]=t; 00792 for(int j=0;j<mh;j++) { 00793 if(j == k){} 00794 else{ 00795 t=fpi[kmax][j]*amax; 00796 fpi[kmax][j]=fpi[k][j]; 00797 if(fabs(t) <= 0){} 00798 else{ 00799 for(int i=0;i<mh;i++) { 00800 fpi[i][j]=fpi[i][j]+t*fpi[i][k]; 00801 } 00802 } 00803 fpi[k][j]=-t; 00804 } 00805 } 00806 for(int i=0;i<mh;i++) { 00807 fpi[i][k]=fpi[i][k]*amax; 00808 } 00809 fpi[k][k]=-amax; 00810 } 00811 00812 for(int l=0;l<mh;l++) { 00813 int k=mh-l-1; 00814 if(k == iwk[k]){} 00815 else{ 00816 int kmax=iwk[k]; 00817 for(int i=0;i<mh;i++) { 00818 t=fpi[i][k]; 00819 fpi[i][k]=fpi[i][kmax]; 00820 fpi[i][kmax]=t; 00821 } 00822 } 00823 } 00824 00825 //////function 00826 for(int jh=0;jh<mh;jh++){ 00827 sum=zer; 00828 for(int ih=0;ih<mh;ih++){ 00829 sum=sum-fpi[jh][ih]*fn[ih]; 00830 } 00831 dph3[jh]=sum; 00832 } 00833 00834 //k4 00835 00836 for(int it=0;it<mh;it++){ 00837 phi[it]= phf[it]+dt1*dph3[it]; 00838 } 00839 ////SABUNSHIKI////// 00840 for(int it=0;it<mt;it++){ 00841 int i0=mv*it; 00842 x1[0][it]=phi[i0+0]; 00843 x1[1][it]=phi[i0+1]; 00844 x1[2][it]=phi[i0+2]; 00845 x1[3][it]=phi[i0+3]; 00846 rl[0][it]=phi[i0+4]; 00847 rl[1][it]=phi[i0+5]; 00848 rl[2][it]=phi[i0+6]; 00849 rl[3][it]=phi[i0+7]; 00850 } 00851 00852 fn[0]=x1[0][0]; 00853 fn[1]=x1[1][0]; 00854 fn[2]=x1[2][0]; 00855 fn[3]=x1[3][0]; 00856 for(int it=0;it<md;it++){ 00857 int i0=mx+mv*it; 00858 int it1=it+1; 00859 double bkp = hal*(x1[0][it]+x1[0][it1]); 00860 double bps = hal*(x1[1][it]+x1[1][it1]); 00861 double br1 = hal*(rl[0][it]+rl[0][it1]); 00862 double br2 = hal*(rl[1][it]+rl[1][it1]); 00863 double br3 = hal*(rl[2][it]+rl[2][it1]); 00864 double br4 = hal*(rl[3][it]+rl[3][it1]); 00865 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; 00866 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; 00867 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); 00868 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); 00869 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; 00870 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); 00871 fn[i0+6] = rl[2][it1]-rl[2][it]; 00872 fn[i0+7] = rl[3][it1]-rl[3][it]; 00873 } 00874 fn[mh-4]=x1[0][mt-1]; 00875 fn[mh-3]=x1[1][mt-1]-ps0; 00876 fn[mh-2]=x1[2][mt-1]-xi0; 00877 fn[mh-1]=x1[3][mt-1]-et0; 00878 00879 00880 ////HENBIBUN//////////// 00881 //// I 0 ////// 00882 //// A B 0 //// 00883 //// 0 A B 0 // 00884 /////////////// 00885 ////////////I 0 00886 //I 00887 fph[0][0]=one; 00888 fph[1][1]=one; 00889 fph[2][2]=one; 00890 fph[3][3]=one; 00891 //A 00892 for(int iu=0;iu<md;iu++){ 00893 int i0=mx+mv*iu; 00894 int j0=mv*iu; 00895 int j1=j0+mv; 00896 double bps=hal*(phi[j0+1]+phi[j1+1]); 00897 double br3=hal*(phi[j0+6]+phi[j1+6]); 00898 double br4=hal*(phi[j0+7]+phi[j1+7]); 00899 double cps=cos(bps); 00900 double sps=sin(bps); 00901 for(int iv=0;iv<mv;iv++){ 00902 fph[i0+iv][j0+iv]=-one; 00903 } 00904 fph[i0+1][j0] = hdt; 00905 fph[i0+2][j0+1] =-hdt*sps; 00906 fph[i0+3][j0+1] = hdt*cps; 00907 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); 00908 fph[i0][j0+4] = hdt; 00909 fph[i0+4][j0+5] =-hdt; 00910 fph[i0+5][j0+6] = hdt*sps; 00911 fph[i0+5][j0+7] =-hdt*cps; 00912 } 00913 //B 00914 //j0=j0+mv; 00915 for(int iu=0;iu<md;iu++){ 00916 int i0=mx+mv*iu; 00917 int j0=mv*(iu+1); 00918 int j1=j0+mv; 00919 double bps=hal*(phi[j0+1]+phi[j1+1]); 00920 double br3=hal*(phi[j0+6]+phi[j1+6]); 00921 double br4=hal*(phi[j0+7]+phi[j1+7]); 00922 double cps=cos(bps); 00923 double sps=sin(bps); 00924 for(int iv=0;iv<mv;iv++){ 00925 fph[i0+iv][j0+iv]=one; 00926 } 00927 fph[i0+1][j0]=hdt; 00928 fph[i0+2][j0+1]=-hdt*sps; 00929 fph[i0+3][j0+1]=hdt*cps; 00930 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); 00931 fph[i0][j0+4]=hdt; 00932 fph[i0+4][j0+5]=-hdt; 00933 fph[i0+5][j0+6]=hdt*sps; 00934 fph[i0+5][j0+7]=-hdt*cps; 00935 } 00936 //I 00937 fph[mh-4][mh-8]=one; 00938 fph[mh-3][mh-7]=one; 00939 fph[mh-2][mh-6]=one; 00940 fph[mh-1][mh-5]=one; 00941 00942 //GYAKUGYOURETSU 00943 for(int i=0;i<mh;i++){ 00944 for(int j=0;j<mh;j++){ 00945 fpi[i][j]=fph[i][j]; 00946 } 00947 } 00948 for(int j=0;j<mh;j++) { 00949 for(int i=0;i<mh;i++) { 00950 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } 00951 } 00952 } 00953 00954 for(int i=0;i<mh;i++) { 00955 double q=wk[i]; 00956 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} 00957 wk[i]=1.0/q; 00958 if(q > qmax){qmax=q;} 00959 } 00960 00961 if(eps < 0){eps=qmax*mh*ueps;} 00962 rdet =1.0; 00963 idet =0.0; 00964 for(int k=0;k<mh;k++) { 00965 double amax = 0.0; 00966 int kmax = k; 00967 for(int i=k;i<mh;i++) { 00968 if(fabs(fpi[i][k])*wk[i] <= amax){} 00969 else{ 00970 amax=fabs(fpi[i][k])*wk[i]; 00971 kmax=i; 00972 } 00973 } 00974 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} 00975 rdet=rdet*fpi[kmax][k]; 00976 if(k != kmax){rdet=-rdet;} 00977 double adet =fabs(rdet); 00978 while(adet > 1.0){ 00979 adet = adet*0.0625; 00980 idet = idet + 4; 00981 } 00982 if(adet >= 0.0625){} 00983 else{ 00984 adet=adet*16.0; 00985 idet=idet-4; 00986 } 00987 if(rdet < 0.0){adet=-adet;} 00988 rdet=adet; 00989 iwk[k]=kmax; 00990 amax=-1.0/fpi[kmax][k]; 00991 t=fpi[kmax][k]; 00992 fpi[kmax][k]=fpi[k][k]; 00993 fpi[k][k]=t; 00994 for(int j=0;j<mh;j++) { 00995 if(j == k){} 00996 else{ 00997 t=fpi[kmax][j]*amax; 00998 fpi[kmax][j]=fpi[k][j]; 00999 if(fabs(t) <= 0){} 01000 else{ 01001 for(int i=0;i<mh;i++) { 01002 fpi[i][j]=fpi[i][j]+t*fpi[i][k]; 01003 } 01004 } 01005 fpi[k][j]=-t; 01006 } 01007 } 01008 for(int i=0;i<mh;i++) { 01009 fpi[i][k]=fpi[i][k]*amax; 01010 } 01011 fpi[k][k]=-amax; 01012 } 01013 01014 for(int l=0;l<mh;l++) { 01015 int k=mh-l-1; 01016 if(k == iwk[k]){} 01017 else{ 01018 int kmax=iwk[k]; 01019 for(int i=0;i<mh;i++) { 01020 t=fpi[i][k]; 01021 fpi[i][k]=fpi[i][kmax]; 01022 fpi[i][kmax]=t; 01023 } 01024 } 01025 } 01026 01027 //////function 01028 for(int jh=0;jh<mh;jh++){ 01029 sum=zer; 01030 for(int ih=0;ih<mh;ih++){ 01031 sum=sum-fpi[jh][ih]*fn[ih]; 01032 } 01033 dph4[jh]=sum; 01034 } 01035 01036 //saitekikai 01037 01038 for(int it=0;it<mh;it++){ 01039 phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0; 01040 } 01041 01042 } 01043 01044 for (int id=0; id< mt; ++id){ 01045 int ii = mt-id-1; 01046 int i0 = mv*ii; 01047 ap[id]=phi[i0+0]; 01048 ps[id]=phi[i0+1]; 01049 xi1[id]=phi[i0+2]*drf; 01050 et1[id]=phi[i0+3]*drf; 01051 xi[id]=xi1[id]-xi1[0]; 01052 et[id]=et1[id]-et1[0]; 01053 } 01054 led1 = 1; 01055 wait(1.0); 01056 01057 /////////////////// 01058 flag_lc = 1; // end of homotopy flag 01059 // ******************** homotopy ******************** 01060 01061 } 01062 } 01063 01064 while(flag_cm<loop){ 01065 01066 // ******************** sequence No.1 ******************** 01067 buf_char = mavc.getc(); 01068 buf_hex_rx[0] = (unsigned int)buf_char; 01069 for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; } 01070 if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x43 && buf_hex_rx[27]==0x4D && buf_hex_rx[26]==0x0D){ 01071 // ******************** sequence No.1 ******************** 01072 01073 // ******************** sequence No.2 ******************** 01074 // send to mavc 01075 mavc.putc(H); 01076 for(int i=0;i<data;i++){ mavc.putc(D[i]); } 01077 // ******************** sequence No.2 ******************** 01078 01079 // ******************** sequence No.3 ******************** 01080 // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 | 01081 // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY | 01082 // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] | 01083 // transrating hex to dec 01084 for(int i=0;i<10;i++){ buf_dec_rx[i] = buf_hex_rx[24-i*2]*256+buf_hex_rx[23-i*2]; } 01085 for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec_rx[i]-32768)/65535*20*9.8; } 01086 for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec_rx[i+3]-32768)/65535*600/180*pi; } 01087 azi = (double)buf_dec_rx[6]/65535*2*pi; 01088 alt = ((double)buf_dec_rx[7]-32768)*0.1; 01089 for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec_rx[i+8]-32768)*0.1; } 01090 // ******************** sequence No.3 ******************** 01091 01092 // ******************** initialization ******************* 01093 for(int i=0;i<mt;i++){ 01094 dr[i]=i*drf/md; 01095 } 01096 if(t == 0){ 01097 x0 = GPS[0]; 01098 y0 = GPS[1]; 01099 ps0 = azi; 01100 ht0 = alt; 01101 } 01102 ets = (GPS[0] - x0)*cos(ps0); 01103 xis = (GPS[1] - y0)*sin(ps0); 01104 psr = pi/2 -(azi - ps0); 01105 htr = 50+(alt-ht0); 01106 avp = gyro[0]; 01107 avq = gyro[1]; 01108 avr = gyro[2]; 01109 // ******************** initialization ******************* 01110 01111 // ******************** interpolation ******************** 01112 drp = drp + ua * dt; 01113 xir = xir + ua * dt * cos(psr); 01114 etr = etr + ua * dt * sin(psr); 01115 01116 if (xis != xis1 && fabs(xis-xis1)<50) { 01117 s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1); 01118 ua = 0.5*(ua+sqrt(s)/(time-time1)); 01119 drc = drc+ua*(time-time1); 01120 drp= drc; 01121 xir=xis; 01122 etr=ets; 01123 xis1=xis; 01124 ets1=ets; 01125 } 01126 // ******************** interpolation ******************** 01127 01128 // ******************** guidance law WO homo ************* 01129 htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp; 01130 01131 for(int i=0;i<mt;i++){ 01132 c = drp-dr[i]; 01133 c1= dr[i+1]-drp; 01134 01135 if(c >=0 && c1 >= 0){ 01136 c0=c/(dr[i+1]-dr[i]); 01137 c1=c1/(dr[i+1]-dr[i]); 01138 psc=c0*ps[i+1]+c1*ps[i]; 01139 xic=c0*xi[i+1]+c1*xi[i]; 01140 etc=c0*et[i+1]+c1*et[i]; 01141 break; 01142 } 01143 } 01144 01145 // ******************** guidance law WO homo ************* 01146 01147 // ******************** control law ********************** 01148 pse=(psc-psr)*dtr; 01149 ete=etc-etr; 01150 hte=htc-htr; 01151 01152 //horizontal 01153 /* 01154 gkp=omg1*omg1/grv; 01155 gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr; 01156 avpc=1/tt1*(gkd*pse+gkp*ete);// 01157 */ 01158 01159 //avpc=kp*hte; 01160 01161 //vertical 01162 /* 01163 v2=vmr*vmr; 01164 dhdt=(hte-hte00)/dt 01165 */ 01166 01167 // to translate deg/s to rad/s, add *pi/180. TORATANI 01168 //avqc=(-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte)))*pi/180; 01169 //hte00=hte; 01170 01171 //avqc=kq*(pse+ete); 01172 01173 // dummy output 01174 avpc = avpc+0.001*pi; 01175 avqc = 0; 01176 avrc = avrc-0.001*pi; 01177 01178 com[0] = avpc; 01179 com[1] = avqc; 01180 com[2] = avrc; 01181 01182 // transrating dec to hex 01183 for(int i=0;i<3;i++){ 01184 buf_dec_tx[i] = 19660.5/pi*com[i]+32768; 01185 01186 buf_hex_tx[i][0] = buf_dec_tx[i]/256; 01187 buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][0]*256; 01188 buf_hex_tx[i][1] = buf_dec_tx[i]; 01189 01190 for(int j=0;j<2;j++){ D[j+2*i] = buf_hex_tx[i][j]; } 01191 01192 } 01193 // ******************** control law ********************** 01194 01195 // !!! HOSHINO !!! fprintf add guidance, velocity and control 01196 fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n",time, gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]); 01197 01198 time = time + 0.1; 01199 flag_cm++; 01200 01201 } 01202 01203 } 01204 01205 fclose(fp); 01206 led1 = 1; led2 = 1; led3 = 1; led4 = 1; 01207 01208 }
Generated on Thu Jul 14 2022 22:32:34 by
1.7.2