Ok
Dependencies: mbed_rtos_types Mutex mbed_rtos_storage mbed Semaphore
Mbed_Adafruit_GPS.cpp
00001 #include "MBed_Adafruit_GPS.h" 00002 00003 // how long are max NMEA lines to parse? 00004 #define MAXLINELENGTH 120 00005 00006 // we double buffer: read one line in and leave one for the main program 00007 volatile char line1[MAXLINELENGTH]; 00008 volatile char line2[MAXLINELENGTH]; 00009 // our index into filling the current line 00010 volatile uint16_t lineidx=0; 00011 // pointers to the double buffers 00012 volatile char *currentline; 00013 volatile char *lastline; 00014 volatile bool recvdflag; 00015 volatile bool inStandbyMode; 00016 00017 00018 bool Adafruit_GPS::parse(char *nmea) { 00019 // do checksum check 00020 00021 // first look if we even have one 00022 if (nmea[strlen(nmea)-4] == '*') { 00023 uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16; 00024 sum += parseHex(nmea[strlen(nmea)-2]); 00025 00026 // check checksum 00027 for (uint8_t i=1; i < (strlen(nmea)-4); i++) { 00028 sum ^= nmea[i]; 00029 } 00030 if (sum != 0) { 00031 // bad checksum :( 00032 //return false; 00033 } 00034 } 00035 00036 // look for a few common sentences 00037 if (strstr(nmea, "$GPGGA")) { 00038 // found GGA 00039 char *p = nmea; 00040 // get time 00041 p = strchr(p, ',')+1; 00042 float timef = atof(p); 00043 uint32_t time = timef; 00044 hour = (time / 10000)+1; //Adjust timezone to Madrid (UTC+1) 00045 if (hour==24){hour=0;} 00046 minute = (time % 10000) / 100; 00047 seconds = (time % 100); 00048 00049 milliseconds = fmod((double) timef, 1.0) * 1000; 00050 00051 // parse out latitude 00052 p = strchr(p, ',')+1; 00053 latitude = atof(p); 00054 00055 p = strchr(p, ',')+1; 00056 if (p[0] == 'N') lat = 'N'; 00057 else if (p[0] == 'S') lat = 'S'; 00058 else if (p[0] == ',') lat = 0; 00059 else return false; 00060 00061 // parse out longitude 00062 p = strchr(p, ',')+1; 00063 longitude = atof(p); 00064 00065 p = strchr(p, ',')+1; 00066 if (p[0] == 'W') lon = 'W'; 00067 else if (p[0] == 'E') lon = 'E'; 00068 else if (p[0] == ',') lon = 0; 00069 else return false; 00070 00071 p = strchr(p, ',')+1; 00072 fixquality = atoi(p); 00073 00074 p = strchr(p, ',')+1; 00075 satellites = atoi(p); 00076 00077 p = strchr(p, ',')+1; 00078 HDOP = atof(p); 00079 00080 p = strchr(p, ',')+1; 00081 altitude = atof(p); 00082 p = strchr(p, ',')+1; 00083 p = strchr(p, ',')+1; 00084 geoidheight = atof(p); 00085 return true; 00086 } 00087 if (strstr(nmea, "$GPRMC")) { 00088 // found RMC 00089 char *p = nmea; 00090 00091 // get time 00092 p = strchr(p, ',')+1; 00093 float timef = atof(p); 00094 uint32_t time = timef; 00095 hour = (time / 10000)+1; //Adjust timezone to Madrid (UTC+1) 00096 if (hour==24){hour=0;} 00097 minute = (time % 10000) / 100; 00098 seconds = (time % 100); 00099 00100 milliseconds = fmod((double) timef, 1.0) * 1000; 00101 00102 p = strchr(p, ',')+1; 00103 // Serial.println(p); 00104 if (p[0] == 'A') 00105 fix = true; 00106 else if (p[0] == 'V') 00107 fix = false; 00108 else 00109 return false; 00110 00111 // parse out latitude 00112 p = strchr(p, ',')+1; 00113 latitude = atof(p); 00114 00115 p = strchr(p, ',')+1; 00116 if (p[0] == 'N') lat = 'N'; 00117 else if (p[0] == 'S') lat = 'S'; 00118 else if (p[0] == ',') lat = 0; 00119 else return false; 00120 00121 // parse out longitude 00122 p = strchr(p, ',')+1; 00123 longitude = atof(p); 00124 00125 p = strchr(p, ',')+1; 00126 if (p[0] == 'W') lon = 'W'; 00127 else if (p[0] == 'E') lon = 'E'; 00128 else if (p[0] == ',') lon = 0; 00129 else return false; 00130 00131 // speed 00132 p = strchr(p, ',')+1; 00133 speed = atof(p); 00134 00135 // angle 00136 p = strchr(p, ',')+1; 00137 angle = atof(p); 00138 00139 p = strchr(p, ',')+1; 00140 uint32_t fulldate = atof(p); 00141 day = fulldate / 10000; 00142 month = (fulldate % 10000) / 100; 00143 year = (fulldate % 100); 00144 00145 // we dont parse the remaining, yet! 00146 return true; 00147 } 00148 00149 return false; 00150 } 00151 00152 char Adafruit_GPS::read(void) { 00153 char c = 0; 00154 00155 if (paused) return c; 00156 00157 if(!gpsSerial->readable()) return c; 00158 c = gpsSerial->getc(); 00159 00160 //Serial.print(c); 00161 00162 if (c == '$') { 00163 currentline[lineidx] = 0; 00164 lineidx = 0; 00165 } 00166 if (c == '\n') { 00167 currentline[lineidx] = 0; 00168 00169 if (currentline == line1) { 00170 currentline = line2; 00171 lastline = line1; 00172 } else { 00173 currentline = line1; 00174 lastline = line2; 00175 } 00176 00177 lineidx = 0; 00178 recvdflag = true; 00179 } 00180 00181 currentline[lineidx++] = c; 00182 if (lineidx >= MAXLINELENGTH) 00183 lineidx = MAXLINELENGTH-1; 00184 00185 return c; 00186 } 00187 00188 Adafruit_GPS::Adafruit_GPS (Serial *ser) 00189 { 00190 common_init(); // Set everything to common state, then... 00191 gpsSerial = ser; // ...override gpsSwSerial with value passed. 00192 } 00193 00194 // Initialization code used by all constructor types 00195 void Adafruit_GPS::common_init(void) { 00196 gpsSerial = NULL; 00197 recvdflag = false; 00198 paused = false; 00199 lineidx = 0; 00200 currentline = line1; 00201 lastline = line2; 00202 00203 hour = minute = seconds = year = month = day = 00204 fixquality = satellites = 0; // uint8_t 00205 lat = lon = mag = 0; // char 00206 fix = false; // bool 00207 milliseconds = 0; // uint16_t 00208 latitude = longitude = geoidheight = altitude = 00209 speed = angle = magvariation = HDOP = 0.0; // float 00210 } 00211 00212 void Adafruit_GPS::begin(int baud) 00213 { 00214 gpsSerial->baud(baud); 00215 wait_ms(10); 00216 } 00217 00218 void Adafruit_GPS::sendCommand(char *str) { 00219 gpsSerial->printf("%s",str); 00220 } 00221 00222 bool Adafruit_GPS::newNMEAreceived(void) { 00223 return recvdflag; 00224 } 00225 00226 void Adafruit_GPS::pause(bool p) { 00227 paused = p; 00228 } 00229 00230 char *Adafruit_GPS::lastNMEA(void) { 00231 recvdflag = false; 00232 return (char *)lastline; 00233 } 00234 00235 // read a Hex value and return the decimal equivalent 00236 uint8_t Adafruit_GPS::parseHex(char c) { 00237 if (c < '0') 00238 return 0; 00239 if (c <= '9') 00240 return c - '0'; 00241 if (c < 'A') 00242 return 0; 00243 if (c <= 'F') 00244 return (c - 'A')+10; 00245 } 00246 00247 bool Adafruit_GPS::waitForSentence(char *wait4me, uint8_t max) { 00248 char str[20]; 00249 00250 uint8_t i=0; 00251 while (i < max) { 00252 if (newNMEAreceived()) { 00253 char *nmea = lastNMEA(); 00254 strncpy(str, nmea, 20); 00255 str[19] = 0; 00256 i++; 00257 00258 if (strstr(str, wait4me)) 00259 return true; 00260 } 00261 } 00262 00263 return false; 00264 } 00265 00266 bool Adafruit_GPS::LOCUS_StartLogger(void) { 00267 sendCommand(PMTK_LOCUS_STARTLOG); 00268 recvdflag = false; 00269 return waitForSentence(PMTK_LOCUS_LOGSTARTED); 00270 } 00271 00272 bool Adafruit_GPS::LOCUS_ReadStatus(void) { 00273 sendCommand(PMTK_LOCUS_QUERY_STATUS); 00274 00275 if (! waitForSentence("$PMTKLOG")) 00276 return false; 00277 00278 char *response = lastNMEA(); 00279 uint16_t parsed[10]; 00280 uint8_t i; 00281 00282 for (i=0; i<10; i++) parsed[i] = -1; 00283 00284 response = strchr(response, ','); 00285 for (i=0; i<10; i++) { 00286 if (!response || (response[0] == 0) || (response[0] == '*')) 00287 break; 00288 response++; 00289 parsed[i]=0; 00290 while ((response[0] != ',') && 00291 (response[0] != '*') && (response[0] != 0)) { 00292 parsed[i] *= 10; 00293 char c = response[0]; 00294 if (isdigit(c)) 00295 parsed[i] += c - '0'; 00296 else 00297 parsed[i] = c; 00298 response++; 00299 } 00300 } 00301 LOCUS_serial = parsed[0]; 00302 LOCUS_type = parsed[1]; 00303 if (isalpha(parsed[2])) { 00304 parsed[2] = parsed[2] - 'a' + 10; 00305 } 00306 LOCUS_mode = parsed[2]; 00307 LOCUS_config = parsed[3]; 00308 LOCUS_interval = parsed[4]; 00309 LOCUS_distance = parsed[5]; 00310 LOCUS_speed = parsed[6]; 00311 LOCUS_status = !parsed[7]; 00312 LOCUS_records = parsed[8]; 00313 LOCUS_percent = parsed[9]; 00314 00315 return true; 00316 } 00317 00318 // Standby Mode Switches 00319 bool Adafruit_GPS::standby(void) { 00320 if (inStandbyMode) { 00321 return false; // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS 00322 } 00323 else { 00324 inStandbyMode = true; 00325 sendCommand(PMTK_STANDBY); 00326 //return waitForSentence(PMTK_STANDBY_SUCCESS); // don't seem to be fast enough to catch the message, or something else just is not working 00327 return true; 00328 } 00329 } 00330 00331 bool Adafruit_GPS::wakeup(void) { 00332 if (inStandbyMode) { 00333 inStandbyMode = false; 00334 sendCommand(""); // send byte to wake it up 00335 return waitForSentence(PMTK_AWAKE); 00336 } 00337 else { 00338 return false; // Returns false if not in standby mode, nothing to wakeup 00339 } 00340 } 00341 00342
Generated on Tue Jul 12 2022 22:06:19 by
1.7.2