PQ_Hybrid_Electrical_Equipment_Team
2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
Diff: GPS_interrupt.cpp
- Revision:
- 2:7be89bab6db9
- Parent:
- 0:74d8e952a3bd
- Child:
- 3:8e66ec281888
--- a/GPS_interrupt.cpp Mon Jan 02 01:05:31 2017 +0000
+++ b/GPS_interrupt.cpp Mon Jan 02 18:48:44 2017 +0000
@@ -21,10 +21,80 @@
GPS_interrupt* GPS_interrupt::gps_irq;
-GPS_interrupt::GPS_interrupt(RawSerial *_gps, int baudrate){
+//Timeout timeout_clear;
+
+void GPS_interrupt::debug(char *str){
+ printf("%s\r\n", str);
+}
+
+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(RawSerial *_gps, int _baudrate, int _frequency, int start_baudrate){
+
+ baudrate = _baudrate;
+ frequency = _frequency;
gps_irq = this;
gps = _gps;
- gps->baud(baudrate);
+ gps->baud(start_baudrate);
+ wait(0.1);
+ switch (_baudrate){
+ case 9600:
+ gps->printf("$PMTK251,9600*17\r\n");
+ break;
+ case 19200:
+ gps->printf("$PMTK251,19200*22\r\n");
+ break;
+ case 38400:
+ gps->printf("$PMTK251,38400*27\r\n");
+ break;
+ case 57600:
+ gps->printf("$PMTK251,57600*2C\r\n");
+ break;
+ case 115200:
+ gps->printf("$PMTK251,115200*1F\r\n");
+ break;
+ default:
+ gps->printf("$PMTK251,115200*1F\r\n");
+ break;
+ }
+ gps->baud(_baudrate);
+ 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);
+ switch(_frequency){
+ case 1:
+ gps->printf("$PMTK220,1000*1F\r\n");
+ break;
+ case 2:
+ gps->printf("$PMTK220,500*2B\r\n");
+ break;
+ case 3:
+ gps->printf("$PMTK220,333*2D\r\n");
+ break;
+ case 4:
+ gps->printf("$PMTK220,250*29\r\n");
+ break;
+ case 5:
+ gps->printf("$PMTK220,200*2C\r\n");
+ break;
+ case 10:
+ gps->printf("$PMTK220,100*2F\r\n");
+ break;
+ default:
+ gps->printf("$PMTK220,1000*1F\r\n");
+ break;
+ }
+ wait(0.1);
gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
}
@@ -37,10 +107,40 @@
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);
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'};
@@ -51,6 +151,9 @@
static char* save_buffer = gps_buffer_A;
temp = gps->getc();
+
+ //printf("%c", temp);
+
if(temp == '$'){
current = 1;
start_flag = true;
@@ -64,6 +167,10 @@
start_flag = false;
current = 1;
gps_read_buffer = save_buffer;
+
+ //debug(gps_read_buffer);
+ //printf("%s\r\n", save_buffer);
+
if(classify_AB){
save_buffer = gps_buffer_A;//バッファ切換
classify_AB = ~classify_AB;
@@ -73,11 +180,22 @@
classify_AB = ~classify_AB;
}
if(strncmp(gps_read_buffer, "$GPRMC", 6) == 0){
+ memset(str_temp, '\0', 128);
strcpy(str_temp, gps_read_buffer);
- if(processGPS(str_temp)){
+ if(processGPRMC(str_temp)){
gps_readable = true;
}
}
+ 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;
+ }
+ }
+ //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){
+ //printf("%s\r\n", gps_read_buffer);
+ //}
}
else{
save_buffer[current] = temp;
@@ -91,7 +209,7 @@
}
-bool GPS_interrupt::processGPS(char *line){
+bool GPS_interrupt::processGPRMC(char *line){
char *tok; //strtokで帰ってくる文字列のポインター
bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
@@ -102,22 +220,22 @@
char angle[8], speed[8];
char ido[16] = "", keido[16] = "";
- double X = 0, Y = 0;
+ 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;
- initialize();
+ rmc_initialize();
//_____GPRMC___________________________________
-
//データ文字列の先頭から6文字目までが$GPRMCなら０が返ってくる
- if (strncmp(line, "$GPRMC", 6) == 0){
+ //if (strncmp(line, "$GPRMC", 6) == 0){
tok = strtok(line, ",");
- char tok_count = 0;
+ tok_count = 0;
//$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A
@@ -127,38 +245,37 @@
// 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;
+ 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;
}
- 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;
@@ -191,17 +308,126 @@
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);//南緯なら緯度を負に
+ if (lonSign) longitude *= (-1);//西経なら経度を負に
+ if (latSign) latitude *= (-1);//南緯なら緯度を負に
knot = atof(speed);
degree = atof(angle);
- if(longitude > 129.0 && latitude > 29.0){
+ //if(longitude > 129.0 && latitude > 29.0){
return true;
- }
- else return false;
+ //}
+ //else return false;
}
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);//南緯なら緯度を負に
+
+ return true;
+ }
+ else return false;
+ //}
+
+}