2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Diff: GPS_interrupt.cpp
- Revision:
- 8:3f32df2b66c0
- Parent:
- 7:4b893ac95ae1
- Child:
- 9:dab13bd20f43
--- a/GPS_interrupt.cpp Sat Feb 25 10:09:08 2017 +0000 +++ b/GPS_interrupt.cpp Mon Jun 26 08:14:47 2017 +0000 @@ -14,9 +14,11 @@ 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_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;*/ @@ -56,7 +58,7 @@ debugFlag = false; initialize(); - baudrate = 115200;//_baudrate; + baudrate = 9600;//_baudrate; frequency = _frequency; gps_irq = this; gps = _gps; @@ -81,8 +83,8 @@ gps->printf("$PMTK251,115200*1F\r\n"); wait(0.2); } - gps->baud(115200); - baudrate = 115200; + gps->baud(9600); + baudrate = 9600; wait(0.1); //gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format @@ -121,6 +123,7 @@ 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(){ @@ -151,15 +154,15 @@ void GPS_interrupt::gps_auto_receive(){ static char str_temp[128] = {'\0'}; - static char temp = 0; + static unsigned char temp = 0; static bool start_flag = false; - static unsigned char current = 0; - static char classify_AB = 0; + static int current = 0; + static char classify_ABC = 0; static char* save_buffer = gps_buffer_A; temp = gps->getc(); - //printf("%c", temp); + if(debugFlag) printf("%c", temp); if(temp == '$'){ current = 1; @@ -176,17 +179,33 @@ gps_read_buffer = save_buffer; //debug(gps_read_buffer); - printf("%s\r\n", save_buffer); + //printf("%s\r\n", save_buffer); + //printf("debug \"%s\"\r\n", gps_read_buffer); - if(classify_AB){ - save_buffer = gps_buffer_A;//バッファ切換 - classify_AB = ~classify_AB; + /*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{ - save_buffer = gps_buffer_B; - classify_AB = ~classify_AB; + 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)){ @@ -196,6 +215,9 @@ gps_readable = true; return;//データが取得できればここで終了 } + else{ + gps_readable = false; + } } else if(strncmp(gps_read_buffer, "$GPGGA", 6) == 0){ memset(str_temp, '\0', 128); @@ -204,9 +226,11 @@ 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); //} @@ -217,6 +241,7 @@ if(current >= 127){ current = 1; start_flag = false; + memset(save_buffer, '\0', 128); } } } @@ -325,9 +350,10 @@ if (latSign) latitude *= (-1);//南緯なら緯度を負に knot = atof(speed); degree = atof(angle); - - return true; - + if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){ + return true; + } + else return false; } else{ return false; @@ -439,11 +465,37 @@ if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に - return true; + 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); }