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@48:5d9c63364c94, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:56:28 2015 +0000
Revision:
48:5d9c63364c94
Parent:
47:d3123bb4f673
Child:
50:07dfcda65732
splitted

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 48:5d9c63364c94 6 //gga:
maximbolduc 47:d3123bb4f673 7 int num_of_gps_sats;
maximbolduc 47:d3123bb4f673 8 double decimal_latitude;
maximbolduc 47:d3123bb4f673 9 int gps_satellite_quality;
maximbolduc 47:d3123bb4f673 10 double decimal_lon;
maximbolduc 47:d3123bb4f673 11
maximbolduc 48:5d9c63364c94 12 Point position;
maximbolduc 48:5d9c63364c94 13 //rmc:
maximbolduc 48:5d9c63364c94 14 Point old_position;
maximbolduc 48:5d9c63364c94 15 int day;
maximbolduc 48:5d9c63364c94 16 int hour;
maximbolduc 48:5d9c63364c94 17 int minute;
maximbolduc 48:5d9c63364c94 18 int second;
maximbolduc 48:5d9c63364c94 19 int tenths;
maximbolduc 48:5d9c63364c94 20 int hundreths;
maximbolduc 48:5d9c63364c94 21 char status;
maximbolduc 48:5d9c63364c94 22 double track; // track made good . angle
maximbolduc 48:5d9c63364c94 23 char magvar_dir;
maximbolduc 48:5d9c63364c94 24 double magvar;
maximbolduc 48:5d9c63364c94 25 int year;
maximbolduc 48:5d9c63364c94 26 int month;
maximbolduc 48:5d9c63364c94 27 double speed_km = 0;
maximbolduc 48:5d9c63364c94 28 double speed_m_s = 0;
maximbolduc 48:5d9c63364c94 29 double velocity; // speed in knot
maximbolduc 48:5d9c63364c94 30 char *time_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 31 char *date = (char *)NULL;
maximbolduc 48:5d9c63364c94 32 char *stat = (char *)NULL;
maximbolduc 48:5d9c63364c94 33 char *vel = (char *)NULL;
maximbolduc 48:5d9c63364c94 34 char *trk = (char *)NULL;
maximbolduc 48:5d9c63364c94 35 char *magv = (char *)NULL;
maximbolduc 48:5d9c63364c94 36 char *magd = (char *)NULL;
maximbolduc 48:5d9c63364c94 37 char *latit = "";
maximbolduc 48:5d9c63364c94 38 char *longit = "";
maximbolduc 48:5d9c63364c94 39 char *latitude_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 40 char *longitude_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 41 char *lat_dir = (char *)NULL;
maximbolduc 48:5d9c63364c94 42 char *lon_dir = (char *)NULL;
maximbolduc 48:5d9c63364c94 43
maximbolduc 47:d3123bb4f673 44 Point point_add(Point a, Point b)
maximbolduc 47:d3123bb4f673 45 {
maximbolduc 47:d3123bb4f673 46 return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
maximbolduc 47:d3123bb4f673 47 }
maximbolduc 47:d3123bb4f673 48
maximbolduc 47:d3123bb4f673 49 Point point_sub(Point a , Point b)
maximbolduc 47:d3123bb4f673 50 {
maximbolduc 47:d3123bb4f673 51 return Point(a.GetX() - b.GetX(), a.GetY() - b.GetY());
maximbolduc 47:d3123bb4f673 52 }
maximbolduc 47:d3123bb4f673 53
maximbolduc 47:d3123bb4f673 54 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
maximbolduc 47:d3123bb4f673 55 {
maximbolduc 47:d3123bb4f673 56 Point v = point_sub(line_end,line_start);
maximbolduc 47:d3123bb4f673 57 Point w = point_sub(P,line_start);
maximbolduc 47:d3123bb4f673 58
maximbolduc 47:d3123bb4f673 59 double c1 = dot(w,v);
maximbolduc 47:d3123bb4f673 60 double c2 = dot(v,v);
maximbolduc 47:d3123bb4f673 61 double b = c1 / c2;
maximbolduc 47:d3123bb4f673 62
maximbolduc 47:d3123bb4f673 63 Point resulting(b * v.GetX(),b*v.GetY());
maximbolduc 47:d3123bb4f673 64 Point Pb = point_add(line_start, resulting);
maximbolduc 47:d3123bb4f673 65
maximbolduc 47:d3123bb4f673 66 return d(P, Pb);
maximbolduc 47:d3123bb4f673 67 }
maximbolduc 47:d3123bb4f673 68
maximbolduc 47:d3123bb4f673 69 double lat_to_deg(char *s, char north_south)
maximbolduc 47:d3123bb4f673 70 {
maximbolduc 47:d3123bb4f673 71 int deg, min, sec;
maximbolduc 47:d3123bb4f673 72 double fsec, val;
maximbolduc 47:d3123bb4f673 73
maximbolduc 47:d3123bb4f673 74 deg = ( (s[0] - '0') * 10) + s[1] - '0';
maximbolduc 47:d3123bb4f673 75 min = ( (s[2] - '0') * 10) + s[3] - '0';
maximbolduc 47:d3123bb4f673 76 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
maximbolduc 47:d3123bb4f673 77 fsec = (double)((double)sec /10000.0);
maximbolduc 47:d3123bb4f673 78 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 47:d3123bb4f673 79 if (north_south == 'S') {
maximbolduc 47:d3123bb4f673 80 val *= -1.0;
maximbolduc 47:d3123bb4f673 81 }
maximbolduc 47:d3123bb4f673 82 return val;
maximbolduc 47:d3123bb4f673 83 }
maximbolduc 47:d3123bb4f673 84
maximbolduc 47:d3123bb4f673 85 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
maximbolduc 47:d3123bb4f673 86 // Input: three points P0, P1, and P2
maximbolduc 47:d3123bb4f673 87 // Return: >0 for P2 left of the line through P0 to P1
maximbolduc 47:d3123bb4f673 88 // =0 for P2 on the line
maximbolduc 47:d3123bb4f673 89 // <0 for P2 right of the line
maximbolduc 47:d3123bb4f673 90 int isLeft( Point P0, Point P1, Point P2 )
maximbolduc 47:d3123bb4f673 91 {
maximbolduc 47:d3123bb4f673 92 double isleft = ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX()) - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
maximbolduc 47:d3123bb4f673 93 if ( isleft > 0 ) {
maximbolduc 47:d3123bb4f673 94 isleft = 1;
maximbolduc 47:d3123bb4f673 95 } else {
maximbolduc 47:d3123bb4f673 96 isleft = -1;
maximbolduc 47:d3123bb4f673 97 }
maximbolduc 47:d3123bb4f673 98 return (int)isleft;
maximbolduc 47:d3123bb4f673 99 }
maximbolduc 47:d3123bb4f673 100
maximbolduc 47:d3123bb4f673 101 double lon_to_deg(char *s, char east_west)
maximbolduc 47:d3123bb4f673 102 {
maximbolduc 47:d3123bb4f673 103 int deg, min, sec;
maximbolduc 47:d3123bb4f673 104 double fsec, val;
maximbolduc 47:d3123bb4f673 105 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
maximbolduc 47:d3123bb4f673 106 min = ( (s[3] - '0') * 10) + s[4] - '0';
maximbolduc 47:d3123bb4f673 107 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
maximbolduc 47:d3123bb4f673 108 fsec = (double)((double)sec /10000.0);
maximbolduc 47:d3123bb4f673 109 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
maximbolduc 47:d3123bb4f673 110
maximbolduc 47:d3123bb4f673 111 if (east_west == 'W') {
maximbolduc 47:d3123bb4f673 112 val *= -1.0;
maximbolduc 47:d3123bb4f673 113 }
maximbolduc 47:d3123bb4f673 114 return val;
maximbolduc 47:d3123bb4f673 115 }
maximbolduc 47:d3123bb4f673 116
maximbolduc 47:d3123bb4f673 117 void nmea_gga(char *s)
maximbolduc 47:d3123bb4f673 118 {
maximbolduc 47:d3123bb4f673 119 char *token;
maximbolduc 47:d3123bb4f673 120 int token_counter = 0;
maximbolduc 47:d3123bb4f673 121 char *latitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 122 char *longitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 123 char *lat_dir = (char *)NULL;
maximbolduc 47:d3123bb4f673 124 char *lon_dir = (char *)NULL;
maximbolduc 47:d3123bb4f673 125 char *qual = (char *)NULL;
maximbolduc 47:d3123bb4f673 126 char *altitude = (char *)NULL;
maximbolduc 47:d3123bb4f673 127 char *sats = (char *)NULL;
maximbolduc 47:d3123bb4f673 128
maximbolduc 47:d3123bb4f673 129 token = strtok(s, ",");
maximbolduc 47:d3123bb4f673 130 while (token) {
maximbolduc 47:d3123bb4f673 131 switch (token_counter) {
maximbolduc 47:d3123bb4f673 132 case 2:
maximbolduc 47:d3123bb4f673 133 latitude = token;
maximbolduc 47:d3123bb4f673 134 break;
maximbolduc 47:d3123bb4f673 135 case 4:
maximbolduc 47:d3123bb4f673 136 longitude = token;
maximbolduc 47:d3123bb4f673 137 break;
maximbolduc 47:d3123bb4f673 138 case 3:
maximbolduc 47:d3123bb4f673 139 lat_dir = token;
maximbolduc 47:d3123bb4f673 140 break;
maximbolduc 47:d3123bb4f673 141 case 5:
maximbolduc 47:d3123bb4f673 142 lon_dir = token;
maximbolduc 47:d3123bb4f673 143 break;
maximbolduc 47:d3123bb4f673 144 case 6:
maximbolduc 47:d3123bb4f673 145 qual = token;
maximbolduc 47:d3123bb4f673 146 break;
maximbolduc 47:d3123bb4f673 147 case 7:
maximbolduc 47:d3123bb4f673 148 sats = token;
maximbolduc 47:d3123bb4f673 149 break;
maximbolduc 47:d3123bb4f673 150 case 9:
maximbolduc 47:d3123bb4f673 151 altitude = token;
maximbolduc 47:d3123bb4f673 152 break;
maximbolduc 47:d3123bb4f673 153 }
maximbolduc 47:d3123bb4f673 154 token = strtok((char *)NULL, ",");
maximbolduc 47:d3123bb4f673 155 token_counter++;
maximbolduc 47:d3123bb4f673 156 }
maximbolduc 47:d3123bb4f673 157 if (latitude && longitude && altitude && sats) {
maximbolduc 47:d3123bb4f673 158 decimal_latitude = lat_to_deg(latitude, lat_dir[0]);
maximbolduc 47:d3123bb4f673 159 decimal_lon = lon_to_deg(longitude, lon_dir[0]);
maximbolduc 47:d3123bb4f673 160 num_of_gps_sats = atoi(sats);
maximbolduc 47:d3123bb4f673 161 gps_satellite_quality = atoi(qual);
maximbolduc 47:d3123bb4f673 162 } else {
maximbolduc 47:d3123bb4f673 163 gps_satellite_quality = 0;
maximbolduc 47:d3123bb4f673 164 }
maximbolduc 47:d3123bb4f673 165 }
maximbolduc 47:d3123bb4f673 166
maximbolduc 47:d3123bb4f673 167 char dms[128];
maximbolduc 47:d3123bb4f673 168 char* To_DMS(double dec_deg)
maximbolduc 47:d3123bb4f673 169 {
maximbolduc 47:d3123bb4f673 170 dec_deg = abs(dec_deg);
maximbolduc 47:d3123bb4f673 171 int d = (int)(dec_deg);
maximbolduc 47:d3123bb4f673 172 sprintf(dms,"%0.2i\0",d);
maximbolduc 47:d3123bb4f673 173 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 47:d3123bb4f673 174 if (m < 10 ) {
maximbolduc 47:d3123bb4f673 175 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 176 } else {
maximbolduc 47:d3123bb4f673 177 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 178 }
maximbolduc 47:d3123bb4f673 179 return dms;
maximbolduc 47:d3123bb4f673 180 }
maximbolduc 47:d3123bb4f673 181
maximbolduc 47:d3123bb4f673 182 char* To_DMS_lon(double dec_deg)
maximbolduc 47:d3123bb4f673 183 {
maximbolduc 47:d3123bb4f673 184 dec_deg = abs(dec_deg);
maximbolduc 47:d3123bb4f673 185 int d = (int)(dec_deg);
maximbolduc 47:d3123bb4f673 186 sprintf(dms,"%0.3i\0",d);
maximbolduc 47:d3123bb4f673 187 double m = (double)(((double)dec_deg - (double)d) * 60.0);
maximbolduc 47:d3123bb4f673 188 if (m < 10 ) {
maximbolduc 47:d3123bb4f673 189 sprintf(dms,"%s0%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 190 } else {
maximbolduc 47:d3123bb4f673 191 sprintf(dms,"%s%0.9f\0",dms,m);
maximbolduc 47:d3123bb4f673 192 }
maximbolduc 47:d3123bb4f673 193 return dms;
maximbolduc 47:d3123bb4f673 194 }
maximbolduc 47:d3123bb4f673 195
maximbolduc 47:d3123bb4f673 196 //from farmerGPS code
maximbolduc 47:d3123bb4f673 197 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
maximbolduc 47:d3123bb4f673 198 {
maximbolduc 47:d3123bb4f673 199 double ydist = 0;
maximbolduc 47:d3123bb4f673 200 double xdist = 0;
maximbolduc 47:d3123bb4f673 201 angle = angle + 180;
maximbolduc 47:d3123bb4f673 202 double radiant = angle * 3.14159265359 / 180;
maximbolduc 47:d3123bb4f673 203 double sinr = sin(radiant);
maximbolduc 47:d3123bb4f673 204 double cosr = cos(radiant);
maximbolduc 47:d3123bb4f673 205 xdist = cosr * distance;
maximbolduc 47:d3123bb4f673 206 ydist = sinr * distance;
maximbolduc 47:d3123bb4f673 207 lat2 = lat1 + (ydist / (69.09 * -1609.344));
maximbolduc 47:d3123bb4f673 208 lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
maximbolduc 48:5d9c63364c94 209 }
maximbolduc 48:5d9c63364c94 210
maximbolduc 48:5d9c63364c94 211 bool nmea_rmc(char *s)
maximbolduc 48:5d9c63364c94 212 {
maximbolduc 48:5d9c63364c94 213 char *token;
maximbolduc 48:5d9c63364c94 214 int token_counter = 0;
maximbolduc 48:5d9c63364c94 215 time_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 216 date = (char *)NULL;
maximbolduc 48:5d9c63364c94 217 stat = (char *)NULL;
maximbolduc 48:5d9c63364c94 218 vel = (char *)NULL;
maximbolduc 48:5d9c63364c94 219 trk = (char *)NULL;
maximbolduc 48:5d9c63364c94 220 magv = (char *)NULL;
maximbolduc 48:5d9c63364c94 221 magd = (char *)NULL;
maximbolduc 48:5d9c63364c94 222 latit = "";
maximbolduc 48:5d9c63364c94 223 longit = "";
maximbolduc 48:5d9c63364c94 224 latitude_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 225 longitude_s = (char *)NULL;
maximbolduc 48:5d9c63364c94 226 lat_dir = (char *)NULL;
maximbolduc 48:5d9c63364c94 227 lon_dir = (char *)NULL;
maximbolduc 48:5d9c63364c94 228 while ((token = strsep(&s, ",")) != NULL) {
maximbolduc 48:5d9c63364c94 229 switch (token_counter) {
maximbolduc 48:5d9c63364c94 230 case 1:
maximbolduc 48:5d9c63364c94 231 time_s = token;
maximbolduc 48:5d9c63364c94 232 break;
maximbolduc 48:5d9c63364c94 233 case 2:
maximbolduc 48:5d9c63364c94 234 stat = token;
maximbolduc 48:5d9c63364c94 235 break;
maximbolduc 48:5d9c63364c94 236 case 3:
maximbolduc 48:5d9c63364c94 237 if ( token ) {
maximbolduc 48:5d9c63364c94 238 latit = token;
maximbolduc 48:5d9c63364c94 239 latitude_s = token;
maximbolduc 48:5d9c63364c94 240 }
maximbolduc 48:5d9c63364c94 241 break;
maximbolduc 48:5d9c63364c94 242 case 4:
maximbolduc 48:5d9c63364c94 243 lat_dir = token;
maximbolduc 48:5d9c63364c94 244 break;
maximbolduc 48:5d9c63364c94 245 case 5:
maximbolduc 48:5d9c63364c94 246 longit = token;
maximbolduc 48:5d9c63364c94 247 longitude_s = token;
maximbolduc 48:5d9c63364c94 248 break;
maximbolduc 48:5d9c63364c94 249 case 6:
maximbolduc 48:5d9c63364c94 250 lon_dir = token;
maximbolduc 48:5d9c63364c94 251 break;
maximbolduc 48:5d9c63364c94 252 case 7:
maximbolduc 48:5d9c63364c94 253 vel = token;
maximbolduc 48:5d9c63364c94 254 break;
maximbolduc 48:5d9c63364c94 255 case 8:
maximbolduc 48:5d9c63364c94 256 trk = token;
maximbolduc 48:5d9c63364c94 257 break;
maximbolduc 48:5d9c63364c94 258 case 9:
maximbolduc 48:5d9c63364c94 259 date = token;
maximbolduc 48:5d9c63364c94 260 break;
maximbolduc 48:5d9c63364c94 261 case 10:
maximbolduc 48:5d9c63364c94 262 magv = token;
maximbolduc 48:5d9c63364c94 263 break;
maximbolduc 48:5d9c63364c94 264 case 11:
maximbolduc 48:5d9c63364c94 265 magd = token;
maximbolduc 48:5d9c63364c94 266 break;
maximbolduc 48:5d9c63364c94 267 }
maximbolduc 48:5d9c63364c94 268 token_counter++;
maximbolduc 48:5d9c63364c94 269 }
maximbolduc 48:5d9c63364c94 270 if (stat!= '\0' && date!= '\0' && time_s!= '\0') {
maximbolduc 48:5d9c63364c94 271 hour = (char)((time_s[0] - '0') * 10) + (time_s[1] - '0');
maximbolduc 48:5d9c63364c94 272 minute = (char)((time_s[2] - '0') * 10) + (time_s[3] - '0');
maximbolduc 48:5d9c63364c94 273 second = (char)((time_s[4] - '0') * 10) + (time_s[5] - '0');
maximbolduc 48:5d9c63364c94 274 day = (char)((date[0] - '0') * 10) + (date[1] - '0');
maximbolduc 48:5d9c63364c94 275 month = (char)((date[2] - '0') * 10) + (date[3] - '0');
maximbolduc 48:5d9c63364c94 276 year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
maximbolduc 48:5d9c63364c94 277 status = stat[0];
maximbolduc 48:5d9c63364c94 278 velocity = atof(vel);
maximbolduc 48:5d9c63364c94 279 speed_km = velocity * 1.852;
maximbolduc 48:5d9c63364c94 280 speed_m_s = speed_km * 3600.0 / 1000.0;
maximbolduc 48:5d9c63364c94 281 track = atof(trk);
maximbolduc 48:5d9c63364c94 282 magvar = atof(magv);
maximbolduc 48:5d9c63364c94 283 return true;
maximbolduc 48:5d9c63364c94 284 }
maximbolduc 48:5d9c63364c94 285 if ( longit != '\0' && latit != '\0' ) {
maximbolduc 48:5d9c63364c94 286 old_position = position;
maximbolduc 48:5d9c63364c94 287 position.SetX(lat_to_deg(latitude_s, lat_dir[0]));
maximbolduc 48:5d9c63364c94 288 position.SetY(lon_to_deg(longitude_s, lon_dir[0]));
maximbolduc 48:5d9c63364c94 289 return true;
maximbolduc 48:5d9c63364c94 290 } else {
maximbolduc 48:5d9c63364c94 291 return false;
maximbolduc 48:5d9c63364c94 292 }
maximbolduc 47:d3123bb4f673 293 }