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@3:8e66ec281888, 2017-01-03 (annotated)

Committer:
Gaku0606
Date:
Tue Jan 03 09:22:54 2017 +0000
Revision:
3:8e66ec281888
Parent:
2:7be89bab6db9
Child:
4:758f97bee95a
GPS_interrupt.lib ver 1.2.6

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 0:74d8e952a3bd 3 double GPS_interrupt::latitude;
Gaku0606 0:74d8e952a3bd 4 double GPS_interrupt::longitude;
Gaku0606 0:74d8e952a3bd 5 int GPS_interrupt::year;
Gaku0606 0:74d8e952a3bd 6 int GPS_interrupt::month;
Gaku0606 0:74d8e952a3bd 7 int GPS_interrupt::day;
Gaku0606 0:74d8e952a3bd 8 int GPS_interrupt::hour;
Gaku0606 0:74d8e952a3bd 9 int GPS_interrupt::minutes;
Gaku0606 0:74d8e952a3bd 10 double GPS_interrupt::seconds;
Gaku0606 0:74d8e952a3bd 11 double GPS_interrupt::degree = 0;
Gaku0606 0:74d8e952a3bd 12 double GPS_interrupt::knot = 0;
Gaku0606 0:74d8e952a3bd 13 double GPS_interrupt::height = 0;
Gaku0606 0:74d8e952a3bd 14 double GPS_interrupt::geoid = 0;
Gaku0606 0:74d8e952a3bd 15 int GPS_interrupt::number = 0;
Gaku0606 0:74d8e952a3bd 16
Gaku0606 0:74d8e952a3bd 17 char GPS_interrupt::gps_buffer_A[128];
Gaku0606 0:74d8e952a3bd 18 char GPS_interrupt::gps_buffer_B[128];
Gaku0606 0:74d8e952a3bd 19 char* GPS_interrupt::gps_read_buffer = gps_buffer_B;
Gaku0606 0:74d8e952a3bd 20 bool GPS_interrupt::gps_readable = false;
Gaku0606 0:74d8e952a3bd 21
Gaku0606 0:74d8e952a3bd 22 GPS_interrupt* GPS_interrupt::gps_irq;
Gaku0606 0:74d8e952a3bd 23
Gaku0606 2:7be89bab6db9 24 //Timeout timeout_clear;
Gaku0606 2:7be89bab6db9 25
Gaku0606 2:7be89bab6db9 26 void GPS_interrupt::debug(char *str){
Gaku0606 2:7be89bab6db9 27 printf("%s\r\n", str);
Gaku0606 2:7be89bab6db9 28 }
Gaku0606 2:7be89bab6db9 29
Gaku0606 2:7be89bab6db9 30 unsigned char GPS_interrupt::checkSum(char *str){
Gaku0606 2:7be89bab6db9 31 int num = strlen(str);
Gaku0606 2:7be89bab6db9 32 unsigned char val = 0;
Gaku0606 2:7be89bab6db9 33 for(int i = 0; i< num; i++){
Gaku0606 2:7be89bab6db9 34 val = val ^ str[i];
Gaku0606 2:7be89bab6db9 35 }
Gaku0606 2:7be89bab6db9 36 return val;
Gaku0606 2:7be89bab6db9 37 }
Gaku0606 2:7be89bab6db9 38
Gaku0606 2:7be89bab6db9 39 GPS_interrupt::GPS_interrupt(RawSerial *_gps, int _baudrate, int _frequency, int start_baudrate){
Gaku0606 2:7be89bab6db9 40
Gaku0606 2:7be89bab6db9 41 baudrate = _baudrate;
Gaku0606 2:7be89bab6db9 42 frequency = _frequency;
Gaku0606 0:74d8e952a3bd 43 gps_irq = this;
Gaku0606 0:74d8e952a3bd 44 gps = _gps;
Gaku0606 2:7be89bab6db9 45 gps->baud(start_baudrate);
Gaku0606 2:7be89bab6db9 46 wait(0.1);
Gaku0606 2:7be89bab6db9 47 switch (_baudrate){
Gaku0606 2:7be89bab6db9 48 case 9600:
Gaku0606 2:7be89bab6db9 49 gps->printf("$PMTK251,9600*17\r\n");
Gaku0606 2:7be89bab6db9 50 break;
Gaku0606 2:7be89bab6db9 51 case 19200:
Gaku0606 2:7be89bab6db9 52 gps->printf("$PMTK251,19200*22\r\n");
Gaku0606 2:7be89bab6db9 53 break;
Gaku0606 2:7be89bab6db9 54 case 38400:
Gaku0606 2:7be89bab6db9 55 gps->printf("$PMTK251,38400*27\r\n");
Gaku0606 2:7be89bab6db9 56 break;
Gaku0606 2:7be89bab6db9 57 case 57600:
Gaku0606 2:7be89bab6db9 58 gps->printf("$PMTK251,57600*2C\r\n");
Gaku0606 2:7be89bab6db9 59 break;
Gaku0606 2:7be89bab6db9 60 case 115200:
Gaku0606 2:7be89bab6db9 61 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 2:7be89bab6db9 62 break;
Gaku0606 2:7be89bab6db9 63 default:
Gaku0606 2:7be89bab6db9 64 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 2:7be89bab6db9 65 break;
Gaku0606 2:7be89bab6db9 66 }
Gaku0606 2:7be89bab6db9 67 gps->baud(_baudrate);
Gaku0606 2:7be89bab6db9 68 wait(0.1);
Gaku0606 3:8e66ec281888 69 /*
Gaku0606 2:7be89bab6db9 70 gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format
Gaku0606 2:7be89bab6db9 71 unsigned char checksum = 0;
Gaku0606 3:8e66ec281888 72 checksum = checkSum("PMTK314,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1");
Gaku0606 2:7be89bab6db9 73 //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム
Gaku0606 3:8e66ec281888 74 gps->printf("$PMTK314,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1*%02X\r\n", checksum);*/
Gaku0606 2:7be89bab6db9 75 switch(_frequency){
Gaku0606 2:7be89bab6db9 76 case 1:
Gaku0606 2:7be89bab6db9 77 gps->printf("$PMTK220,1000*1F\r\n");
Gaku0606 2:7be89bab6db9 78 break;
Gaku0606 2:7be89bab6db9 79 case 2:
Gaku0606 2:7be89bab6db9 80 gps->printf("$PMTK220,500*2B\r\n");
Gaku0606 2:7be89bab6db9 81 break;
Gaku0606 2:7be89bab6db9 82 case 3:
Gaku0606 2:7be89bab6db9 83 gps->printf("$PMTK220,333*2D\r\n");
Gaku0606 2:7be89bab6db9 84 break;
Gaku0606 2:7be89bab6db9 85 case 4:
Gaku0606 2:7be89bab6db9 86 gps->printf("$PMTK220,250*29\r\n");
Gaku0606 2:7be89bab6db9 87 break;
Gaku0606 2:7be89bab6db9 88 case 5:
Gaku0606 2:7be89bab6db9 89 gps->printf("$PMTK220,200*2C\r\n");
Gaku0606 2:7be89bab6db9 90 break;
Gaku0606 2:7be89bab6db9 91 case 10:
Gaku0606 2:7be89bab6db9 92 gps->printf("$PMTK220,100*2F\r\n");
Gaku0606 2:7be89bab6db9 93 break;
Gaku0606 2:7be89bab6db9 94 default:
Gaku0606 2:7be89bab6db9 95 gps->printf("$PMTK220,1000*1F\r\n");
Gaku0606 2:7be89bab6db9 96 break;
Gaku0606 2:7be89bab6db9 97 }
Gaku0606 2:7be89bab6db9 98 wait(0.1);
Gaku0606 0:74d8e952a3bd 99 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 0:74d8e952a3bd 100 }
Gaku0606 0:74d8e952a3bd 101
Gaku0606 0:74d8e952a3bd 102 void GPS_interrupt::initialize(){
Gaku0606 0:74d8e952a3bd 103 latitude = 0.0f;
Gaku0606 0:74d8e952a3bd 104 longitude = 0.0f;
Gaku0606 0:74d8e952a3bd 105 year = 0;
Gaku0606 0:74d8e952a3bd 106 month = 0;
Gaku0606 0:74d8e952a3bd 107 day = 0;
Gaku0606 0:74d8e952a3bd 108 hour = 0;
Gaku0606 0:74d8e952a3bd 109 minutes = 0;
Gaku0606 0:74d8e952a3bd 110 seconds = 0;
Gaku0606 2:7be89bab6db9 111 knot = 0;
Gaku0606 2:7be89bab6db9 112 degree = 0;
Gaku0606 2:7be89bab6db9 113 number = 0;
Gaku0606 2:7be89bab6db9 114 height = 0;
Gaku0606 2:7be89bab6db9 115 geoid = 0;
Gaku0606 0:74d8e952a3bd 116 memset(gps_buffer_A, '\0', 128);
Gaku0606 0:74d8e952a3bd 117 memset(gps_buffer_B, '\0', 128);
Gaku0606 0:74d8e952a3bd 118 gps_readable = false;
Gaku0606 0:74d8e952a3bd 119 }
Gaku0606 2:7be89bab6db9 120 void GPS_interrupt::rmc_initialize(){
Gaku0606 2:7be89bab6db9 121 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 122 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 123 year = 0;
Gaku0606 2:7be89bab6db9 124 month = 0;
Gaku0606 2:7be89bab6db9 125 day = 0;
Gaku0606 2:7be89bab6db9 126 hour = 0;
Gaku0606 2:7be89bab6db9 127 minutes = 0;
Gaku0606 2:7be89bab6db9 128 seconds = 0;
Gaku0606 2:7be89bab6db9 129 knot = 0;
Gaku0606 2:7be89bab6db9 130 degree = 0;
Gaku0606 2:7be89bab6db9 131 }
Gaku0606 2:7be89bab6db9 132 void GPS_interrupt::gga_initialize(){
Gaku0606 2:7be89bab6db9 133 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 134 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 135 /*year = 0;
Gaku0606 2:7be89bab6db9 136 month = 0;
Gaku0606 2:7be89bab6db9 137 day = 0;
Gaku0606 2:7be89bab6db9 138 hour = 0;
Gaku0606 2:7be89bab6db9 139 minutes = 0;
Gaku0606 2:7be89bab6db9 140 seconds = 0;*/
Gaku0606 2:7be89bab6db9 141 number = 0;
Gaku0606 2:7be89bab6db9 142 height = 0;
Gaku0606 2:7be89bab6db9 143 geoid = 0;
Gaku0606 2:7be89bab6db9 144 }
Gaku0606 0:74d8e952a3bd 145 void GPS_interrupt::gps_auto_receive(){
Gaku0606 0:74d8e952a3bd 146
Gaku0606 0:74d8e952a3bd 147 static char str_temp[128] = {'\0'};
Gaku0606 0:74d8e952a3bd 148 static char temp = 0;
Gaku0606 0:74d8e952a3bd 149 static bool start_flag = false;
Gaku0606 0:74d8e952a3bd 150 static unsigned char current = 0;
Gaku0606 0:74d8e952a3bd 151 static char classify_AB = 0;
Gaku0606 0:74d8e952a3bd 152 static char* save_buffer = gps_buffer_A;
Gaku0606 0:74d8e952a3bd 153
Gaku0606 0:74d8e952a3bd 154 temp = gps->getc();
Gaku0606 2:7be89bab6db9 155
Gaku0606 2:7be89bab6db9 156 //printf("%c", temp);
Gaku0606 2:7be89bab6db9 157
Gaku0606 0:74d8e952a3bd 158 if(temp == '$'){
Gaku0606 0:74d8e952a3bd 159 current = 1;
Gaku0606 0:74d8e952a3bd 160 start_flag = true;
Gaku0606 0:74d8e952a3bd 161 memset(save_buffer, '\0', 128);//初期化
Gaku0606 0:74d8e952a3bd 162 save_buffer[0] = '$';
Gaku0606 0:74d8e952a3bd 163 return;
Gaku0606 0:74d8e952a3bd 164 }
Gaku0606 0:74d8e952a3bd 165 if(start_flag){//1行スタート
Gaku0606 0:74d8e952a3bd 166 if(temp == '\r'){//1行終了
Gaku0606 0:74d8e952a3bd 167 save_buffer[current] = '\0';
Gaku0606 0:74d8e952a3bd 168 start_flag = false;
Gaku0606 0:74d8e952a3bd 169 current = 1;
Gaku0606 0:74d8e952a3bd 170 gps_read_buffer = save_buffer;
Gaku0606 2:7be89bab6db9 171
Gaku0606 2:7be89bab6db9 172 //debug(gps_read_buffer);
Gaku0606 2:7be89bab6db9 173 //printf("%s\r\n", save_buffer);
Gaku0606 2:7be89bab6db9 174
Gaku0606 0:74d8e952a3bd 175 if(classify_AB){
Gaku0606 0:74d8e952a3bd 176 save_buffer = gps_buffer_A;//バッファ切換
Gaku0606 0:74d8e952a3bd 177 classify_AB = ~classify_AB;
Gaku0606 0:74d8e952a3bd 178 }
Gaku0606 0:74d8e952a3bd 179 else{
Gaku0606 0:74d8e952a3bd 180 save_buffer = gps_buffer_B;
Gaku0606 0:74d8e952a3bd 181 classify_AB = ~classify_AB;
Gaku0606 0:74d8e952a3bd 182 }
Gaku0606 3:8e66ec281888 183 if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){
Gaku0606 2:7be89bab6db9 184 memset(str_temp, '\0', 128);
Gaku0606 0:74d8e952a3bd 185 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 186 if(processGPRMC(str_temp)){
Gaku0606 0:74d8e952a3bd 187 gps_readable = true;
Gaku0606 0:74d8e952a3bd 188 }
Gaku0606 0:74d8e952a3bd 189 }
Gaku0606 2:7be89bab6db9 190 else if(strncmp(gps_read_buffer, "$GPGGA", 6) == 0){
Gaku0606 2:7be89bab6db9 191 memset(str_temp, '\0', 128);
Gaku0606 2:7be89bab6db9 192 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 193 if(processGPGGA(str_temp)){
Gaku0606 2:7be89bab6db9 194 gps_readable = true;
Gaku0606 2:7be89bab6db9 195 }
Gaku0606 2:7be89bab6db9 196 }
Gaku0606 2:7be89bab6db9 197 //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){
Gaku0606 2:7be89bab6db9 198 //printf("%s\r\n", gps_read_buffer);
Gaku0606 2:7be89bab6db9 199 //}
Gaku0606 0:74d8e952a3bd 200 }
Gaku0606 0:74d8e952a3bd 201 else{
Gaku0606 0:74d8e952a3bd 202 save_buffer[current] = temp;
Gaku0606 0:74d8e952a3bd 203 current++;
Gaku0606 0:74d8e952a3bd 204 if(current >= 127){
Gaku0606 0:74d8e952a3bd 205 current = 1;
Gaku0606 0:74d8e952a3bd 206 start_flag = false;
Gaku0606 0:74d8e952a3bd 207 }
Gaku0606 0:74d8e952a3bd 208 }
Gaku0606 0:74d8e952a3bd 209 }
Gaku0606 0:74d8e952a3bd 210
Gaku0606 0:74d8e952a3bd 211 }
Gaku0606 0:74d8e952a3bd 212
Gaku0606 2:7be89bab6db9 213 bool GPS_interrupt::processGPRMC(char *line){
Gaku0606 0:74d8e952a3bd 214
Gaku0606 0:74d8e952a3bd 215 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 0:74d8e952a3bd 216 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 0:74d8e952a3bd 217 //bool result = false;
Gaku0606 0:74d8e952a3bd 218 //char num[4] = "";//衛星数の文字データ
Gaku0606 0:74d8e952a3bd 219 //char kaibatsu[8], g_height[8];
Gaku0606 0:74d8e952a3bd 220 //char data_GPS[256] ="";
Gaku0606 0:74d8e952a3bd 221 char angle[8], speed[8];
Gaku0606 0:74d8e952a3bd 222 char ido[16] = "", keido[16] = "";
Gaku0606 0:74d8e952a3bd 223
Gaku0606 2:7be89bab6db9 224 double X = 0, Y = 0;
Gaku0606 0:74d8e952a3bd 225 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 0:74d8e952a3bd 226 char zikann[11], hizuke[7];
Gaku0606 0:74d8e952a3bd 227
Gaku0606 0:74d8e952a3bd 228 //bool gga_flag = false;
Gaku0606 0:74d8e952a3bd 229 bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 230 char tok_count = 0;
Gaku0606 0:74d8e952a3bd 231
Gaku0606 2:7be89bab6db9 232 rmc_initialize();
Gaku0606 0:74d8e952a3bd 233
Gaku0606 0:74d8e952a3bd 234 //_____GPRMC___________________________________
Gaku0606 0:74d8e952a3bd 235 //データ文字列の先頭から6文字目までが$GPRMCなら０が返ってくる
Gaku0606 2:7be89bab6db9 236 //if (strncmp(line, "$GPRMC", 6) == 0){
Gaku0606 0:74d8e952a3bd 237
Gaku0606 0:74d8e952a3bd 238 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 239 tok_count = 0;
Gaku0606 0:74d8e952a3bd 240
Gaku0606 0:74d8e952a3bd 241 //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A
Gaku0606 0:74d8e952a3bd 242
Gaku0606 0:74d8e952a3bd 243 // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12
Gaku0606 0:74d8e952a3bd 244 //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C
Gaku0606 0:74d8e952a3bd 245
Gaku0606 0:74d8e952a3bd 246 // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1
Gaku0606 0:74d8e952a3bd 247 // 0 1 2
Gaku0606 0:74d8e952a3bd 248 while (1){
Gaku0606 2:7be89bab6db9 249 switch (tok_count){
Gaku0606 2:7be89bab6db9 250 case 1://時分秒
Gaku0606 2:7be89bab6db9 251 strcpy(zikann, tok);//zikann = "233514.000\0"
Gaku0606 2:7be89bab6db9 252 break;
Gaku0606 2:7be89bab6db9 253 case 2://有効か無効か判定
Gaku0606 2:7be89bab6db9 254 if (strncmp(tok, "V", 1) == 0) return false;
Gaku0606 2:7be89bab6db9 255 else if (strcmp(tok, "A") == 0) rmc_flag = true;
Gaku0606 2:7be89bab6db9 256 break;
Gaku0606 2:7be89bab6db9 257 case 3://緯度
Gaku0606 2:7be89bab6db9 258 strcpy(ido, tok);
Gaku0606 2:7be89bab6db9 259 break;
Gaku0606 2:7be89bab6db9 260 case 4://北緯か南緯か
Gaku0606 2:7be89bab6db9 261 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 262 break;
Gaku0606 2:7be89bab6db9 263 case 5://経度
Gaku0606 2:7be89bab6db9 264 strcpy(keido, tok);
Gaku0606 2:7be89bab6db9 265 break;
Gaku0606 2:7be89bab6db9 266 case 6://東経か西経か
Gaku0606 2:7be89bab6db9 267 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 268 break;
Gaku0606 2:7be89bab6db9 269 case 7://速度（キロノット）
Gaku0606 2:7be89bab6db9 270 strcpy(speed, tok);
Gaku0606 2:7be89bab6db9 271 break;
Gaku0606 2:7be89bab6db9 272 case 8://角度
Gaku0606 2:7be89bab6db9 273 strcpy(angle, tok);
Gaku0606 2:7be89bab6db9 274 break;
Gaku0606 2:7be89bab6db9 275 case 9://日付
Gaku0606 2:7be89bab6db9 276 strcpy(hizuke, tok);//hizuke = "030316\0"
Gaku0606 2:7be89bab6db9 277 break;
Gaku0606 0:74d8e952a3bd 278 }
Gaku0606 2:7be89bab6db9 279
Gaku0606 0:74d8e952a3bd 280 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 0:74d8e952a3bd 281 tok_count++;
Gaku0606 0:74d8e952a3bd 282 if (tok == NULL) break;
Gaku0606 0:74d8e952a3bd 283 }
Gaku0606 0:74d8e952a3bd 284 if (rmc_flag){
Gaku0606 0:74d8e952a3bd 285 rmc_flag = false;
Gaku0606 0:74d8e952a3bd 286 //ddmmyy
Gaku0606 0:74d8e952a3bd 287 int dmy = 0;
Gaku0606 0:74d8e952a3bd 288 dmy = atoi(hizuke);
Gaku0606 0:74d8e952a3bd 289 day = dmy / 10000; //030316 / 10000 = 3
Gaku0606 0:74d8e952a3bd 290 month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3
Gaku0606 0:74d8e952a3bd 291 year = dmy - month * 100 - day * 10000 + 2000;
Gaku0606 0:74d8e952a3bd 292
Gaku0606 0:74d8e952a3bd 293 //hhmmss.ss
Gaku0606 0:74d8e952a3bd 294 seconds = atof(zikann);//sec = 233514.000
Gaku0606 0:74d8e952a3bd 295 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 0:74d8e952a3bd 296 minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 0:74d8e952a3bd 297 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 0:74d8e952a3bd 298
Gaku0606 0:74d8e952a3bd 299 // getLonLatのとき有効
Gaku0606 0:74d8e952a3bd 300 Y_m = atof(ido);//*Y_m = 3457.5571
Gaku0606 0:74d8e952a3bd 301 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 0:74d8e952a3bd 302 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 0:74d8e952a3bd 303
Gaku0606 0:74d8e952a3bd 304 X_m = atof(keido);//*X = 13057.6142
Gaku0606 0:74d8e952a3bd 305 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 0:74d8e952a3bd 306 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 0:74d8e952a3bd 307
Gaku0606 0:74d8e952a3bd 308 //GPS calculation
Gaku0606 0:74d8e952a3bd 309 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 0:74d8e952a3bd 310 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 0:74d8e952a3bd 311
Gaku0606 2:7be89bab6db9 312 if (lonSign) longitude *= (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 313 if (latSign) latitude *= (-1);//南緯なら緯度を負に
Gaku0606 0:74d8e952a3bd 314 knot = atof(speed);
Gaku0606 0:74d8e952a3bd 315 degree = atof(angle);
Gaku0606 2:7be89bab6db9 316 //if(longitude > 129.0 && latitude > 29.0){
Gaku0606 0:74d8e952a3bd 317 return true;
Gaku0606 2:7be89bab6db9 318 //}
Gaku0606 2:7be89bab6db9 319 //else return false;
Gaku0606 0:74d8e952a3bd 320 }
Gaku0606 0:74d8e952a3bd 321 else return false;
Gaku0606 2:7be89bab6db9 322 //}
Gaku0606 2:7be89bab6db9 323 //else return false;
Gaku0606 0:74d8e952a3bd 324 }
Gaku0606 0:74d8e952a3bd 325
Gaku0606 2:7be89bab6db9 326 bool GPS_interrupt::processGPGGA(char *line){
Gaku0606 2:7be89bab6db9 327
Gaku0606 2:7be89bab6db9 328 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 2:7be89bab6db9 329 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 2:7be89bab6db9 330 //bool result = false;
Gaku0606 2:7be89bab6db9 331 char num[4] = "";//衛星数の文字データ
Gaku0606 2:7be89bab6db9 332 char kaibatsu[8], g_height[8];
Gaku0606 2:7be89bab6db9 333 //char data_GPS[256] ="";
Gaku0606 2:7be89bab6db9 334 //char angle[8], speed[8];
Gaku0606 2:7be89bab6db9 335 char ido[16] = "", keido[16] = "";
Gaku0606 2:7be89bab6db9 336
Gaku0606 2:7be89bab6db9 337 double X = 0, Y = 0;
Gaku0606 2:7be89bab6db9 338 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 2:7be89bab6db9 339 //char zikann[11], hizuke[7];
Gaku0606 2:7be89bab6db9 340 char status[2];
Gaku0606 2:7be89bab6db9 341
Gaku0606 2:7be89bab6db9 342 bool gga_flag = false;
Gaku0606 2:7be89bab6db9 343 //bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 344 char tok_count = 0;
Gaku0606 2:7be89bab6db9 345
Gaku0606 2:7be89bab6db9 346 gga_initialize();
Gaku0606 2:7be89bab6db9 347
Gaku0606 2:7be89bab6db9 348 //データ文字列の先頭から6文字目までが$GPGLLなら０が返ってくる
Gaku0606 2:7be89bab6db9 349 //if (strncmp(data_GPS, "$GPGGA", 6) == 0){
Gaku0606 2:7be89bab6db9 350
Gaku0606 2:7be89bab6db9 351 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 352 tok_count = 0;
Gaku0606 2:7be89bab6db9 353
Gaku0606 2:7be89bab6db9 354 //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40
Gaku0606 2:7be89bab6db9 355
Gaku0606 2:7be89bab6db9 356 // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6
Gaku0606 2:7be89bab6db9 357
Gaku0606 2:7be89bab6db9 358 //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63
Gaku0606 2:7be89bab6db9 359
Gaku0606 2:7be89bab6db9 360 // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1
Gaku0606 2:7be89bab6db9 361 // 0 2
Gaku0606 2:7be89bab6db9 362 while (1){
Gaku0606 2:7be89bab6db9 363 switch (tok_count){
Gaku0606 2:7be89bab6db9 364 case 2://緯度
Gaku0606 2:7be89bab6db9 365 strcpy(ido, tok);//ido = "30"
Gaku0606 2:7be89bab6db9 366 if (strcmp(ido, "0") == 0) return false;//GPS無効
Gaku0606 2:7be89bab6db9 367 break;
Gaku0606 2:7be89bab6db9 368 case 3://北緯か南緯か
Gaku0606 2:7be89bab6db9 369 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 370 break;
Gaku0606 2:7be89bab6db9 371 case 4:
Gaku0606 2:7be89bab6db9 372 strcpy(keido, tok);//keido = "130"
Gaku0606 2:7be89bab6db9 373 break;
Gaku0606 2:7be89bab6db9 374 case 5://東経か西経か
Gaku0606 2:7be89bab6db9 375 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 376 break;
Gaku0606 2:7be89bab6db9 377 case 6:
Gaku0606 2:7be89bab6db9 378 strcpy(status, tok);
Gaku0606 2:7be89bab6db9 379 if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効
Gaku0606 2:7be89bab6db9 380 break;
Gaku0606 2:7be89bab6db9 381 case 7:
Gaku0606 2:7be89bab6db9 382 strcpy(num, tok);
Gaku0606 2:7be89bab6db9 383 break;
Gaku0606 2:7be89bab6db9 384 case 9:
Gaku0606 2:7be89bab6db9 385 strcpy(kaibatsu, tok);
Gaku0606 2:7be89bab6db9 386 break;
Gaku0606 2:7be89bab6db9 387 case 11:
Gaku0606 2:7be89bab6db9 388 strcpy(g_height, tok);
Gaku0606 2:7be89bab6db9 389 break;
Gaku0606 2:7be89bab6db9 390 }
Gaku0606 2:7be89bab6db9 391 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 2:7be89bab6db9 392 tok_count++;
Gaku0606 2:7be89bab6db9 393 if (tok == NULL) break;
Gaku0606 2:7be89bab6db9 394 }
Gaku0606 2:7be89bab6db9 395 if (gga_flag){
Gaku0606 2:7be89bab6db9 396
Gaku0606 2:7be89bab6db9 397 height = atof(kaibatsu);
Gaku0606 2:7be89bab6db9 398 geoid = atof(g_height);
Gaku0606 2:7be89bab6db9 399 number = atoi(num);
Gaku0606 2:7be89bab6db9 400 /*
Gaku0606 2:7be89bab6db9 401 //ddmmyy
Gaku0606 2:7be89bab6db9 402 long dmy = 0;
Gaku0606 2:7be89bab6db9 403 dmy = atol(hizuke);
Gaku0606 2:7be89bab6db9 404 day = (int)(dmy / 10000); //030316 / 10000 = 3
Gaku0606 2:7be89bab6db9 405 month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3
Gaku0606 2:7be89bab6db9 406 year = dmy - month * 100 - (long)day * 10000 + 2000;
Gaku0606 2:7be89bab6db9 407
Gaku0606 2:7be89bab6db9 408 //hhmmss.ss
Gaku0606 2:7be89bab6db9 409 seconds = atof(zikann);//sec = 233514.000
Gaku0606 2:7be89bab6db9 410 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 2:7be89bab6db9 411 minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 2:7be89bab6db9 412 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 2:7be89bab6db9 413 */
Gaku0606 2:7be89bab6db9 414 // getLonLatのとき有効
Gaku0606 2:7be89bab6db9 415 Y_m = atof(ido);//Y_m = 3457.5571
Gaku0606 2:7be89bab6db9 416 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 2:7be89bab6db9 417 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 2:7be89bab6db9 418
Gaku0606 2:7be89bab6db9 419 X_m = atof(keido);//X = 13057.6142
Gaku0606 2:7be89bab6db9 420 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 2:7be89bab6db9 421 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 2:7be89bab6db9 422
Gaku0606 2:7be89bab6db9 423 //GPS calculation
Gaku0606 2:7be89bab6db9 424 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 2:7be89bab6db9 425 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 2:7be89bab6db9 426 if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 427 if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に
Gaku0606 2:7be89bab6db9 428
Gaku0606 2:7be89bab6db9 429 return true;
Gaku0606 2:7be89bab6db9 430 }
Gaku0606 2:7be89bab6db9 431 else return false;
Gaku0606 2:7be89bab6db9 432 //}
Gaku0606 2:7be89bab6db9 433
Gaku0606 2:7be89bab6db9 434 }