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