GPS,LPC,xbee

Dependencies:   mbed LPS25HB_I2C

Revision:
0:f20bcf1efbc2
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 05 06:25:10 2022 +0000
@@ -0,0 +1,278 @@
+#include "mbed.h"
+
+//#include "" //SDcardで使用
+
+#include "LPS.h"    //気圧で使用
+
+#include "math.h"   //GPSで使用
+
+Serial xbee(PA_9, PA_10); //xbee pin
+Serial  gps(PA_9,PA_10);    //GPS後で変える
+Serial pc(USBRX,USBTX); //衛星側のpin
+
+I2C i2c(D0,D1); //気圧 pin
+LPS ps(i2c);
+
+
+Timer t;    //時間を定義
+const float dt = 1;   //wait_us(dt)で使用:データの間隔
+
+
+int i; //回数カウント用
+
+float data1[10];    //気圧の較正用データ
+float sum = 0;  //合計値(初期化)
+float ave;  //気圧の標高平均
+
+
+//データ格納
+//気圧センサ
+float dataPa[10]; //気圧 圧力
+float dataAl[10]; //気圧 標高
+float dataTe[10]; //気圧 温度
+
+//GPSセンサ
+float dataLat[10];  //GSP 緯度
+float dataLon[10];  //GPS 経度
+float dataH[10];    //GPS ジオイド高さ
+
+
+
+
+//GPS
+Timer timer_open;
+Timer timer_log;
+Ticker tic_open;
+Ticker tic_log;
+
+float    _DMS2DEG(float raw_data);
+int      _imput(char cha);
+
+float Time;
+char gps_data[256];
+int cnt_gps;
+int Cnt_GPS=0;
+
+
+int main() {    //1
+    
+    pc.printf("settig start\r\n");
+    xbee.printf("settig start\r\n");
+    
+    /**************************************************************/
+                //ここに気圧の標高の較正とGPSの接続を行う。
+                
+    //気圧(所要時間約20秒)
+    if (!ps.init()){    //2
+        printf("Failed to autodetect pressure sensor!\r\n");
+        while (1);
+    }   //2
+    
+    ps.enableDefault();
+    
+    for(i=0;i<10;i++){  //2
+        float pressure = ps.readPressureMillibars();
+        float altitude = ps.pressureToAltitudeMeters(pressure);
+        float temperature = ps.readTemperatureC();
+        wait(1);
+        }   //2
+    
+    for(i=0;i<10;i++){  //2
+        float pressure = ps.readPressureMillibars();
+        float altitude = ps.pressureToAltitudeMeters(pressure);
+        data1[i] = altitude;
+        sum = sum + data1[i];
+        wait(1);
+        }   //2
+    
+    ave = sum/10;
+    xbee.printf("0 altitude = %f\r\n",ave);
+    
+    //GPS
+    while(1){   //2
+        if(gps.readable()){ //3
+            gps_data[cnt_gps]=gps.getc();
+            if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
+                cnt_gps=0;
+                memset(gps_data,'\0',256);
+         }else if (gps_data[cnt_gps]=='\r'){    //4
+            float world_time, lon_east, lat_north;
+            int rlock, sat_num;
+            char lat,lon;
+            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
+                if(rlock==1){   //6
+                    lat_north=_DMS2DEG(lat_north);
+                    lon_east=_DMS2DEG(lon_east);
+                    //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",
+                    //lat_north,lon_east,world_time,sat_num);
+                    
+                    /*******************************/
+                    
+                    goto lav;
+                    
+                    /*******************************/
+                    
+                    
+                    }else{//6
+                        //printf("%s\r\n",gps_data);
+                    }   //6
+            }   //5
+        }else{  //4
+              cnt_gps++;
+        }   //4
+        }//3
+        if(timer_log.read()>=30.0*60.0) timer_log.reset();
+    }//2
+        
+        
+    lav:    //goto lav;より
+        
+    /**************************************************************/
+    
+    
+    //準備完了
+    
+    pc.printf("Ok\r\n");
+    xbee.printf("Ok\r\n");
+ 
+    for (;;) {  //2
+        
+        //if (pc.readable()) xbee.putc(pc.getc());
+        if (xbee.readable()) pc.putc(xbee.getc());
+        wait_us(1);
+        
+        char cmd1 = xbee.getc();
+        
+        //↓↓↓継続してGPSをとるため
+        if(gps.readable()){ //3
+            gps_data[cnt_gps]=gps.getc();
+            if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
+                cnt_gps=0;
+                memset(gps_data,'\0',256);
+         }else if (gps_data[cnt_gps]=='\r'){    //4
+            float world_time, lon_east, lat_north;
+            int rlock, sat_num;
+            char lat,lon;
+            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
+                if(rlock==1){   //6
+                    lat_north=_DMS2DEG(lat_north);
+                    lon_east=_DMS2DEG(lon_east);
+                    
+                    //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+                    
+                    }else{  //6
+                        //printf("%s\r\n",gps_data);
+                    }   //6
+            }   //5
+        }else{  //4
+              cnt_gps++;
+        }   //3
+        if(timer_log.read()>=30.0*60.0) timer_log.reset(); 
+    }   //2
+   
+        
+        
+
+        //機体の放出後→データを取る
+        if(cmd1 == 'm'){    //3
+            pc.printf("start\r\n");
+            t.start();
+            cmd1 = 0;
+            
+            while(1){   //4
+                //if (pc.readable()) xbee.putc(pc.getc());
+                if (xbee.readable()) pc.putc(xbee.getc());
+                wait_us(1);
+                char cmd2 = xbee.getc();
+                
+                /***********************************************************/
+                                //データ(気圧,GPS,地磁気)を取る。
+                                //データの保存を10回行い, 保存する。
+                
+                
+                for(i=0;i<10;i++){  //データ10回とる  //5
+                    if(gps.readable()){ //6
+                        gps_data[cnt_gps]=gps.getc();
+                        if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //7
+                            cnt_gps=0;
+                            memset(gps_data,'\0',256);
+                    }else if (gps_data[cnt_gps]=='\r'){ //7
+                                
+                                float world_time, lon_east, lat_north, rate, sea_level, g_h; //rate 水平精度低下率 sea_level アンテナ海抜高さ g_h ジオイド高さ
+                                char lat,lon,sea,h;
+                                int rlock, sat_num;               
+                                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
+                                    if(rlock==1){   //9
+                                        lat_north=_DMS2DEG(lat_north);
+                                        lon_east=_DMS2DEG(lon_east);
+                    
+                                        //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);
+                                        
+                                        dataLat[i] = lat_north;
+                                        dataLon[i] = lon_east;
+                                        dataH[i] = g_h;
+                                        
+                                        }else{  //9
+                                        //printf("%s\r\n",gps_data);
+                                        
+                                        dataLat[i] = 0;
+                                        dataLon[i] = 0;
+                                        dataH[i] = 0;
+                                        
+                                        
+                                        }   //9
+                                }   //8
+                            }else{  //7
+                            cnt_gps++;
+                    }   //7
+                    }   //6
+                    
+                    if(timer_log.read()>=30.0*60.0) timer_log.reset();
+                    
+                    //GPS
+                    float pressure = ps.readPressureMillibars();
+                    float altitude = ps.pressureToAltitudeMeters(pressure);
+                    float temperature = ps.readTemperatureC();
+                
+                    dataPa[i] = pressure; 
+                    dataAl[i] = altitude;
+                    dataTe[i] = temperature;
+                    
+                    
+                    wait_us(dt);
+                    }   //5
+                
+                //ここで保存と表示する
+                for(i=0;i<10;i++){  //5
+                    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]);
+                    }   //5
+                
+                
+                
+                /***********************************************************/
+                
+                //機体の着陸後→プログラムを終わらす。
+                if(cmd2 == 'n'){    //5
+                    pc.printf("end\r\n");
+                    xbee.printf("ned\r\n");
+                    pc.printf("time = %f",t.read());
+                    xbee.printf("time = %f",t.read());
+                    t.stop();
+                    cmd2 = 0;
+                    return 0;
+                    }   //5
+                
+                }   //4
+            
+            
+            }   //3
+    }   //2
+}   //1
+
+
+float _DMS2DEG(float raw_data){//GPSで使用 //1
+    float bb = raw_data/100;
+    int d=(int)bb;
+    float m=(raw_data-(float)d*100);
+    return (float)d+m/60;
+}   //1
\ No newline at end of file