User | Revision | Line number | New contents of line |
pd0wm |
0:bec310bde899
|
1
|
/*
|
pd0wm |
0:bec310bde899
|
2
|
Copyright (c) 2010 Andy Kirkham
|
pd0wm |
0:bec310bde899
|
3
|
|
pd0wm |
0:bec310bde899
|
4
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
pd0wm |
0:bec310bde899
|
5
|
of this software and associated documentation files (the "Software"), to deal
|
pd0wm |
0:bec310bde899
|
6
|
in the Software without restriction, including without limitation the rights
|
pd0wm |
0:bec310bde899
|
7
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
pd0wm |
0:bec310bde899
|
8
|
copies of the Software, and to permit persons to whom the Software is
|
pd0wm |
0:bec310bde899
|
9
|
furnished to do so, subject to the following conditions:
|
pd0wm |
0:bec310bde899
|
10
|
|
pd0wm |
0:bec310bde899
|
11
|
The above copyright notice and this permission notice shall be included in
|
pd0wm |
0:bec310bde899
|
12
|
all copies or substantial portions of the Software.
|
pd0wm |
0:bec310bde899
|
13
|
|
pd0wm |
0:bec310bde899
|
14
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
pd0wm |
0:bec310bde899
|
15
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
pd0wm |
0:bec310bde899
|
16
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
pd0wm |
0:bec310bde899
|
17
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
pd0wm |
0:bec310bde899
|
18
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
pd0wm |
0:bec310bde899
|
19
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
pd0wm |
0:bec310bde899
|
20
|
THE SOFTWARE.
|
pd0wm |
0:bec310bde899
|
21
|
*/
|
pd0wm |
0:bec310bde899
|
22
|
|
pd0wm |
0:bec310bde899
|
23
|
#include "GPS_Geodetic.h"
|
pd0wm |
0:bec310bde899
|
24
|
|
pd0wm |
0:bec310bde899
|
25
|
void
|
pd0wm |
0:bec310bde899
|
26
|
GPS_Geodetic::nmea_gga(char *s) {
|
pd0wm |
0:bec310bde899
|
27
|
char *token;
|
pd0wm |
0:bec310bde899
|
28
|
int token_counter = 0;
|
pd0wm |
0:bec310bde899
|
29
|
char *latitude = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
30
|
char *longitude = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
31
|
char *lat_dir = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
32
|
char *lon_dir = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
33
|
char *qual = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
34
|
char *altitude = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
35
|
char *sats = (char *)NULL;
|
pd0wm |
0:bec310bde899
|
36
|
|
pd0wm |
0:bec310bde899
|
37
|
int nw = 0;
|
pd0wm |
0:bec310bde899
|
38
|
int n = 0;
|
pd0wm |
0:bec310bde899
|
39
|
|
pd0wm |
0:bec310bde899
|
40
|
while (s[n] != '\n' && n < 128){
|
pd0wm |
0:bec310bde899
|
41
|
if (s[n] == 'N' || s[n] == 'S' || s[n] == 'E' || s[n] == 'W')
|
pd0wm |
0:bec310bde899
|
42
|
nw++;
|
pd0wm |
0:bec310bde899
|
43
|
n++;
|
pd0wm |
0:bec310bde899
|
44
|
}
|
pd0wm |
0:bec310bde899
|
45
|
|
pd0wm |
0:bec310bde899
|
46
|
token = strtok(s, ",");
|
pd0wm |
0:bec310bde899
|
47
|
while (token) {
|
pd0wm |
0:bec310bde899
|
48
|
switch (token_counter) {
|
pd0wm |
0:bec310bde899
|
49
|
case 2: latitude = token; break;
|
pd0wm |
0:bec310bde899
|
50
|
case 4: longitude = token; break;
|
pd0wm |
0:bec310bde899
|
51
|
case 3: lat_dir = token; break;
|
pd0wm |
0:bec310bde899
|
52
|
case 5: lon_dir = token; break;
|
pd0wm |
0:bec310bde899
|
53
|
case 6: qual = token; break;
|
pd0wm |
0:bec310bde899
|
54
|
case 7: sats = token; break;
|
pd0wm |
0:bec310bde899
|
55
|
case 9: altitude = token; break;
|
pd0wm |
0:bec310bde899
|
56
|
}
|
pd0wm |
0:bec310bde899
|
57
|
token = strtok((char *)NULL, ",");
|
pd0wm |
0:bec310bde899
|
58
|
token_counter++;
|
pd0wm |
0:bec310bde899
|
59
|
}
|
pd0wm |
0:bec310bde899
|
60
|
|
pd0wm |
0:bec310bde899
|
61
|
// If the fix quality is valid set our location information.
|
pd0wm |
0:bec310bde899
|
62
|
if (latitude && longitude && altitude && sats && nw == 2) {
|
pd0wm |
0:bec310bde899
|
63
|
lat = convert_lat_coord(latitude, lat_dir[0]);
|
pd0wm |
0:bec310bde899
|
64
|
lon = convert_lon_coord(longitude, lon_dir[0]);
|
pd0wm |
0:bec310bde899
|
65
|
alt = convert_height(altitude);
|
pd0wm |
0:bec310bde899
|
66
|
num_of_gps_sats = atoi(sats);
|
pd0wm |
0:bec310bde899
|
67
|
gps_satellite_quality = atoi(qual);
|
pd0wm |
0:bec310bde899
|
68
|
}
|
pd0wm |
0:bec310bde899
|
69
|
else {
|
pd0wm |
0:bec310bde899
|
70
|
gps_satellite_quality = 0;
|
pd0wm |
0:bec310bde899
|
71
|
//lat = 0;
|
pd0wm |
0:bec310bde899
|
72
|
//lon = 0;
|
pd0wm |
0:bec310bde899
|
73
|
}
|
pd0wm |
0:bec310bde899
|
74
|
}
|
pd0wm |
0:bec310bde899
|
75
|
|
pd0wm |
0:bec310bde899
|
76
|
double
|
pd0wm |
0:bec310bde899
|
77
|
GPS_Geodetic::convert_lat_coord(char *s, char north_south)
|
pd0wm |
0:bec310bde899
|
78
|
{
|
pd0wm |
0:bec310bde899
|
79
|
int deg, min, sec;
|
pd0wm |
0:bec310bde899
|
80
|
double fsec, val;
|
pd0wm |
0:bec310bde899
|
81
|
|
pd0wm |
0:bec310bde899
|
82
|
deg = ( (s[0] - '0') * 10) + s[1] - '0';
|
pd0wm |
0:bec310bde899
|
83
|
min = ( (s[2] - '0') * 10) + s[3] - '0';
|
pd0wm |
0:bec310bde899
|
84
|
sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
|
pd0wm |
0:bec310bde899
|
85
|
fsec = (double)((double)sec /10000.0);
|
pd0wm |
0:bec310bde899
|
86
|
val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
|
pd0wm |
0:bec310bde899
|
87
|
if (north_south == 'S') { val *= -1.0; }
|
pd0wm |
0:bec310bde899
|
88
|
lat = val;
|
pd0wm |
0:bec310bde899
|
89
|
return val;
|
pd0wm |
0:bec310bde899
|
90
|
}
|
pd0wm |
0:bec310bde899
|
91
|
|
pd0wm |
0:bec310bde899
|
92
|
double
|
pd0wm |
0:bec310bde899
|
93
|
GPS_Geodetic::convert_lon_coord(char *s, char east_west)
|
pd0wm |
0:bec310bde899
|
94
|
{
|
pd0wm |
0:bec310bde899
|
95
|
int deg, min, sec;
|
pd0wm |
0:bec310bde899
|
96
|
double fsec, val;
|
pd0wm |
0:bec310bde899
|
97
|
|
pd0wm |
0:bec310bde899
|
98
|
deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0');
|
pd0wm |
0:bec310bde899
|
99
|
min = ( (s[3] - '0') * 10) + s[4] - '0';
|
pd0wm |
0:bec310bde899
|
100
|
sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
|
pd0wm |
0:bec310bde899
|
101
|
fsec = (double)((double)sec /10000.0);
|
pd0wm |
0:bec310bde899
|
102
|
val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
|
pd0wm |
0:bec310bde899
|
103
|
if (east_west == 'W') { val *= -1.0; }
|
pd0wm |
0:bec310bde899
|
104
|
lon = val;
|
pd0wm |
0:bec310bde899
|
105
|
return val;
|
pd0wm |
0:bec310bde899
|
106
|
|
pd0wm |
0:bec310bde899
|
107
|
}
|
pd0wm |
0:bec310bde899
|
108
|
|
pd0wm |
0:bec310bde899
|
109
|
double
|
pd0wm |
0:bec310bde899
|
110
|
GPS_Geodetic::convert_height(char *s)
|
pd0wm |
0:bec310bde899
|
111
|
{
|
pd0wm |
0:bec310bde899
|
112
|
double val = (double)(atof(s) / 1000.0);
|
pd0wm |
0:bec310bde899
|
113
|
alt = val;
|
pd0wm |
0:bec310bde899
|
114
|
return val;
|
pd0wm |
0:bec310bde899
|
115
|
}
|
pd0wm |
0:bec310bde899
|
116
|
|
pd0wm |
0:bec310bde899
|
117
|
|