Diff: gps.h

Revision:

1:a091e3ca8443

Parent:

0:7f20540d3281

--- 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