File content as of revision 17:4b09630703c0:

#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){

    if(_gps == NULL){
        error("GPS UART BUS ERROR!!\r\n");   
    }
    debugFlag = false;
    initialize();
    //baudrate = 9600;//_baudrate;
    //frequency = 1;
    gps_irq = this;
    gps = _gps;
    //gps->baud(baudrate);
    gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);    
}

/*
GPS_interrupt::GPS_interrupt(Serial *_gps, int _baudrate){
    
    if(_gps == NULL){
        error("GPS UART BUS ERROR!!\r\n");   
    }
    debugFlag = false;
    initialize();
    baudrate = _baudrate;
    //frequency = _frequency;
    gps_irq = this;
    gps = _gps;
    gps->baud(baudrate);
    
    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::changeGPSBaud(int _baudrate){
    baudrate = _baudrate;
    switch(baudrate){
        case 4800:
        gps -> printf("$PMTK251,4800*14\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,4800*14\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,4800*14\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,4800*14\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,4800*14\r\n");
        wait(0.2f);
        gps->baud(4800);
        break;
        
        case 9600:
        gps->printf("$PMTK251,9600*17\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,9600*17\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,9600*17\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,9600*17\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,9600*17\r\n");
        wait(0.2f);
        gps->baud(9600);
        break;
        
        case 14400:
        gps->printf("$PMTK251,14400*29\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,14400*29\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,14400*29\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,14400*29\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,14400*29\r\n");
        wait(0.2f);
        gps->baud(14400);
        break;
        
        case 19200:
        gps->printf("$PMTK251,19200*22\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,19200*22\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,19200*22\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,19200*22\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,19200*22\r\n");
        wait(0.2f);
        gps->baud(19200);
        break;
        
        case 38400:
        wait(0.2f);
        gps->printf("$PMTK251,38400*27\r\n");
        wait(0.2f);
        /*
        gps->printf("$PMTK251,38400*27\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,38400*27\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,38400*27\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,38400*27\r\n");
        wait(0.2f);
        */
        gps->baud(38400);
        break;
        
        case 57600:
        gps->printf("$PMTK251,57600*2C\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,57600*2C\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,57600*2C\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,57600*2C\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,57600*2C\r\n");
        wait(0.2f);
        gps->baud(57600);
        break;
        
        case 115200:
        gps->printf("$PMTK251,115200*1F\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,115200*1F\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,115200*1F\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,115200*1F\r\n");
        wait(0.2f);
        gps->printf("$PMTK251,115200*1F\r\n");
        wait(0.2f);
        gps->baud(115200);
        break;
    }
}

bool GPS_interrupt::changeGPSFreq(int _frequency){
    frequency = _frequency;
    flag_change_freq = 0;
    switch(frequency){
        case 1:
        gps->printf("$PMTK220,1000*1F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,1000*1F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,1000*1F\r\n");
        wait(0.1f);
        break;
        
        case 2:
        gps->printf("$PMTK220,500*2B\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,500*2B\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,500*2B\r\n");
        wait(0.1f);
        break;
        
        case 3:
        gps->printf("$PMTK220,333*2D\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,333*2D\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,333*2D\r\n");
        wait(0.1f);
        break;
        
        case 4:
        wait(0.2f);
        gps->printf("$PMTK220,250*29\r\n");
        wait(0.2f);
        /*
        wait(0.1f);
        gps->printf("$PMTK220,250*29\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,250*29\r\n");
        wait(0.1f);
        */
        break;
        
        case 5:
        gps->printf("$PMTK220,200*2C\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,200*2C\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,200*2C\r\n");
        wait(0.1f);
        break;
        
        case 6:
        gps->printf("$PMTK220,167*2E\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,167*2E\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,167*2E\r\n");
        wait(0.1f);
        break;
        
        case 7:
        gps->printf("$PMTK220,143*28\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,143*28\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,143*28\r\n");
        wait(0.1f);
        break;
        
        case 8:
        gps->printf("$PMTK220,125*28\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,125*28\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,125*28\r\n");
        wait(0.1f);
        break;
        
        case 9:
        gps->printf("$PMTK220,111*2F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,111*2F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,111*2F\r\n");
        wait(0.1f);
        break;
        
        case 10:
        gps->printf("$PMTK220,100*2F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,100*2F\r\n");
        wait(0.1f);
        gps->printf("$PMTK220,100*2F\r\n");
        wait(0.1f);
        break;
    }
    
    for(wait_change = 0; wait_change < 10; wait_change ++){
        if(flag_change_freq == 1){
            return true;
        }
        else if(flag_change_freq == -1){
            return false;
        }
        wait(0.1f);
    }
    return false;
}

bool GPS_interrupt::changeGPSData(int _GLL, int _RMC, int _VTG, int _GGA, int _GSA, int _GSV){
    data_sum = _GLL + _RMC + _VTG + _GGA + _GSA + _GSV;
    data_sum = data_sum % 2;
    data_sum += 28;
    gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
    wait(0.1f);
    gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
    wait(0.1f);
    gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum);
    wait(0.1f);
    
    for(wait_change = 0; wait_change < 10; wait_change ++){
        if(flag_change_data == 1){
            return true;
        }
        else if(flag_change_data == -1){
            return false;
        }
        wait(0.1f);
    }
    return false;
}

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) || (strncmp(gps_read_buffer, "$GNGGA", 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 if(strncmp(gps_read_buffer, "$PMTK001", 8) == 0){
                memset(str_temp, '\0', 128);
                strcpy(str_temp, gps_read_buffer);
                processPMTK(str_temp);
            }
        }
        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なら０が返ってくる
    //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 - ((float)minutes * 100.0 + (float)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);
            //printf("\r\n%f\t%f\r\n",knot, degree);
            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なら０が返ってくる
    //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;
}

void GPS_interrupt::processPMTK(char *line){
    
    char *tok; //strtokで帰ってくる文字列のポインター
    int tok_count = 0;
    
    char status[2];
    char mode_change_s[5] = "";
    int mode_change = 0;

    tok = strtok(line, ",");
    tok_count = 0;

    //$PMTK001, 334, 3*34
    //   0    ,  1 ,  2

    while (1){
        switch (tok_count){
            case 1: //変更内容
            if(strcmp(tok, "220") == 0){
                mode_change = 220;
            }
            else if(strcmp(tok, "314") == 0){
                mode_change = 314;
            }
            break;
            
            case 2:
            if(tok[0] = '3'){
                switch(mode_change){
                    case 220:
                    flag_change_freq = 1;
                    break;
                    case 314:
                    flag_change_data = 1;
                    break;
                }
            }
            else{
                switch(mode_change){
                    case 220:
                    flag_change_freq = -1;
                    break;
                    case 314:
                    flag_change_data = -1;
                    break;
                }
            }
            break;
        }
        tok = strtok(NULL, ",");
        tok_count ++;
        if(tok == NULL){
            break;
        }
    }
}



float 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 (float)sqrt(dLat * dLat + dLng * dLng);
}