GPS.cpp

00001 #include "GPS.h"
00002 
00003 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
00004     _gps.baud(9600);
00005     nmea_longitude = 0.0;
00006     nmea_latitude = 0.0;
00007     utc_time = 0;
00008     ns = ' ';
00009     ew = ' ';
00010     lock = 0;
00011     satelites = 0;
00012     hdop = 0.0;
00013     msl_altitude = 0.0;
00014     msl_units = ' ';
00015 
00016     rmc_status = ' ';
00017     speed_k = 0.0;
00018     course_d = 0.0;
00019     date = 0;
00020 
00021     dec_longitude = 0.0;
00022     dec_latitude = 0.0;
00023 
00024     gll_status = ' ';
00025 
00026     course_t = 0.0; // ground speed true
00027     course_t_unit = ' ';
00028     course_m = 0.0; // magnetic
00029     course_m_unit = ' ';
00030     speed_k_unit = ' ';
00031     speed_km = 0.0; // speek km/hr
00032     speed_km_unit = ' ';
00033 
00034     altitude_ft = 0.0;
00035 #ifdef OPEN_LOG
00036     is_logging = false;
00037 #endif
00038 }
00039 
00040 #ifdef OPEN_LOG
00041 void GPS::start_log() {
00042     is_logging = true;
00043 }
00044 
00045 void GPS::new_file(void) {
00046     _openLog.newFile();
00047 }
00048 
00049 void GPS::stop_log(void) {
00050     is_logging = false;
00051 }
00052 #endif
00053 
00054 float GPS::nmea_to_dec(float deg_coord, char nsew) {
00055     int degree = (int)(deg_coord/100);
00056     float minutes = deg_coord - degree*100;
00057     float dec_deg = minutes / 60;
00058     float decimal = degree + dec_deg;
00059     if (nsew == 'S' || nsew == 'W') { // return negative
00060         decimal *= -1;
00061     }
00062     return decimal;
00063 }
00064 
00065 int GPS::sample() {
00066     int line_parsed = 0;
00067 
00068     if (_gps.readable()) {
00069         getline();
00070     
00071 #ifdef OPEN_LOG
00072         if (is_logging && lock) {
00073             format_for_log();
00074             _openLog.write(bfr);
00075         }
00076 #endif
00077 
00078         FILE *fp= fopen("/sd/mydir/gps_data.txt", "a");
00079         if(fp!=NULL){
00080         fprintf(fp,"%s\n",msg);
00081         fclose(fp);
00082         }
00083         // Check if it is a GPGGA msg (matches both locked and non-locked msg)
00084         if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &lock, &satelites, &hdop, &msl_altitude, &msl_units) >= 1) {
00085             line_parsed = GGA;
00086         }
00087         // Check if it is a GPRMC msg
00088         else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%f,%d", &utc_time, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
00089             line_parsed = RMC;
00090         }
00091         // GLL - Geographic Position-Lat/Lon
00092         else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
00093             line_parsed = GLL;
00094         }
00095         // VTG-Course Over Ground and Ground Speed
00096         else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
00097             line_parsed = VTG;
00098         }
00099         
00100         if(satelites == 0) {
00101             lock = 0;
00102         }
00103     }
00104     if (!lock) {
00105         return NO_LOCK;
00106     } else if (line_parsed) {
00107         return line_parsed;
00108     } else {
00109         return NOT_PARSED;
00110     }
00111 }
00112 
00113 
00114 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
00115 float GPS::trunc(float v) {
00116     if (v < 0.0) {
00117         v*= -1.0;
00118         v = floor(v);
00119         v*=-1.0;
00120     } else {
00121         v = floor(v);
00122     }
00123     return v;
00124 }
00125 
00126 void GPS::getline() {
00127     while (_gps.getc() != '$');   // wait for the start of a line
00128     for (int i=0; i<1022; i++) {
00129         msg[i] = _gps.getc();
00130         if (msg[i] == '\r') {
00131             msg[i] = 0;
00132             return;
00133         }else if(msg[i]=='$'){
00134             msg[i]=0;
00135             return;
00136             }
00137     }
00138     error("Overflow in getline");
00139 }
00140 
00141 void GPS::format_for_log() {
00142     bfr[0] = '$';
00143     for (int i = 0; i < 1022; i++) {
00144         bfr[i+1] = msg[i];
00145         if (msg[i] == 0 || msg[i] =='$') {
00146             bfr[i+1] = '\r';
00147             bfr[i+2] = '\n';
00148             bfr[i+3] = 0;
00149             return;
00150         }
00151     }
00152     error("Overflow in format");
00153 }
00154 
00155 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
00156 float GPS::get_msl_altitude() {
00157     if (!lock)
00158         return 0.0;
00159     else
00160         return msl_altitude;
00161 }
00162 
00163 int GPS::get_satelites() {
00164     if (!lock)
00165         return 0;
00166     else
00167         return satelites;
00168 }
00169 
00170 float GPS::get_nmea_longitude() {
00171     if (!lock)
00172         return 0.0;
00173     else
00174         return nmea_longitude;
00175 }
00176 
00177 float GPS::get_dec_longitude() {
00178     dec_longitude = nmea_to_dec(nmea_longitude, ew);
00179     if (!lock)
00180         return 0.0;
00181     else
00182         return dec_longitude;
00183 }
00184 
00185 float GPS::get_nmea_latitude() {
00186     if (!lock)
00187         return 0.0;
00188     else
00189         return nmea_latitude;
00190 }
00191 
00192 float GPS::get_dec_latitude() {
00193     dec_latitude = nmea_to_dec(nmea_latitude, ns);
00194     if (!lock)
00195         return 0.0;
00196     else
00197         return dec_latitude;
00198 }
00199 
00200 float GPS::get_course_t() {
00201     if (!lock)
00202         return 0.0;
00203     else
00204         return course_t;
00205 }
00206 
00207 float GPS::get_course_m() {
00208     if (!lock)
00209         return 0.0;
00210     else
00211         return course_m;
00212 }
00213 
00214 float GPS::get_speed_k() {
00215     if (!lock)
00216         return 0.0;
00217     else
00218         return speed_k;
00219 }
00220 
00221 float GPS::get_speed_km() {
00222     if (!lock)
00223         return 0.0;
00224     else
00225         return speed_km;
00226 }
00227 
00228 float GPS::get_altitude_ft() {
00229     if (!lock)
00230         return 0.0;
00231     else
00232         return 3.280839895*msl_altitude;
00233 }
00234 
00235 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
00236 float GPS::calc_course_to(float pointLat, float pontLong) {
00237     const double d2r = PI / 180.0;
00238     const double r2d = 180.0 / PI;
00239     double dlat = abs(pointLat - get_dec_latitude()) * d2r;
00240     double dlong = abs(pontLong - get_dec_longitude()) * d2r;
00241     double y = sin(dlong) * cos(pointLat * d2r);
00242     double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
00243     return atan2(y,x)*r2d;
00244 }    
00245 
00246 /*
00247 var y = Math.sin(dLon) * Math.cos(lat2);
00248 var x = Math.cos(lat1)*Math.sin(lat2) -
00249         Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
00250 var brng = Math.atan2(y, x).toDeg();
00251 */
00252 
00253 /*
00254             The Haversine formula according to Dr. Math.
00255             http://mathforum.org/library/drmath/view/51879.html
00256                 
00257             dlon = lon2 - lon1
00258             dlat = lat2 - lat1
00259             a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
00260             c = 2 * atan2(sqrt(a), sqrt(1-a)) 
00261             d = R * c
00262                 
00263             Where
00264                 * dlon is the change in longitude
00265                 * dlat is the change in latitude
00266                 * c is the great circle distance in Radians.
00267                 * R is the radius of a spherical Earth.
00268                 * The locations of the two points in 
00269                     spherical coordinates (longitude and 
00270                     latitude) are lon1,lat1 and lon2, lat2.
00271 */
00272 double GPS::calc_dist_to_mi(float pointLat, float pontLong) {
00273     const double d2r = PI / 180.0;
00274     double dlat = pointLat - get_dec_latitude();
00275     double dlong = pontLong - get_dec_longitude();
00276     double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
00277     double c = 2.0 * asin(sqrt(abs(a)));
00278     double d = 63.765 * c;
00279     
00280     return d;
00281 }
00282 
00283 double GPS::calc_dist_to_ft(float pointLat, float pontLong) {
00284     return calc_dist_to_mi(pointLat, pontLong)*5280.0;
00285 }
00286 
00287 double GPS::calc_dist_to_km(float pointLat, float pontLong) {
00288     return calc_dist_to_mi(pointLat, pontLong)*1.609344;
00289 }
00290 
00291 double GPS::calc_dist_to_m(float pointLat, float pontLong) {
00292     return calc_dist_to_mi(pointLat, pontLong)*1609.344;
00293 }
00294 
00295