Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreePilot PinDetect mbed-src
Fork of FreePilot_V2-2 by
gps.h
- Committer:
- maximbolduc
- Date:
- 2015-03-14
- Revision:
- 47:d3123bb4f673
- Child:
- 48:5d9c63364c94
File content as of revision 47:d3123bb4f673:
#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))); }