PQ_Hybrid_Electrical_Equipment_Team / GPS_interrupt

2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム

Dependents:   Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more

GPS_interrupt.cpp@0:74d8e952a3bd, 2017-01-02 (annotated)

Committer:
Gaku0606
Date:
Mon Jan 02 00:40:46 2017 +0000
Revision:
0:74d8e952a3bd
Child:
2:7be89bab6db9
GPS_interrupt.lib ver1.0.3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:74d8e952a3bd 1 #include "mbed.h"
Gaku0606 0:74d8e952a3bd 2 #include "GPS_interrupt.h"
Gaku0606 0:74d8e952a3bd 3 double GPS_interrupt::latitude;
Gaku0606 0:74d8e952a3bd 4 double GPS_interrupt::longitude;
Gaku0606 0:74d8e952a3bd 5 int GPS_interrupt::year;
Gaku0606 0:74d8e952a3bd 6 int GPS_interrupt::month;
Gaku0606 0:74d8e952a3bd 7 int GPS_interrupt::day;
Gaku0606 0:74d8e952a3bd 8 int GPS_interrupt::hour;
Gaku0606 0:74d8e952a3bd 9 int GPS_interrupt::minutes;
Gaku0606 0:74d8e952a3bd 10 double GPS_interrupt::seconds;
Gaku0606 0:74d8e952a3bd 11 double GPS_interrupt::degree = 0;
Gaku0606 0:74d8e952a3bd 12 double GPS_interrupt::knot = 0;
Gaku0606 0:74d8e952a3bd 13 double GPS_interrupt::height = 0;
Gaku0606 0:74d8e952a3bd 14 double GPS_interrupt::geoid = 0;
Gaku0606 0:74d8e952a3bd 15 int GPS_interrupt::number = 0;
Gaku0606 0:74d8e952a3bd 16
Gaku0606 0:74d8e952a3bd 17 char GPS_interrupt::gps_buffer_A[128];
Gaku0606 0:74d8e952a3bd 18 char GPS_interrupt::gps_buffer_B[128];
Gaku0606 0:74d8e952a3bd 19 char* GPS_interrupt::gps_read_buffer = gps_buffer_B;
Gaku0606 0:74d8e952a3bd 20 bool GPS_interrupt::gps_readable = false;
Gaku0606 0:74d8e952a3bd 21
Gaku0606 0:74d8e952a3bd 22 GPS_interrupt* GPS_interrupt::gps_irq;
Gaku0606 0:74d8e952a3bd 23
Gaku0606 0:74d8e952a3bd 24 GPS_interrupt::GPS_interrupt(RawSerial *_gps, int baudrate){
Gaku0606 0:74d8e952a3bd 25 gps_irq = this;
Gaku0606 0:74d8e952a3bd 26 gps = _gps;
Gaku0606 0:74d8e952a3bd 27 gps->baud(baudrate);
Gaku0606 0:74d8e952a3bd 28 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 0:74d8e952a3bd 29 }
Gaku0606 0:74d8e952a3bd 30
Gaku0606 0:74d8e952a3bd 31 void GPS_interrupt::initialize(){
Gaku0606 0:74d8e952a3bd 32 latitude = 0.0f;
Gaku0606 0:74d8e952a3bd 33 longitude = 0.0f;
Gaku0606 0:74d8e952a3bd 34 year = 0;
Gaku0606 0:74d8e952a3bd 35 month = 0;
Gaku0606 0:74d8e952a3bd 36 day = 0;
Gaku0606 0:74d8e952a3bd 37 hour = 0;
Gaku0606 0:74d8e952a3bd 38 minutes = 0;
Gaku0606 0:74d8e952a3bd 39 seconds = 0;
Gaku0606 0:74d8e952a3bd 40 memset(gps_buffer_A, '\0', 128);
Gaku0606 0:74d8e952a3bd 41 memset(gps_buffer_B, '\0', 128);
Gaku0606 0:74d8e952a3bd 42 gps_readable = false;
Gaku0606 0:74d8e952a3bd 43 }
Gaku0606 0:74d8e952a3bd 44 void GPS_interrupt::gps_auto_receive(){
Gaku0606 0:74d8e952a3bd 45
Gaku0606 0:74d8e952a3bd 46 static char str_temp[128] = {'\0'};
Gaku0606 0:74d8e952a3bd 47 static char temp = 0;
Gaku0606 0:74d8e952a3bd 48 static bool start_flag = false;
Gaku0606 0:74d8e952a3bd 49 static unsigned char current = 0;
Gaku0606 0:74d8e952a3bd 50 static char classify_AB = 0;
Gaku0606 0:74d8e952a3bd 51 static char* save_buffer = gps_buffer_A;
Gaku0606 0:74d8e952a3bd 52
Gaku0606 0:74d8e952a3bd 53 temp = gps->getc();
Gaku0606 0:74d8e952a3bd 54 if(temp == '$'){
Gaku0606 0:74d8e952a3bd 55 current = 1;
Gaku0606 0:74d8e952a3bd 56 start_flag = true;
Gaku0606 0:74d8e952a3bd 57 memset(save_buffer, '\0', 128);//初期化
Gaku0606 0:74d8e952a3bd 58 save_buffer[0] = '$';
Gaku0606 0:74d8e952a3bd 59 return;
Gaku0606 0:74d8e952a3bd 60 }
Gaku0606 0:74d8e952a3bd 61 if(start_flag){//1行スタート
Gaku0606 0:74d8e952a3bd 62 if(temp == '\r'){//1行終了
Gaku0606 0:74d8e952a3bd 63 save_buffer[current] = '\0';
Gaku0606 0:74d8e952a3bd 64 start_flag = false;
Gaku0606 0:74d8e952a3bd 65 current = 1;
Gaku0606 0:74d8e952a3bd 66 gps_read_buffer = save_buffer;
Gaku0606 0:74d8e952a3bd 67 if(classify_AB){
Gaku0606 0:74d8e952a3bd 68 save_buffer = gps_buffer_A;//バッファ切換
Gaku0606 0:74d8e952a3bd 69 classify_AB = ~classify_AB;
Gaku0606 0:74d8e952a3bd 70 }
Gaku0606 0:74d8e952a3bd 71 else{
Gaku0606 0:74d8e952a3bd 72 save_buffer = gps_buffer_B;
Gaku0606 0:74d8e952a3bd 73 classify_AB = ~classify_AB;
Gaku0606 0:74d8e952a3bd 74 }
Gaku0606 0:74d8e952a3bd 75 if(strncmp(gps_read_buffer, "$GPRMC", 6) == 0){
Gaku0606 0:74d8e952a3bd 76 strcpy(str_temp, gps_read_buffer);
Gaku0606 0:74d8e952a3bd 77 if(processGPS(str_temp)){
Gaku0606 0:74d8e952a3bd 78 gps_readable = true;
Gaku0606 0:74d8e952a3bd 79 }
Gaku0606 0:74d8e952a3bd 80 }
Gaku0606 0:74d8e952a3bd 81 }
Gaku0606 0:74d8e952a3bd 82 else{
Gaku0606 0:74d8e952a3bd 83 save_buffer[current] = temp;
Gaku0606 0:74d8e952a3bd 84 current++;
Gaku0606 0:74d8e952a3bd 85 if(current >= 127){
Gaku0606 0:74d8e952a3bd 86 current = 1;
Gaku0606 0:74d8e952a3bd 87 start_flag = false;
Gaku0606 0:74d8e952a3bd 88 }
Gaku0606 0:74d8e952a3bd 89 }
Gaku0606 0:74d8e952a3bd 90 }
Gaku0606 0:74d8e952a3bd 91
Gaku0606 0:74d8e952a3bd 92 }
Gaku0606 0:74d8e952a3bd 93
Gaku0606 0:74d8e952a3bd 94 bool GPS_interrupt::processGPS(char *line){
Gaku0606 0:74d8e952a3bd 95
Gaku0606 0:74d8e952a3bd 96 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 0:74d8e952a3bd 97 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 0:74d8e952a3bd 98 //bool result = false;
Gaku0606 0:74d8e952a3bd 99 //char num[4] = "";//衛星数の文字データ
Gaku0606 0:74d8e952a3bd 100 //char kaibatsu[8], g_height[8];
Gaku0606 0:74d8e952a3bd 101 //char data_GPS[256] ="";
Gaku0606 0:74d8e952a3bd 102 char angle[8], speed[8];
Gaku0606 0:74d8e952a3bd 103 char ido[16] = "", keido[16] = "";
Gaku0606 0:74d8e952a3bd 104
Gaku0606 0:74d8e952a3bd 105 double X = 0, Y = 0;
Gaku0606 0:74d8e952a3bd 106 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 0:74d8e952a3bd 107 char zikann[11], hizuke[7];
Gaku0606 0:74d8e952a3bd 108
Gaku0606 0:74d8e952a3bd 109 //bool gga_flag = false;
Gaku0606 0:74d8e952a3bd 110 bool rmc_flag = false;
Gaku0606 0:74d8e952a3bd 111
Gaku0606 0:74d8e952a3bd 112 initialize();
Gaku0606 0:74d8e952a3bd 113
Gaku0606 0:74d8e952a3bd 114 //_____GPRMC___________________________________
Gaku0606 0:74d8e952a3bd 115
Gaku0606 0:74d8e952a3bd 116 //データ文字列の先頭から6文字目までが$GPRMCなら０が返ってくる
Gaku0606 0:74d8e952a3bd 117 if (strncmp(line, "$GPRMC", 6) == 0){
Gaku0606 0:74d8e952a3bd 118
Gaku0606 0:74d8e952a3bd 119 tok = strtok(line, ",");
Gaku0606 0:74d8e952a3bd 120 char tok_count = 0;
Gaku0606 0:74d8e952a3bd 121
Gaku0606 0:74d8e952a3bd 122 //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A
Gaku0606 0:74d8e952a3bd 123
Gaku0606 0:74d8e952a3bd 124 // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12
Gaku0606 0:74d8e952a3bd 125 //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C
Gaku0606 0:74d8e952a3bd 126
Gaku0606 0:74d8e952a3bd 127 // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1
Gaku0606 0:74d8e952a3bd 128 // 0 1 2
Gaku0606 0:74d8e952a3bd 129 while (1){
Gaku0606 0:74d8e952a3bd 130
Gaku0606 0:74d8e952a3bd 131 //___UTC時間_____________
Gaku0606 0:74d8e952a3bd 132 if (tok_count == 1){
Gaku0606 0:74d8e952a3bd 133 strcpy(zikann, tok);//zikann = "233514.000\0"
Gaku0606 0:74d8e952a3bd 134 }
Gaku0606 0:74d8e952a3bd 135 //___確認,経度___________
Gaku0606 0:74d8e952a3bd 136 else if (tok_count == 2){
Gaku0606 0:74d8e952a3bd 137 if (strncmp(tok, "V", 1) == 0) return false;
Gaku0606 0:74d8e952a3bd 138 else if (strcmp(tok, "A") == 0) rmc_flag = true;
Gaku0606 0:74d8e952a3bd 139 }
Gaku0606 0:74d8e952a3bd 140 else if (tok_count == 3){//緯度
Gaku0606 0:74d8e952a3bd 141 strcpy(ido, tok);
Gaku0606 0:74d8e952a3bd 142 }
Gaku0606 0:74d8e952a3bd 143 else if (tok_count == 4){//北緯か南緯か
Gaku0606 0:74d8e952a3bd 144 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 0:74d8e952a3bd 145 }
Gaku0606 0:74d8e952a3bd 146 else if (tok_count == 5){//経度
Gaku0606 0:74d8e952a3bd 147 strcpy(keido, tok);
Gaku0606 0:74d8e952a3bd 148 }
Gaku0606 0:74d8e952a3bd 149 else if (tok_count == 6){//東経か西経か
Gaku0606 0:74d8e952a3bd 150 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 0:74d8e952a3bd 151 }
Gaku0606 0:74d8e952a3bd 152 else if (tok_count == 7){//速度（キロノット）
Gaku0606 0:74d8e952a3bd 153 strcpy(speed, tok);
Gaku0606 0:74d8e952a3bd 154 }
Gaku0606 0:74d8e952a3bd 155 else if (tok_count == 8){//角度
Gaku0606 0:74d8e952a3bd 156 strcpy(angle, tok);
Gaku0606 0:74d8e952a3bd 157 }
Gaku0606 0:74d8e952a3bd 158 //___日付________________
Gaku0606 0:74d8e952a3bd 159 else if (tok_count == 9){
Gaku0606 0:74d8e952a3bd 160 strcpy(hizuke, tok);//hizuke = "030316\0"
Gaku0606 0:74d8e952a3bd 161 }
Gaku0606 0:74d8e952a3bd 162 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 0:74d8e952a3bd 163 tok_count++;
Gaku0606 0:74d8e952a3bd 164 if (tok == NULL) break;
Gaku0606 0:74d8e952a3bd 165 }
Gaku0606 0:74d8e952a3bd 166 if (rmc_flag){
Gaku0606 0:74d8e952a3bd 167 rmc_flag = false;
Gaku0606 0:74d8e952a3bd 168 //ddmmyy
Gaku0606 0:74d8e952a3bd 169 int dmy = 0;
Gaku0606 0:74d8e952a3bd 170 dmy = atoi(hizuke);
Gaku0606 0:74d8e952a3bd 171 day = dmy / 10000; //030316 / 10000 = 3
Gaku0606 0:74d8e952a3bd 172 month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3
Gaku0606 0:74d8e952a3bd 173 year = dmy - month * 100 - day * 10000 + 2000;
Gaku0606 0:74d8e952a3bd 174
Gaku0606 0:74d8e952a3bd 175 //hhmmss.ss
Gaku0606 0:74d8e952a3bd 176 seconds = atof(zikann);//sec = 233514.000
Gaku0606 0:74d8e952a3bd 177 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 0:74d8e952a3bd 178 minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 0:74d8e952a3bd 179 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 0:74d8e952a3bd 180
Gaku0606 0:74d8e952a3bd 181 // getLonLatのとき有効
Gaku0606 0:74d8e952a3bd 182 Y_m = atof(ido);//*Y_m = 3457.5571
Gaku0606 0:74d8e952a3bd 183 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 0:74d8e952a3bd 184 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 0:74d8e952a3bd 185
Gaku0606 0:74d8e952a3bd 186 X_m = atof(keido);//*X = 13057.6142
Gaku0606 0:74d8e952a3bd 187 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 0:74d8e952a3bd 188 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 0:74d8e952a3bd 189
Gaku0606 0:74d8e952a3bd 190 //GPS calculation
Gaku0606 0:74d8e952a3bd 191 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 0:74d8e952a3bd 192 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 0:74d8e952a3bd 193
Gaku0606 0:74d8e952a3bd 194 if (lonSign == true) longitude *= (-1);//西経なら経度を負に
Gaku0606 0:74d8e952a3bd 195 if (latSign == true) latitude *= (-1);//南緯なら緯度を負に
Gaku0606 0:74d8e952a3bd 196 knot = atof(speed);
Gaku0606 0:74d8e952a3bd 197 degree = atof(angle);
Gaku0606 0:74d8e952a3bd 198 if(longitude > 129.0 && latitude > 29.0){
Gaku0606 0:74d8e952a3bd 199 return true;
Gaku0606 0:74d8e952a3bd 200 }
Gaku0606 0:74d8e952a3bd 201 else return false;
Gaku0606 0:74d8e952a3bd 202 }
Gaku0606 0:74d8e952a3bd 203 else return false;
Gaku0606 0:74d8e952a3bd 204 }
Gaku0606 0:74d8e952a3bd 205 else return false;
Gaku0606 0:74d8e952a3bd 206 }
Gaku0606 0:74d8e952a3bd 207