2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Diff: GPS_interrupt.cpp
- Revision:
- 0:74d8e952a3bd
- Child:
- 2:7be89bab6db9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS_interrupt.cpp Mon Jan 02 00:40:46 2017 +0000 @@ -0,0 +1,207 @@ +#include "mbed.h" +#include "GPS_interrupt.h" +double GPS_interrupt::latitude; +double GPS_interrupt::longitude; +int GPS_interrupt::year; +int GPS_interrupt::month; +int GPS_interrupt::day; +int GPS_interrupt::hour; +int GPS_interrupt::minutes; +double GPS_interrupt::seconds; +double GPS_interrupt::degree = 0; +double GPS_interrupt::knot = 0; +double GPS_interrupt::height = 0; +double GPS_interrupt::geoid = 0; +int GPS_interrupt::number = 0; + +char GPS_interrupt::gps_buffer_A[128]; +char GPS_interrupt::gps_buffer_B[128]; +char* GPS_interrupt::gps_read_buffer = gps_buffer_B; +bool GPS_interrupt::gps_readable = false; + +GPS_interrupt* GPS_interrupt::gps_irq; + +GPS_interrupt::GPS_interrupt(RawSerial *_gps, int baudrate){ + gps_irq = this; + gps = _gps; + gps->baud(baudrate); + gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); +} + +void GPS_interrupt::initialize(){ + latitude = 0.0f; + longitude = 0.0f; + year = 0; + month = 0; + day = 0; + hour = 0; + minutes = 0; + seconds = 0; + memset(gps_buffer_A, '\0', 128); + memset(gps_buffer_B, '\0', 128); + gps_readable = false; +} +void GPS_interrupt::gps_auto_receive(){ + + static char str_temp[128] = {'\0'}; + static char temp = 0; + static bool start_flag = false; + static unsigned char current = 0; + static char classify_AB = 0; + static char* save_buffer = gps_buffer_A; + + temp = gps->getc(); + if(temp == '$'){ + current = 1; + start_flag = true; + memset(save_buffer, '\0', 128);//初期化 + save_buffer[0] = '$'; + return; + } + if(start_flag){//1行スタート + if(temp == '\r'){//1行終了 + save_buffer[current] = '\0'; + start_flag = false; + current = 1; + gps_read_buffer = save_buffer; + if(classify_AB){ + save_buffer = gps_buffer_A;//バッファ切換 + classify_AB = ~classify_AB; + } + else{ + save_buffer = gps_buffer_B; + classify_AB = ~classify_AB; + } + if(strncmp(gps_read_buffer, "$GPRMC", 6) == 0){ + strcpy(str_temp, gps_read_buffer); + if(processGPS(str_temp)){ + gps_readable = true; + } + } + } + else{ + save_buffer[current] = temp; + current++; + if(current >= 127){ + current = 1; + start_flag = false; + } + } + } + +} + +bool GPS_interrupt::processGPS(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]; + + //bool gga_flag = false; + bool rmc_flag = false; + + initialize(); + +//_____GPRMC___________________________________ + + //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる + if (strncmp(line, "$GPRMC", 6) == 0){ + + tok = strtok(line, ","); + char tok_count = 0; + + //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A + + // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12 + //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C + + // 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; + } + 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; + } + if (rmc_flag){ + rmc_flag = false; + //ddmmyy + int dmy = 0; + dmy = atoi(hizuke); + day = dmy / 10000; //030316 / 10000 = 3 + month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3 + year = dmy - month * 100 - day * 10000 + 2000; + + //hhmmss.ss + seconds = atof(zikann);//sec = 233514.000 + hour = (int)(seconds / 10000.0); //233514/10000=(int)23 + minutes = (seconds - hour * 10000) / 100;//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 *= (-1);//西経なら経度を負に + if (latSign == true) latitude *= (-1);//南緯なら緯度を負に + knot = atof(speed); + degree = atof(angle); + if(longitude > 129.0 && latitude > 29.0){ + return true; + } + else return false; + } + else return false; + } + else return false; +} +