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:
- 15:6b1ed321c1be
- Parent:
- 14:23611bb30bc8
- Child:
- 17:4b09630703c0
--- a/GPS_interrupt.cpp Sun Feb 25 19:29:26 2018 +0000
+++ b/GPS_interrupt.cpp Fri May 25 21:29:17 2018 +0000
@@ -43,6 +43,7 @@
}
return val;
}
+
GPS_interrupt::GPS_interrupt(Serial *_gps){
if(_gps == NULL){
@@ -50,14 +51,15 @@
}
debugFlag = false;
initialize();
- baudrate = 9600;//_baudrate;
- frequency = 1;
+ //baudrate = 9600;//_baudrate;
+ //frequency = 1;
gps_irq = this;
gps = _gps;
- gps->baud(baudrate);
+ //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){
@@ -70,7 +72,8 @@
gps_irq = this;
gps = _gps;
gps->baud(baudrate);
- /*if(baudrate == 9600){
+
+ if(baudrate == 9600){
gps->printf("$PMTK251,9600*17\r\n");
}
else if(baudrate == 19200){
@@ -113,8 +116,243 @@
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:
+ 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:
+ gps->printf("$PMTK220,250*29\r\n");
+ 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;
@@ -244,6 +482,11 @@
//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;
@@ -486,9 +729,68 @@
else{
return false;
}
- 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;//相対経度