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