![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2015/06/09
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Base_ver3_1 by
Diff: main.cpp
- Revision:
- 14:3aa6d735a2fa
- Parent:
- 13:82f4f00f11f6
- Child:
- 15:35e3917fcbf5
--- a/main.cpp Sun May 24 08:10:08 2015 +0000 +++ b/main.cpp Mon May 25 11:38:09 2015 +0000 @@ -39,7 +39,7 @@ #define SIGMA_MIN 0.0001 //************ID Number***************** -const char MyID = 'b'; +const char MyID = 'd'; //************ID Number***************** ///////////////////////////////////////// @@ -85,8 +85,13 @@ double Kx=0,Ky=0;//カルマンゲイン double zx,zy;//観測値 void Kalman(double Latitude,double Longitude); +int change = 0; - +/* +LocalFileSystem local("local"); // マウントポイントを定義(ディレクトリパスになる) +FILE *fp; +char filename[16] = "/local/out0.txt"; +*/ ///////////////////////////////////////// // //Send_Status @@ -134,7 +139,8 @@ ///////////////////////////////////////// void Get_GPS(Adafruit_GPS *myGPS){ - static int flag = 0; + static int flag = 0; +// static int save_counter = 0; if (myGPS->fix) { agz.nowStatus = GPS_AVAIL; @@ -149,7 +155,28 @@ } if(flag >= 16){ - Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); +// if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001){ + Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); +/* change = 1; + } + else{ + change = 0; + } + + if(save_counter < 10){ + fp = fopen(filename, "a"); + fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n", + change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), + agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), + agz.get_agzCov_lati(),agz.get_agzCov_longi()); + fclose(fp); + + if((flag - 16) % 500 == 0){ + filename[10]++; + save_counter++; + } + } +*/ } printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",