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);
 }