2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
GPS_interrupt.cpp
- Committer:
- Gaku0606
- Date:
- 2017-11-23
- Revision:
- 10:a006445dc76d
- Parent:
- 9:dab13bd20f43
- Child:
- 11:1897b52fa8a1
File content as of revision 10:a006445dc76d:
#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] = {'\0'}; //char GPS_interrupt::gps_buffer_B[128] = {'\0'}; //char GPS_interrupt::gps_buffer_C[128] = {'\0'}; /* char* GPS_interrupt::gps_read_buffer = gps_buffer_B; bool GPS_interrupt::gps_readable = false;*/ //GPS_interrupt* GPS_interrupt::gps_irq; //Timeout timeout_clear; void GPS_interrupt::debug(bool tf){ if(tf){ debugFlag = true; } else{ debugFlag = false; } } 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(Serial &_gps){ debugFlag = false; initialize(); baudrate = 115200;//_baudrate; frequency = 10; gps_irq = this; gps = &_gps; gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); } GPS_interrupt::GPS_interrupt(Serial *_gps, int _frequency){ debugFlag = false; initialize(); baudrate = 9600;//_baudrate; frequency = _frequency; gps_irq = this; gps = _gps; gps->baud(9600); if(baudrate == 9600){ gps->printf("$PMTK251,9600*17\r\n"); } else if(baudrate == 19200){ gps->printf("$PMTK251,19200*22\r\n"); } else if( baudrate == 38400){ gps->printf("$PMTK251,38400*27\r\n"); } else if(baudrate == 56700){ gps->printf("$PMTK251,57600*2C\r\n"); } else if(baudrate == 115200){ gps->printf("$PMTK251,115200*1F\r\n"); wait(0.1); gps->printf("$PMTK251,115200*1F\r\n"); wait(0.2); gps->printf("$PMTK251,115200*1F\r\n"); wait(0.2); } gps->baud(9600); baudrate = 9600; 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); wait(0.2); 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); wait(0.2); 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); wait(0.2); if(_frequency == 1) gps->printf("$PMTK220,1000*1F\r\n"); else if(_frequency == 2) gps->printf("$PMTK220,500*2B\r\n"); else if(_frequency == 3) gps->printf("$PMTK220,333*2D\r\n"); else if(_frequency == 4) gps->printf("$PMTK220,250*29\r\n"); else if(_frequency == 5) gps->printf("$PMTK220,200*2C\r\n"); else if(_frequency == 10) gps->printf("$PMTK220,100*2F\r\n"); wait(0.2); 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; knot = 0; degree = 0; number = 0; height = 0; geoid = 0; memset(gps_buffer_A, '\0', 128); memset(gps_buffer_B, '\0', 128); // memset(gps_buffer_C, '\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'}; static unsigned char temp = 0; static bool start_flag = false; static int current = 0; static char classify_ABC = 0; static char* save_buffer = gps_buffer_A; temp = gps->getc(); //if(debugFlag) printf("%c", temp); 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; //debug(gps_read_buffer); //printf("%s\r\n", save_buffer); //printf("debug \"%s\"\r\n", gps_read_buffer); /*switch(classify_ABC){ case 0: save_buffer = gps_buffer_B; classify_ABC = 1; break; case 1: save_buffer = gps_buffer_C;//バッファ切換 classify_ABC = 2; break; case 2: save_buffer = gps_buffer_A; classify_ABC = 0; }*/ if(classify_ABC == 0){ save_buffer = gps_buffer_B; classify_ABC = 1; } else if(classify_ABC == 1){ save_buffer = gps_buffer_A;//バッファ切換 classify_ABC = 0; } memset(save_buffer, '\0', 128); //if(debugFlag) printf("debug \"%s\"\r\n", gps_read_buffer); if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){ memset(str_temp, '\0', 128); strcpy(str_temp, gps_read_buffer); if(processGPRMC(str_temp)){ gps_readable = true; return;//データが取得できればここで終了 } else{ gps_readable = false; } } 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; return;//データが取得できればここで終了 } else{ gps_readable = false; return; } } //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){ //printf("%s\r\n", gps_read_buffer); //} } else{ save_buffer[current] = temp; current++; if(current >= 127){ current = 1; start_flag = false; memset(save_buffer, '\0', 128); } } } } bool GPS_interrupt::processGPRMC(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; char tok_count = 0; //rmc_initialize(); //_____GPRMC___________________________________ //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる //if (strncmp(line, "$GPRMC", 6) == 0){ tok = strtok(line, ","); 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){ 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; } 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) longitude *= (-1);//西経なら経度を負に if (latSign) latitude *= (-1);//南緯なら緯度を負に knot = atof(speed); degree = atof(angle); if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){ return true; } 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);//南緯なら緯度を負に if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){ return true; } else{ return false; } } else{ return false; } return false; } double GPS_interrupt::Distance(double x, double y){ double dLat = x - longitude;//相対経度 double dLng = y - latitude;//相対緯度 double radLat = latitude * GPS_PI / 180.0;//今の緯度をラジアンにしたもの double F = EIRTH_AspectRatioInverse; // 離心率の2乗 double E = ((2.0 * F) - 1.0) / (F * F); // π * 赤道半径 double PI_ER = GPS_PI * EARTH_EQUATOR_RADIUS; // 1 - e^2 sin^2 (θ) double TMP = 1.0 - E * sin(radLat) * sin(radLat); // 経度1°あたりの長さ double arc_lat = (PI_ER * (1.0 - E)) / ( 180.0 * pow(TMP, 1.5)); // 緯度1°あたりの長さ double arc_lng = (PI_ER * cos(radLat)) / (180.0 * pow(TMP, 0.5)); dLat *= arc_lat; dLng *= arc_lng; return sqrt(dLat * dLat + dLng * dLng); }