gps.h

00001 #ifndef GPS_H
00002 #define GPS_H
00003 
00004 #include "mbed.h"
00005 #define knots_to_ms 0.514444444f
00006 #define pi 3.14159265359f
00007 #include "arm_math.h"
00008 #include "math_helper.h"
00009 #include "arm_common_tables.h"
00010 
00011 #include "arm_math.h"
00012 #include "math_helper.h"
00013 #include "arm_common_tables.h"
00014 
00015 const float a = 6378137.0f;
00016 const float b = 6356752.3141f; //Major and minor radii for WGS84 grid
00017 const float e2 = (a*a-b*b)/a*a;
00018 const float degrad = pi / 180.0f; //Convert degrees (GPS delivered as decimal degrees) to radians
00019 
00020 struct coords{
00021     float x;
00022     float y;
00023 };
00024 
00025 class GPS{
00026 public:
00027 
00028     GPS(void);
00029     void parse(char *nmea);
00030 
00031     float radius;
00032     float latitude;
00033     float longitude;
00034     float speed;
00035     float mph;
00036     float angle;
00037     uint8_t hour, minute, seconds, year, month, day;
00038     uint16_t milliseconds;
00039     bool fix;
00040     char currentline[120];
00041     uint16_t lineidx;
00042     bool newSentence;
00043     bool firstfix;
00044 
00045 private:
00046     
00047     uint16_t parseHex(char c) {
00048 //Converts Hex character to value - for checksum calculation
00049     if (c <= '9')
00050       return c - '0';
00051     if (c < 'A')
00052        return 0;
00053     if (c <= 'F')
00054        return (c - 'A')+10;
00055     return 0;
00056     }
00057     
00058     float timef;
00059     bool checksum;
00060     char lat, lon;
00061     float mins;
00062 
00063     struct coords positions[3];
00064 
00065 coords Cartesian(float lat, float lon){
00066     coords result;
00067     float phi = lat*degrad;
00068     float lambda = lon*degrad;
00069     float sinphi = arm_sin_f32(phi);
00070     float cosphi = arm_cos_f32(phi);
00071     float nu = sqrt(a/(1-e2*sinphi*sinphi));
00072     result.x = nu * cosphi * arm_cos_f32(lambda);
00073     result.y = nu * cosphi * arm_sin_f32(lambda);
00074     return result;
00075 }
00076 
00077 float CalcRadius(coords p1, coords p2, coords p3){
00078 // Calculate the radius of turn from the last three gps coordinates
00079     float dx1 = p2.x - p1.x;
00080     float dx2 = p2.x - p3.x;
00081     float dx3 = p3.x - p1.x;
00082     float dy1 = p2.y - p1.y;
00083     float dy2 = p2.y - p3.y;
00084     float dy3 = p3.y - p1.y;
00085     float numerator = sqrt((dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2)*(dx3*dx3+dy3*dy3)); 
00086     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);
00087     if (denominator < 0.000001f) return 500.0f;
00088     else if (denominator > (numerator/500.0f)) return numerator/denominator;
00089     else return 500.0f;
00090 }
00091 
00092 };
00093 #endif