2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Diff: GPS_interrupt.cpp
- Revision:
- 2:7be89bab6db9
- Parent:
- 0:74d8e952a3bd
- Child:
- 3:8e66ec281888
--- a/GPS_interrupt.cpp Mon Jan 02 01:05:31 2017 +0000 +++ b/GPS_interrupt.cpp Mon Jan 02 18:48:44 2017 +0000 @@ -21,10 +21,80 @@ GPS_interrupt* GPS_interrupt::gps_irq; -GPS_interrupt::GPS_interrupt(RawSerial *_gps, int baudrate){ +//Timeout timeout_clear; + +void GPS_interrupt::debug(char *str){ + printf("%s\r\n", str); +} + +unsigned char GPS_interrupt::checkSum(char *str){ + int num = strlen(str); + unsigned char val = 0; + for(int i = 0; i< num; i++){ + val = val ^ str[i]; + } + return val; +} + +GPS_interrupt::GPS_interrupt(RawSerial *_gps, int _baudrate, int _frequency, int start_baudrate){ + + baudrate = _baudrate; + frequency = _frequency; gps_irq = this; gps = _gps; - gps->baud(baudrate); + gps->baud(start_baudrate); + wait(0.1); + switch (_baudrate){ + case 9600: + gps->printf("$PMTK251,9600*17\r\n"); + break; + case 19200: + gps->printf("$PMTK251,19200*22\r\n"); + break; + case 38400: + gps->printf("$PMTK251,38400*27\r\n"); + break; + case 57600: + gps->printf("$PMTK251,57600*2C\r\n"); + break; + case 115200: + gps->printf("$PMTK251,115200*1F\r\n"); + break; + default: + gps->printf("$PMTK251,115200*1F\r\n"); + break; + } + gps->baud(_baudrate); + wait(0.1); + gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format + unsigned char checksum = 0; + checksum = checkSum("PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0"); + //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム + 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); + switch(_frequency){ + case 1: + gps->printf("$PMTK220,1000*1F\r\n"); + break; + case 2: + gps->printf("$PMTK220,500*2B\r\n"); + break; + case 3: + gps->printf("$PMTK220,333*2D\r\n"); + break; + case 4: + gps->printf("$PMTK220,250*29\r\n"); + break; + case 5: + gps->printf("$PMTK220,200*2C\r\n"); + break; + case 10: + gps->printf("$PMTK220,100*2F\r\n"); + break; + default: + gps->printf("$PMTK220,1000*1F\r\n"); + break; + } + wait(0.1); gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); } @@ -37,10 +107,40 @@ hour = 0; minutes = 0; seconds = 0; + knot = 0; + degree = 0; + number = 0; + height = 0; + geoid = 0; memset(gps_buffer_A, '\0', 128); memset(gps_buffer_B, '\0', 128); gps_readable = false; } +void GPS_interrupt::rmc_initialize(){ + latitude = 0.0f; + longitude = 0.0f; + year = 0; + month = 0; + day = 0; + hour = 0; + minutes = 0; + seconds = 0; + knot = 0; + degree = 0; +} +void GPS_interrupt::gga_initialize(){ + latitude = 0.0f; + longitude = 0.0f; + /*year = 0; + month = 0; + day = 0; + hour = 0; + minutes = 0; + seconds = 0;*/ + number = 0; + height = 0; + geoid = 0; +} void GPS_interrupt::gps_auto_receive(){ static char str_temp[128] = {'\0'}; @@ -51,6 +151,9 @@ static char* save_buffer = gps_buffer_A; temp = gps->getc(); + + //printf("%c", temp); + if(temp == '$'){ current = 1; start_flag = true; @@ -64,6 +167,10 @@ start_flag = false; current = 1; gps_read_buffer = save_buffer; + + //debug(gps_read_buffer); + //printf("%s\r\n", save_buffer); + if(classify_AB){ save_buffer = gps_buffer_A;//バッファ切換 classify_AB = ~classify_AB; @@ -73,11 +180,22 @@ classify_AB = ~classify_AB; } if(strncmp(gps_read_buffer, "$GPRMC", 6) == 0){ + memset(str_temp, '\0', 128); strcpy(str_temp, gps_read_buffer); - if(processGPS(str_temp)){ + if(processGPRMC(str_temp)){ gps_readable = true; } } + else if(strncmp(gps_read_buffer, "$GPGGA", 6) == 0){ + memset(str_temp, '\0', 128); + strcpy(str_temp, gps_read_buffer); + if(processGPGGA(str_temp)){ + gps_readable = true; + } + } + //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){ + //printf("%s\r\n", gps_read_buffer); + //} } else{ save_buffer[current] = temp; @@ -91,7 +209,7 @@ } -bool GPS_interrupt::processGPS(char *line){ +bool GPS_interrupt::processGPRMC(char *line){ char *tok; //strtokで帰ってくる文字列のポインター bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue @@ -102,22 +220,22 @@ char angle[8], speed[8]; char ido[16] = "", keido[16] = ""; - double X = 0, Y = 0; + double X = 0, Y = 0; double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 char zikann[11], hizuke[7]; //bool gga_flag = false; bool rmc_flag = false; + char tok_count = 0; - initialize(); + rmc_initialize(); //_____GPRMC___________________________________ - //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる - if (strncmp(line, "$GPRMC", 6) == 0){ + //if (strncmp(line, "$GPRMC", 6) == 0){ tok = strtok(line, ","); - char tok_count = 0; + tok_count = 0; //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A @@ -127,38 +245,37 @@ // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1 // 0 1 2 while (1){ - - //___UTC時間_____________ - if (tok_count == 1){ - strcpy(zikann, tok);//zikann = "233514.000\0" - } - //___確認,経度___________ - else if (tok_count == 2){ - if (strncmp(tok, "V", 1) == 0) return false; - else if (strcmp(tok, "A") == 0) rmc_flag = true; - } - else if (tok_count == 3){//緯度 - strcpy(ido, tok); - } - else if (tok_count == 4){//北緯か南緯か - if (strcmp(tok, "S") == 0) latSign = true; + switch (tok_count){ + case 1://時分秒 + strcpy(zikann, tok);//zikann = "233514.000\0" + break; + case 2://有効か無効か判定 + if (strncmp(tok, "V", 1) == 0) return false; + else if (strcmp(tok, "A") == 0) rmc_flag = true; + break; + case 3://緯度 + strcpy(ido, tok); + break; + case 4://北緯か南緯か + if (strcmp(tok, "S") == 0) latSign = true; + break; + case 5://経度 + strcpy(keido, tok); + break; + case 6://東経か西経か + if (strcmp(tok, "W") == 0) lonSign = true; + break; + case 7://速度(キロノット) + strcpy(speed, tok); + break; + case 8://角度 + strcpy(angle, tok); + break; + case 9://日付 + strcpy(hizuke, tok);//hizuke = "030316\0" + break; } - else if (tok_count == 5){//経度 - strcpy(keido, tok); - } - else if (tok_count == 6){//東経か西経か - if (strcmp(tok, "W") == 0) lonSign = true; - } - else if (tok_count == 7){//速度(キロノット) - strcpy(speed, tok); - } - else if (tok_count == 8){//角度 - strcpy(angle, tok); - } - //___日付________________ - else if (tok_count == 9){ - strcpy(hizuke, tok);//hizuke = "030316\0" - } + tok = strtok(NULL, ",");//comma = ",\0" tok_count++; if (tok == NULL) break; @@ -191,17 +308,126 @@ longitude = X + X_m / 60.0;// 34.959285 latitude = Y + Y_m / 60.0;//137.090290 - if (lonSign == true) longitude *= (-1);//西経なら経度を負に - if (latSign == true) latitude *= (-1);//南緯なら緯度を負に + if (lonSign) longitude *= (-1);//西経なら経度を負に + if (latSign) latitude *= (-1);//南緯なら緯度を負に knot = atof(speed); degree = atof(angle); - if(longitude > 129.0 && latitude > 29.0){ + //if(longitude > 129.0 && latitude > 29.0){ return true; - } - else return false; + //} + //else return false; } else return false; - } - else return false; + //} + //else return false; } +bool GPS_interrupt::processGPGGA(char *line){ + + char *tok; //strtokで帰ってくる文字列のポインター + bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue + //bool result = false; + char num[4] = "";//衛星数の文字データ + char kaibatsu[8], g_height[8]; + //char data_GPS[256] =""; + //char angle[8], speed[8]; + char ido[16] = "", keido[16] = ""; + + double X = 0, Y = 0; + double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 + //char zikann[11], hizuke[7]; + char status[2]; + + bool gga_flag = false; + //bool rmc_flag = false; + char tok_count = 0; + + gga_initialize(); + +//データ文字列の先頭から6文字目までが$GPGLLなら0が返ってくる + //if (strncmp(data_GPS, "$GPGGA", 6) == 0){ + + tok = strtok(line, ","); + tok_count = 0; + + //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40 + + // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6 + + //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63 + + // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1 + // 0 2 + while (1){ + switch (tok_count){ + case 2://緯度 + strcpy(ido, tok);//ido = "30" + if (strcmp(ido, "0") == 0) return false;//GPS無効 + break; + case 3://北緯か南緯か + if (strcmp(tok, "S") == 0) latSign = true; + break; + case 4: + strcpy(keido, tok);//keido = "130" + break; + case 5://東経か西経か + if (strcmp(tok, "W") == 0) lonSign = true; + break; + case 6: + strcpy(status, tok); + if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効 + break; + case 7: + strcpy(num, tok); + break; + case 9: + strcpy(kaibatsu, tok); + break; + case 11: + strcpy(g_height, tok); + break; + } + tok = strtok(NULL, ",");//comma = ",\0" + tok_count++; + if (tok == NULL) break; + } + if (gga_flag){ + + height = atof(kaibatsu); + geoid = atof(g_height); + number = atoi(num); + /* + //ddmmyy + long dmy = 0; + dmy = atol(hizuke); + day = (int)(dmy / 10000); //030316 / 10000 = 3 + month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3 + year = dmy - month * 100 - (long)day * 10000 + 2000; + + //hhmmss.ss + seconds = atof(zikann);//sec = 233514.000 + hour = (int)(seconds / 10000.0); //233514/10000=(int)23 + minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35 + seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0); + */ + // getLonLatのとき有効 + Y_m = atof(ido);//Y_m = 3457.5571 + Y = (int)(Y_m / 100.0);//Y = 34.0 + Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 + + X_m = atof(keido);//X = 13057.6142 + X = (int)(X_m / 100.0);//X = 130.0 + X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 + + //GPS calculation + longitude = X + X_m / 60.0;// 34.959285 + latitude = Y + Y_m / 60.0;//137.090290 + if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に + if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に + + return true; + } + else return false; + //} + +}