コウスケ スズキ / Mbed 2 deprecated 20220804-integration1-1

GPS,LPC,xbee

Dependencies:   mbed LPS25HB_I2C

main.cpp@0:f20bcf1efbc2, 21 months ago (annotated)

Committer:
kosukesuzuki
Date:
Fri Aug 05 06:25:10 2022 +0000
Revision:
0:f20bcf1efbc2
xbee,LPC,GPS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosukesuzuki 0:f20bcf1efbc2 1 #include "mbed.h"
kosukesuzuki 0:f20bcf1efbc2 2
kosukesuzuki 0:f20bcf1efbc2 3 //#include "" //SDcardで使用
kosukesuzuki 0:f20bcf1efbc2 4
kosukesuzuki 0:f20bcf1efbc2 5 #include "LPS.h" //気圧で使用
kosukesuzuki 0:f20bcf1efbc2 6
kosukesuzuki 0:f20bcf1efbc2 7 #include "math.h" //GPSで使用
kosukesuzuki 0:f20bcf1efbc2 8
kosukesuzuki 0:f20bcf1efbc2 9 Serial xbee(PA_9, PA_10); //xbee pin
kosukesuzuki 0:f20bcf1efbc2 10 Serial gps(PA_9,PA_10); //GPS後で変える
kosukesuzuki 0:f20bcf1efbc2 11 Serial pc(USBRX,USBTX); //衛星側のpin
kosukesuzuki 0:f20bcf1efbc2 12
kosukesuzuki 0:f20bcf1efbc2 13 I2C i2c(D0,D1); //気圧 pin
kosukesuzuki 0:f20bcf1efbc2 14 LPS ps(i2c);
kosukesuzuki 0:f20bcf1efbc2 15
kosukesuzuki 0:f20bcf1efbc2 16
kosukesuzuki 0:f20bcf1efbc2 17 Timer t; //時間を定義
kosukesuzuki 0:f20bcf1efbc2 18 const float dt = 1; //wait_us(dt)で使用:データの間隔
kosukesuzuki 0:f20bcf1efbc2 19
kosukesuzuki 0:f20bcf1efbc2 20
kosukesuzuki 0:f20bcf1efbc2 21 int i; //回数カウント用
kosukesuzuki 0:f20bcf1efbc2 22
kosukesuzuki 0:f20bcf1efbc2 23 float data1[10]; //気圧の較正用データ
kosukesuzuki 0:f20bcf1efbc2 24 float sum = 0; //合計値(初期化)
kosukesuzuki 0:f20bcf1efbc2 25 float ave; //気圧の標高平均
kosukesuzuki 0:f20bcf1efbc2 26
kosukesuzuki 0:f20bcf1efbc2 27
kosukesuzuki 0:f20bcf1efbc2 28 //データ格納
kosukesuzuki 0:f20bcf1efbc2 29 //気圧センサ
kosukesuzuki 0:f20bcf1efbc2 30 float dataPa[10]; //気圧　圧力
kosukesuzuki 0:f20bcf1efbc2 31 float dataAl[10]; //気圧　標高
kosukesuzuki 0:f20bcf1efbc2 32 float dataTe[10]; //気圧　温度
kosukesuzuki 0:f20bcf1efbc2 33
kosukesuzuki 0:f20bcf1efbc2 34 //GPSセンサ
kosukesuzuki 0:f20bcf1efbc2 35 float dataLat[10]; //GSP 緯度
kosukesuzuki 0:f20bcf1efbc2 36 float dataLon[10]; //GPS 経度
kosukesuzuki 0:f20bcf1efbc2 37 float dataH[10]; //GPS ジオイド高さ
kosukesuzuki 0:f20bcf1efbc2 38
kosukesuzuki 0:f20bcf1efbc2 39
kosukesuzuki 0:f20bcf1efbc2 40
kosukesuzuki 0:f20bcf1efbc2 41
kosukesuzuki 0:f20bcf1efbc2 42 //GPS
kosukesuzuki 0:f20bcf1efbc2 43 Timer timer_open;
kosukesuzuki 0:f20bcf1efbc2 44 Timer timer_log;
kosukesuzuki 0:f20bcf1efbc2 45 Ticker tic_open;
kosukesuzuki 0:f20bcf1efbc2 46 Ticker tic_log;
kosukesuzuki 0:f20bcf1efbc2 47
kosukesuzuki 0:f20bcf1efbc2 48 float _DMS2DEG(float raw_data);
kosukesuzuki 0:f20bcf1efbc2 49 int _imput(char cha);
kosukesuzuki 0:f20bcf1efbc2 50
kosukesuzuki 0:f20bcf1efbc2 51 float Time;
kosukesuzuki 0:f20bcf1efbc2 52 char gps_data[256];
kosukesuzuki 0:f20bcf1efbc2 53 int cnt_gps;
kosukesuzuki 0:f20bcf1efbc2 54 int Cnt_GPS=0;
kosukesuzuki 0:f20bcf1efbc2 55
kosukesuzuki 0:f20bcf1efbc2 56
kosukesuzuki 0:f20bcf1efbc2 57 int main() { //1
kosukesuzuki 0:f20bcf1efbc2 58
kosukesuzuki 0:f20bcf1efbc2 59 pc.printf("settig start\r\n");
kosukesuzuki 0:f20bcf1efbc2 60 xbee.printf("settig start\r\n");
kosukesuzuki 0:f20bcf1efbc2 61
kosukesuzuki 0:f20bcf1efbc2 62 /**************************************************************/
kosukesuzuki 0:f20bcf1efbc2 63 //ここに気圧の標高の較正とGPSの接続を行う。
kosukesuzuki 0:f20bcf1efbc2 64
kosukesuzuki 0:f20bcf1efbc2 65 //気圧(所要時間約20秒)
kosukesuzuki 0:f20bcf1efbc2 66 if (!ps.init()){ //2
kosukesuzuki 0:f20bcf1efbc2 67 printf("Failed to autodetect pressure sensor!\r\n");
kosukesuzuki 0:f20bcf1efbc2 68 while (1);
kosukesuzuki 0:f20bcf1efbc2 69 } //2
kosukesuzuki 0:f20bcf1efbc2 70
kosukesuzuki 0:f20bcf1efbc2 71 ps.enableDefault();
kosukesuzuki 0:f20bcf1efbc2 72
kosukesuzuki 0:f20bcf1efbc2 73 for(i=0;i<10;i++){ //2
kosukesuzuki 0:f20bcf1efbc2 74 float pressure = ps.readPressureMillibars();
kosukesuzuki 0:f20bcf1efbc2 75 float altitude = ps.pressureToAltitudeMeters(pressure);
kosukesuzuki 0:f20bcf1efbc2 76 float temperature = ps.readTemperatureC();
kosukesuzuki 0:f20bcf1efbc2 77 wait(1);
kosukesuzuki 0:f20bcf1efbc2 78 } //2
kosukesuzuki 0:f20bcf1efbc2 79
kosukesuzuki 0:f20bcf1efbc2 80 for(i=0;i<10;i++){ //2
kosukesuzuki 0:f20bcf1efbc2 81 float pressure = ps.readPressureMillibars();
kosukesuzuki 0:f20bcf1efbc2 82 float altitude = ps.pressureToAltitudeMeters(pressure);
kosukesuzuki 0:f20bcf1efbc2 83 data1[i] = altitude;
kosukesuzuki 0:f20bcf1efbc2 84 sum = sum + data1[i];
kosukesuzuki 0:f20bcf1efbc2 85 wait(1);
kosukesuzuki 0:f20bcf1efbc2 86 } //2
kosukesuzuki 0:f20bcf1efbc2 87
kosukesuzuki 0:f20bcf1efbc2 88 ave = sum/10;
kosukesuzuki 0:f20bcf1efbc2 89 xbee.printf("0 altitude = %f\r\n",ave);
kosukesuzuki 0:f20bcf1efbc2 90
kosukesuzuki 0:f20bcf1efbc2 91 //GPS
kosukesuzuki 0:f20bcf1efbc2 92 while(1){ //2
kosukesuzuki 0:f20bcf1efbc2 93 if(gps.readable()){ //3
kosukesuzuki 0:f20bcf1efbc2 94 gps_data[cnt_gps]=gps.getc();
kosukesuzuki 0:f20bcf1efbc2 95 if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
kosukesuzuki 0:f20bcf1efbc2 96 cnt_gps=0;
kosukesuzuki 0:f20bcf1efbc2 97 memset(gps_data,'\0',256);
kosukesuzuki 0:f20bcf1efbc2 98 }else if (gps_data[cnt_gps]=='\r'){ //4
kosukesuzuki 0:f20bcf1efbc2 99 float world_time, lon_east, lat_north;
kosukesuzuki 0:f20bcf1efbc2 100 int rlock, sat_num;
kosukesuzuki 0:f20bcf1efbc2 101 char lat,lon;
kosukesuzuki 0:f20bcf1efbc2 102 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
kosukesuzuki 0:f20bcf1efbc2 103 if(rlock==1){ //6
kosukesuzuki 0:f20bcf1efbc2 104 lat_north=_DMS2DEG(lat_north);
kosukesuzuki 0:f20bcf1efbc2 105 lon_east=_DMS2DEG(lon_east);
kosukesuzuki 0:f20bcf1efbc2 106 //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",
kosukesuzuki 0:f20bcf1efbc2 107 //lat_north,lon_east,world_time,sat_num);
kosukesuzuki 0:f20bcf1efbc2 108
kosukesuzuki 0:f20bcf1efbc2 109 /*******************************/
kosukesuzuki 0:f20bcf1efbc2 110
kosukesuzuki 0:f20bcf1efbc2 111 goto lav;
kosukesuzuki 0:f20bcf1efbc2 112
kosukesuzuki 0:f20bcf1efbc2 113 /*******************************/
kosukesuzuki 0:f20bcf1efbc2 114
kosukesuzuki 0:f20bcf1efbc2 115
kosukesuzuki 0:f20bcf1efbc2 116 }else{//6
kosukesuzuki 0:f20bcf1efbc2 117 //printf("%s\r\n",gps_data);
kosukesuzuki 0:f20bcf1efbc2 118 } //6
kosukesuzuki 0:f20bcf1efbc2 119 } //5
kosukesuzuki 0:f20bcf1efbc2 120 }else{ //4
kosukesuzuki 0:f20bcf1efbc2 121 cnt_gps++;
kosukesuzuki 0:f20bcf1efbc2 122 } //4
kosukesuzuki 0:f20bcf1efbc2 123 }//3
kosukesuzuki 0:f20bcf1efbc2 124 if(timer_log.read()>=30.0*60.0) timer_log.reset();
kosukesuzuki 0:f20bcf1efbc2 125 }//2
kosukesuzuki 0:f20bcf1efbc2 126
kosukesuzuki 0:f20bcf1efbc2 127
kosukesuzuki 0:f20bcf1efbc2 128 lav: //goto lav;より
kosukesuzuki 0:f20bcf1efbc2 129
kosukesuzuki 0:f20bcf1efbc2 130 /**************************************************************/
kosukesuzuki 0:f20bcf1efbc2 131
kosukesuzuki 0:f20bcf1efbc2 132
kosukesuzuki 0:f20bcf1efbc2 133 //準備完了
kosukesuzuki 0:f20bcf1efbc2 134
kosukesuzuki 0:f20bcf1efbc2 135 pc.printf("Ok\r\n");
kosukesuzuki 0:f20bcf1efbc2 136 xbee.printf("Ok\r\n");
kosukesuzuki 0:f20bcf1efbc2 137
kosukesuzuki 0:f20bcf1efbc2 138 for (;;) { //2
kosukesuzuki 0:f20bcf1efbc2 139
kosukesuzuki 0:f20bcf1efbc2 140 //if (pc.readable()) xbee.putc(pc.getc());
kosukesuzuki 0:f20bcf1efbc2 141 if (xbee.readable()) pc.putc(xbee.getc());
kosukesuzuki 0:f20bcf1efbc2 142 wait_us(1);
kosukesuzuki 0:f20bcf1efbc2 143
kosukesuzuki 0:f20bcf1efbc2 144 char cmd1 = xbee.getc();
kosukesuzuki 0:f20bcf1efbc2 145
kosukesuzuki 0:f20bcf1efbc2 146 //↓↓↓継続してGPSをとるため
kosukesuzuki 0:f20bcf1efbc2 147 if(gps.readable()){ //3
kosukesuzuki 0:f20bcf1efbc2 148 gps_data[cnt_gps]=gps.getc();
kosukesuzuki 0:f20bcf1efbc2 149 if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //4
kosukesuzuki 0:f20bcf1efbc2 150 cnt_gps=0;
kosukesuzuki 0:f20bcf1efbc2 151 memset(gps_data,'\0',256);
kosukesuzuki 0:f20bcf1efbc2 152 }else if (gps_data[cnt_gps]=='\r'){ //4
kosukesuzuki 0:f20bcf1efbc2 153 float world_time, lon_east, lat_north;
kosukesuzuki 0:f20bcf1efbc2 154 int rlock, sat_num;
kosukesuzuki 0:f20bcf1efbc2 155 char lat,lon;
kosukesuzuki 0:f20bcf1efbc2 156 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
kosukesuzuki 0:f20bcf1efbc2 157 if(rlock==1){ //6
kosukesuzuki 0:f20bcf1efbc2 158 lat_north=_DMS2DEG(lat_north);
kosukesuzuki 0:f20bcf1efbc2 159 lon_east=_DMS2DEG(lon_east);
kosukesuzuki 0:f20bcf1efbc2 160
kosukesuzuki 0:f20bcf1efbc2 161 //printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
kosukesuzuki 0:f20bcf1efbc2 162
kosukesuzuki 0:f20bcf1efbc2 163 }else{ //6
kosukesuzuki 0:f20bcf1efbc2 164 //printf("%s\r\n",gps_data);
kosukesuzuki 0:f20bcf1efbc2 165 } //6
kosukesuzuki 0:f20bcf1efbc2 166 } //5
kosukesuzuki 0:f20bcf1efbc2 167 }else{ //4
kosukesuzuki 0:f20bcf1efbc2 168 cnt_gps++;
kosukesuzuki 0:f20bcf1efbc2 169 } //3
kosukesuzuki 0:f20bcf1efbc2 170 if(timer_log.read()>=30.0*60.0) timer_log.reset();
kosukesuzuki 0:f20bcf1efbc2 171 } //2
kosukesuzuki 0:f20bcf1efbc2 172
kosukesuzuki 0:f20bcf1efbc2 173
kosukesuzuki 0:f20bcf1efbc2 174
kosukesuzuki 0:f20bcf1efbc2 175
kosukesuzuki 0:f20bcf1efbc2 176 //機体の放出後→データを取る
kosukesuzuki 0:f20bcf1efbc2 177 if(cmd1 == 'm'){ //3
kosukesuzuki 0:f20bcf1efbc2 178 pc.printf("start\r\n");
kosukesuzuki 0:f20bcf1efbc2 179 t.start();
kosukesuzuki 0:f20bcf1efbc2 180 cmd1 = 0;
kosukesuzuki 0:f20bcf1efbc2 181
kosukesuzuki 0:f20bcf1efbc2 182 while(1){ //4
kosukesuzuki 0:f20bcf1efbc2 183 //if (pc.readable()) xbee.putc(pc.getc());
kosukesuzuki 0:f20bcf1efbc2 184 if (xbee.readable()) pc.putc(xbee.getc());
kosukesuzuki 0:f20bcf1efbc2 185 wait_us(1);
kosukesuzuki 0:f20bcf1efbc2 186 char cmd2 = xbee.getc();
kosukesuzuki 0:f20bcf1efbc2 187
kosukesuzuki 0:f20bcf1efbc2 188 /***********************************************************/
kosukesuzuki 0:f20bcf1efbc2 189 //データ(気圧,GPS,地磁気)を取る。
kosukesuzuki 0:f20bcf1efbc2 190 //データの保存を10回行い, 保存する。
kosukesuzuki 0:f20bcf1efbc2 191
kosukesuzuki 0:f20bcf1efbc2 192
kosukesuzuki 0:f20bcf1efbc2 193 for(i=0;i<10;i++){ //データ10回とる //5
kosukesuzuki 0:f20bcf1efbc2 194 if(gps.readable()){ //6
kosukesuzuki 0:f20bcf1efbc2 195 gps_data[cnt_gps]=gps.getc();
kosukesuzuki 0:f20bcf1efbc2 196 if(gps_data[cnt_gps]=='$' || cnt_gps==256){ //7
kosukesuzuki 0:f20bcf1efbc2 197 cnt_gps=0;
kosukesuzuki 0:f20bcf1efbc2 198 memset(gps_data,'\0',256);
kosukesuzuki 0:f20bcf1efbc2 199 }else if (gps_data[cnt_gps]=='\r'){ //7
kosukesuzuki 0:f20bcf1efbc2 200
kosukesuzuki 0:f20bcf1efbc2 201 float world_time, lon_east, lat_north, rate, sea_level, g_h; //rate 水平精度低下率　sea_level アンテナ海抜高さ g_h ジオイド高さ
kosukesuzuki 0:f20bcf1efbc2 202 char lat,lon,sea,h;
kosukesuzuki 0:f20bcf1efbc2 203 int rlock, sat_num;
kosukesuzuki 0:f20bcf1efbc2 204 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
kosukesuzuki 0:f20bcf1efbc2 205 if(rlock==1){ //9
kosukesuzuki 0:f20bcf1efbc2 206 lat_north=_DMS2DEG(lat_north);
kosukesuzuki 0:f20bcf1efbc2 207 lon_east=_DMS2DEG(lon_east);
kosukesuzuki 0:f20bcf1efbc2 208
kosukesuzuki 0:f20bcf1efbc2 209 //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);
kosukesuzuki 0:f20bcf1efbc2 210
kosukesuzuki 0:f20bcf1efbc2 211 dataLat[i] = lat_north;
kosukesuzuki 0:f20bcf1efbc2 212 dataLon[i] = lon_east;
kosukesuzuki 0:f20bcf1efbc2 213 dataH[i] = g_h;
kosukesuzuki 0:f20bcf1efbc2 214
kosukesuzuki 0:f20bcf1efbc2 215 }else{ //9
kosukesuzuki 0:f20bcf1efbc2 216 //printf("%s\r\n",gps_data);
kosukesuzuki 0:f20bcf1efbc2 217
kosukesuzuki 0:f20bcf1efbc2 218 dataLat[i] = 0;
kosukesuzuki 0:f20bcf1efbc2 219 dataLon[i] = 0;
kosukesuzuki 0:f20bcf1efbc2 220 dataH[i] = 0;
kosukesuzuki 0:f20bcf1efbc2 221
kosukesuzuki 0:f20bcf1efbc2 222
kosukesuzuki 0:f20bcf1efbc2 223 } //9
kosukesuzuki 0:f20bcf1efbc2 224 } //8
kosukesuzuki 0:f20bcf1efbc2 225 }else{ //7
kosukesuzuki 0:f20bcf1efbc2 226 cnt_gps++;
kosukesuzuki 0:f20bcf1efbc2 227 } //7
kosukesuzuki 0:f20bcf1efbc2 228 } //6
kosukesuzuki 0:f20bcf1efbc2 229
kosukesuzuki 0:f20bcf1efbc2 230 if(timer_log.read()>=30.0*60.0) timer_log.reset();
kosukesuzuki 0:f20bcf1efbc2 231
kosukesuzuki 0:f20bcf1efbc2 232 //GPS
kosukesuzuki 0:f20bcf1efbc2 233 float pressure = ps.readPressureMillibars();
kosukesuzuki 0:f20bcf1efbc2 234 float altitude = ps.pressureToAltitudeMeters(pressure);
kosukesuzuki 0:f20bcf1efbc2 235 float temperature = ps.readTemperatureC();
kosukesuzuki 0:f20bcf1efbc2 236
kosukesuzuki 0:f20bcf1efbc2 237 dataPa[i] = pressure;
kosukesuzuki 0:f20bcf1efbc2 238 dataAl[i] = altitude;
kosukesuzuki 0:f20bcf1efbc2 239 dataTe[i] = temperature;
kosukesuzuki 0:f20bcf1efbc2 240
kosukesuzuki 0:f20bcf1efbc2 241
kosukesuzuki 0:f20bcf1efbc2 242 wait_us(dt);
kosukesuzuki 0:f20bcf1efbc2 243 } //5
kosukesuzuki 0:f20bcf1efbc2 244
kosukesuzuki 0:f20bcf1efbc2 245 //ここで保存と表示する
kosukesuzuki 0:f20bcf1efbc2 246 for(i=0;i<10;i++){ //5
kosukesuzuki 0:f20bcf1efbc2 247 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]);
kosukesuzuki 0:f20bcf1efbc2 248 } //5
kosukesuzuki 0:f20bcf1efbc2 249
kosukesuzuki 0:f20bcf1efbc2 250
kosukesuzuki 0:f20bcf1efbc2 251
kosukesuzuki 0:f20bcf1efbc2 252 /***********************************************************/
kosukesuzuki 0:f20bcf1efbc2 253
kosukesuzuki 0:f20bcf1efbc2 254 //機体の着陸後→プログラムを終わらす。
kosukesuzuki 0:f20bcf1efbc2 255 if(cmd2 == 'n'){ //5
kosukesuzuki 0:f20bcf1efbc2 256 pc.printf("end\r\n");
kosukesuzuki 0:f20bcf1efbc2 257 xbee.printf("ned\r\n");
kosukesuzuki 0:f20bcf1efbc2 258 pc.printf("time = %f",t.read());
kosukesuzuki 0:f20bcf1efbc2 259 xbee.printf("time = %f",t.read());
kosukesuzuki 0:f20bcf1efbc2 260 t.stop();
kosukesuzuki 0:f20bcf1efbc2 261 cmd2 = 0;
kosukesuzuki 0:f20bcf1efbc2 262 return 0;
kosukesuzuki 0:f20bcf1efbc2 263 } //5
kosukesuzuki 0:f20bcf1efbc2 264
kosukesuzuki 0:f20bcf1efbc2 265 } //4
kosukesuzuki 0:f20bcf1efbc2 266
kosukesuzuki 0:f20bcf1efbc2 267
kosukesuzuki 0:f20bcf1efbc2 268 } //3
kosukesuzuki 0:f20bcf1efbc2 269 } //2
kosukesuzuki 0:f20bcf1efbc2 270 } //1
kosukesuzuki 0:f20bcf1efbc2 271
kosukesuzuki 0:f20bcf1efbc2 272
kosukesuzuki 0:f20bcf1efbc2 273 float _DMS2DEG(float raw_data){//GPSで使用 //1
kosukesuzuki 0:f20bcf1efbc2 274 float bb = raw_data/100;
kosukesuzuki 0:f20bcf1efbc2 275 int d=(int)bb;
kosukesuzuki 0:f20bcf1efbc2 276 float m=(raw_data-(float)d*100);
kosukesuzuki 0:f20bcf1efbc2 277 return (float)d+m/60;
kosukesuzuki 0:f20bcf1efbc2 278 } //1