Haruki Sashida / Mbed 2 deprecated 201903_pf_lower_judgment

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

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@10:1a626929850e, 2019-03-10 (annotated)

Committer:
sashida_h
Date:
Sun Mar 10 03:41:56 2019 +0000
Revision:
10:1a626929850e
Parent:
9:42b4d337d4cc
Child:
11:c90db1500720
2019_0310;

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