CORE / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@8:15a1b22df82f, 2019-03-07 (annotated)

Committer:
sashida_h
Date:
Thu Mar 07 12:13:33 2019 +0000
Revision:
8:15a1b22df82f
Parent:
7:9953d922499d
Child:
9:42b4d337d4cc
20190307;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:0ff20d8e9090 1 #include "mbed.h"
Yukina 4:4b3ae90ec778 2 #include "MPU6050.h"
mikawataru 0:0ff20d8e9090 3 #include "BMP180.h"
Yukina 5:f6e956e8a060 4 #include "Kalman.h"
Yukina 5:f6e956e8a060 5 #include "MadgwickAHRS.h"
mikawataru 0:0ff20d8e9090 6
Yukina 5:f6e956e8a060 7 /*しきい値など*/
sashida_h 8:15a1b22df82f 8 #define ACC_JUDGE_LAUNCH 3.0 //発射判定のしきい値
sashida_h 8:15a1b22df82f 9 #define TIME_BURNING 6 //開放判定しない時間（燃焼時間）
sashida_h 7:9953d922499d 10 #define ALT_JUDGE_FIRE 0
sashida_h 8:15a1b22df82f 11 #define ALT_JUDGE_OPEN 1 //落下判定のカウントを１増やす高度差
sashida_h 8:15a1b22df82f 12 #define TIME_OPEN 25 //強制的に開放させる時間
sashida_h 8:15a1b22df82f 13 #define TIME_SEND 1.0
sashida_h 7:9953d922499d 14 #define ANGLE_JUDGE_FIRE_MIN 15
sashida_h 7:9953d922499d 15 #define ANGLE_JUDGE_FIRE_MAX 70
sashida_h 8:15a1b22df82f 16 #define CNT_JUDGE 10
Yukina 5:f6e956e8a060 17 #define TIME_JUDGE_CNT 1.5
Yukina 5:f6e956e8a060 18 #define NUM_CNT_MEDIAN 10
sashida_h 7:9953d922499d 19 #define RATE_GPS 1.0
sashida_h 7:9953d922499d 20 #define RATE_DATA 10
sashida_h 7:9953d922499d 21 #define TIMER_NOTFIRE 15.0
Yukina 5:f6e956e8a060 22 #define p0 1013.25f
Yukina 5:f6e956e8a060 23 #define RadToDeg 57.295779513082320876798154814105
Yukina 5:f6e956e8a060 24
sashida_h 7:9953d922499d 25 MPU6050 mpu(PB_7,PB_6);
sashida_h 7:9953d922499d 26 BMP180 bmp(PB_7,PB_6);
Yukina 5:f6e956e8a060 27 KalmanFilter gKfx, gKfy;
Yukina 5:f6e956e8a060 28 Madgwick MadgwickFilter;
sashida_h 7:9953d922499d 29 Serial pc(PA_2, PA_3);
sashida_h 7:9953d922499d 30 Serial gps(PA_9, PA_10);
Yukina 5:f6e956e8a060 31 Timer timer_open;
Yukina 5:f6e956e8a060 32 Timer timer_data;
Yukina 5:f6e956e8a060 33 Ticker tic_data;
Yukina 5:f6e956e8a060 34 Ticker tic_gps;
mikawataru 0:0ff20d8e9090 35
Yukina 5:f6e956e8a060 36 /*自作関数*/
Yukina 5:f6e956e8a060 37 float _getAlt();
Yukina 5:f6e956e8a060 38 float _median(float data[],int num);
Yukina 5:f6e956e8a060 39 void _SendData();
Yukina 5:f6e956e8a060 40 void _SendGPS();
Yukina 5:f6e956e8a060 41 float _DMS2DEG(float raw_data);
Yukina 4:4b3ae90ec778 42
Yukina 5:f6e956e8a060 43 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
Yukina 4:4b3ae90ec778 44
Yukina 5:f6e956e8a060 45 /*グローバル変数*/
sashida_h 8:15a1b22df82f 46 float t = 0;
sashida_h 7:9953d922499d 47 //地上高度
sashida_h 7:9953d922499d 48 float alt_gnd;
sashida_h 7:9953d922499d 49 //GPS
sashida_h 7:9953d922499d 50 int cnt_gps=0;
sashida_h 7:9953d922499d 51
sashida_h 7:9953d922499d 52 int p = 1;
mikawataru 0:0ff20d8e9090 53
Yukina 4:4b3ae90ec778 54 int main(){
Yukina 5:f6e956e8a060 55 /*ローカル変数*/
Yukina 5:f6e956e8a060 56 float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
sashida_h 7:9953d922499d 57 float alt_buff[10],alt_md,alt_max=0;
Yukina 5:f6e956e8a060 58 float time_judge;
Yukina 5:f6e956e8a060 59 int cnt_data=0,cnt_judge=0;
sashida_h 7:9953d922499d 60 int i=0;
Yukina 5:f6e956e8a060 61
Yukina 5:f6e956e8a060 62 /*センサの初期化等*/
sashida_h 7:9953d922499d 63 pc.baud(38400);
Yukina 5:f6e956e8a060 64 mpu.setAcceleroRange(3);
mikawataru 0:0ff20d8e9090 65 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
Yukina 5:f6e956e8a060 66
Yukina 5:f6e956e8a060 67 /*初期位置の設定*/
Yukina 5:f6e956e8a060 68 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 69 mpu.getGyro(gyro);
Yukina 5:f6e956e8a060 70
Yukina 5:f6e956e8a060 71 Phase = STANDBY;
sashida_h 7:9953d922499d 72 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
sashida_h 7:9953d922499d 73 alt_buff[cnt_data] = _getAlt();
sashida_h 7:9953d922499d 74 }
sashida_h 7:9953d922499d 75 alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN);
sashida_h 7:9953d922499d 76 wait(2.0);
sashida_h 7:9953d922499d 77 pc.printf("Hello World!\r\n");
sashida_h 8:15a1b22df82f 78 wait(1.0);
sashida_h 8:15a1b22df82f 79 pc.printf("f:Flight_mode_on\r\n");
sashida_h 8:15a1b22df82f 80 wait(1.0);
sashida_h 8:15a1b22df82f 81
Yukina 5:f6e956e8a060 82 while(1){
Yukina 5:f6e956e8a060 83 switch(Phase){
Yukina 5:f6e956e8a060 84 case STANDBY:
Yukina 5:f6e956e8a060 85 /*入力待ち*/
sashida_h 7:9953d922499d 86 //tic_data.attach(&_SendData, 1.0/RATE_DATA);
sashida_h 8:15a1b22df82f 87 char c = pc.getc();
sashida_h 7:9953d922499d 88 if(c=='f'){
sashida_h 7:9953d922499d 89 pc.printf("Flight_mode\r\n");
sashida_h 8:15a1b22df82f 90 timer_data.start();
sashida_h 7:9953d922499d 91 Phase = LAUNCH;
sashida_h 7:9953d922499d 92 }
sashida_h 7:9953d922499d 93 wait(1.0);
Yukina 5:f6e956e8a060 94 break;
sashida_h 8:15a1b22df82f 95
Yukina 5:f6e956e8a060 96 case LAUNCH:
Yukina 5:f6e956e8a060 97 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 98 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 99 acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
Yukina 5:f6e956e8a060 100 }
Yukina 5:f6e956e8a060 101 acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
sashida_h 8:15a1b22df82f 102 if(timer_data.read() - t > TIME_SEND){
sashida_h 8:15a1b22df82f 103 pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge);
sashida_h 8:15a1b22df82f 104 t = timer_data.read();
sashida_h 7:9953d922499d 105 }
Yukina 5:f6e956e8a060 106 /*加速度判定*/
Yukina 5:f6e956e8a060 107 if(acc_abs>ACC_JUDGE_LAUNCH){
Yukina 5:f6e956e8a060 108 cnt_judge++;
Yukina 4:4b3ae90ec778 109 }
sashida_h 8:15a1b22df82f 110 if(cnt_judge==CNT_JUDGE){
Yukina 5:f6e956e8a060 111 cnt_judge=0;
Yukina 5:f6e956e8a060 112 timer_open.start();
sashida_h 7:9953d922499d 113 Phase = RISE;
Yukina 5:f6e956e8a060 114 }
Yukina 5:f6e956e8a060 115 break;
sashida_h 8:15a1b22df82f 116
Yukina 5:f6e956e8a060 117 case RISE:
Yukina 5:f6e956e8a060 118 while(timer_open.read() < TIME_BURNING){
sashida_h 8:15a1b22df82f 119 pc.printf("RISE,time from launch:%f\r\n",timer_open.read());
sashida_h 8:15a1b22df82f 120 wait(1.0);
sashida_h 7:9953d922499d 121 i=0;
sashida_h 8:15a1b22df82f 122 timer_data.reset();
Yukina 5:f6e956e8a060 123 }
sashida_h 8:15a1b22df82f 124 Phase = OPEN;
sashida_h 8:15a1b22df82f 125 t = 0.0;
Yukina 5:f6e956e8a060 126 break;
Yukina 5:f6e956e8a060 127 case FIRE:
Yukina 6:f17310205c1f 128
Yukina 6:f17310205c1f 129 //カルマン
Yukina 6:f17310205c1f 130 /*
Yukina 5:f6e956e8a060 131 gPrevMicros = timer_open.read();
Yukina 4:4b3ae90ec778 132 mpu.getAccelero(acc);
Yukina 5:f6e956e8a060 133 mpu.getGyro(gyro);
Yukina 5:f6e956e8a060 134 degRoll = atan2(acc[1], acc[2]) * RadToDeg;
Yukina 5:f6e956e8a060 135 degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
Yukina 5:f6e956e8a060 136
Yukina 5:f6e956e8a060 137 float dpsX = gyro[0] * RadToDeg;
Yukina 5:f6e956e8a060 138 float dpsY = gyro[1] * RadToDeg;
Yukina 5:f6e956e8a060 139 float dpsZ = gyro[2] * RadToDeg;
Yukina 5:f6e956e8a060 140
Yukina 5:f6e956e8a060 141 float curMicros = timer_open.read();
Yukina 5:f6e956e8a060 142 float dt = curMicros - gPrevMicros;
Yukina 5:f6e956e8a060 143 gPrevMicros = curMicros;
Yukina 4:4b3ae90ec778 144
Yukina 5:f6e956e8a060 145 float degX = gKfx.calcAngle(degRoll, dpsX, dt);
Yukina 5:f6e956e8a060 146 float degY = gKfy.calcAngle(degPitch, dpsY, dt);
Yukina 5:f6e956e8a060 147 degY -= gCalibrateY;
Yukina 5:f6e956e8a060 148 degX -= gCalibrateX;
Yukina 5:f6e956e8a060 149 if(degY>ANGLE_JUDGE_FIRE){
Yukina 5:f6e956e8a060 150 Phase = OPEN;
Yukina 5:f6e956e8a060 151 }
Yukina 6:f17310205c1f 152 */
Yukina 6:f17310205c1f 153
Yukina 6:f17310205c1f 154 //madgwick
Yukina 6:f17310205c1f 155 float Roll,Pitch,Yaw;
Yukina 6:f17310205c1f 156 MadgwickFilter.begin(2);
Yukina 6:f17310205c1f 157 mpu.getAccelero(acc);
Yukina 6:f17310205c1f 158 mpu.getGyro(gyro);
Yukina 6:f17310205c1f 159 gyro[0] *= RadToDeg;
Yukina 6:f17310205c1f 160 gyro[1] *= RadToDeg;
Yukina 6:f17310205c1f 161 gyro[2] *= RadToDeg;
Yukina 6:f17310205c1f 162
Yukina 6:f17310205c1f 163 MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]);
Yukina 6:f17310205c1f 164 Roll = MadgwickFilter.getRoll();
Yukina 6:f17310205c1f 165 Pitch = MadgwickFilter.getPitch();
Yukina 6:f17310205c1f 166 Yaw = MadgwickFilter.getYaw();
sashida_h 7:9953d922499d 167 i++;
sashida_h 7:9953d922499d 168 if(i==400){
sashida_h 7:9953d922499d 169 //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch);
sashida_h 7:9953d922499d 170 pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read());
sashida_h 7:9953d922499d 171 i=0;
sashida_h 7:9953d922499d 172 }
sashida_h 7:9953d922499d 173 /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){
Yukina 6:f17310205c1f 174 Phase = OPEN;
sashida_h 7:9953d922499d 175 i=0;
Yukina 6:f17310205c1f 176 }
sashida_h 7:9953d922499d 177 */
Yukina 6:f17310205c1f 178
sashida_h 7:9953d922499d 179 //if(timer_open.read()>TIMER_NOTFIRE){
sashida_h 7:9953d922499d 180 if(timer_data.read()>60.0){
sashida_h 7:9953d922499d 181 Phase = OPEN;
sashida_h 7:9953d922499d 182 pc.printf("NOT_FIRE!!\r\n");
sashida_h 7:9953d922499d 183 }
Yukina 5:f6e956e8a060 184 break;
Yukina 5:f6e956e8a060 185 case OPEN:
Yukina 5:f6e956e8a060 186 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
Yukina 5:f6e956e8a060 187 alt_buff[cnt_data] = _getAlt();
Yukina 5:f6e956e8a060 188 }
Yukina 5:f6e956e8a060 189 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
sashida_h 7:9953d922499d 190 alt_md = alt_md - alt_gnd;
sashida_h 8:15a1b22df82f 191 if(timer_open.read() - t > TIME_SEND){
sashida_h 8:15a1b22df82f 192 pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge);
sashida_h 8:15a1b22df82f 193 t = timer_open.read();
sashida_h 7:9953d922499d 194 }
Yukina 5:f6e956e8a060 195 if(alt_md > alt_max){
Yukina 5:f6e956e8a060 196 alt_max = alt_md;
Yukina 5:f6e956e8a060 197 cnt_judge = 0;
Yukina 5:f6e956e8a060 198 }
Yukina 5:f6e956e8a060 199 else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
Yukina 5:f6e956e8a060 200 cnt_judge++;
sashida_h 8:15a1b22df82f 201 //time_judge = timer_open.read();
Yukina 5:f6e956e8a060 202 }
Yukina 4:4b3ae90ec778 203
sashida_h 7:9953d922499d 204 //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
sashida_h 8:15a1b22df82f 205 if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){
Yukina 5:f6e956e8a060 206 Phase = RECOVERY;
Yukina 5:f6e956e8a060 207 }
Yukina 5:f6e956e8a060 208 break;
Yukina 5:f6e956e8a060 209 case RECOVERY:
sashida_h 8:15a1b22df82f 210 char gps_data[256];
sashida_h 8:15a1b22df82f 211 while(1){
sashida_h 8:15a1b22df82f 212 if(gps.readable()){
sashida_h 8:15a1b22df82f 213 gps_data[cnt_gps] = gps.getc();
sashida_h 8:15a1b22df82f 214 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 8:15a1b22df82f 215 cnt_gps = 0;
sashida_h 8:15a1b22df82f 216 memset(gps_data,'\0',256);
sashida_h 8:15a1b22df82f 217 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 8:15a1b22df82f 218 float world_time, lon_east, lat_north;
sashida_h 8:15a1b22df82f 219 int rlock, sat_num;
sashida_h 8:15a1b22df82f 220 char lat,lon;
sashida_h 8:15a1b22df82f 221 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
sashida_h 8:15a1b22df82f 222 if(rlock==1){
sashida_h 8:15a1b22df82f 223 lat_north = _DMS2DEG(lat_north);
sashida_h 8:15a1b22df82f 224 lon_east = _DMS2DEG(lon_east);
sashida_h 8:15a1b22df82f 225 //pc.printf("%s\r\n",gps_data);
sashida_h 8:15a1b22df82f 226 //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
sashida_h 8:15a1b22df82f 227 int japan_time = int(world_time) - 9;
sashida_h 8:15a1b22df82f 228 pc.printf("Lat:%f,Lon:%f,time:%d,max_alt:%f\r\n",lat_north,lon_east,world_time,alt_max);
sashida_h 8:15a1b22df82f 229 break;
sashida_h 8:15a1b22df82f 230 }else{
sashida_h 8:15a1b22df82f 231 //pc.printf("%s\r\n",gps_data);
sashida_h 8:15a1b22df82f 232 pc.printf("NoGPSSignal,max_alt:%f\r\n",alt_max);
sashida_h 8:15a1b22df82f 233 break;
sashida_h 8:15a1b22df82f 234 }
sashida_h 8:15a1b22df82f 235 }else{
sashida_h 8:15a1b22df82f 236 //ffpc.printf("No_Satellite_signal\r\n");
sashida_h 8:15a1b22df82f 237 }
sashida_h 8:15a1b22df82f 238 }else{
sashida_h 8:15a1b22df82f 239 cnt_gps++;
sashida_h 8:15a1b22df82f 240 }
sashida_h 8:15a1b22df82f 241 }
sashida_h 8:15a1b22df82f 242 }
sashida_h 8:15a1b22df82f 243
Yukina 5:f6e956e8a060 244 Phase = SEA;
Yukina 5:f6e956e8a060 245 break;
Yukina 5:f6e956e8a060 246 case SEA:
sashida_h 7:9953d922499d 247 while(1){
sashida_h 7:9953d922499d 248 if(gps.readable()){
sashida_h 7:9953d922499d 249 c = gps.getc();
sashida_h 7:9953d922499d 250 pc.printf("%c",c);
sashida_h 7:9953d922499d 251 }
sashida_h 7:9953d922499d 252 }
sashida_h 7:9953d922499d 253
Yukina 5:f6e956e8a060 254 break;
Yukina 4:4b3ae90ec778 255 }
Yukina 4:4b3ae90ec778 256 }
Yukina 4:4b3ae90ec778 257 }
Yukina 4:4b3ae90ec778 258
Yukina 4:4b3ae90ec778 259 float _getAlt(){
Yukina 4:4b3ae90ec778 260 float altitude,pressure,temperature;
Yukina 4:4b3ae90ec778 261 bmp.ReadData(&temperature,&pressure);
Yukina 4:4b3ae90ec778 262 altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
Yukina 4:4b3ae90ec778 263 return altitude;
Yukina 4:4b3ae90ec778 264 }
Yukina 4:4b3ae90ec778 265
Yukina 5:f6e956e8a060 266 void _SendData(){
Yukina 5:f6e956e8a060 267 float pretime,a[3],g[3],alt;
Yukina 5:f6e956e8a060 268 //カルマン
Yukina 5:f6e956e8a060 269 /*
Yukina 5:f6e956e8a060 270 pretime = timer_data.read();
Yukina 5:f6e956e8a060 271 mpu.getAccelero(a);
Yukina 5:f6e956e8a060 272 mpu.getGyro(g);
Yukina 5:f6e956e8a060 273 float degroll = atan2(a[1], a[2]) * RadToDeg;
Yukina 5:f6e956e8a060 274 float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
Yukina 5:f6e956e8a060 275
Yukina 5:f6e956e8a060 276 float dpsx = g[0] * RadToDeg;
Yukina 5:f6e956e8a060 277 float dpsy = g[1] * RadToDeg;
Yukina 5:f6e956e8a060 278 float dpsz = g[2] * RadToDeg;
Yukina 5:f6e956e8a060 279
Yukina 5:f6e956e8a060 280 float curtime = timer_data.read();
Yukina 5:f6e956e8a060 281 float t = curtime - pretime;
Yukina 5:f6e956e8a060 282 pretime = curtime;
Yukina 5:f6e956e8a060 283
Yukina 5:f6e956e8a060 284 float degx = gKfx.calcAngle(degroll, dpsx, t);
Yukina 5:f6e956e8a060 285 float degy = gKfy.calcAngle(degpitch, dpsx, t);
Yukina 5:f6e956e8a060 286 degy -= gCalibrateY;
Yukina 5:f6e956e8a060 287 degx -= gCalibrateX;
Yukina 5:f6e956e8a060 288 */
Yukina 5:f6e956e8a060 289 /*madgwick*/
Yukina 5:f6e956e8a060 290 float Roll,Pitch,Yaw;
Yukina 5:f6e956e8a060 291 MadgwickFilter.begin(2);
Yukina 5:f6e956e8a060 292 mpu.getAccelero(a);
Yukina 5:f6e956e8a060 293 mpu.getGyro(g);
Yukina 5:f6e956e8a060 294 g[0] *= RadToDeg;
Yukina 5:f6e956e8a060 295 g[1] *= RadToDeg;
Yukina 5:f6e956e8a060 296 g[2] *= RadToDeg;
Yukina 5:f6e956e8a060 297
Yukina 5:f6e956e8a060 298 MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]);
Yukina 5:f6e956e8a060 299 Roll = MadgwickFilter.getRoll();
Yukina 5:f6e956e8a060 300 Pitch = MadgwickFilter.getPitch();
Yukina 5:f6e956e8a060 301 Yaw = MadgwickFilter.getYaw();
Yukina 5:f6e956e8a060 302
Yukina 5:f6e956e8a060 303 alt = _getAlt();
sashida_h 7:9953d922499d 304 alt = alt - alt_gnd;
Yukina 5:f6e956e8a060 305 //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy);
Yukina 5:f6e956e8a060 306 pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
Yukina 5:f6e956e8a060 307 }
Yukina 5:f6e956e8a060 308
sashida_h 7:9953d922499d 309
Yukina 5:f6e956e8a060 310 void _SendGPS(){
sashida_h 7:9953d922499d 311 char gps_data[256];
sashida_h 7:9953d922499d 312 while(1){
sashida_h 7:9953d922499d 313 if(gps.readable()){
sashida_h 7:9953d922499d 314 gps_data[cnt_gps] = gps.getc();
sashida_h 7:9953d922499d 315 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
sashida_h 7:9953d922499d 316 cnt_gps = 0;
sashida_h 7:9953d922499d 317 memset(gps_data,'\0',256);
sashida_h 7:9953d922499d 318 }else if(gps_data[cnt_gps] == '\r'){
sashida_h 7:9953d922499d 319 float world_time, lon_east, lat_north;
sashida_h 7:9953d922499d 320 int rlock, sat_num;
sashida_h 7:9953d922499d 321 char lat,lon;
sashida_h 7:9953d922499d 322 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
sashida_h 7:9953d922499d 323 if(rlock==1){
sashida_h 7:9953d922499d 324 lat_north = _DMS2DEG(lat_north);
sashida_h 7:9953d922499d 325 lon_east = _DMS2DEG(lon_east);
sashida_h 7:9953d922499d 326 //pc.printf("%s\r\n",gps_data);
sashida_h 7:9953d922499d 327 //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
sashida_h 7:9953d922499d 328 int japan_time = int(world_time) - 9;
sashida_h 7:9953d922499d 329 pc.printf("Lat:%f,Lon:%f,ntime:%d\r\n",lat_north,lon_east,world_time);
sashida_h 7:9953d922499d 330 break;
sashida_h 7:9953d922499d 331 }else{
sashida_h 7:9953d922499d 332 //pc.printf("%s\r\n",gps_data);
sashida_h 7:9953d922499d 333 pc.printf("NoGPSSignal\r\n");
sashida_h 7:9953d922499d 334 break;
sashida_h 7:9953d922499d 335 }
Yukina 5:f6e956e8a060 336 }else{
sashida_h 7:9953d922499d 337 //ffpc.printf("No_Satellite_signal\r\n");
Yukina 5:f6e956e8a060 338 }
sashida_h 7:9953d922499d 339 }else{
sashida_h 7:9953d922499d 340 cnt_gps++;
Yukina 5:f6e956e8a060 341 }
Yukina 5:f6e956e8a060 342 }
Yukina 4:4b3ae90ec778 343 }
sashida_h 7:9953d922499d 344
Yukina 4:4b3ae90ec778 345 }
Yukina 4:4b3ae90ec778 346
Yukina 4:4b3ae90ec778 347 float _DMS2DEG(float raw_data){
Yukina 4:4b3ae90ec778 348 int d=(int)(raw_data/100);
Yukina 4:4b3ae90ec778 349 float m=(raw_data-(float)d*100);
Yukina 4:4b3ae90ec778 350 return (float)d+m/60;
Yukina 4:4b3ae90ec778 351 }
sashida_h 7:9953d922499d 352
Yukina 4:4b3ae90ec778 353
Yukina 4:4b3ae90ec778 354 float _median(float data[], int num){
Yukina 4:4b3ae90ec778 355 float *data_cpy, ans;
Yukina 4:4b3ae90ec778 356 data_cpy = new float[num];
Yukina 4:4b3ae90ec778 357 memcpy(data_cpy,data,sizeof(float)*num);
Yukina 4:4b3ae90ec778 358
Yukina 4:4b3ae90ec778 359 for(int i=0; i<num; i++){
Yukina 4:4b3ae90ec778 360 for(int j=0; j<num-i-1; j++){
Yukina 4:4b3ae90ec778 361 if(data_cpy[j]>data_cpy[j+1]){
Yukina 4:4b3ae90ec778 362 float buff = data_cpy[j+1];
Yukina 4:4b3ae90ec778 363 data_cpy[j+1] = data_cpy[j];
Yukina 4:4b3ae90ec778 364 data_cpy[j] = buff;
Yukina 4:4b3ae90ec778 365 }
Yukina 4:4b3ae90ec778 366 }
Yukina 4:4b3ae90ec778 367 }
Yukina 4:4b3ae90ec778 368
Yukina 4:4b3ae90ec778 369 if(num%2!=0) ans = data_cpy[num/2];
Yukina 4:4b3ae90ec778 370 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
Yukina 4:4b3ae90ec778 371 delete[] data_cpy;
Yukina 4:4b3ae90ec778 372 return ans;
sashida_h 7:9953d922499d 373 }