Revision 1:a091e3ca8443, committed 2015-06-03

Comitter:

StressedDave

Date:

Wed Jun 03 11:12:47 2015 +0000

Parent:

0:7f20540d3281

Commit message:

Initial parsing and processing. Based of Adafruit GPS library

Changed in this revision

diff -r 7f20540d3281 -r a091e3ca8443 gps.cpp
--- a/gps.cpp	Wed Apr 08 19:06:39 2015 +0000
+++ b/gps.cpp	Wed Jun 03 11:12:47 2015 +0000
@@ -6,6 +6,7 @@
     latitude = 0.0f;
     longitude = 0.0f;
     speed = 0.0f;
+    mph = 0.0f;
     angle = 0.0f;    
     hour = 0;
     minute = 0;
@@ -18,9 +19,13 @@
     lineidx = 0;
     newSentence = false;
     firstfix = false;
+    for (int i = 0; i<3; i++){
+        positions[i].x = 0;
+        positions[i].y = 0;
+        }
     }
     
-bool GPS::parse(char *nmea) {
+void GPS::parse(char *nmea) {
     uint16_t sum;
 // do checksum check
 // first look if we even have one
@@ -33,16 +38,16 @@
     }
     if (sum != 0) {
 // bad checksum :(
-      return false;
+    newSentence = false;
+    return;
     }
   }
 // bad string
-  else return false;
-
   float minutes;
   char degreebuff[10];
 // look for RMC sentence only
   if (strstr(nmea, "$GPRMC")) {
+     newSentence = true;
      char *p = nmea;
 // get time
     p = strchr(p, ',')+1;
@@ -54,14 +59,16 @@
     milliseconds = time % 10;
 //Check for valid fix
     p = strchr(p, ',')+1;
-    if (p[0] == 'A') fix = true;
-    else if (p[0] == 'V') fix = false;
-    else return false;
+    if (p[0] == 'A') 
+        fix = true;
+    else{
+        fix = false;
+        return;
+        }
+    firstfix = true;
 // parse out latitude
     p = strchr(p, ',')+1;
-    if (',' != *p)
-//Not empty value due to no fix
-    {
+    if (',' != *p){
       strncpy(degreebuff, p, 2);
       p += 2;
       degreebuff[2] = '\0';
@@ -70,7 +77,7 @@
       degreebuff[7] = '\0';
       minutes = atof(degreebuff)/60;
       latitude += minutes;
-    }
+        }
     p = strchr(p, ',')+1;
     if (',' != *p)
     {
@@ -100,6 +107,7 @@
     if (',' != *p)
     {
       speed = atof(p)*knots_to_ms;
+      mph = speed / 0.44704f;
     }
     
     // angle
@@ -117,7 +125,13 @@
       month = (fulldate % 10000) / 100;
       year = (fulldate % 100);
     }
-    return true;
+    positions[0] = positions[1];
+    positions[1] = positions[2];
+    positions[2] = Cartesian(latitude,longitude);
+    if (mph > 10) radius = CalcRadius(positions[0], positions[1], positions[2]);
+    else radius = 300.0f;
+    if (radius > 300.0f) radius = 300.0f;
+    return;
   }
-  return false;
+  newSentence = false;
 }
\ No newline at end of file

diff -r 7f20540d3281 -r a091e3ca8443 gps.h
--- a/gps.h	Wed Apr 08 19:06:39 2015 +0000
+++ b/gps.h	Wed Jun 03 11:12:47 2015 +0000
@@ -8,15 +8,31 @@
 #include "math_helper.h"
 #include "arm_common_tables.h"
 
+#include "arm_math.h"
+#include "math_helper.h"
+#include "arm_common_tables.h"
+
+const float a = 6378137.0f;
+const float b = 6356752.3141f; //Major and minor radii for WGS84 grid
+const float e2 = (a*a-b*b)/a*a;
+const float degrad = pi / 180.0f; //Convert degrees (GPS delivered as decimal degrees) to radians
+
+struct coords{
+    float x;
+    float y;
+};
+
 class GPS{
 public:
 
     GPS(void);
-    bool parse(char *nmea);
+    void parse(char *nmea);
 
+    float radius;
     float latitude;
     float longitude;
     float speed;
+    float mph;
     float angle;
     uint8_t hour, minute, seconds, year, month, day;
     uint16_t milliseconds;
@@ -44,5 +60,34 @@
     char lat, lon;
     float mins;
 
+    struct coords positions[3];
+
+coords Cartesian(float lat, float lon){
+    coords result;
+    float phi = lat*degrad;
+    float lambda = lon*degrad;
+    float sinphi = arm_sin_f32(phi);
+    float cosphi = arm_cos_f32(phi);
+    float nu = sqrt(a/(1-e2*sinphi*sinphi));
+    result.x = nu * cosphi * arm_cos_f32(lambda);
+    result.y = nu * cosphi * arm_sin_f32(lambda);
+    return result;
+}
+
+float CalcRadius(coords p1, coords p2, coords p3){
+// Calculate the radius of turn from the last three gps coordinates
+    float dx1 = p2.x - p1.x;
+    float dx2 = p2.x - p3.x;
+    float dx3 = p3.x - p1.x;
+    float dy1 = p2.y - p1.y;
+    float dy2 = p2.y - p3.y;
+    float dy3 = p3.y - p1.y;
+    float numerator = sqrt((dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2)*(dx3*dx3+dy3*dy3)); 
+    float denominator = 2.0f*abs(p1.x*p2.y+p2.x*p3.y*p3.x*p1.y-p1.x*p3.y-p2.x*p1.y-p3.x*p2.y);
+    if (denominator < 0.000001f) return 500.0f;
+    else if (denominator > (numerator/500.0f)) return numerator/denominator;
+    else return 500.0f;
+}
+
 };
 #endif
\ No newline at end of file