Tomo Suzu / GPS

2018/06/18

Dependents:   library

GPS.cpp@0:2c0d50143d85, 2018-05-03 (annotated)

Committer:
Tomo073
Date:
Thu May 03 14:19:36 2018 +0000
Revision:
0:2c0d50143d85
Child:
1:2f131675f872
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tomo073 0:2c0d50143d85 1 #include "mbed.h"
Tomo073 0:2c0d50143d85 2 #include "GPS.h"
Tomo073 0:2c0d50143d85 3 #include "math.h"
Tomo073 0:2c0d50143d85 4 #include "stdio.h"
Tomo073 0:2c0d50143d85 5
Tomo073 0:2c0d50143d85 6 GPS::GPS(PinName gpstx,PinName gpsrx): serial(gpstx,gpsrx){
Tomo073 0:2c0d50143d85 7
Tomo073 0:2c0d50143d85 8 gpscmd("314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
Tomo073 0:2c0d50143d85 9 int odr=ODR;
Tomo073 0:2c0d50143d85 10 odr=1000/odr;
Tomo073 0:2c0d50143d85 11 sprintf(buf,"300,%d,0,0,0,0",odr);
Tomo073 0:2c0d50143d85 12 gpscmd(buf);
Tomo073 0:2c0d50143d85 13
Tomo073 0:2c0d50143d85 14 int baud=GPSBAUD;
Tomo073 0:2c0d50143d85 15
Tomo073 0:2c0d50143d85 16 for(int i=0;i<100;i++){
Tomo073 0:2c0d50143d85 17 buf[i]=0;
Tomo073 0:2c0d50143d85 18 }
Tomo073 0:2c0d50143d85 19
Tomo073 0:2c0d50143d85 20 sprintf(buf,"251,%d",baud);
Tomo073 0:2c0d50143d85 21
Tomo073 0:2c0d50143d85 22 gpscmd(buf);
Tomo073 0:2c0d50143d85 23 gpscmd(buf);
Tomo073 0:2c0d50143d85 24
Tomo073 0:2c0d50143d85 25
Tomo073 0:2c0d50143d85 26 serial.baud(GPSBAUD);
Tomo073 0:2c0d50143d85 27
Tomo073 0:2c0d50143d85 28 NVIC_SetPriority(UART2_IRQn,0);
Tomo073 0:2c0d50143d85 29
Tomo073 0:2c0d50143d85 30 //m = 1;
Tomo073 0:2c0d50143d85 31 //reasonable = false;
Tomo073 0:2c0d50143d85 32
Tomo073 0:2c0d50143d85 33 //serial.attach(this,ExtractionData,Serial::RxIrq);
Tomo073 0:2c0d50143d85 34
Tomo073 0:2c0d50143d85 35 }
Tomo073 0:2c0d50143d85 36
Tomo073 0:2c0d50143d85 37 void GPS::gpscmd(char *command)
Tomo073 0:2c0d50143d85 38 {
Tomo073 0:2c0d50143d85 39 int len=strlen(command);
Tomo073 0:2c0d50143d85 40 char xordata[len+4];
Tomo073 0:2c0d50143d85 41 sprintf(xordata,"PMTK%s",command);
Tomo073 0:2c0d50143d85 42
Tomo073 0:2c0d50143d85 43
Tomo073 0:2c0d50143d85 44 char xorbuf=0x00;
Tomo073 0:2c0d50143d85 45 for(int i=0;i<len+4;i++){
Tomo073 0:2c0d50143d85 46 xorbuf=xorbuf^xordata[i];
Tomo073 0:2c0d50143d85 47 }
Tomo073 0:2c0d50143d85 48
Tomo073 0:2c0d50143d85 49
Tomo073 0:2c0d50143d85 50 char packet[len+8];
Tomo073 0:2c0d50143d85 51 if(xorbuf>0x0f){
Tomo073 0:2c0d50143d85 52 sprintf(packet,"$%s*%x",xordata,xorbuf);
Tomo073 0:2c0d50143d85 53 }
Tomo073 0:2c0d50143d85 54 else {
Tomo073 0:2c0d50143d85 55 sprintf(packet,"$%s*0%x",xordata,xorbuf);
Tomo073 0:2c0d50143d85 56 }
Tomo073 0:2c0d50143d85 57
Tomo073 0:2c0d50143d85 58
Tomo073 0:2c0d50143d85 59 serial.printf(packet);
Tomo073 0:2c0d50143d85 60 serial.putc(0x0D);
Tomo073 0:2c0d50143d85 61 serial.putc(0x0A);
Tomo073 0:2c0d50143d85 62 }
Tomo073 0:2c0d50143d85 63
Tomo073 0:2c0d50143d85 64 void GPS::attach(void(*fn)()){
Tomo073 0:2c0d50143d85 65 serial.attach(fn, Serial::RxIrq);
Tomo073 0:2c0d50143d85 66 }
Tomo073 0:2c0d50143d85 67
Tomo073 0:2c0d50143d85 68 void GPS::GetDefault(){
Tomo073 0:2c0d50143d85 69 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 70 }
Tomo073 0:2c0d50143d85 71
Tomo073 0:2c0d50143d85 72 void GPS::ExtractionData(){
Tomo073 0:2c0d50143d85 73
Tomo073 0:2c0d50143d85 74 Initialization();
Tomo073 0:2c0d50143d85 75
Tomo073 0:2c0d50143d85 76 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 77
Tomo073 0:2c0d50143d85 78 if(datadefault == '$'){
Tomo073 0:2c0d50143d85 79
Tomo073 0:2c0d50143d85 80 char DATA[20] = "\0";
Tomo073 0:2c0d50143d85 81 Initialization();
Tomo073 0:2c0d50143d85 82 getstring(DATA);
Tomo073 0:2c0d50143d85 83
Tomo073 0:2c0d50143d85 84 if(!strcmp(DATA, "GPGGA")){
Tomo073 0:2c0d50143d85 85 //int m;
Tomo073 0:2c0d50143d85 86 for(m = 1; m < 6;m++){
Tomo073 0:2c0d50143d85 87 getstring(DATA);
Tomo073 0:2c0d50143d85 88 switch(m){
Tomo073 0:2c0d50143d85 89 case 1:
Tomo073 0:2c0d50143d85 90 time = strtof(DATA, NULL) + 90000.0;
Tomo073 0:2c0d50143d85 91 break;
Tomo073 0:2c0d50143d85 92 case 2:
Tomo073 0:2c0d50143d85 93 //latitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 94 fMinutes = modff(strtof(DATA, NULL), &latitude)*60.0;
Tomo073 0:2c0d50143d85 95 latitude = latitude*100.0 + fMinutes;
Tomo073 0:2c0d50143d85 96 break;
Tomo073 0:2c0d50143d85 97 case 3:
Tomo073 0:2c0d50143d85 98 if(!strcmp(DATA, "S")){
Tomo073 0:2c0d50143d85 99 latitude = latitude*-1.0;
Tomo073 0:2c0d50143d85 100 }
Tomo073 0:2c0d50143d85 101 break;
Tomo073 0:2c0d50143d85 102 case 4:
Tomo073 0:2c0d50143d85 103 //longtitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 104 fMinutes = modff(strtof(DATA, NULL), &longtitude)*60.0;
Tomo073 0:2c0d50143d85 105 longtitude = longtitude*100.0 + fMinutes;
Tomo073 0:2c0d50143d85 106 break;
Tomo073 0:2c0d50143d85 107 case 5:
Tomo073 0:2c0d50143d85 108 if(!strcmp(DATA, "W")){
Tomo073 0:2c0d50143d85 109 longtitude = longtitude*-1.0;
Tomo073 0:2c0d50143d85 110 }
Tomo073 0:2c0d50143d85 111 break;
Tomo073 0:2c0d50143d85 112 case 9:
Tomo073 0:2c0d50143d85 113 altitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 114 break;
Tomo073 0:2c0d50143d85 115 case 10:
Tomo073 0:2c0d50143d85 116 if(strcmp(DATA, "M")){
Tomo073 0:2c0d50143d85 117 altitude = altitude*-1.0;
Tomo073 0:2c0d50143d85 118 }
Tomo073 0:2c0d50143d85 119 //m = 1;
Tomo073 0:2c0d50143d85 120 readable = true;
Tomo073 0:2c0d50143d85 121 break;
Tomo073 0:2c0d50143d85 122 }
Tomo073 0:2c0d50143d85 123 }
Tomo073 0:2c0d50143d85 124 }
Tomo073 0:2c0d50143d85 125 }
Tomo073 0:2c0d50143d85 126 }
Tomo073 0:2c0d50143d85 127
Tomo073 0:2c0d50143d85 128 void GPS::ExtractionData_FULL(){
Tomo073 0:2c0d50143d85 129
Tomo073 0:2c0d50143d85 130 char DATA[20] = "\0";
Tomo073 0:2c0d50143d85 131
Tomo073 0:2c0d50143d85 132 Initialization();
Tomo073 0:2c0d50143d85 133 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 134
Tomo073 0:2c0d50143d85 135 if(datadefault == '$'){
Tomo073 0:2c0d50143d85 136 getstring(DATA);
Tomo073 0:2c0d50143d85 137 if(!strcmp(DATA, "GPGGA")){
Tomo073 0:2c0d50143d85 138 reasonable = true;
Tomo073 0:2c0d50143d85 139 }
Tomo073 0:2c0d50143d85 140 }
Tomo073 0:2c0d50143d85 141
Tomo073 0:2c0d50143d85 142 if(reasonable){
Tomo073 0:2c0d50143d85 143
Tomo073 0:2c0d50143d85 144 Initialization();
Tomo073 0:2c0d50143d85 145 getstring(DATA);
Tomo073 0:2c0d50143d85 146
Tomo073 0:2c0d50143d85 147 switch(m){
Tomo073 0:2c0d50143d85 148 case 1:
Tomo073 0:2c0d50143d85 149 Seconds = modff((strtof(DATA, NULL) + 90000.0)/100.0, &fHours)*100;
Tomo073 0:2c0d50143d85 150 fMinutes = modff(fHours/100.0, &fHours)*100;
Tomo073 0:2c0d50143d85 151 Hours = fHours;
Tomo073 0:2c0d50143d85 152 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 153 sprintf(timeFULL, "%d:%d:%f", Hours, Minutes, Seconds);
Tomo073 0:2c0d50143d85 154 break;
Tomo073 0:2c0d50143d85 155 case 2:
Tomo073 0:2c0d50143d85 156 fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0;
Tomo073 0:2c0d50143d85 157 Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0;
Tomo073 0:2c0d50143d85 158 Degrees = fDegrees;
Tomo073 0:2c0d50143d85 159 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 160 sprintf(latitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds);
Tomo073 0:2c0d50143d85 161 break;
Tomo073 0:2c0d50143d85 162 case 3:
Tomo073 0:2c0d50143d85 163 sprintf(latitudeFULL, "%s%s", latitudeFULL, DATA);
Tomo073 0:2c0d50143d85 164 break;
Tomo073 0:2c0d50143d85 165 case 4:
Tomo073 0:2c0d50143d85 166 fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0;
Tomo073 0:2c0d50143d85 167 Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0;
Tomo073 0:2c0d50143d85 168 Degrees = fDegrees;
Tomo073 0:2c0d50143d85 169 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 170 sprintf(longtitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds);
Tomo073 0:2c0d50143d85 171 break;
Tomo073 0:2c0d50143d85 172 case 5:
Tomo073 0:2c0d50143d85 173 sprintf(longtitudeFULL, "%s%s", longtitudeFULL, DATA);
Tomo073 0:2c0d50143d85 174 break;
Tomo073 0:2c0d50143d85 175 case 9:
Tomo073 0:2c0d50143d85 176 strcpy(altitudeFULL, DATA);
Tomo073 0:2c0d50143d85 177 break;
Tomo073 0:2c0d50143d85 178 case 10:
Tomo073 0:2c0d50143d85 179 sprintf(altitudeFULL, "%s%s", altitudeFULL, DATA);
Tomo073 0:2c0d50143d85 180 m = 1;
Tomo073 0:2c0d50143d85 181 reasonable = false;
Tomo073 0:2c0d50143d85 182 readable = true;
Tomo073 0:2c0d50143d85 183 break;
Tomo073 0:2c0d50143d85 184 }
Tomo073 0:2c0d50143d85 185
Tomo073 0:2c0d50143d85 186 m++;
Tomo073 0:2c0d50143d85 187 //sizeof
Tomo073 0:2c0d50143d85 188 }
Tomo073 0:2c0d50143d85 189 }
Tomo073 0:2c0d50143d85 190
Tomo073 0:2c0d50143d85 191 void GPS::getstring(char a[]){
Tomo073 0:2c0d50143d85 192 int n;
Tomo073 0:2c0d50143d85 193 for(n = 0;;n++){
Tomo073 0:2c0d50143d85 194 char ch = serial.getc();
Tomo073 0:2c0d50143d85 195 if(ch == ',' ||ch == '\n'){
Tomo073 0:2c0d50143d85 196 a[n] = '\0';
Tomo073 0:2c0d50143d85 197 break;
Tomo073 0:2c0d50143d85 198 }
Tomo073 0:2c0d50143d85 199 a[n] = ch;
Tomo073 0:2c0d50143d85 200 }
Tomo073 0:2c0d50143d85 201 }
Tomo073 0:2c0d50143d85 202
Tomo073 0:2c0d50143d85 203 void GPS::Initialization(){
Tomo073 0:2c0d50143d85 204 readable = false;
Tomo073 0:2c0d50143d85 205 Degrees = -1;
Tomo073 0:2c0d50143d85 206 Minutes = -1;
Tomo073 0:2c0d50143d85 207 Hours = -1;
Tomo073 0:2c0d50143d85 208 longtitude = -1.0;
Tomo073 0:2c0d50143d85 209 latitude = -1.0;
Tomo073 0:2c0d50143d85 210 altitude = -1.0;
Tomo073 0:2c0d50143d85 211 time = -1.0;
Tomo073 0:2c0d50143d85 212 fDegrees = -1.0;
Tomo073 0:2c0d50143d85 213 fMinutes = -1.0;
Tomo073 0:2c0d50143d85 214 Seconds = -1.0;
Tomo073 0:2c0d50143d85 215 fHours = -1.0;
Tomo073 0:2c0d50143d85 216 longtitudeFULL = "\0";
Tomo073 0:2c0d50143d85 217 latitudeFULL = "\0";
Tomo073 0:2c0d50143d85 218 timeFULL = "\0";
Tomo073 0:2c0d50143d85 219 altitudeFULL = "\0";
Tomo073 0:2c0d50143d85 220 }