Diff: gps.h

Revision:

47:d3123bb4f673

Child:

48:5d9c63364c94

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.h	Sat Mar 14 01:28:37 2015 +0000
@@ -0,0 +1,176 @@
+
+#define dot(u,v)   ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
+#define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
+#define d(u,v)      norm(point_sub(u,v))          // distance = norm of difference
+
+int num_of_gps_sats;
+double decimal_latitude;
+int  gps_satellite_quality;
+double decimal_lon;
+
+Point point_add(Point a, Point b)
+{
+    return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
+}
+
+Point point_sub(Point a , Point b)
+{
+    return Point(a.GetX() - b.GetX(),  a.GetY() - b.GetY());
+}
+
+double dist_Point_to_Line( Point P, Point line_start, Point line_end)
+{
+    Point v = point_sub(line_end,line_start);
+    Point w = point_sub(P,line_start);
+
+    double c1 = dot(w,v);
+    double c2 = dot(v,v);
+    double b = c1 / c2;
+
+    Point resulting(b * v.GetX(),b*v.GetY());
+    Point Pb = point_add(line_start, resulting);
+
+    return d(P, Pb);
+}
+
+double lat_to_deg(char *s, char north_south)
+{
+    int deg, min, sec;
+    double fsec, val;
+
+    deg  = ( (s[0] - '0') * 10) + s[1] - '0';
+    min  = ( (s[2] - '0') * 10) + s[3] - '0';
+    sec  = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
+    fsec = (double)((double)sec /10000.0);
+    val  = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
+    if (north_south == 'S') {
+        val *= -1.0;
+    }
+    return val;
+}
+
+// isLeft(): test if a point is Left|On|Right of an infinite 2D line.
+//    Input:  three points P0, P1, and P2
+//    Return: >0 for P2 left of the line through P0 to P1
+//          =0 for P2 on the line
+//          <0 for P2 right of the line
+int isLeft( Point P0, Point P1, Point P2 )
+{
+    double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
+    if ( isleft > 0 ) {
+        isleft = 1;
+    } else {
+        isleft = -1;
+    }
+    return (int)isleft;
+}
+
+double lon_to_deg(char *s, char east_west)
+{
+    int deg, min, sec;
+    double fsec, val;
+    deg  = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
+    min  = ( (s[3] - '0') * 10) + s[4] - '0';
+    sec  = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
+    fsec = (double)((double)sec /10000.0);
+    val  = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
+
+    if (east_west == 'W') {
+        val *= -1.0;
+    }
+    return val;
+}
+
+void nmea_gga(char *s)
+{
+    char *token;
+    int  token_counter = 0;
+    char *latitude  = (char *)NULL;
+    char *longitude = (char *)NULL;
+    char *lat_dir   = (char *)NULL;
+    char *lon_dir   = (char *)NULL;
+    char *qual      = (char *)NULL;
+    char *altitude  = (char *)NULL;
+    char *sats      = (char *)NULL;
+
+    token = strtok(s, ",");
+    while (token) {
+        switch (token_counter) {
+            case 2:
+                latitude  = token;
+                break;
+            case 4:
+                longitude = token;
+                break;
+            case 3:
+                lat_dir   = token;
+                break;
+            case 5:
+                lon_dir   = token;
+                break;
+            case 6:
+                qual      = token;
+                break;
+            case 7:
+                sats      = token;
+                break;
+            case 9:
+                altitude  = token;
+                break;
+        }
+        token = strtok((char *)NULL, ",");
+        token_counter++;
+    }
+    if (latitude && longitude && altitude && sats) {
+        decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
+        decimal_lon = lon_to_deg(longitude, lon_dir[0]);
+        num_of_gps_sats = atoi(sats);
+        gps_satellite_quality = atoi(qual);
+    } else {
+        gps_satellite_quality = 0;
+    }
+}
+
+char dms[128];
+char* To_DMS(double dec_deg)
+{
+    dec_deg = abs(dec_deg);
+    int d = (int)(dec_deg);
+    sprintf(dms,"%0.2i\0",d);
+    double m = (double)(((double)dec_deg - (double)d) * 60.0);
+    if (m < 10 ) {
+        sprintf(dms,"%s0%0.9f\0",dms,m);
+    } else {
+        sprintf(dms,"%s%0.9f\0",dms,m);
+    }
+    return dms;
+}
+
+char* To_DMS_lon(double dec_deg)
+{
+    dec_deg = abs(dec_deg);
+    int d = (int)(dec_deg);
+    sprintf(dms,"%0.3i\0",d);
+    double m = (double)(((double)dec_deg - (double)d) * 60.0);
+    if (m < 10 ) {
+        sprintf(dms,"%s0%0.9f\0",dms,m);
+    } else {
+        sprintf(dms,"%s%0.9f\0",dms,m);
+    }
+    return dms;
+}
+
+//from farmerGPS code
+void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
+{
+    double ydist = 0;
+    double xdist = 0;
+    angle = angle + 180;
+    double radiant = angle * 3.14159265359 / 180;
+    double sinr = sin(radiant);
+    double cosr = cos(radiant);
+    xdist = cosr * distance;
+    ydist = sinr * distance;
+    lat2 = lat1 + (ydist / (69.09 * -1609.344));
+    lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
+}
\ No newline at end of file