PQ_Hybrid_Electrical_Equipment_Team / GPS_interrupt

2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム

Dependents:   Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more

GPS_interrupt.cpp@17:4b09630703c0, 2018-11-14 (annotated)

Committer:
Sigma884
Date:
Wed Nov 14 09:16:15 2018 +0000
Revision:
17:4b09630703c0
Parent:
15:6b1ed321c1be 
????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:74d8e952a3bd 1 #include "mbed.h"
Gaku0606 0:74d8e952a3bd 2 #include "GPS_interrupt.h"
Gaku0606 4:758f97bee95a 3
Gaku0606 4:758f97bee95a 4 /*double GPS_interrupt::latitude;
Gaku0606 0:74d8e952a3bd 5 double GPS_interrupt::longitude;
Gaku0606 0:74d8e952a3bd 6 int GPS_interrupt::year;
Gaku0606 0:74d8e952a3bd 7 int GPS_interrupt::month;
Gaku0606 0:74d8e952a3bd 8 int GPS_interrupt::day;
Gaku0606 0:74d8e952a3bd 9 int GPS_interrupt::hour;
Gaku0606 0:74d8e952a3bd 10 int GPS_interrupt::minutes;
Gaku0606 0:74d8e952a3bd 11 double GPS_interrupt::seconds;
Gaku0606 0:74d8e952a3bd 12 double GPS_interrupt::degree = 0;
Gaku0606 0:74d8e952a3bd 13 double GPS_interrupt::knot = 0;
Gaku0606 0:74d8e952a3bd 14 double GPS_interrupt::height = 0;
Gaku0606 0:74d8e952a3bd 15 double GPS_interrupt::geoid = 0;
Gaku0606 0:74d8e952a3bd 16 int GPS_interrupt::number = 0;
Gaku0606 8:3f32df2b66c0 17 */
Gaku0606 8:3f32df2b66c0 18 //char GPS_interrupt::gps_buffer_A[128] = {'\0'};
Gaku0606 8:3f32df2b66c0 19 //char GPS_interrupt::gps_buffer_B[128] = {'\0'};
Gaku0606 8:3f32df2b66c0 20 //char GPS_interrupt::gps_buffer_C[128] = {'\0'};
Gaku0606 8:3f32df2b66c0 21 /*
Gaku0606 0:74d8e952a3bd 22 char* GPS_interrupt::gps_read_buffer = gps_buffer_B;
Gaku0606 4:758f97bee95a 23 bool GPS_interrupt::gps_readable = false;*/
Gaku0606 0:74d8e952a3bd 24
Gaku0606 4:758f97bee95a 25 //GPS_interrupt* GPS_interrupt::gps_irq;
Gaku0606 0:74d8e952a3bd 26
Gaku0606 2:7be89bab6db9 27 //Timeout timeout_clear;
Gaku0606 2:7be89bab6db9 28
Gaku0606 4:758f97bee95a 29 void GPS_interrupt::debug(bool tf){
Gaku0606 4:758f97bee95a 30 if(tf){
Gaku0606 4:758f97bee95a 31 debugFlag = true;
Gaku0606 4:758f97bee95a 32 }
Gaku0606 4:758f97bee95a 33 else{
Gaku0606 4:758f97bee95a 34 debugFlag = false;
Gaku0606 4:758f97bee95a 35 }
Gaku0606 2:7be89bab6db9 36 }
Gaku0606 2:7be89bab6db9 37
Gaku0606 2:7be89bab6db9 38 unsigned char GPS_interrupt::checkSum(char *str){
Gaku0606 2:7be89bab6db9 39 int num = strlen(str);
Gaku0606 2:7be89bab6db9 40 unsigned char val = 0;
Gaku0606 2:7be89bab6db9 41 for(int i = 0; i< num; i++){
Gaku0606 2:7be89bab6db9 42 val = val ^ str[i];
Gaku0606 2:7be89bab6db9 43 }
Gaku0606 2:7be89bab6db9 44 return val;
Gaku0606 2:7be89bab6db9 45 }
Sigma884 15:6b1ed321c1be 46
Gaku0606 11:1897b52fa8a1 47 GPS_interrupt::GPS_interrupt(Serial *_gps){
Gaku0606 2:7be89bab6db9 48
Gaku0606 11:1897b52fa8a1 49 if(_gps == NULL){
Gaku0606 11:1897b52fa8a1 50 error("GPS UART BUS ERROR!!\r\n");
Gaku0606 11:1897b52fa8a1 51 }
Gaku0606 6:2f91c71d64b1 52 debugFlag = false;
Gaku0606 6:2f91c71d64b1 53 initialize();
Sigma884 15:6b1ed321c1be 54 //baudrate = 9600;//_baudrate;
Sigma884 15:6b1ed321c1be 55 //frequency = 1;
Gaku0606 6:2f91c71d64b1 56 gps_irq = this;
Gaku0606 11:1897b52fa8a1 57 gps = _gps;
Sigma884 15:6b1ed321c1be 58 //gps->baud(baudrate);
Gaku0606 6:2f91c71d64b1 59 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 6:2f91c71d64b1 60 }
Gaku0606 6:2f91c71d64b1 61
Sigma884 15:6b1ed321c1be 62 /*
Gaku0606 11:1897b52fa8a1 63 GPS_interrupt::GPS_interrupt(Serial *_gps, int _baudrate){
Gaku0606 2:7be89bab6db9 64
Gaku0606 11:1897b52fa8a1 65 if(_gps == NULL){
Gaku0606 11:1897b52fa8a1 66 error("GPS UART BUS ERROR!!\r\n");
Gaku0606 11:1897b52fa8a1 67 }
Gaku0606 4:758f97bee95a 68 debugFlag = false;
Gaku0606 4:758f97bee95a 69 initialize();
Gaku0606 11:1897b52fa8a1 70 baudrate = _baudrate;
Gaku0606 11:1897b52fa8a1 71 //frequency = _frequency;
Gaku0606 0:74d8e952a3bd 72 gps_irq = this;
Gaku0606 0:74d8e952a3bd 73 gps = _gps;
Gaku0606 11:1897b52fa8a1 74 gps->baud(baudrate);
Sigma884 15:6b1ed321c1be 75
Sigma884 15:6b1ed321c1be 76 if(baudrate == 9600){
Gaku0606 4:758f97bee95a 77 gps->printf("$PMTK251,9600*17\r\n");
Gaku0606 4:758f97bee95a 78 }
Gaku0606 4:758f97bee95a 79 else if(baudrate == 19200){
Gaku0606 4:758f97bee95a 80 gps->printf("$PMTK251,19200*22\r\n");
Gaku0606 4:758f97bee95a 81 }
Gaku0606 4:758f97bee95a 82 else if( baudrate == 38400){
Gaku0606 4:758f97bee95a 83 gps->printf("$PMTK251,38400*27\r\n");
Gaku0606 4:758f97bee95a 84 }
Gaku0606 4:758f97bee95a 85 else if(baudrate == 56700){
Gaku0606 4:758f97bee95a 86 gps->printf("$PMTK251,57600*2C\r\n");
Gaku0606 4:758f97bee95a 87 }
Gaku0606 4:758f97bee95a 88 else if(baudrate == 115200){
Gaku0606 4:758f97bee95a 89 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 90 wait(0.1);
Gaku0606 4:758f97bee95a 91 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 92 wait(0.2);
Gaku0606 4:758f97bee95a 93 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 94 wait(0.2);
Gaku0606 4:758f97bee95a 95 }
Gaku0606 8:3f32df2b66c0 96 gps->baud(9600);
Gaku0606 11:1897b52fa8a1 97 */
Gaku0606 11:1897b52fa8a1 98 //baudrate = 9600;
Gaku0606 11:1897b52fa8a1 99 /*wait(0.1);
Gaku0606 4:758f97bee95a 100
Gaku0606 4:758f97bee95a 101 //gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format
Gaku0606 2:7be89bab6db9 102 unsigned char checksum = 0;
Gaku0606 4:758f97bee95a 103 checksum = checkSum("PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
Gaku0606 2:7be89bab6db9 104 //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム
Gaku0606 4:758f97bee95a 105 gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum);
Gaku0606 4:758f97bee95a 106 wait(0.2);
Gaku0606 4:758f97bee95a 107 gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum);
Gaku0606 4:758f97bee95a 108 wait(0.2);
Gaku0606 4:758f97bee95a 109 gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum);
Gaku0606 4:758f97bee95a 110 wait(0.2);
Gaku0606 4:758f97bee95a 111 if(_frequency == 1) gps->printf("$PMTK220,1000*1F\r\n");
Gaku0606 4:758f97bee95a 112 else if(_frequency == 2) gps->printf("$PMTK220,500*2B\r\n");
Gaku0606 4:758f97bee95a 113 else if(_frequency == 3) gps->printf("$PMTK220,333*2D\r\n");
Gaku0606 4:758f97bee95a 114 else if(_frequency == 4) gps->printf("$PMTK220,250*29\r\n");
Gaku0606 4:758f97bee95a 115 else if(_frequency == 5) gps->printf("$PMTK220,200*2C\r\n");
Gaku0606 4:758f97bee95a 116 else if(_frequency == 10) gps->printf("$PMTK220,100*2F\r\n");
Gaku0606 4:758f97bee95a 117 wait(0.2);
Gaku0606 11:1897b52fa8a1 118 */
Sigma884 15:6b1ed321c1be 119 /*
Gaku0606 0:74d8e952a3bd 120 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 0:74d8e952a3bd 121 }
Sigma884 15:6b1ed321c1be 122 */
Sigma884 15:6b1ed321c1be 123
Sigma884 15:6b1ed321c1be 124 void GPS_interrupt::changeGPSBaud(int _baudrate){
Sigma884 15:6b1ed321c1be 125 baudrate = _baudrate;
Sigma884 15:6b1ed321c1be 126 switch(baudrate){
Sigma884 15:6b1ed321c1be 127 case 4800:
Sigma884 15:6b1ed321c1be 128 gps -> printf("$PMTK251,4800*14\r\n");
Sigma884 15:6b1ed321c1be 129 wait(0.2f);
Sigma884 15:6b1ed321c1be 130 gps->printf("$PMTK251,4800*14\r\n");
Sigma884 15:6b1ed321c1be 131 wait(0.2f);
Sigma884 15:6b1ed321c1be 132 gps->printf("$PMTK251,4800*14\r\n");
Sigma884 15:6b1ed321c1be 133 wait(0.2f);
Sigma884 15:6b1ed321c1be 134 gps->printf("$PMTK251,4800*14\r\n");
Sigma884 15:6b1ed321c1be 135 wait(0.2f);
Sigma884 15:6b1ed321c1be 136 gps->printf("$PMTK251,4800*14\r\n");
Sigma884 15:6b1ed321c1be 137 wait(0.2f);
Sigma884 15:6b1ed321c1be 138 gps->baud(4800);
Sigma884 15:6b1ed321c1be 139 break;
Sigma884 15:6b1ed321c1be 140
Sigma884 15:6b1ed321c1be 141 case 9600:
Sigma884 15:6b1ed321c1be 142 gps->printf("$PMTK251,9600*17\r\n");
Sigma884 15:6b1ed321c1be 143 wait(0.2f);
Sigma884 15:6b1ed321c1be 144 gps->printf("$PMTK251,9600*17\r\n");
Sigma884 15:6b1ed321c1be 145 wait(0.2f);
Sigma884 15:6b1ed321c1be 146 gps->printf("$PMTK251,9600*17\r\n");
Sigma884 15:6b1ed321c1be 147 wait(0.2f);
Sigma884 15:6b1ed321c1be 148 gps->printf("$PMTK251,9600*17\r\n");
Sigma884 15:6b1ed321c1be 149 wait(0.2f);
Sigma884 15:6b1ed321c1be 150 gps->printf("$PMTK251,9600*17\r\n");
Sigma884 15:6b1ed321c1be 151 wait(0.2f);
Sigma884 15:6b1ed321c1be 152 gps->baud(9600);
Sigma884 15:6b1ed321c1be 153 break;
Sigma884 15:6b1ed321c1be 154
Sigma884 15:6b1ed321c1be 155 case 14400:
Sigma884 15:6b1ed321c1be 156 gps->printf("$PMTK251,14400*29\r\n");
Sigma884 15:6b1ed321c1be 157 wait(0.2f);
Sigma884 15:6b1ed321c1be 158 gps->printf("$PMTK251,14400*29\r\n");
Sigma884 15:6b1ed321c1be 159 wait(0.2f);
Sigma884 15:6b1ed321c1be 160 gps->printf("$PMTK251,14400*29\r\n");
Sigma884 15:6b1ed321c1be 161 wait(0.2f);
Sigma884 15:6b1ed321c1be 162 gps->printf("$PMTK251,14400*29\r\n");
Sigma884 15:6b1ed321c1be 163 wait(0.2f);
Sigma884 15:6b1ed321c1be 164 gps->printf("$PMTK251,14400*29\r\n");
Sigma884 15:6b1ed321c1be 165 wait(0.2f);
Sigma884 15:6b1ed321c1be 166 gps->baud(14400);
Sigma884 15:6b1ed321c1be 167 break;
Sigma884 15:6b1ed321c1be 168
Sigma884 15:6b1ed321c1be 169 case 19200:
Sigma884 15:6b1ed321c1be 170 gps->printf("$PMTK251,19200*22\r\n");
Sigma884 15:6b1ed321c1be 171 wait(0.2f);
Sigma884 15:6b1ed321c1be 172 gps->printf("$PMTK251,19200*22\r\n");
Sigma884 15:6b1ed321c1be 173 wait(0.2f);
Sigma884 15:6b1ed321c1be 174 gps->printf("$PMTK251,19200*22\r\n");
Sigma884 15:6b1ed321c1be 175 wait(0.2f);
Sigma884 15:6b1ed321c1be 176 gps->printf("$PMTK251,19200*22\r\n");
Sigma884 15:6b1ed321c1be 177 wait(0.2f);
Sigma884 15:6b1ed321c1be 178 gps->printf("$PMTK251,19200*22\r\n");
Sigma884 15:6b1ed321c1be 179 wait(0.2f);
Sigma884 15:6b1ed321c1be 180 gps->baud(19200);
Sigma884 15:6b1ed321c1be 181 break;
Sigma884 15:6b1ed321c1be 182
Sigma884 15:6b1ed321c1be 183 case 38400:
Sigma884 17:4b09630703c0 184 wait(0.2f);
Sigma884 17:4b09630703c0 185 gps->printf("$PMTK251,38400*27\r\n");
Sigma884 17:4b09630703c0 186 wait(0.2f);
Sigma884 17:4b09630703c0 187 /*
Sigma884 15:6b1ed321c1be 188 gps->printf("$PMTK251,38400*27\r\n");
Sigma884 15:6b1ed321c1be 189 wait(0.2f);
Sigma884 15:6b1ed321c1be 190 gps->printf("$PMTK251,38400*27\r\n");
Sigma884 15:6b1ed321c1be 191 wait(0.2f);
Sigma884 15:6b1ed321c1be 192 gps->printf("$PMTK251,38400*27\r\n");
Sigma884 15:6b1ed321c1be 193 wait(0.2f);
Sigma884 15:6b1ed321c1be 194 gps->printf("$PMTK251,38400*27\r\n");
Sigma884 15:6b1ed321c1be 195 wait(0.2f);
Sigma884 17:4b09630703c0 196 */
Sigma884 15:6b1ed321c1be 197 gps->baud(38400);
Sigma884 15:6b1ed321c1be 198 break;
Sigma884 15:6b1ed321c1be 199
Sigma884 15:6b1ed321c1be 200 case 57600:
Sigma884 15:6b1ed321c1be 201 gps->printf("$PMTK251,57600*2C\r\n");
Sigma884 15:6b1ed321c1be 202 wait(0.2f);
Sigma884 15:6b1ed321c1be 203 gps->printf("$PMTK251,57600*2C\r\n");
Sigma884 15:6b1ed321c1be 204 wait(0.2f);
Sigma884 15:6b1ed321c1be 205 gps->printf("$PMTK251,57600*2C\r\n");
Sigma884 15:6b1ed321c1be 206 wait(0.2f);
Sigma884 15:6b1ed321c1be 207 gps->printf("$PMTK251,57600*2C\r\n");
Sigma884 15:6b1ed321c1be 208 wait(0.2f);
Sigma884 15:6b1ed321c1be 209 gps->printf("$PMTK251,57600*2C\r\n");
Sigma884 15:6b1ed321c1be 210 wait(0.2f);
Sigma884 15:6b1ed321c1be 211 gps->baud(57600);
Sigma884 15:6b1ed321c1be 212 break;
Sigma884 15:6b1ed321c1be 213
Sigma884 15:6b1ed321c1be 214 case 115200:
Sigma884 15:6b1ed321c1be 215 gps->printf("$PMTK251,115200*1F\r\n");
Sigma884 15:6b1ed321c1be 216 wait(0.2f);
Sigma884 15:6b1ed321c1be 217 gps->printf("$PMTK251,115200*1F\r\n");
Sigma884 15:6b1ed321c1be 218 wait(0.2f);
Sigma884 15:6b1ed321c1be 219 gps->printf("$PMTK251,115200*1F\r\n");
Sigma884 15:6b1ed321c1be 220 wait(0.2f);
Sigma884 15:6b1ed321c1be 221 gps->printf("$PMTK251,115200*1F\r\n");
Sigma884 15:6b1ed321c1be 222 wait(0.2f);
Sigma884 15:6b1ed321c1be 223 gps->printf("$PMTK251,115200*1F\r\n");
Sigma884 15:6b1ed321c1be 224 wait(0.2f);
Sigma884 15:6b1ed321c1be 225 gps->baud(115200);
Sigma884 15:6b1ed321c1be 226 break;
Sigma884 15:6b1ed321c1be 227 }
Sigma884 15:6b1ed321c1be 228 }
Sigma884 15:6b1ed321c1be 229
Sigma884 15:6b1ed321c1be 230 bool GPS_interrupt::changeGPSFreq(int _frequency){
Sigma884 15:6b1ed321c1be 231 frequency = _frequency;
Sigma884 15:6b1ed321c1be 232 flag_change_freq = 0;
Sigma884 15:6b1ed321c1be 233 switch(frequency){
Sigma884 15:6b1ed321c1be 234 case 1:
Sigma884 15:6b1ed321c1be 235 gps->printf("$PMTK220,1000*1F\r\n");
Sigma884 15:6b1ed321c1be 236 wait(0.1f);
Sigma884 15:6b1ed321c1be 237 gps->printf("$PMTK220,1000*1F\r\n");
Sigma884 15:6b1ed321c1be 238 wait(0.1f);
Sigma884 15:6b1ed321c1be 239 gps->printf("$PMTK220,1000*1F\r\n");
Sigma884 15:6b1ed321c1be 240 wait(0.1f);
Sigma884 15:6b1ed321c1be 241 break;
Sigma884 15:6b1ed321c1be 242
Sigma884 15:6b1ed321c1be 243 case 2:
Sigma884 15:6b1ed321c1be 244 gps->printf("$PMTK220,500*2B\r\n");
Sigma884 15:6b1ed321c1be 245 wait(0.1f);
Sigma884 15:6b1ed321c1be 246 gps->printf("$PMTK220,500*2B\r\n");
Sigma884 15:6b1ed321c1be 247 wait(0.1f);
Sigma884 15:6b1ed321c1be 248 gps->printf("$PMTK220,500*2B\r\n");
Sigma884 15:6b1ed321c1be 249 wait(0.1f);
Sigma884 15:6b1ed321c1be 250 break;
Sigma884 15:6b1ed321c1be 251
Sigma884 15:6b1ed321c1be 252 case 3:
Sigma884 15:6b1ed321c1be 253 gps->printf("$PMTK220,333*2D\r\n");
Sigma884 15:6b1ed321c1be 254 wait(0.1f);
Sigma884 15:6b1ed321c1be 255 gps->printf("$PMTK220,333*2D\r\n");
Sigma884 15:6b1ed321c1be 256 wait(0.1f);
Sigma884 15:6b1ed321c1be 257 gps->printf("$PMTK220,333*2D\r\n");
Sigma884 15:6b1ed321c1be 258 wait(0.1f);
Sigma884 15:6b1ed321c1be 259 break;
Sigma884 15:6b1ed321c1be 260
Sigma884 15:6b1ed321c1be 261 case 4:
Sigma884 17:4b09630703c0 262 wait(0.2f);
Sigma884 15:6b1ed321c1be 263 gps->printf("$PMTK220,250*29\r\n");
Sigma884 17:4b09630703c0 264 wait(0.2f);
Sigma884 17:4b09630703c0 265 /*
Sigma884 15:6b1ed321c1be 266 wait(0.1f);
Sigma884 15:6b1ed321c1be 267 gps->printf("$PMTK220,250*29\r\n");
Sigma884 15:6b1ed321c1be 268 wait(0.1f);
Sigma884 15:6b1ed321c1be 269 gps->printf("$PMTK220,250*29\r\n");
Sigma884 15:6b1ed321c1be 270 wait(0.1f);
Sigma884 17:4b09630703c0 271 */
Sigma884 15:6b1ed321c1be 272 break;
Sigma884 15:6b1ed321c1be 273
Sigma884 15:6b1ed321c1be 274 case 5:
Sigma884 15:6b1ed321c1be 275 gps->printf("$PMTK220,200*2C\r\n");
Sigma884 15:6b1ed321c1be 276 wait(0.1f);
Sigma884 15:6b1ed321c1be 277 gps->printf("$PMTK220,200*2C\r\n");
Sigma884 15:6b1ed321c1be 278 wait(0.1f);
Sigma884 15:6b1ed321c1be 279 gps->printf("$PMTK220,200*2C\r\n");
Sigma884 15:6b1ed321c1be 280 wait(0.1f);
Sigma884 15:6b1ed321c1be 281 break;
Sigma884 15:6b1ed321c1be 282
Sigma884 15:6b1ed321c1be 283 case 6:
Sigma884 15:6b1ed321c1be 284 gps->printf("$PMTK220,167*2E\r\n");
Sigma884 15:6b1ed321c1be 285 wait(0.1f);
Sigma884 15:6b1ed321c1be 286 gps->printf("$PMTK220,167*2E\r\n");
Sigma884 15:6b1ed321c1be 287 wait(0.1f);
Sigma884 15:6b1ed321c1be 288 gps->printf("$PMTK220,167*2E\r\n");
Sigma884 15:6b1ed321c1be 289 wait(0.1f);
Sigma884 15:6b1ed321c1be 290 break;
Sigma884 15:6b1ed321c1be 291
Sigma884 15:6b1ed321c1be 292 case 7:
Sigma884 15:6b1ed321c1be 293 gps->printf("$PMTK220,143*28\r\n");
Sigma884 15:6b1ed321c1be 294 wait(0.1f);
Sigma884 15:6b1ed321c1be 295 gps->printf("$PMTK220,143*28\r\n");
Sigma884 15:6b1ed321c1be 296 wait(0.1f);
Sigma884 15:6b1ed321c1be 297 gps->printf("$PMTK220,143*28\r\n");
Sigma884 15:6b1ed321c1be 298 wait(0.1f);
Sigma884 15:6b1ed321c1be 299 break;
Sigma884 15:6b1ed321c1be 300
Sigma884 15:6b1ed321c1be 301 case 8:
Sigma884 15:6b1ed321c1be 302 gps->printf("$PMTK220,125*28\r\n");
Sigma884 15:6b1ed321c1be 303 wait(0.1f);
Sigma884 15:6b1ed321c1be 304 gps->printf("$PMTK220,125*28\r\n");
Sigma884 15:6b1ed321c1be 305 wait(0.1f);
Sigma884 15:6b1ed321c1be 306 gps->printf("$PMTK220,125*28\r\n");
Sigma884 15:6b1ed321c1be 307 wait(0.1f);
Sigma884 15:6b1ed321c1be 308 break;
Sigma884 15:6b1ed321c1be 309
Sigma884 15:6b1ed321c1be 310 case 9:
Sigma884 15:6b1ed321c1be 311 gps->printf("$PMTK220,111*2F\r\n");
Sigma884 15:6b1ed321c1be 312 wait(0.1f);
Sigma884 15:6b1ed321c1be 313 gps->printf("$PMTK220,111*2F\r\n");
Sigma884 15:6b1ed321c1be 314 wait(0.1f);
Sigma884 15:6b1ed321c1be 315 gps->printf("$PMTK220,111*2F\r\n");
Sigma884 15:6b1ed321c1be 316 wait(0.1f);
Sigma884 15:6b1ed321c1be 317 break;
Sigma884 15:6b1ed321c1be 318
Sigma884 15:6b1ed321c1be 319 case 10:
Sigma884 15:6b1ed321c1be 320 gps->printf("$PMTK220,100*2F\r\n");
Sigma884 15:6b1ed321c1be 321 wait(0.1f);
Sigma884 15:6b1ed321c1be 322 gps->printf("$PMTK220,100*2F\r\n");
Sigma884 15:6b1ed321c1be 323 wait(0.1f);
Sigma884 15:6b1ed321c1be 324 gps->printf("$PMTK220,100*2F\r\n");
Sigma884 15:6b1ed321c1be 325 wait(0.1f);
Sigma884 15:6b1ed321c1be 326 break;
Sigma884 15:6b1ed321c1be 327 }
Sigma884 15:6b1ed321c1be 328
Sigma884 15:6b1ed321c1be 329 for(wait_change = 0; wait_change < 10; wait_change ++){
Sigma884 15:6b1ed321c1be 330 if(flag_change_freq == 1){
Sigma884 15:6b1ed321c1be 331 return true;
Sigma884 15:6b1ed321c1be 332 }
Sigma884 15:6b1ed321c1be 333 else if(flag_change_freq == -1){
Sigma884 15:6b1ed321c1be 334 return false;
Sigma884 15:6b1ed321c1be 335 }
Sigma884 15:6b1ed321c1be 336 wait(0.1f);
Sigma884 15:6b1ed321c1be 337 }
Sigma884 15:6b1ed321c1be 338 return false;
Sigma884 15:6b1ed321c1be 339 }
Sigma884 15:6b1ed321c1be 340
Sigma884 15:6b1ed321c1be 341 bool GPS_interrupt::changeGPSData(int _GLL, int _RMC, int _VTG, int _GGA, int _GSA, int _GSV){
Sigma884 15:6b1ed321c1be 342 data_sum = _GLL + _RMC + _VTG + _GGA + _GSA + _GSV;
Sigma884 15:6b1ed321c1be 343 data_sum = data_sum % 2;
Sigma884 15:6b1ed321c1be 344 data_sum += 28;
Sigma884 15:6b1ed321c1be 345 gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
Sigma884 15:6b1ed321c1be 346 wait(0.1f);
Sigma884 15:6b1ed321c1be 347 gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
Sigma884 15:6b1ed321c1be 348 wait(0.1f);
Sigma884 15:6b1ed321c1be 349 gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
Sigma884 15:6b1ed321c1be 350 wait(0.1f);
Sigma884 15:6b1ed321c1be 351
Sigma884 15:6b1ed321c1be 352 for(wait_change = 0; wait_change < 10; wait_change ++){
Sigma884 15:6b1ed321c1be 353 if(flag_change_data == 1){
Sigma884 15:6b1ed321c1be 354 return true;
Sigma884 15:6b1ed321c1be 355 }
Sigma884 15:6b1ed321c1be 356 else if(flag_change_data == -1){
Sigma884 15:6b1ed321c1be 357 return false;
Sigma884 15:6b1ed321c1be 358 }
Sigma884 15:6b1ed321c1be 359 wait(0.1f);
Sigma884 15:6b1ed321c1be 360 }
Sigma884 15:6b1ed321c1be 361 return false;
Sigma884 15:6b1ed321c1be 362 }
Gaku0606 0:74d8e952a3bd 363
Gaku0606 0:74d8e952a3bd 364 void GPS_interrupt::initialize(){
Gaku0606 0:74d8e952a3bd 365 latitude = 0.0f;
Gaku0606 0:74d8e952a3bd 366 longitude = 0.0f;
Gaku0606 0:74d8e952a3bd 367 year = 0;
Gaku0606 0:74d8e952a3bd 368 month = 0;
Gaku0606 0:74d8e952a3bd 369 day = 0;
Gaku0606 0:74d8e952a3bd 370 hour = 0;
Gaku0606 0:74d8e952a3bd 371 minutes = 0;
Gaku0606 0:74d8e952a3bd 372 seconds = 0;
Gaku0606 2:7be89bab6db9 373 knot = 0;
Gaku0606 2:7be89bab6db9 374 degree = 0;
Gaku0606 2:7be89bab6db9 375 number = 0;
Gaku0606 2:7be89bab6db9 376 height = 0;
Gaku0606 2:7be89bab6db9 377 geoid = 0;
Gaku0606 0:74d8e952a3bd 378 memset(gps_buffer_A, '\0', 128);
Gaku0606 0:74d8e952a3bd 379 memset(gps_buffer_B, '\0', 128);
Gaku0606 8:3f32df2b66c0 380 // memset(gps_buffer_C, '\0', 128);
Gaku0606 0:74d8e952a3bd 381 gps_readable = false;
Gaku0606 0:74d8e952a3bd 382 }
Gaku0606 2:7be89bab6db9 383 void GPS_interrupt::rmc_initialize(){
Gaku0606 2:7be89bab6db9 384 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 385 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 386 year = 0;
Gaku0606 2:7be89bab6db9 387 month = 0;
Gaku0606 2:7be89bab6db9 388 day = 0;
Gaku0606 2:7be89bab6db9 389 hour = 0;
Gaku0606 2:7be89bab6db9 390 minutes = 0;
Gaku0606 2:7be89bab6db9 391 seconds = 0;
Gaku0606 2:7be89bab6db9 392 knot = 0;
Gaku0606 2:7be89bab6db9 393 degree = 0;
Gaku0606 2:7be89bab6db9 394 }
Gaku0606 2:7be89bab6db9 395 void GPS_interrupt::gga_initialize(){
Gaku0606 2:7be89bab6db9 396 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 397 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 398 /*year = 0;
Gaku0606 2:7be89bab6db9 399 month = 0;
Gaku0606 2:7be89bab6db9 400 day = 0;
Gaku0606 2:7be89bab6db9 401 hour = 0;
Gaku0606 2:7be89bab6db9 402 minutes = 0;
Gaku0606 2:7be89bab6db9 403 seconds = 0;*/
Gaku0606 2:7be89bab6db9 404 number = 0;
Gaku0606 2:7be89bab6db9 405 height = 0;
Gaku0606 2:7be89bab6db9 406 geoid = 0;
Gaku0606 2:7be89bab6db9 407 }
Gaku0606 9:dab13bd20f43 408
Gaku0606 0:74d8e952a3bd 409 void GPS_interrupt::gps_auto_receive(){
Gaku0606 0:74d8e952a3bd 410
Gaku0606 0:74d8e952a3bd 411 static char str_temp[128] = {'\0'};
Gaku0606 8:3f32df2b66c0 412 static unsigned char temp = 0;
Gaku0606 0:74d8e952a3bd 413 static bool start_flag = false;
Gaku0606 8:3f32df2b66c0 414 static int current = 0;
Gaku0606 8:3f32df2b66c0 415 static char classify_ABC = 0;
Gaku0606 0:74d8e952a3bd 416 static char* save_buffer = gps_buffer_A;
Gaku0606 0:74d8e952a3bd 417
Gaku0606 0:74d8e952a3bd 418 temp = gps->getc();
Gaku0606 2:7be89bab6db9 419
Gaku0606 11:1897b52fa8a1 420 if(debugFlag) printf("%c", temp);
Gaku0606 2:7be89bab6db9 421
Gaku0606 0:74d8e952a3bd 422 if(temp == '$'){
Gaku0606 0:74d8e952a3bd 423 current = 1;
Gaku0606 0:74d8e952a3bd 424 start_flag = true;
Gaku0606 0:74d8e952a3bd 425 memset(save_buffer, '\0', 128);//初期化
Gaku0606 0:74d8e952a3bd 426 save_buffer[0] = '$';
Gaku0606 0:74d8e952a3bd 427 return;
Gaku0606 0:74d8e952a3bd 428 }
Gaku0606 0:74d8e952a3bd 429 if(start_flag){//1行スタート
Gaku0606 0:74d8e952a3bd 430 if(temp == '\r'){//1行終了
Gaku0606 0:74d8e952a3bd 431 save_buffer[current] = '\0';
Gaku0606 0:74d8e952a3bd 432 start_flag = false;
Gaku0606 0:74d8e952a3bd 433 current = 1;
Gaku0606 0:74d8e952a3bd 434 gps_read_buffer = save_buffer;
Gaku0606 2:7be89bab6db9 435
Gaku0606 2:7be89bab6db9 436 //debug(gps_read_buffer);
Gaku0606 8:3f32df2b66c0 437 //printf("%s\r\n", save_buffer);
Gaku0606 8:3f32df2b66c0 438 //printf("debug \"%s\"\r\n", gps_read_buffer);
Gaku0606 2:7be89bab6db9 439
Gaku0606 8:3f32df2b66c0 440 /*switch(classify_ABC){
Gaku0606 8:3f32df2b66c0 441 case 0:
Gaku0606 8:3f32df2b66c0 442 save_buffer = gps_buffer_B;
Gaku0606 8:3f32df2b66c0 443 classify_ABC = 1;
Gaku0606 8:3f32df2b66c0 444 break;
Gaku0606 8:3f32df2b66c0 445
Gaku0606 8:3f32df2b66c0 446 case 1:
Gaku0606 8:3f32df2b66c0 447 save_buffer = gps_buffer_C;//バッファ切換
Gaku0606 8:3f32df2b66c0 448 classify_ABC = 2;
Gaku0606 8:3f32df2b66c0 449 break;
Gaku0606 8:3f32df2b66c0 450
Gaku0606 8:3f32df2b66c0 451 case 2:
Gaku0606 8:3f32df2b66c0 452 save_buffer = gps_buffer_A;
Gaku0606 8:3f32df2b66c0 453 classify_ABC = 0;
Gaku0606 8:3f32df2b66c0 454 }*/
Gaku0606 8:3f32df2b66c0 455 if(classify_ABC == 0){
Gaku0606 8:3f32df2b66c0 456 save_buffer = gps_buffer_B;
Gaku0606 8:3f32df2b66c0 457 classify_ABC = 1;
Gaku0606 0:74d8e952a3bd 458 }
Gaku0606 8:3f32df2b66c0 459 else if(classify_ABC == 1){
Gaku0606 8:3f32df2b66c0 460 save_buffer = gps_buffer_A;//バッファ切換
Gaku0606 8:3f32df2b66c0 461 classify_ABC = 0;
Gaku0606 0:74d8e952a3bd 462 }
Gaku0606 8:3f32df2b66c0 463 memset(save_buffer, '\0', 128);
Gaku0606 7:4b893ac95ae1 464 //if(debugFlag) printf("debug \"%s\"\r\n", gps_read_buffer);
Gaku0606 6:2f91c71d64b1 465
Gaku0606 3:8e66ec281888 466 if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){
Gaku0606 2:7be89bab6db9 467 memset(str_temp, '\0', 128);
Gaku0606 0:74d8e952a3bd 468 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 469 if(processGPRMC(str_temp)){
Gaku0606 0:74d8e952a3bd 470 gps_readable = true;
Gaku0606 4:758f97bee95a 471 return;//データが取得できればここで終了
Gaku0606 0:74d8e952a3bd 472 }
Gaku0606 8:3f32df2b66c0 473 else{
Gaku0606 8:3f32df2b66c0 474 gps_readable = false;
Gaku0606 8:3f32df2b66c0 475 }
Gaku0606 0:74d8e952a3bd 476 }
Gaku0606 14:23611bb30bc8 477 else if((strncmp(gps_read_buffer, "$GPGGA", 6) == 0) || (strncmp(gps_read_buffer, "$GNGGA", 6) == 0)){
Gaku0606 2:7be89bab6db9 478 memset(str_temp, '\0', 128);
Gaku0606 2:7be89bab6db9 479 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 480 if(processGPGGA(str_temp)){
Gaku0606 2:7be89bab6db9 481 gps_readable = true;
Gaku0606 4:758f97bee95a 482 return;//データが取得できればここで終了
Gaku0606 4:758f97bee95a 483 }
Gaku0606 8:3f32df2b66c0 484 else{
Gaku0606 8:3f32df2b66c0 485 gps_readable = false;
Gaku0606 8:3f32df2b66c0 486 return;
Gaku0606 8:3f32df2b66c0 487 }
Gaku0606 2:7be89bab6db9 488 }
Gaku0606 2:7be89bab6db9 489 //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){
Gaku0606 2:7be89bab6db9 490 //printf("%s\r\n", gps_read_buffer);
Gaku0606 2:7be89bab6db9 491 //}
Sigma884 15:6b1ed321c1be 492 else if(strncmp(gps_read_buffer, "$PMTK001", 8) == 0){
Sigma884 15:6b1ed321c1be 493 memset(str_temp, '\0', 128);
Sigma884 15:6b1ed321c1be 494 strcpy(str_temp, gps_read_buffer);
Sigma884 15:6b1ed321c1be 495 processPMTK(str_temp);
Sigma884 15:6b1ed321c1be 496 }
Gaku0606 0:74d8e952a3bd 497 }
Gaku0606 0:74d8e952a3bd 498 else{
Gaku0606 0:74d8e952a3bd 499 save_buffer[current] = temp;
Gaku0606 0:74d8e952a3bd 500 current++;
Gaku0606 0:74d8e952a3bd 501 if(current >= 127){
Gaku0606 0:74d8e952a3bd 502 current = 1;
Gaku0606 0:74d8e952a3bd 503 start_flag = false;
Gaku0606 8:3f32df2b66c0 504 memset(save_buffer, '\0', 128);
Gaku0606 0:74d8e952a3bd 505 }
Gaku0606 0:74d8e952a3bd 506 }
Gaku0606 0:74d8e952a3bd 507 }
Gaku0606 0:74d8e952a3bd 508 }
Gaku0606 0:74d8e952a3bd 509
Gaku0606 2:7be89bab6db9 510 bool GPS_interrupt::processGPRMC(char *line){
Gaku0606 0:74d8e952a3bd 511
Gaku0606 0:74d8e952a3bd 512 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 0:74d8e952a3bd 513 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 0:74d8e952a3bd 514 //bool result = false;
Gaku0606 0:74d8e952a3bd 515 //char num[4] = "";//衛星数の文字データ
Gaku0606 0:74d8e952a3bd 516 //char kaibatsu[8], g_height[8];
Gaku0606 0:74d8e952a3bd 517 //char data_GPS[256] ="";
Gaku0606 0:74d8e952a3bd 518 char angle[8], speed[8];
Gaku0606 0:74d8e952a3bd 519 char ido[16] = "", keido[16] = "";
Gaku0606 0:74d8e952a3bd 520
Gaku0606 2:7be89bab6db9 521 double X = 0, Y = 0;
Gaku0606 0:74d8e952a3bd 522 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 0:74d8e952a3bd 523 char zikann[11], hizuke[7];
Gaku0606 0:74d8e952a3bd 524
Gaku0606 0:74d8e952a3bd 525 //bool gga_flag = false;
Gaku0606 0:74d8e952a3bd 526 bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 527 char tok_count = 0;
Gaku0606 0:74d8e952a3bd 528
Gaku0606 4:758f97bee95a 529 //rmc_initialize();
Gaku0606 0:74d8e952a3bd 530
Gaku0606 0:74d8e952a3bd 531 //_____GPRMC___________________________________
Gaku0606 0:74d8e952a3bd 532 //データ文字列の先頭から6文字目までが$GPRMCなら０が返ってくる
Gaku0606 2:7be89bab6db9 533 //if (strncmp(line, "$GPRMC", 6) == 0){
Gaku0606 0:74d8e952a3bd 534
Gaku0606 0:74d8e952a3bd 535 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 536 tok_count = 0;
Gaku0606 0:74d8e952a3bd 537
Gaku0606 0:74d8e952a3bd 538 //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A
Gaku0606 0:74d8e952a3bd 539
Gaku0606 0:74d8e952a3bd 540 // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12
Gaku0606 0:74d8e952a3bd 541 //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C
Gaku0606 0:74d8e952a3bd 542
Gaku0606 0:74d8e952a3bd 543 // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1
Gaku0606 0:74d8e952a3bd 544 // 0 1 2
Gaku0606 0:74d8e952a3bd 545 while (1){
Gaku0606 2:7be89bab6db9 546 switch (tok_count){
Gaku0606 2:7be89bab6db9 547 case 1://時分秒
Gaku0606 2:7be89bab6db9 548 strcpy(zikann, tok);//zikann = "233514.000\0"
Gaku0606 2:7be89bab6db9 549 break;
Gaku0606 2:7be89bab6db9 550 case 2://有効か無効か判定
Gaku0606 2:7be89bab6db9 551 if (strncmp(tok, "V", 1) == 0) return false;
Gaku0606 2:7be89bab6db9 552 else if (strcmp(tok, "A") == 0) rmc_flag = true;
Gaku0606 2:7be89bab6db9 553 break;
Gaku0606 2:7be89bab6db9 554 case 3://緯度
Gaku0606 2:7be89bab6db9 555 strcpy(ido, tok);
Gaku0606 2:7be89bab6db9 556 break;
Gaku0606 2:7be89bab6db9 557 case 4://北緯か南緯か
Gaku0606 2:7be89bab6db9 558 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 559 break;
Gaku0606 2:7be89bab6db9 560 case 5://経度
Gaku0606 2:7be89bab6db9 561 strcpy(keido, tok);
Gaku0606 2:7be89bab6db9 562 break;
Gaku0606 2:7be89bab6db9 563 case 6://東経か西経か
Gaku0606 2:7be89bab6db9 564 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 565 break;
Gaku0606 2:7be89bab6db9 566 case 7://速度（キロノット）
Gaku0606 2:7be89bab6db9 567 strcpy(speed, tok);
Gaku0606 2:7be89bab6db9 568 break;
Gaku0606 2:7be89bab6db9 569 case 8://角度
Gaku0606 2:7be89bab6db9 570 strcpy(angle, tok);
Gaku0606 2:7be89bab6db9 571 break;
Gaku0606 2:7be89bab6db9 572 case 9://日付
Gaku0606 2:7be89bab6db9 573 strcpy(hizuke, tok);//hizuke = "030316\0"
Gaku0606 2:7be89bab6db9 574 break;
Gaku0606 0:74d8e952a3bd 575 }
Gaku0606 2:7be89bab6db9 576
Gaku0606 0:74d8e952a3bd 577 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 0:74d8e952a3bd 578 tok_count++;
Gaku0606 0:74d8e952a3bd 579 if (tok == NULL) break;
Gaku0606 0:74d8e952a3bd 580 }
Gaku0606 0:74d8e952a3bd 581 if (rmc_flag){
Gaku0606 0:74d8e952a3bd 582 rmc_flag = false;
Gaku0606 0:74d8e952a3bd 583 //ddmmyy
Gaku0606 0:74d8e952a3bd 584 int dmy = 0;
Gaku0606 0:74d8e952a3bd 585 dmy = atoi(hizuke);
Gaku0606 0:74d8e952a3bd 586 day = dmy / 10000; //030316 / 10000 = 3
Gaku0606 0:74d8e952a3bd 587 month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3
Gaku0606 0:74d8e952a3bd 588 year = dmy - month * 100 - day * 10000 + 2000;
Gaku0606 0:74d8e952a3bd 589
Gaku0606 0:74d8e952a3bd 590 //hhmmss.ss
Gaku0606 0:74d8e952a3bd 591 seconds = atof(zikann);//sec = 233514.000
Gaku0606 0:74d8e952a3bd 592 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 0:74d8e952a3bd 593 minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 12:935b21d30ec2 594 seconds = seconds - ((float)minutes * 100.0 + (float)hour * 10000.0);
Gaku0606 0:74d8e952a3bd 595
Gaku0606 0:74d8e952a3bd 596 // getLonLatのとき有効
Gaku0606 0:74d8e952a3bd 597 Y_m = atof(ido);//*Y_m = 3457.5571
Gaku0606 0:74d8e952a3bd 598 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 0:74d8e952a3bd 599 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 0:74d8e952a3bd 600
Gaku0606 0:74d8e952a3bd 601 X_m = atof(keido);//*X = 13057.6142
Gaku0606 0:74d8e952a3bd 602 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 0:74d8e952a3bd 603 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 0:74d8e952a3bd 604
Gaku0606 0:74d8e952a3bd 605 //GPS calculation
Gaku0606 0:74d8e952a3bd 606 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 0:74d8e952a3bd 607 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 0:74d8e952a3bd 608
Gaku0606 2:7be89bab6db9 609 if (lonSign) longitude *= (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 610 if (latSign) latitude *= (-1);//南緯なら緯度を負に
Gaku0606 0:74d8e952a3bd 611 knot = atof(speed);
Gaku0606 0:74d8e952a3bd 612 degree = atof(angle);
Gaku0606 14:23611bb30bc8 613 //printf("\r\n%f\t%f\r\n",knot, degree);
Gaku0606 8:3f32df2b66c0 614 if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){
Gaku0606 8:3f32df2b66c0 615 return true;
Gaku0606 8:3f32df2b66c0 616 }
Gaku0606 8:3f32df2b66c0 617 else return false;
Gaku0606 0:74d8e952a3bd 618 }
Gaku0606 4:758f97bee95a 619 else{
Gaku0606 4:758f97bee95a 620 return false;
Gaku0606 4:758f97bee95a 621 }
Gaku0606 2:7be89bab6db9 622 //}
Gaku0606 2:7be89bab6db9 623 //else return false;
Gaku0606 0:74d8e952a3bd 624 }
Gaku0606 0:74d8e952a3bd 625
Gaku0606 2:7be89bab6db9 626 bool GPS_interrupt::processGPGGA(char *line){
Gaku0606 2:7be89bab6db9 627
Gaku0606 2:7be89bab6db9 628 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 2:7be89bab6db9 629 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 2:7be89bab6db9 630 //bool result = false;
Gaku0606 2:7be89bab6db9 631 char num[4] = "";//衛星数の文字データ
Gaku0606 2:7be89bab6db9 632 char kaibatsu[8], g_height[8];
Gaku0606 2:7be89bab6db9 633 //char data_GPS[256] ="";
Gaku0606 2:7be89bab6db9 634 //char angle[8], speed[8];
Gaku0606 2:7be89bab6db9 635 char ido[16] = "", keido[16] = "";
Gaku0606 2:7be89bab6db9 636
Gaku0606 2:7be89bab6db9 637 double X = 0, Y = 0;
Gaku0606 2:7be89bab6db9 638 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 2:7be89bab6db9 639 //char zikann[11], hizuke[7];
Gaku0606 2:7be89bab6db9 640 char status[2];
Gaku0606 2:7be89bab6db9 641
Gaku0606 2:7be89bab6db9 642 bool gga_flag = false;
Gaku0606 2:7be89bab6db9 643 //bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 644 char tok_count = 0;
Gaku0606 2:7be89bab6db9 645
Gaku0606 4:758f97bee95a 646 //gga_initialize();
Gaku0606 2:7be89bab6db9 647
Gaku0606 2:7be89bab6db9 648 //データ文字列の先頭から6文字目までが$GPGLLなら０が返ってくる
Gaku0606 2:7be89bab6db9 649 //if (strncmp(data_GPS, "$GPGGA", 6) == 0){
Gaku0606 2:7be89bab6db9 650
Gaku0606 2:7be89bab6db9 651 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 652 tok_count = 0;
Gaku0606 2:7be89bab6db9 653
Gaku0606 2:7be89bab6db9 654 //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40
Gaku0606 2:7be89bab6db9 655
Gaku0606 2:7be89bab6db9 656 // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6
Gaku0606 2:7be89bab6db9 657
Gaku0606 2:7be89bab6db9 658 //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63
Gaku0606 2:7be89bab6db9 659
Gaku0606 2:7be89bab6db9 660 // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1
Gaku0606 2:7be89bab6db9 661 // 0 2
Gaku0606 2:7be89bab6db9 662 while (1){
Gaku0606 2:7be89bab6db9 663 switch (tok_count){
Gaku0606 2:7be89bab6db9 664 case 2://緯度
Gaku0606 2:7be89bab6db9 665 strcpy(ido, tok);//ido = "30"
Gaku0606 2:7be89bab6db9 666 if (strcmp(ido, "0") == 0) return false;//GPS無効
Gaku0606 2:7be89bab6db9 667 break;
Gaku0606 2:7be89bab6db9 668 case 3://北緯か南緯か
Gaku0606 2:7be89bab6db9 669 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 670 break;
Gaku0606 2:7be89bab6db9 671 case 4:
Gaku0606 2:7be89bab6db9 672 strcpy(keido, tok);//keido = "130"
Gaku0606 2:7be89bab6db9 673 break;
Gaku0606 2:7be89bab6db9 674 case 5://東経か西経か
Gaku0606 2:7be89bab6db9 675 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 676 break;
Gaku0606 2:7be89bab6db9 677 case 6:
Gaku0606 2:7be89bab6db9 678 strcpy(status, tok);
Gaku0606 2:7be89bab6db9 679 if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効
Gaku0606 2:7be89bab6db9 680 break;
Gaku0606 2:7be89bab6db9 681 case 7:
Gaku0606 2:7be89bab6db9 682 strcpy(num, tok);
Gaku0606 2:7be89bab6db9 683 break;
Gaku0606 2:7be89bab6db9 684 case 9:
Gaku0606 2:7be89bab6db9 685 strcpy(kaibatsu, tok);
Gaku0606 2:7be89bab6db9 686 break;
Gaku0606 2:7be89bab6db9 687 case 11:
Gaku0606 2:7be89bab6db9 688 strcpy(g_height, tok);
Gaku0606 2:7be89bab6db9 689 break;
Gaku0606 2:7be89bab6db9 690 }
Gaku0606 2:7be89bab6db9 691 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 2:7be89bab6db9 692 tok_count++;
Gaku0606 2:7be89bab6db9 693 if (tok == NULL) break;
Gaku0606 2:7be89bab6db9 694 }
Gaku0606 2:7be89bab6db9 695 if (gga_flag){
Gaku0606 2:7be89bab6db9 696
Gaku0606 2:7be89bab6db9 697 height = atof(kaibatsu);
Gaku0606 2:7be89bab6db9 698 geoid = atof(g_height);
Gaku0606 2:7be89bab6db9 699 number = atoi(num);
Gaku0606 2:7be89bab6db9 700 /*
Gaku0606 2:7be89bab6db9 701 //ddmmyy
Gaku0606 2:7be89bab6db9 702 long dmy = 0;
Gaku0606 2:7be89bab6db9 703 dmy = atol(hizuke);
Gaku0606 2:7be89bab6db9 704 day = (int)(dmy / 10000); //030316 / 10000 = 3
Gaku0606 2:7be89bab6db9 705 month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3
Gaku0606 2:7be89bab6db9 706 year = dmy - month * 100 - (long)day * 10000 + 2000;
Gaku0606 2:7be89bab6db9 707
Gaku0606 2:7be89bab6db9 708 //hhmmss.ss
Gaku0606 2:7be89bab6db9 709 seconds = atof(zikann);//sec = 233514.000
Gaku0606 2:7be89bab6db9 710 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 2:7be89bab6db9 711 minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 2:7be89bab6db9 712 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 2:7be89bab6db9 713 */
Gaku0606 2:7be89bab6db9 714 // getLonLatのとき有効
Gaku0606 2:7be89bab6db9 715 Y_m = atof(ido);//Y_m = 3457.5571
Gaku0606 2:7be89bab6db9 716 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 2:7be89bab6db9 717 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 2:7be89bab6db9 718
Gaku0606 2:7be89bab6db9 719 X_m = atof(keido);//X = 13057.6142
Gaku0606 2:7be89bab6db9 720 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 2:7be89bab6db9 721 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 2:7be89bab6db9 722
Gaku0606 2:7be89bab6db9 723 //GPS calculation
Gaku0606 2:7be89bab6db9 724 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 2:7be89bab6db9 725 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 2:7be89bab6db9 726 if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 727 if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に
Gaku0606 2:7be89bab6db9 728
Gaku0606 8:3f32df2b66c0 729 if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){
Gaku0606 8:3f32df2b66c0 730 return true;
Gaku0606 8:3f32df2b66c0 731 }
Gaku0606 8:3f32df2b66c0 732 else{
Gaku0606 8:3f32df2b66c0 733 return false;
Gaku0606 8:3f32df2b66c0 734 }
Gaku0606 2:7be89bab6db9 735 }
Gaku0606 4:758f97bee95a 736 else{
Gaku0606 4:758f97bee95a 737 return false;
Gaku0606 4:758f97bee95a 738 }
Sigma884 15:6b1ed321c1be 739 //return false;
Gaku0606 8:3f32df2b66c0 740 }
Gaku0606 8:3f32df2b66c0 741
Sigma884 15:6b1ed321c1be 742 void GPS_interrupt::processPMTK(char *line){
Sigma884 15:6b1ed321c1be 743
Sigma884 15:6b1ed321c1be 744 char *tok; //strtokで帰ってくる文字列のポインター
Sigma884 15:6b1ed321c1be 745 int tok_count = 0;
Sigma884 15:6b1ed321c1be 746
Sigma884 15:6b1ed321c1be 747 char status[2];
Sigma884 15:6b1ed321c1be 748 char mode_change_s[5] = "";
Sigma884 15:6b1ed321c1be 749 int mode_change = 0;
Sigma884 15:6b1ed321c1be 750
Sigma884 15:6b1ed321c1be 751 tok = strtok(line, ",");
Sigma884 15:6b1ed321c1be 752 tok_count = 0;
Sigma884 15:6b1ed321c1be 753
Sigma884 15:6b1ed321c1be 754 //$PMTK001, 334, 3*34
Sigma884 15:6b1ed321c1be 755 // 0 , 1 , 2
Sigma884 15:6b1ed321c1be 756
Sigma884 15:6b1ed321c1be 757 while (1){
Sigma884 15:6b1ed321c1be 758 switch (tok_count){
Sigma884 15:6b1ed321c1be 759 case 1: //変更内容
Sigma884 15:6b1ed321c1be 760 if(strcmp(tok, "220") == 0){
Sigma884 15:6b1ed321c1be 761 mode_change = 220;
Sigma884 15:6b1ed321c1be 762 }
Sigma884 15:6b1ed321c1be 763 else if(strcmp(tok, "314") == 0){
Sigma884 15:6b1ed321c1be 764 mode_change = 314;
Sigma884 15:6b1ed321c1be 765 }
Sigma884 15:6b1ed321c1be 766 break;
Sigma884 15:6b1ed321c1be 767
Sigma884 15:6b1ed321c1be 768 case 2:
Sigma884 15:6b1ed321c1be 769 if(tok[0] = '3'){
Sigma884 15:6b1ed321c1be 770 switch(mode_change){
Sigma884 15:6b1ed321c1be 771 case 220:
Sigma884 15:6b1ed321c1be 772 flag_change_freq = 1;
Sigma884 15:6b1ed321c1be 773 break;
Sigma884 15:6b1ed321c1be 774 case 314:
Sigma884 15:6b1ed321c1be 775 flag_change_data = 1;
Sigma884 15:6b1ed321c1be 776 break;
Sigma884 15:6b1ed321c1be 777 }
Sigma884 15:6b1ed321c1be 778 }
Sigma884 15:6b1ed321c1be 779 else{
Sigma884 15:6b1ed321c1be 780 switch(mode_change){
Sigma884 15:6b1ed321c1be 781 case 220:
Sigma884 15:6b1ed321c1be 782 flag_change_freq = -1;
Sigma884 15:6b1ed321c1be 783 break;
Sigma884 15:6b1ed321c1be 784 case 314:
Sigma884 15:6b1ed321c1be 785 flag_change_data = -1;
Sigma884 15:6b1ed321c1be 786 break;
Sigma884 15:6b1ed321c1be 787 }
Sigma884 15:6b1ed321c1be 788 }
Sigma884 15:6b1ed321c1be 789 break;
Sigma884 15:6b1ed321c1be 790 }
Sigma884 15:6b1ed321c1be 791 tok = strtok(NULL, ",");
Sigma884 15:6b1ed321c1be 792 tok_count ++;
Sigma884 15:6b1ed321c1be 793 if(tok == NULL){
Sigma884 15:6b1ed321c1be 794 break;
Sigma884 15:6b1ed321c1be 795 }
Sigma884 15:6b1ed321c1be 796 }
Sigma884 15:6b1ed321c1be 797 }
Sigma884 15:6b1ed321c1be 798
Sigma884 15:6b1ed321c1be 799
Sigma884 15:6b1ed321c1be 800
Gaku0606 12:935b21d30ec2 801 float GPS_interrupt::Distance(double x, double y){
Gaku0606 2:7be89bab6db9 802
Gaku0606 8:3f32df2b66c0 803 double dLat = x - longitude;//相対経度
Gaku0606 8:3f32df2b66c0 804 double dLng = y - latitude;//相対緯度
Gaku0606 8:3f32df2b66c0 805 double radLat = latitude * GPS_PI / 180.0;//今の緯度をラジアンにしたもの
Gaku0606 8:3f32df2b66c0 806 double F = EIRTH_AspectRatioInverse;
Gaku0606 8:3f32df2b66c0 807 // 離心率の2乗
Gaku0606 8:3f32df2b66c0 808 double E = ((2.0 * F) - 1.0) / (F * F);
Gaku0606 8:3f32df2b66c0 809 // π * 赤道半径
Gaku0606 8:3f32df2b66c0 810 double PI_ER = GPS_PI * EARTH_EQUATOR_RADIUS;
Gaku0606 8:3f32df2b66c0 811 // 1 - e^2 sin^2 (θ)
Gaku0606 8:3f32df2b66c0 812 double TMP = 1.0 - E * sin(radLat) * sin(radLat);
Gaku0606 8:3f32df2b66c0 813 // 経度1°あたりの長さ
Gaku0606 8:3f32df2b66c0 814 double arc_lat = (PI_ER * (1.0 - E)) / ( 180.0 * pow(TMP, 1.5));
Gaku0606 8:3f32df2b66c0 815 // 緯度1°あたりの長さ
Gaku0606 8:3f32df2b66c0 816 double arc_lng = (PI_ER * cos(radLat)) / (180.0 * pow(TMP, 0.5));
Gaku0606 8:3f32df2b66c0 817
Gaku0606 8:3f32df2b66c0 818 dLat *= arc_lat;
Gaku0606 8:3f32df2b66c0 819 dLng *= arc_lng;
Gaku0606 12:935b21d30ec2 820 return (float)sqrt(dLat * dLat + dLng * dLng);
Gaku0606 2:7be89bab6db9 821 }