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なら０が返ってくる
+    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;
+}
+