Diff: main.cpp

Revision:

3:dd2e91231e3c

Parent:

2:13ef79e40e45

--- a/main.cpp	Sun Apr 26 11:51:49 2015 +0000
+++ b/main.cpp	Sun Apr 26 19:05:38 2015 +0000
@@ -1,13 +1,100 @@
 /*
  * Author: Edoardo De Marchi
- * Date: 22-08-14
+ * Editor : Say Pulung
+ * Country : Indonesia (Lampung)
+ * Date: 26-04-15
  * Notes: Firmware for GPS U-Blox NEO-6M
 */
 
 #include "main.h"
 #include "string.h"
 #include "rtos.h"
-char gpsData[10][500];
+#include "mbed.h"
+#include <stdint.h>
+#include <math.h>
+#include <ctype.h>
+uint8_t hour, minute, seconds, year, month, day;
+uint16_t milliseconds;
+float latitude, longitude, geoidheight, altitude;
+float speed, angle, magvariation, HDOP;
+char lat, lon, mag;
+bool fix;
+float trunc(float v) {
+    if(v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+void parseRMC(char* buffer)
+{
+    char *p = buffer;
+
+    // get time
+    p = strchr(p, ',')+1;
+    float 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
+      fix= 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 lat=0;
+
+    // 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 lon=0;
+    if(lat == 'S') {    latitude  *= -1; }
+    if(lon == 'W') {    longitude *= -1; }
+    float degrees = trunc(latitude / 100.0f);
+    float minutes = latitude - (degrees * 100.0f);
+    latitude = degrees + minutes / 60.0f;    
+    
+    degrees = trunc(longitude / 100.0f );
+    minutes = longitude - (degrees * 100.0f);
+    longitude = 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);
+
+}
 void Init()
 {
     gps.baud(9600);
@@ -30,20 +117,13 @@
                     c = gps.getc();
                     if( c == '\r' )
                     {
-                        //int a = sizeof(cDataBuffer);
-                        //char data[500];
-                        //int ccS = sprintf(data,"%s",cDataBuffer);
                         
-                        pc.printf("Message : %s\n", cDataBuffer);
+                        
                         if(strncmp(cDataBuffer,"$GPRMC",6)==0)
                         {
-                            
+                            parseRMC(cDataBuffer);
                         }
-                        //int count = getDataCount(data,",");
-                        //pc.printf("Length  : %d\n",count);
-                        //fflush(stdin);
-                        //fflush(stdout);
-                        //parse(cDataBuffer, i);
+                        //pc.printf("Longitude = %f, Latitude = %f, Speed = %f,Angle = %f, Day = %d, Month = %d, Year = %d,H = %d, M = %d, S = %d\n",longitude,latitude,speed,angle,day,month,year,hour,minute,seconds);
                         i = sizeof(cDataBuffer);
                     }
                     else
@@ -52,6 +132,7 @@
                     }                 
                 }
             }
+            
          } 
     }
 }
@@ -67,62 +148,12 @@
         wait(0.5);
         l1=0;
         wait(0.5);
+        pc.printf("Longitude = %f, Latitude = %f, Speed = %f,Angle = %f, Day = %d, Month = %d, Year = %d,H = %d, M = %d, S = %d\n",longitude,latitude,speed,angle,day,month,year,hour,minute,seconds);
     }
     
 }
 
 
-void parse(char *cmd, int n)
-{
-    
-    char ns, ew, tf, status;
-    int fq, nst, fix, date;                                     // fix quality, Number of satellites being tracked, 3D fix
-    float latitude, longitude, timefix, speed, altitude;
-    
-    pc.printf("Parser cooyy\n");
-    // Global Positioning System Fix Data
-    if(strncmp(cmd,"$GPGGA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
-        pc.printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
-    }
-    
-    // Satellite status
-    if(strncmp(cmd,"$GPGSA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
-        pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
-    }
-    
-    // Geographic position, Latitude and Longitude
-    if(strncmp(cmd,"$GPGLL", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
-        pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
-    }
-    
-    // Geographic position, Latitude and Longitude
-    if(strncmp(cmd,"$GPRMC", 6) == 0) 
-    {
-        sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
-        pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
-    }
-}
-
-void splitString(char str[],char *delimiter)
-{
-    int count=0;
-    char *tok = strtok(str,delimiter);
-    int idx=0;
-    while(0!=tok)
-    {
-        int cpy = sprintf(gpsData[idx],"%s",tok);
-        pc.printf("Data %d = %s\n",idx,gpsData[idx]);
-        tok = strtok(NULL,delimiter);
-        idx++;
-    }   
-    //return count;
-}