201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp@20:89fdec3731f8, 2019-06-23 (annotated)
- Committer:
- sashida_h
- Date:
- Sun Jun 23 09:52:57 2019 +0000
- Revision:
- 20:89fdec3731f8
- Parent:
- 19:1e1a9f1829a9
add description;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sashida_h | 20:89fdec3731f8 | 1 | /*Designed by CORE in HINO*/ |
sashida_h | 20:89fdec3731f8 | 2 | |
mikawataru | 0:0ff20d8e9090 | 3 | #include "mbed.h" |
Yukina | 4:4b3ae90ec778 | 4 | #include "MPU6050.h" |
mikawataru | 0:0ff20d8e9090 | 5 | #include "BMP180.h" |
mikawataru | 0:0ff20d8e9090 | 6 | |
Yukina | 5:f6e956e8a060 | 7 | /*しきい値など*/ |
sashida_h | 13:3e6411bfd581 | 8 | #define CNT_LAUNCH 10 //発射判定するときのしきい値 |
sashida_h | 11:c90db1500720 | 9 | #define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値 |
sashida_h | 8:15a1b22df82f | 10 | #define TIME_BURNING 6 //開放判定しない時間(燃焼時間) |
sashida_h | 19:1e1a9f1829a9 | 11 | #define ALT_JUDGE_OPEN 5 //落下判定のカウントを1増やす高度差 |
sashida_h | 8:15a1b22df82f | 12 | #define TIME_OPEN 25 //強制的に開放させる時間 |
sashida_h | 11:c90db1500720 | 13 | #define TIME_SEND 1.0 //無線送信する間隔 |
sashida_h | 19:1e1a9f1829a9 | 14 | #define CNT_JUDGE 10 //頂点判定する時に落下のカウント数 |
sashida_h | 11:c90db1500720 | 15 | #define NUM_CNT_MEDIAN 10 //中央値をとる個数 |
sashida_h | 11:c90db1500720 | 16 | #define RATE_GPS 1.0 //GPSのTickerを動作させるタイミング |
sashida_h | 11:c90db1500720 | 17 | #define p0 1013.25f //海面気圧 |
Yukina | 5:f6e956e8a060 | 18 | |
sashida_h | 7:9953d922499d | 19 | MPU6050 mpu(PB_7,PB_6); |
sashida_h | 7:9953d922499d | 20 | BMP180 bmp(PB_7,PB_6); |
sashida_h | 7:9953d922499d | 21 | Serial pc(PA_2, PA_3); |
sashida_h | 7:9953d922499d | 22 | Serial gps(PA_9, PA_10); |
sashida_h | 9:42b4d337d4cc | 23 | DigitalOut myled(PA_15); |
sashida_h | 10:1a626929850e | 24 | DigitalOut ES920_RST(PA_5); |
sashida_h | 11:c90db1500720 | 25 | PwmOut servo1(PB_4); |
sashida_h | 11:c90db1500720 | 26 | PwmOut servo2(PB_5); |
sashida_h | 11:c90db1500720 | 27 | DigitalOut servo1_signal(PA_0); |
sashida_h | 11:c90db1500720 | 28 | DigitalOut servo2_signal(PA_1); |
Yukina | 5:f6e956e8a060 | 29 | Timer timer_open; |
Yukina | 5:f6e956e8a060 | 30 | Timer timer_data; |
Yukina | 5:f6e956e8a060 | 31 | Ticker tic_gps; |
mikawataru | 0:0ff20d8e9090 | 32 | |
Yukina | 5:f6e956e8a060 | 33 | /*自作関数*/ |
Yukina | 5:f6e956e8a060 | 34 | float _getAlt(); |
Yukina | 5:f6e956e8a060 | 35 | float _median(float data[],int num); |
Yukina | 5:f6e956e8a060 | 36 | void _SendGPS(); |
Yukina | 5:f6e956e8a060 | 37 | float _DMS2DEG(float raw_data); |
Yukina | 4:4b3ae90ec778 | 38 | |
Yukina | 5:f6e956e8a060 | 39 | enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; |
Yukina | 4:4b3ae90ec778 | 40 | |
Yukina | 5:f6e956e8a060 | 41 | /*グローバル変数*/ |
sashida_h | 7:9953d922499d | 42 | //地上高度 |
sashida_h | 7:9953d922499d | 43 | float alt_gnd; |
sashida_h | 9:42b4d337d4cc | 44 | float alt_max; |
sashida_h | 13:3e6411bfd581 | 45 | char c[516]; |
sashida_h | 9:42b4d337d4cc | 46 | int i = 0; |
sashida_h | 13:3e6411bfd581 | 47 | int cnt_gps=0; |
sashida_h | 9:42b4d337d4cc | 48 | void main(){ |
sashida_h | 10:1a626929850e | 49 | ES920_RST = 0; |
sashida_h | 10:1a626929850e | 50 | myled =0; |
Yukina | 5:f6e956e8a060 | 51 | /*ローカル変数*/ |
Yukina | 5:f6e956e8a060 | 52 | float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; |
sashida_h | 9:42b4d337d4cc | 53 | float alt_buff[10],alt_md; |
Yukina | 5:f6e956e8a060 | 54 | float time_judge; |
Yukina | 5:f6e956e8a060 | 55 | int cnt_data=0,cnt_judge=0; |
sashida_h | 11:c90db1500720 | 56 | float t = 0; |
sashida_h | 14:f932a8a297ff | 57 | float t_hour = 0; |
sashida_h | 14:f932a8a297ff | 58 | float t_send = 0; |
Yukina | 5:f6e956e8a060 | 59 | /*センサの初期化等*/ |
sashida_h | 7:9953d922499d | 60 | pc.baud(38400); |
Yukina | 5:f6e956e8a060 | 61 | mpu.setAcceleroRange(3); |
mikawataru | 0:0ff20d8e9090 | 62 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
Yukina | 5:f6e956e8a060 | 63 | |
Yukina | 5:f6e956e8a060 | 64 | /*初期位置の設定*/ |
Yukina | 5:f6e956e8a060 | 65 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 66 | mpu.getGyro(gyro); |
Yukina | 5:f6e956e8a060 | 67 | |
Yukina | 5:f6e956e8a060 | 68 | Phase = STANDBY; |
sashida_h | 7:9953d922499d | 69 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
sashida_h | 7:9953d922499d | 70 | alt_buff[cnt_data] = _getAlt(); |
sashida_h | 7:9953d922499d | 71 | } |
sashida_h | 7:9953d922499d | 72 | alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN); |
sashida_h | 7:9953d922499d | 73 | wait(2.0); |
sashida_h | 12:6a3c0e9075eb | 74 | pc.printf("0,0,0,0,0\r\n"); |
sashida_h | 8:15a1b22df82f | 75 | wait(1.0); |
sashida_h | 9:42b4d337d4cc | 76 | timer_data.start(); |
sashida_h | 11:c90db1500720 | 77 | servo1_signal = 1; |
sashida_h | 11:c90db1500720 | 78 | servo2_signal = 1; |
sashida_h | 13:3e6411bfd581 | 79 | servo1.pulsewidth(0.0015);//close |
sashida_h | 13:3e6411bfd581 | 80 | servo2.pulsewidth(0.0006);//close |
Yukina | 5:f6e956e8a060 | 81 | while(1){ |
Yukina | 5:f6e956e8a060 | 82 | switch(Phase){ |
Yukina | 5:f6e956e8a060 | 83 | case STANDBY: |
sashida_h | 10:1a626929850e | 84 | /*サーボ入力待ち*/ |
sashida_h | 10:1a626929850e | 85 | //servo(); |
sashida_h | 14:f932a8a297ff | 86 | if(pc.readable()){ |
sashida_h | 14:f932a8a297ff | 87 | c[0]=pc.getc(); |
sashida_h | 14:f932a8a297ff | 88 | } |
sashida_h | 14:f932a8a297ff | 89 | t = timer_data.read(); |
sashida_h | 14:f932a8a297ff | 90 | if(t > 1800){ |
sashida_h | 18:37d747f51c0a | 91 | t_hour += 1800; |
sashida_h | 14:f932a8a297ff | 92 | timer_data.reset(); |
sashida_h | 14:f932a8a297ff | 93 | } |
sashida_h | 14:f932a8a297ff | 94 | t = t + t_hour; |
sashida_h | 14:f932a8a297ff | 95 | t = t/60.0; |
sashida_h | 14:f932a8a297ff | 96 | |
sashida_h | 11:c90db1500720 | 97 | if(c[0] == 'o'){ |
sashida_h | 11:c90db1500720 | 98 | myled = 1; |
sashida_h | 13:3e6411bfd581 | 99 | servo1.pulsewidth(0.0006);//open |
sashida_h | 13:3e6411bfd581 | 100 | servo2.pulsewidth(0.0015);//open |
sashida_h | 11:c90db1500720 | 101 | mpu.getAccelero(acc); |
sashida_h | 12:6a3c0e9075eb | 102 | //pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); |
sashida_h | 16:6395de1ad89c | 103 | pc.printf("0,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),t); |
sashida_h | 11:c90db1500720 | 104 | } |
sashida_h | 11:c90db1500720 | 105 | if(c[0] == 'l'){ |
sashida_h | 11:c90db1500720 | 106 | myled = 0; |
sashida_h | 13:3e6411bfd581 | 107 | servo1.pulsewidth(0.0015); |
sashida_h | 13:3e6411bfd581 | 108 | servo2.pulsewidth(0.0006); |
sashida_h | 11:c90db1500720 | 109 | mpu.getAccelero(acc); |
sashida_h | 12:6a3c0e9075eb | 110 | //pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); |
sashida_h | 16:6395de1ad89c | 111 | pc.printf("0,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),t); |
sashida_h | 11:c90db1500720 | 112 | } |
sashida_h | 10:1a626929850e | 113 | if(c[0] == 'f'){ |
sashida_h | 10:1a626929850e | 114 | //pc.printf("flight_mode\r\n"); |
sashida_h | 10:1a626929850e | 115 | Phase = LAUNCH; |
sashida_h | 11:c90db1500720 | 116 | servo1_signal = 0; |
sashida_h | 11:c90db1500720 | 117 | servo2_signal = 0; |
sashida_h | 18:37d747f51c0a | 118 | t = 0; |
sashida_h | 18:37d747f51c0a | 119 | timer_data.reset(); |
sashida_h | 10:1a626929850e | 120 | } |
sashida_h | 13:3e6411bfd581 | 121 | if(c[0] == 'g'){ |
sashida_h | 13:3e6411bfd581 | 122 | pc.printf("9,0,0,0,0\r\n"); |
sashida_h | 13:3e6411bfd581 | 123 | wait(0.5); |
sashida_h | 13:3e6411bfd581 | 124 | tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); |
sashida_h | 13:3e6411bfd581 | 125 | Phase = RECOVERY; |
sashida_h | 13:3e6411bfd581 | 126 | } |
Yukina | 5:f6e956e8a060 | 127 | break; |
sashida_h | 8:15a1b22df82f | 128 | |
Yukina | 5:f6e956e8a060 | 129 | case LAUNCH: |
Yukina | 5:f6e956e8a060 | 130 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 131 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 132 | 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 | 133 | } |
Yukina | 5:f6e956e8a060 | 134 | acc_abs = _median(acc_buff,NUM_CNT_MEDIAN); |
sashida_h | 8:15a1b22df82f | 135 | if(timer_data.read() - t > TIME_SEND){ |
sashida_h | 12:6a3c0e9075eb | 136 | //pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge); |
sashida_h | 16:6395de1ad89c | 137 | pc.printf("1,%f,0,%3f\r\n",acc_abs,timer_data.read()); |
sashida_h | 13:3e6411bfd581 | 138 | cnt_judge = 0; |
sashida_h | 8:15a1b22df82f | 139 | t = timer_data.read(); |
sashida_h | 7:9953d922499d | 140 | } |
Yukina | 5:f6e956e8a060 | 141 | /*加速度判定*/ |
Yukina | 5:f6e956e8a060 | 142 | if(acc_abs>ACC_JUDGE_LAUNCH){ |
Yukina | 5:f6e956e8a060 | 143 | cnt_judge++; |
Yukina | 4:4b3ae90ec778 | 144 | } |
sashida_h | 13:3e6411bfd581 | 145 | if(cnt_judge==CNT_LAUNCH){ |
sashida_h | 11:c90db1500720 | 146 | servo1_signal = 1; |
sashida_h | 11:c90db1500720 | 147 | servo1_signal = 1; |
Yukina | 5:f6e956e8a060 | 148 | cnt_judge=0; |
Yukina | 5:f6e956e8a060 | 149 | timer_open.start(); |
sashida_h | 7:9953d922499d | 150 | Phase = RISE; |
sashida_h | 18:37d747f51c0a | 151 | timer_data.stop(); |
Yukina | 5:f6e956e8a060 | 152 | } |
Yukina | 5:f6e956e8a060 | 153 | break; |
sashida_h | 8:15a1b22df82f | 154 | |
Yukina | 5:f6e956e8a060 | 155 | case RISE: |
Yukina | 5:f6e956e8a060 | 156 | while(timer_open.read() < TIME_BURNING){ |
sashida_h | 12:6a3c0e9075eb | 157 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
sashida_h | 12:6a3c0e9075eb | 158 | alt_buff[cnt_data] = _getAlt(); |
sashida_h | 12:6a3c0e9075eb | 159 | } |
sashida_h | 12:6a3c0e9075eb | 160 | alt_md = _median(alt_buff,NUM_CNT_MEDIAN); |
sashida_h | 12:6a3c0e9075eb | 161 | alt_md = alt_md - alt_gnd; |
sashida_h | 12:6a3c0e9075eb | 162 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
sashida_h | 12:6a3c0e9075eb | 163 | mpu.getAccelero(acc); |
sashida_h | 12:6a3c0e9075eb | 164 | 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)); |
sashida_h | 12:6a3c0e9075eb | 165 | } |
sashida_h | 12:6a3c0e9075eb | 166 | acc_abs = _median(acc_buff,NUM_CNT_MEDIAN); |
sashida_h | 12:6a3c0e9075eb | 167 | //pc.printf("RISE,time from launch:%f\r\n",timer_open.read()); |
sashida_h | 16:6395de1ad89c | 168 | pc.printf("3,%f,%f,%3f\r\n",acc_abs,alt_md,timer_open.read()); |
sashida_h | 12:6a3c0e9075eb | 169 | wait(0.9); |
sashida_h | 7:9953d922499d | 170 | i=0; |
sashida_h | 18:37d747f51c0a | 171 | //timer_data.reset(); |
Yukina | 5:f6e956e8a060 | 172 | } |
sashida_h | 8:15a1b22df82f | 173 | Phase = OPEN; |
sashida_h | 8:15a1b22df82f | 174 | t = 0.0; |
Yukina | 5:f6e956e8a060 | 175 | break; |
Yukina | 5:f6e956e8a060 | 176 | case FIRE: |
Yukina | 5:f6e956e8a060 | 177 | break; |
Yukina | 5:f6e956e8a060 | 178 | case OPEN: |
Yukina | 5:f6e956e8a060 | 179 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 180 | alt_buff[cnt_data] = _getAlt(); |
Yukina | 5:f6e956e8a060 | 181 | } |
Yukina | 5:f6e956e8a060 | 182 | alt_md = _median(alt_buff,NUM_CNT_MEDIAN); |
sashida_h | 7:9953d922499d | 183 | alt_md = alt_md - alt_gnd; |
sashida_h | 8:15a1b22df82f | 184 | if(timer_open.read() - t > TIME_SEND){ |
sashida_h | 12:6a3c0e9075eb | 185 | //pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge); |
sashida_h | 16:6395de1ad89c | 186 | pc.printf("15,%f,%f,%3f\r\n",acc_abs,alt_md,timer_open.read()); |
sashida_h | 8:15a1b22df82f | 187 | t = timer_open.read(); |
sashida_h | 7:9953d922499d | 188 | } |
Yukina | 5:f6e956e8a060 | 189 | if(alt_md > alt_max){ |
Yukina | 5:f6e956e8a060 | 190 | alt_max = alt_md; |
Yukina | 5:f6e956e8a060 | 191 | cnt_judge = 0; |
Yukina | 5:f6e956e8a060 | 192 | } |
Yukina | 5:f6e956e8a060 | 193 | else if((alt_max-alt_md) > ALT_JUDGE_OPEN){ |
Yukina | 5:f6e956e8a060 | 194 | cnt_judge++; |
Yukina | 5:f6e956e8a060 | 195 | } |
Yukina | 4:4b3ae90ec778 | 196 | |
sashida_h | 7:9953d922499d | 197 | //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0; |
sashida_h | 8:15a1b22df82f | 198 | if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){ |
Yukina | 5:f6e956e8a060 | 199 | Phase = RECOVERY; |
sashida_h | 13:3e6411bfd581 | 200 | servo1_signal = 1; |
sashida_h | 13:3e6411bfd581 | 201 | servo2_signal = 1; |
sashida_h | 13:3e6411bfd581 | 202 | wait(0.1); |
sashida_h | 13:3e6411bfd581 | 203 | servo1.pulsewidth(0.0006);//open |
sashida_h | 13:3e6411bfd581 | 204 | servo2.pulsewidth(0.0015);//open |
sashida_h | 14:f932a8a297ff | 205 | pc.printf("9,0,0,0,%d\r\n",cnt_judge); |
sashida_h | 12:6a3c0e9075eb | 206 | wait(0.5); |
sashida_h | 9:42b4d337d4cc | 207 | tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); |
Yukina | 5:f6e956e8a060 | 208 | } |
Yukina | 5:f6e956e8a060 | 209 | break; |
Yukina | 5:f6e956e8a060 | 210 | case RECOVERY: |
Yukina | 5:f6e956e8a060 | 211 | break; |
Yukina | 5:f6e956e8a060 | 212 | case SEA: |
Yukina | 5:f6e956e8a060 | 213 | break; |
Yukina | 4:4b3ae90ec778 | 214 | } |
Yukina | 4:4b3ae90ec778 | 215 | } |
Yukina | 4:4b3ae90ec778 | 216 | } |
Yukina | 4:4b3ae90ec778 | 217 | |
Yukina | 4:4b3ae90ec778 | 218 | float _getAlt(){ |
Yukina | 4:4b3ae90ec778 | 219 | float altitude,pressure,temperature; |
Yukina | 4:4b3ae90ec778 | 220 | bmp.ReadData(&temperature,&pressure); |
Yukina | 4:4b3ae90ec778 | 221 | altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; |
Yukina | 4:4b3ae90ec778 | 222 | return altitude; |
Yukina | 4:4b3ae90ec778 | 223 | } |
Yukina | 4:4b3ae90ec778 | 224 | |
sashida_h | 9:42b4d337d4cc | 225 | float _DMS2DEG(float raw_data){ |
sashida_h | 9:42b4d337d4cc | 226 | int d=(int)(raw_data/100); |
sashida_h | 9:42b4d337d4cc | 227 | float m=(raw_data-(float)d*100); |
sashida_h | 9:42b4d337d4cc | 228 | return (float)d+m/60; |
sashida_h | 9:42b4d337d4cc | 229 | } |
sashida_h | 9:42b4d337d4cc | 230 | |
sashida_h | 9:42b4d337d4cc | 231 | |
sashida_h | 9:42b4d337d4cc | 232 | float _median(float data[], int num){ |
sashida_h | 9:42b4d337d4cc | 233 | float *data_cpy, ans; |
sashida_h | 9:42b4d337d4cc | 234 | data_cpy = new float[num]; |
sashida_h | 9:42b4d337d4cc | 235 | memcpy(data_cpy,data,sizeof(float)*num); |
sashida_h | 9:42b4d337d4cc | 236 | |
sashida_h | 9:42b4d337d4cc | 237 | for(i=0; i<num; i++){ |
sashida_h | 9:42b4d337d4cc | 238 | for(int j=0; j<num-i-1; j++){ |
sashida_h | 9:42b4d337d4cc | 239 | if(data_cpy[j]>data_cpy[j+1]){ |
sashida_h | 9:42b4d337d4cc | 240 | float buff = data_cpy[j+1]; |
sashida_h | 9:42b4d337d4cc | 241 | data_cpy[j+1] = data_cpy[j]; |
sashida_h | 9:42b4d337d4cc | 242 | data_cpy[j] = buff; |
sashida_h | 9:42b4d337d4cc | 243 | } |
sashida_h | 9:42b4d337d4cc | 244 | } |
sashida_h | 9:42b4d337d4cc | 245 | } |
sashida_h | 9:42b4d337d4cc | 246 | |
sashida_h | 9:42b4d337d4cc | 247 | if(num%2!=0) ans = data_cpy[num/2]; |
sashida_h | 9:42b4d337d4cc | 248 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
sashida_h | 9:42b4d337d4cc | 249 | delete[] data_cpy; |
sashida_h | 9:42b4d337d4cc | 250 | return ans; |
sashida_h | 9:42b4d337d4cc | 251 | } |
sashida_h | 9:42b4d337d4cc | 252 | |
Yukina | 5:f6e956e8a060 | 253 | void _SendGPS(){ |
sashida_h | 7:9953d922499d | 254 | char gps_data[256]; |
sashida_h | 7:9953d922499d | 255 | while(1){ |
sashida_h | 7:9953d922499d | 256 | if(gps.readable()){ |
sashida_h | 7:9953d922499d | 257 | gps_data[cnt_gps] = gps.getc(); |
sashida_h | 7:9953d922499d | 258 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
sashida_h | 7:9953d922499d | 259 | cnt_gps = 0; |
sashida_h | 7:9953d922499d | 260 | memset(gps_data,'\0',256); |
sashida_h | 7:9953d922499d | 261 | }else if(gps_data[cnt_gps] == '\r'){ |
sashida_h | 7:9953d922499d | 262 | float world_time, lon_east, lat_north; |
sashida_h | 7:9953d922499d | 263 | int rlock, sat_num; |
sashida_h | 7:9953d922499d | 264 | char lat,lon; |
sashida_h | 7:9953d922499d | 265 | 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 | 266 | if(rlock==1){ |
sashida_h | 7:9953d922499d | 267 | lat_north = _DMS2DEG(lat_north); |
sashida_h | 7:9953d922499d | 268 | lon_east = _DMS2DEG(lon_east); |
sashida_h | 7:9953d922499d | 269 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 7:9953d922499d | 270 | //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 | 271 | int japan_time = int(world_time) - 9; |
sashida_h | 13:3e6411bfd581 | 272 | pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); |
sashida_h | 10:1a626929850e | 273 | for(i=0;i<2;i++){ |
sashida_h | 10:1a626929850e | 274 | c[i]=pc.getc(); |
sashida_h | 10:1a626929850e | 275 | } |
sashida_h | 10:1a626929850e | 276 | //pc.printf("%c",c[1]); |
sashida_h | 10:1a626929850e | 277 | if(c[1]!='O'){ |
sashida_h | 10:1a626929850e | 278 | ES920_RST = 1; |
sashida_h | 10:1a626929850e | 279 | wait(0.1); |
sashida_h | 10:1a626929850e | 280 | ES920_RST = 0; |
sashida_h | 10:1a626929850e | 281 | wait(1.0); |
sashida_h | 10:1a626929850e | 282 | myled = 1; |
sashida_h | 10:1a626929850e | 283 | }else{ |
sashida_h | 10:1a626929850e | 284 | myled = 0; |
sashida_h | 10:1a626929850e | 285 | } |
sashida_h | 10:1a626929850e | 286 | |
sashida_h | 7:9953d922499d | 287 | break; |
sashida_h | 7:9953d922499d | 288 | }else{ |
sashida_h | 7:9953d922499d | 289 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 17:9fc09af2f37a | 290 | pc.printf("NoGPSSignal\r\n"); |
sashida_h | 7:9953d922499d | 291 | break; |
sashida_h | 7:9953d922499d | 292 | } |
Yukina | 5:f6e956e8a060 | 293 | }else{ |
sashida_h | 13:3e6411bfd581 | 294 | //pc.printf("No_Satellite_signal\r\n"); |
Yukina | 5:f6e956e8a060 | 295 | } |
sashida_h | 7:9953d922499d | 296 | }else{ |
sashida_h | 7:9953d922499d | 297 | cnt_gps++; |
Yukina | 5:f6e956e8a060 | 298 | } |
Yukina | 5:f6e956e8a060 | 299 | } |
Yukina | 4:4b3ae90ec778 | 300 | } |
Yukina | 4:4b3ae90ec778 | 301 | |
sashida_h | 7:9953d922499d | 302 | } |