2018/06/18
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?
User | Revision | Line number | New 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 | } |