Agra-GPS / FreePilot_V2-3

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

gps.h@47:d3123bb4f673, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:28:37 2015 +0000
Revision:
47:d3123bb4f673
Child:
48:5d9c63364c94
splitted into classes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 47:d3123bb4f673 1
maximbolduc 47:d3123bb4f673 2 #define dot(u,v) ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
maximbolduc 47:d3123bb4f673 3 #define norm(v) sqrt(dot(v,v)) // norm = length of vector
maximbolduc 47:d3123bb4f673 4 #define d(u,v) norm(point_sub(u,v)) // distance = norm of difference
maximbolduc 47:d3123bb4f673 5
maximbolduc 47:d3123bb4f673 6 int num_of_gps_sats;
maximbolduc 47:d3123bb4f673 7 double decimal_latitude;
maximbolduc 47:d3123bb4f673 8 int gps_satellite_quality;
maximbolduc 47:d3123bb4f673 9 double decimal_lon;
maximbolduc 47:d3123bb4f673 10
maximbolduc 47:d3123bb4f673 11 Point point_add(Point a, Point b)
maximbolduc 47:d3123bb4f673 12 {
maximbolduc 47:d3123bb4f673 13 return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
maximbolduc 47:d3123bb4f673 14 }
maximbolduc 47:d3123bb4f673 15
maximbolduc 47:d3123bb4f673 16 Point point_sub(Point a , Point b)
maximbolduc 47:d3123bb4f673 17 {
maximbolduc 47:d3123bb4f673 18 return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY());
maximbolduc 47:d3123bb4f673 19 }
maximbolduc 47:d3123bb4f673 20
maximbolduc 47:d3123bb4f673 21 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
maximbolduc 47:d3123bb4f673 22 {
maximbolduc 47:d3123bb4f673 23 Point v = point_sub(line_end,line_start);
maximbolduc 47:d3123bb4f673 24 Point w = point_sub(P,line_start);
maximbolduc 47:d3123bb4f673 25
maximbolduc 47:d3123bb4f673 26 double c1 = dot(w,v);
maximbolduc 47:d3123bb4f673 27 double c2 = dot(v,v);
maximbolduc 47:d3123bb4f673 28 double b = c1 / c2;
maximbolduc 47:d3123bb4f673 29
maximbolduc 47:d3123bb4f673 30 Point resulting(b * v.GetX(),b*v.GetY());
maximbolduc 47:d3123bb4f673 31 Point Pb = point_add(line_start, resulting);
maximbolduc 47:d3123bb4f673 32
maximbolduc 47:d3123bb4f673 33 return d(P, Pb);
maximbolduc 47:d3123bb4f673 34 }
maximbolduc 47:d3123bb4f673 35
maximbolduc 47:d3123bb4f673 36 double lat_to_deg(char *s, char north_south)
maximbolduc 47:d3123bb4f673 37 {
maximbolduc 47:d3123bb4f673 38 int deg, min, sec;
maximbolduc 47:d3123bb4f673 39 double fsec, val;
maximbolduc 47:d3123bb4f673 40
maximbolduc 47:d3123bb4f673 41 deg = ( (s[0] - '0') * 10) + s[1] - '0';
maximbolduc 47:d3123bb4f673 42 min = ( (s[2] - '0') * 10) + s[3] - '0';
maximbolduc 47:d3123bb4f673 43 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
maximbolduc 47:d3123bb4f673 44 fsec = (double)((double)sec /10000.0);
maximbolduc 47:d3123bb4f673 45 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 47:d3123bb4f673 46 if (north_south == 'S') {
maximbolduc 47:d3123bb4f673 47 val *= -1.0;
maximbolduc 47:d3123bb4f673 48 }
maximbolduc 47:d3123bb4f673 49 return val;
maximbolduc 47:d3123bb4f673 50 }
maximbolduc 47:d3123bb4f673 51
maximbolduc 47:d3123bb4f673 52 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
maximbolduc 47:d3123bb4f673 53 // Input: three points P0, P1, and P2
maximbolduc 47:d3123bb4f673 54 // Return: >0 for P2 left of the line through P0 to P1
maximbolduc 47:d3123bb4f673 55 // =0 for P2 on the line
maximbolduc 47:d3123bb4f673 56 // <0 for P2 right of the line
maximbolduc 47:d3123bb4f673 57 int isLeft( Point P0, Point P1, Point P2 )
maximbolduc 47:d3123bb4f673 58 {
maximbolduc 47:d3123bb4f673 59 double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
maximbolduc 47:d3123bb4f673 60 if ( isleft > 0 ) {
maximbolduc 47:d3123bb4f673 61 isleft = 1;
maximbolduc 47:d3123bb4f673 62 } else {
maximbolduc 47:d3123bb4f673 63 isleft = -1;
maximbolduc 47:d3123bb4f673 64 }
maximbolduc 47:d3123bb4f673 65 return (int)isleft;
maximbolduc 47:d3123bb4f673 66 }
maximbolduc 47:d3123bb4f673 67
maximbolduc 47:d3123bb4f673 68 double lon_to_deg(char *s, char east_west)
maximbolduc 47:d3123bb4f673 69 {
maximbolduc 47:d3123bb4f673 70 int deg, min, sec;
maximbolduc 47:d3123bb4f673 71 double fsec, val;
maximbolduc 47:d3123bb4f673 72 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
maximbolduc 47:d3123bb4f673 73 min = ( (s[3] - '0') * 10) + s[4] - '0';
maximbolduc 47:d3123bb4f673 74 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
maximbolduc 47:d3123bb4f673 75 fsec = (double)((double)sec /10000.0);
maximbolduc 47:d3123bb4f673 76 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 47:d3123bb4f673 77
maximbolduc 47:d3123bb4f673 78 if (east_west == 'W') {
maximbolduc 47:d3123bb4f673 79 val *= -1.0;
maximbolduc 47:d3123bb4f673 80 }
maximbolduc 47:d3123bb4f673 81 return val;
maximbolduc 47:d3123bb4f673 82 }
maximbolduc 47:d3123bb4f673 83
maximbolduc 47:d3123bb4f673 84 void nmea_gga(char *s)
maximbolduc 47:d3123bb4f673 85 {
maximbolduc 47:d3123bb4f673 86 char *token;
maximbolduc 47:d3123bb4f673 87 int token_counter = 0;
maximbolduc 47:d3123bb4f673 88 char *latitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 89 char *longitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 90 char *lat_dir = (char *)NULL;
maximbolduc 47:d3123bb4f673 91 char *lon_dir = (char *)NULL;
maximbolduc 47:d3123bb4f673 92 char *qual = (char *)NULL;
maximbolduc 47:d3123bb4f673 93 char *altitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 94 char *sats = (char *)NULL;
maximbolduc 47:d3123bb4f673 95
maximbolduc 47:d3123bb4f673 96 token = strtok(s, ",");
maximbolduc 47:d3123bb4f673 97 while (token) {
maximbolduc 47:d3123bb4f673 98 switch (token_counter) {
maximbolduc 47:d3123bb4f673 99 case 2:
maximbolduc 47:d3123bb4f673 100 latitude = token;
maximbolduc 47:d3123bb4f673 101 break;
maximbolduc 47:d3123bb4f673 102 case 4:
maximbolduc 47:d3123bb4f673 103 longitude = token;
maximbolduc 47:d3123bb4f673 104 break;
maximbolduc 47:d3123bb4f673 105 case 3:
maximbolduc 47:d3123bb4f673 106 lat_dir = token;
maximbolduc 47:d3123bb4f673 107 break;
maximbolduc 47:d3123bb4f673 108 case 5:
maximbolduc 47:d3123bb4f673 109 lon_dir = token;
maximbolduc 47:d3123bb4f673 110 break;
maximbolduc 47:d3123bb4f673 111 case 6:
maximbolduc 47:d3123bb4f673 112 qual = token;
maximbolduc 47:d3123bb4f673 113 break;
maximbolduc 47:d3123bb4f673 114 case 7:
maximbolduc 47:d3123bb4f673 115 sats = token;
maximbolduc 47:d3123bb4f673 116 break;
maximbolduc 47:d3123bb4f673 117 case 9:
maximbolduc 47:d3123bb4f673 118 altitude = token;
maximbolduc 47:d3123bb4f673 119 break;
maximbolduc 47:d3123bb4f673 120 }
maximbolduc 47:d3123bb4f673 121 token = strtok((char *)NULL, ",");
maximbolduc 47:d3123bb4f673 122 token_counter++;
maximbolduc 47:d3123bb4f673 123 }
maximbolduc 47:d3123bb4f673 124 if (latitude && longitude && altitude && sats) {
maximbolduc 47:d3123bb4f673 125 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 47:d3123bb4f673 126 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 47:d3123bb4f673 127 num_of_gps_sats = atoi(sats);
maximbolduc 47:d3123bb4f673 128 gps_satellite_quality = atoi(qual);
maximbolduc 47:d3123bb4f673 129 } else {
maximbolduc 47:d3123bb4f673 130 gps_satellite_quality = 0;
maximbolduc 47:d3123bb4f673 131 }
maximbolduc 47:d3123bb4f673 132 }
maximbolduc 47:d3123bb4f673 133
maximbolduc 47:d3123bb4f673 134 char dms[128];
maximbolduc 47:d3123bb4f673 135 char* To_DMS(double dec_deg)
maximbolduc 47:d3123bb4f673 136 {
maximbolduc 47:d3123bb4f673 137 dec_deg = abs(dec_deg);
maximbolduc 47:d3123bb4f673 138 int d = (int)(dec_deg);
maximbolduc 47:d3123bb4f673 139 sprintf(dms,"%0.2i\0",d);
maximbolduc 47:d3123bb4f673 140 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 47:d3123bb4f673 141 if (m < 10 ) {
maximbolduc 47:d3123bb4f673 142 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 143 } else {
maximbolduc 47:d3123bb4f673 144 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 145 }
maximbolduc 47:d3123bb4f673 146 return dms;
maximbolduc 47:d3123bb4f673 147 }
maximbolduc 47:d3123bb4f673 148
maximbolduc 47:d3123bb4f673 149 char* To_DMS_lon(double dec_deg)
maximbolduc 47:d3123bb4f673 150 {
maximbolduc 47:d3123bb4f673 151 dec_deg = abs(dec_deg);
maximbolduc 47:d3123bb4f673 152 int d = (int)(dec_deg);
maximbolduc 47:d3123bb4f673 153 sprintf(dms,"%0.3i\0",d);
maximbolduc 47:d3123bb4f673 154 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 47:d3123bb4f673 155 if (m < 10 ) {
maximbolduc 47:d3123bb4f673 156 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 157 } else {
maximbolduc 47:d3123bb4f673 158 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 159 }
maximbolduc 47:d3123bb4f673 160 return dms;
maximbolduc 47:d3123bb4f673 161 }
maximbolduc 47:d3123bb4f673 162
maximbolduc 47:d3123bb4f673 163 //from farmerGPS code
maximbolduc 47:d3123bb4f673 164 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
maximbolduc 47:d3123bb4f673 165 {
maximbolduc 47:d3123bb4f673 166 double ydist = 0;
maximbolduc 47:d3123bb4f673 167 double xdist = 0;
maximbolduc 47:d3123bb4f673 168 angle = angle + 180;
maximbolduc 47:d3123bb4f673 169 double radiant = angle * 3.14159265359 / 180;
maximbolduc 47:d3123bb4f673 170 double sinr = sin(radiant);
maximbolduc 47:d3123bb4f673 171 double cosr = cos(radiant);
maximbolduc 47:d3123bb4f673 172 xdist = cosr * distance;
maximbolduc 47:d3123bb4f673 173 ydist = sinr * distance;
maximbolduc 47:d3123bb4f673 174 lat2 = lat1 + (ydist / (69.09 * -1609.344));
maximbolduc 47:d3123bb4f673 175 lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
maximbolduc 47:d3123bb4f673 176 }