SDP Code
Fork of MODGPS by
Embed:
(wiki syntax)
Show/hide line numbers
GPS_Geodetic.cpp
00001 /* 00002 Copyright (c) 2010 Andy Kirkham 00003 00004 Permission is hereby granted, free of charge, to any person obtaining a copy 00005 of this software and associated documentation files (the "Software"), to deal 00006 in the Software without restriction, including without limitation the rights 00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00008 copies of the Software, and to permit persons to whom the Software is 00009 furnished to do so, subject to the following conditions: 00010 00011 The above copyright notice and this permission notice shall be included in 00012 all copies or substantial portions of the Software. 00013 00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00020 THE SOFTWARE. 00021 */ 00022 00023 #include "GPS_Geodetic.h" 00024 00025 void 00026 GPS_Geodetic::nmea_gga(char *s) { 00027 char *token; 00028 int token_counter = 0; 00029 char *latitude = (char *)NULL; 00030 char *longitude = (char *)NULL; 00031 char *lat_dir = (char *)NULL; 00032 char *lon_dir = (char *)NULL; 00033 char *qual = (char *)NULL; 00034 char *altitude = (char *)NULL; 00035 char *sats = (char *)NULL; 00036 00037 token = strtok(s, ","); 00038 while (token) { 00039 switch (token_counter) { 00040 case 2: latitude = token; break; 00041 case 4: longitude = token; break; 00042 case 3: lat_dir = token; break; 00043 case 5: lon_dir = token; break; 00044 case 6: qual = token; break; 00045 case 7: sats = token; break; 00046 case 9: altitude = token; break; 00047 } 00048 token = strtok((char *)NULL, ","); 00049 token_counter++; 00050 } 00051 00052 // If the fix quality is valid set our location information. 00053 if (latitude && longitude && altitude && sats) { 00054 lat = convert_lat_coord(latitude, lat_dir[0]); 00055 lon = convert_lon_coord(longitude, lon_dir[0]); 00056 alt = convert_height(altitude); 00057 num_of_gps_sats = atoi(sats); 00058 gps_satellite_quality = atoi(qual); 00059 } 00060 else { 00061 gps_satellite_quality = 0; 00062 } 00063 } 00064 00065 double 00066 GPS_Geodetic::convert_lat_coord(char *s, char north_south) 00067 { 00068 int deg, min, sec; 00069 double fsec, val; 00070 00071 deg = ( (s[0] - '0') * 10) + s[1] - '0'; 00072 min = ( (s[2] - '0') * 10) + s[3] - '0'; 00073 sec = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0')); 00074 fsec = (double)((double)sec /10000.0); 00075 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); 00076 if (north_south == 'S') { val *= -1.0; } 00077 lat = val; 00078 return val; 00079 } 00080 00081 double 00082 GPS_Geodetic::convert_lon_coord(char *s, char east_west) 00083 { 00084 int deg, min, sec; 00085 double fsec, val; 00086 00087 deg = ( (s[0] - '0') * 100) + ((s[1] - '0') * 10) + (s[2] - '0'); 00088 min = ( (s[3] - '0') * 10) + s[4] - '0'; 00089 sec = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0')); 00090 fsec = (double)((double)sec /10000.0); 00091 val = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0); 00092 if (east_west == 'W') { val *= -1.0; } 00093 lon = val; 00094 return val; 00095 00096 } 00097 00098 double 00099 GPS_Geodetic::convert_height(char *s) 00100 { 00101 double val = (double)(atof(s) / 1000.0); 00102 alt = val; 00103 return val; 00104 } 00105 00106
Generated on Thu Jul 14 2022 03:46:43 by 1.7.2