main.cpp

00001 #include "mbed.h"
00002 
00003 //#include "" //SDcardで使用
00004 
00005 #include "LPS.h"    //気圧で使用
00006 
00007 #include "math.h"   //GPSで使用
00008 
00009 Serial xbee(PA_9, PA_10); //xbee pin
00010 Serial  gps(PA_9,PA_10);    //GPS後で変える
00011 Serial pc(USBRX,USBTX); //衛星側のpin
00012 
00013 I2C i2c(D0,D1); //気圧 pin
00014 LPS ps(i2c);
00015 
00016 
00017 Timer t;    //時間を定義
00018 const float dt = 1;   //wait_us(dt)で使用:データの間隔
00019 
00020 
00021 int i; //回数カウント用
00022 
00023 float data1[10];    //気圧の較正用データ
00024 float sum = 0;  //合計値(初期化)
00025 float ave;  //気圧の標高平均
00026 
00027 
00028 //データ格納
00029 //気圧センサ
00030 float dataPa[10]; //気圧　圧力
00031 float dataAl[10]; //気圧　標高
00032 float dataTe[10]; //気圧　温度
00033 
00034 //GPSセンサ
00035 float dataLat[10];  //GSP 緯度
00036 float dataLon[10];  //GPS 経度
00037 float dataH[10];    //GPS ジオイド高さ
00038 
00039 
00040 
00041 
00042 //GPS
00043 Timer timer_open;
00044 Timer timer_log;
00045 Ticker tic_open;
00046 Ticker tic_log;
00047 
00048 float    _DMS2DEG(float raw_data);
00049 int      _imput(char cha);
00050 
00051 float Time;
00052 char gps_data[256];
00053 int cnt_gps;
00054 int Cnt_GPS=0;
00055 
00056 
00057 int main() {    //1
00058     
00059     pc.printf("settig start\r\n");
00060     xbee.printf("settig start\r\n");
00061     
00062     /**************************************************************/
00063                 //ここに気圧の標高の較正とGPSの接続を行う。
00064                 
00065     //気圧(所要時間約20秒)
00066     if (!ps.init()){    //2
00067         printf("Failed to autodetect pressure sensor!\r\n");
00068         while (1);
00069     }   //2
00070     
00071     ps.enableDefault();
00072     
00073     for(i=0;i<10;i++){  //2
00074         float pressure = ps.readPressureMillibars();
00075         float altitude = ps.pressureToAltitudeMeters(pressure);
00076         float temperature = ps.readTemperatureC();
00077         wait(1);
00078         }   //2
00079     
00080     for(i=0;i<10;i++){  //2
00081         float pressure = ps.readPressureMillibars();
00082         float altitude = ps.pressureToAltitudeMeters(pressure);
00083         data1[i] = altitude;
00084         sum = sum + data1[i];
00085         wait(1);
00086         }   //2
00087     
00088     ave = sum/10;
00089     xbee.printf("0 altitude = %f\r\n",ave);
00090     
00091     //GPS
00092     while(1){   //2
00093         if(gps.readable()){ //3
00094             gps_data[cnt_gps]=gps.getc();
00095             if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
00096                 cnt_gps=0;
00097                 memset(gps_data,'\0',256);
00098          }else if (gps_data[cnt_gps]=='\r'){    //4
00099             float world_time, lon_east, lat_north;
00100             int rlock, sat_num;
00101             char lat,lon;
00102             if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){    //5
00103                 if(rlock==1){   //6
00104                     lat_north=_DMS2DEG(lat_north);
00105                     lon_east=_DMS2DEG(lon_east);
00106                     //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",
00107                     //lat_north,lon_east,world_time,sat_num);
00108                     
00109                     /*******************************/
00110                     
00111                     goto lav;
00112                     
00113                     /*******************************/
00114                     
00115                     
00116                     }else{//6
00117                         //printf("%s\r\n",gps_data);
00118                     }   //6
00119             }   //5
00120         }else{  //4
00121               cnt_gps++;
00122         }   //4
00123         }//3
00124         if(timer_log.read()>=30.0*60.0) timer_log.reset();
00125     }//2
00126         
00127         
00128     lav:    //goto lav;より
00129         
00130     /**************************************************************/
00131     
00132     
00133     //準備完了
00134     
00135     pc.printf("Ok\r\n");
00136     xbee.printf("Ok\r\n");
00137  
00138     for (;;) {  //2
00139         
00140         //if (pc.readable()) xbee.putc(pc.getc());
00141         if (xbee.readable()) pc.putc(xbee.getc());
00142         wait_us(1);
00143         
00144         char cmd1 = xbee.getc();
00145         
00146         //↓↓↓継続してGPSをとるため
00147         if(gps.readable()){ //3
00148             gps_data[cnt_gps]=gps.getc();
00149             if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
00150                 cnt_gps=0;
00151                 memset(gps_data,'\0',256);
00152          }else if (gps_data[cnt_gps]=='\r'){    //4
00153             float world_time, lon_east, lat_north;
00154             int rlock, sat_num;
00155             char lat,lon;
00156             if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){    //5
00157                 if(rlock==1){   //6
00158                     lat_north=_DMS2DEG(lat_north);
00159                     lon_east=_DMS2DEG(lon_east);
00160                     
00161                     //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
00162                     
00163                     }else{  //6
00164                         //printf("%s\r\n",gps_data);
00165                     }   //6
00166             }   //5
00167         }else{  //4
00168               cnt_gps++;
00169         }   //3
00170         if(timer_log.read()>=30.0*60.0) timer_log.reset(); 
00171     }   //2
00172    
00173         
00174         
00175 
00176         //機体の放出後→データを取る
00177         if(cmd1 == 'm'){    //3
00178             pc.printf("start\r\n");
00179             t.start();
00180             cmd1 = 0;
00181             
00182             while(1){   //4
00183                 //if (pc.readable()) xbee.putc(pc.getc());
00184                 if (xbee.readable()) pc.putc(xbee.getc());
00185                 wait_us(1);
00186                 char cmd2 = xbee.getc();
00187                 
00188                 /***********************************************************/
00189                                 //データ(気圧,GPS,地磁気)を取る。
00190                                 //データの保存を10回行い, 保存する。
00191                 
00192                 
00193                 for(i=0;i<10;i++){  //データ10回とる  //5
00194                     if(gps.readable()){ //6
00195                         gps_data[cnt_gps]=gps.getc();
00196                         if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //7
00197                             cnt_gps=0;
00198                             memset(gps_data,'\0',256);
00199                     }else if (gps_data[cnt_gps]=='\r'){ //7
00200                                 
00201                                 float world_time, lon_east, lat_north, rate, sea_level, g_h; //rate 水平精度低下率　sea_level アンテナ海抜高さ g_h ジオイド高さ
00202                                 char lat,lon,sea,h;
00203                                 int rlock, sat_num;               
00204                                 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num, &rate,&sea_level,&sea,&g_h,&h)>=1){  //8
00205                                     if(rlock==1){   //9
00206                                         lat_north=_DMS2DEG(lat_north);
00207                                         lon_east=_DMS2DEG(lon_east);
00208                     
00209                                         //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d g_h%f\r\n",lat_north,lon_east,world_time,sat_num,g_h);
00210                                         
00211                                         dataLat[i] = lat_north;
00212                                         dataLon[i] = lon_east;
00213                                         dataH[i] = g_h;
00214                                         
00215                                         }else{  //9
00216                                         //printf("%s\r\n",gps_data);
00217                                         
00218                                         dataLat[i] = 0;
00219                                         dataLon[i] = 0;
00220                                         dataH[i] = 0;
00221                                         
00222                                         
00223                                         }   //9
00224                                 }   //8
00225                             }else{  //7
00226                             cnt_gps++;
00227                     }   //7
00228                     }   //6
00229                     
00230                     if(timer_log.read()>=30.0*60.0) timer_log.reset();
00231                     
00232                     //GPS
00233                     float pressure = ps.readPressureMillibars();
00234                     float altitude = ps.pressureToAltitudeMeters(pressure);
00235                     float temperature = ps.readTemperatureC();
00236                 
00237                     dataPa[i] = pressure; 
00238                     dataAl[i] = altitude;
00239                     dataTe[i] = temperature;
00240                     
00241                     
00242                     wait_us(dt);
00243                     }   //5
00244                 
00245                 //ここで保存と表示する
00246                 for(i=0;i<10;i++){  //5
00247                     xbee.printf("time = %f, Lat,Lon %f,%f gH = %f, Pa = %f, Al = %f, Te = %f\r\n",t.read(),dataLat[i],dataLon[i],dataH[i],dataPa[i],dataAl[i],dataTe[i]);
00248                     }   //5
00249                 
00250                 
00251                 
00252                 /***********************************************************/
00253                 
00254                 //機体の着陸後→プログラムを終わらす。
00255                 if(cmd2 == 'n'){    //5
00256                     pc.printf("end\r\n");
00257                     xbee.printf("ned\r\n");
00258                     pc.printf("time = %f",t.read());
00259                     xbee.printf("time = %f",t.read());
00260                     t.stop();
00261                     cmd2 = 0;
00262                     return 0;
00263                     }   //5
00264                 
00265                 }   //4
00266             
00267             
00268             }   //3
00269     }   //2
00270 }   //1
00271 
00272 
00273 float _DMS2DEG(float raw_data){//GPSで使用 //1
00274     float bb = raw_data/100;
00275     int d=(int)bb;
00276     float m=(raw_data-(float)d*100);
00277     return (float)d+m/60;
00278 }   //1