コウスケ スズキ
GPS,LPC,xbee
Dependencies: mbed LPS25HB_I2C
Diff: main.cpp
- 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