Diff: GPS.cpp

Revision:

1:35fcaa2209af

Parent:

0:15611c7938a3

Child:

2:509abe8eda59

--- a/GPS.cpp	Tue Jun 08 14:10:27 2010 +0000
+++ b/GPS.cpp	Tue Jun 16 12:03:20 2015 +0000
@@ -21,42 +21,42 @@
  */
  
 #include "GPS.h"
+#include "main.h"
+// how long are max NMEA lines to parse?
+#define MAXLINELENGTH 120
+// we double buffer: read one line in and leave one for the main program
+volatile char line1[MAXLINELENGTH];
+volatile char line2[MAXLINELENGTH];
+// our index into filling the current line
+volatile uint16_t lineidx=0;
+// pointers to the double buffers
+volatile char *currentline;
+volatile char *lastline;
+volatile bool recvdflag;
+volatile bool inStandbyMode;
 
 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
-    _gps.baud(4800);    
-    longitude = 0.0;
-    latitude = 0.0;        
+    _gps.baud(9600);    
+    recvdflag   = false;
+    paused      = false;
+    lineidx     = 0;
+    currentline = line1;
+    lastline    = line2;
 }
 
-int GPS::sample() {
-    float time;
-    char ns, ew;
-    int lock;
-
-    while(1) {        
-        getline();
-
-        // Check if it is a GPGGA msg (matches both locked and non-locked msg)
-        if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock) >= 1) { 
-            if(!lock) {
-                longitude = 0.0;
-                latitude = 0.0;        
-                return 0;
-            } else {
-                if(ns == 'S') {    latitude  *= -1.0; }
-                if(ew == 'W') {    longitude *= -1.0; }
-                float degrees = trunc(latitude / 100.0f);
-                float minutes = latitude - (degrees * 100.0f);
-                latitude = degrees + minutes / 60.0f;    
-                degrees = trunc(longitude / 100.0f * 0.01f);
-                minutes = longitude - (degrees * 100.0f);
-                longitude = degrees + minutes / 60.0f;
-                return 1;
-            }
-        }
-    }
+bool GPS::newNMEAreceived(void) {
+  return recvdflag;
 }
 
+void GPS::pause(bool p) {
+  paused = p;
+}
+
+char *GPS::lastNMEA(void) {
+  recvdflag = false;
+  return (char *)lastline;
+}
+  
 float GPS::trunc(float v) {
     if(v < 0.0) {
         v*= -1.0;
@@ -68,14 +68,346 @@
     return v;
 }
 
-void GPS::getline() {
-    while(_gps.getc() != '$');    // wait for the start of a line
-    for(int i=0; i<256; i++) {
-        msg[i] = _gps.getc();
-        if(msg[i] == '\r') {
-            msg[i] = 0;
-            return;
-        }
+
+bool GPS::parse(char *nmea) {
+  // do checksum check
+
+  // first look if we even have one
+  if (nmea[strlen(nmea)-4] == '*') {
+    uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16;
+    sum += parseHex(nmea[strlen(nmea)-2]);
+    
+    // check checksum 
+    for (uint8_t i=1; i < (strlen(nmea)-4); i++) {
+      sum ^= nmea[i];
+    }
+    if (sum != 0) {
+      // bad checksum :(
+      //return false;
     }
-    error("Overflowed message limit");
+  }
+
+  // look for a few common sentences
+  if (strstr(nmea, "$GPGGA")) {
+    // found GGA
+    char *p = nmea;
+    // get time
+    p = strchr(p, ',')+1;
+    timef = atof(p);
+    uint32_t time = timef;
+    hour = time / 10000;
+    minute = (time % 10000) / 100;
+    seconds = (time % 100);
+
+    milliseconds = fmod((double) timef, 1.0) * 1000;
+
+    // parse out latitude
+    p = strchr(p, ',')+1;
+    latitude = atof(p);
+
+    p = strchr(p, ',')+1;
+    if (p[0] == 'N') lat = 'N';
+    else if (p[0] == 'S') lat = 'S';
+    else if (p[0] == ',') lat = 0;
+    else return false;
+
+    // parse out longitude
+    p = strchr(p, ',')+1;
+    longitude = atof(p);
+
+    p = strchr(p, ',')+1;
+    if (p[0] == 'W') lon = 'W';
+    else if (p[0] == 'E') lon = 'E';
+    else if (p[0] == ',') lon = 0;
+    else return false;
+   // convert to degrees
+    lat_deg = latitude;
+    lon_deg = longitude;
+    
+    if(lat == 'S') {    lat_deg  *= -1.0; }
+    if(lon == 'W') {    lon_deg *= -1.0; }
+    float degrees = trunc(lat_deg / 100.0f);
+    float minutes = lat_deg - (degrees * 100.0f);
+    lat_deg = degrees + minutes / 60.0f;    
+    degrees = trunc(lon_deg / 100.0f);
+    //degrees = trunc(longitude / 100.0f * 0.01f);
+    //printf("my lon=%f deg=%f\r\n",longitude, degrees);
+    minutes = lon_deg - (degrees * 100.0f);
+    lon_deg = degrees + minutes / 60.0f;
+    //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude);
+    //printf("gpgga rcvd\r\n");
+
+    p = strchr(p, ',')+1;
+    fixquality = atoi(p);
+
+    p = strchr(p, ',')+1;
+    satellites = atoi(p);
+
+    p = strchr(p, ',')+1;
+    HDOP = atof(p);
+
+    p = strchr(p, ',')+1;
+    altitude = atof(p);
+    p = strchr(p, ',')+1;
+    p = strchr(p, ',')+1;
+    geoidheight = atof(p);
+    return true;
+  }
+  if (strstr(nmea, "$GPRMC")) {
+   // found RMC
+    char *p = nmea;
+
+    // get time
+    p = strchr(p, ',')+1;
+    timef = atof(p);
+    uint32_t time = timef;
+    hour = time / 10000;
+    minute = (time % 10000) / 100;
+    seconds = (time % 100);
+
+    milliseconds = fmod((double) timef, 1.0) * 1000;
+
+    p = strchr(p, ',')+1;
+    // Serial.println(p);
+    if (p[0] == 'A') 
+      fix = true;
+    else if (p[0] == 'V')
+      fix = false;
+    else
+      return false;
+
+    // parse out latitude
+    p = strchr(p, ',')+1;
+    latitude = atof(p);
+
+    p = strchr(p, ',')+1;
+    if (p[0] == 'N') lat = 'N';
+    else if (p[0] == 'S') lat = 'S';
+    else if (p[0] == ',') lat = 0;
+    else return false;
+
+    // parse out longitude
+    p = strchr(p, ',')+1;
+    longitude = atof(p);
+
+    p = strchr(p, ',')+1;
+    if (p[0] == 'W') lon = 'W';
+    else if (p[0] == 'E') lon = 'E';
+    else if (p[0] == ',') lon = 0;
+    else return false;
+   // convert to degrees
+    lat_deg = latitude;
+    lon_deg = longitude;
+    
+    if(lat == 'S') {    lat_deg  *= -1.0; }
+    if(lon == 'W') {    lon_deg *= -1.0; }
+    float degrees = trunc(lat_deg / 100.0f);
+    float minutes = lat_deg - (degrees * 100.0f);
+    lat_deg = degrees + minutes / 60.0f;    
+    degrees = trunc(lon_deg / 100.0f);
+    minutes = lon_deg - (degrees * 100.0f);
+    lon_deg = degrees + minutes / 60.0f;
+     
+    // speed
+    p = strchr(p, ',')+1;
+    speed = atof(p);
+
+    // angle
+    p = strchr(p, ',')+1;
+    angle = atof(p);
+
+    p = strchr(p, ',')+1;
+    uint32_t fulldate = atof(p);
+    day = fulldate / 10000;
+    month = (fulldate % 10000) / 100;
+    year = (fulldate % 100);
+
+    // we dont parse the remaining, yet!
+    return true;
+  }
+
+  return false;
 }
+
+
+char GPS::read(void) {
+  char c = 0;
+  
+  if (paused) return c;
+
+    if(!_gps.readable()) return c;
+    c = _gps.getc();
+
+  //Serial.print(c);
+
+  if (c == '$') {
+    currentline[lineidx] = 0;
+    lineidx = 0;
+  }
+  if (c == '\n') {
+    currentline[lineidx] = 0;
+
+    if (currentline == line1) {
+      currentline = line2;
+      lastline = line1;
+    } else {
+      currentline = line1;
+      lastline = line2;
+    }
+
+    lineidx = 0;
+    recvdflag = true;
+  }
+
+  currentline[lineidx++] = c;
+  if (lineidx >= MAXLINELENGTH)
+    lineidx = MAXLINELENGTH-1;
+
+  return c;
+}
+
+void GPS::setBaud(int baud)
+{
+   
+     switch (baud) {
+        case 9600:
+            _gps.baud(9600);
+            sendCommand(PMTK_SET_BAUD_9600);
+            wait_ms(100);
+            printf("baud (%d), set to 9600\r\n",baud);
+            break;  
+        case 57600:
+            _gps.baud(9600);  // assume device is at default 9600 baud
+            sendCommand(PMTK_SET_BAUD_57600);
+            _gps.baud(57600);
+            wait_ms(100);
+            printf("baud (%d), set to 57600\r\n",baud);
+            break;        
+        default:
+            printf("unsupported baud (%d), set to 9600\r\n",baud);
+            _gps.baud(9600);
+            sendCommand(PMTK_SET_BAUD_9600);
+            break;    
+    }
+  
+  wait_ms(10);
+}
+
+void GPS::sendCommand(char *str) {
+  _gps.printf("%s\r\n",str);
+}
+
+void GPS::setSetup(void) {
+  
+ 
+
+  
+}
+
+// read a Hex value and return the decimal equivalent
+uint8_t GPS::parseHex(char c) {
+    if (c < '0')
+      return 0;
+    if (c <= '9')
+      return c - '0';
+    if (c < 'A')
+       return 0;
+    if (c <= 'F')
+       return (c - 'A')+10;
+    return 0;
+}
+
+bool GPS::waitForSentence(char *wait4me, uint8_t max) {
+  char str[20];
+
+  uint8_t i=0;
+  while (i < max) {
+    if (newNMEAreceived()) { 
+      char *nmea = lastNMEA();
+      strncpy(str, nmea, 20);
+      str[19] = 0;
+      i++;
+
+      if (strstr(str, wait4me))
+    return true;
+    }
+  }
+
+  return false;
+}
+
+bool GPS::LOCUS_StartLogger(void) {
+  sendCommand(PMTK_LOCUS_STARTLOG);
+  recvdflag = false;
+  return waitForSentence(PMTK_LOCUS_LOGSTARTED);
+}
+
+bool GPS::LOCUS_ReadStatus(void) {
+  sendCommand(PMTK_LOCUS_QUERY_STATUS);
+  
+  if (! waitForSentence("$PMTKLOG"))
+    return false;
+
+  char *response = lastNMEA();
+  uint16_t parsed[10];
+  int8_t i;
+  
+  for (i=0; i<10; i++) parsed[i] = -1;
+  
+  response = strchr(response, ',');
+  for (i=0; i<10; i++) {
+    if (!response || (response[0] == 0) || (response[0] == '*')) 
+      break;
+    response++;
+    parsed[i]=0;
+    while ((response[0] != ',') && 
+       (response[0] != '*') && (response[0] != 0)) {
+      parsed[i] *= 10;
+      char c = response[0];
+      if (isdigit(c))
+        parsed[i] += c - '0';
+      else
+        parsed[i] = c;
+      response++;
+    }
+  }
+  LOCUS_serial = parsed[0];
+  LOCUS_type = parsed[1];
+  if (isalpha(parsed[2])) {
+    parsed[2] = parsed[2] - 'a' + 10; 
+  }
+  LOCUS_mode = parsed[2];
+  LOCUS_config = parsed[3];
+  LOCUS_interval = parsed[4];
+  LOCUS_distance = parsed[5];
+  LOCUS_speed = parsed[6];
+  LOCUS_status = !parsed[7];
+  LOCUS_records = parsed[8];
+  LOCUS_percent = parsed[9];
+
+  return true;
+}
+
+// Standby Mode Switches
+bool GPS::standby(void) {
+  if (inStandbyMode) {
+    return false;  // Returns false if already in standby mode, so that you do not wake it up by sending commands to GPS
+  }
+  else {
+    inStandbyMode = true;
+    sendCommand(PMTK_STANDBY);
+    //return waitForSentence(PMTK_STANDBY_SUCCESS);  // don't seem to be fast enough to catch the message, or something else just is not working
+    return true;
+  }
+}
+
+bool GPS::wakeup(void) {
+  if (inStandbyMode) {
+   inStandbyMode = false;
+    sendCommand("");  // send byte to wake it up
+    return waitForSentence(PMTK_AWAKE);
+  }
+  else {
+      return false;  // Returns false if not in standby mode, nothing to wakeup
+  }
+}