GPS.C

Go to the documentation of this file.
00001 /*!@file Devices/GPS.C Serial interface to an NMEA 0183 GPS unit (Garmin Geko 301) */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2002   //
00005 // by the University of Southern California (USC) and the iLab at USC.  //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Laurent Itti <itti@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Devices/GPS.C $
00035 // $Id: GPS.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Devices/GPS.H"
00039 #include "Component/OptionManager.H"
00040 #include "rutz/compat_snprintf.h"
00041 #include <cstdlib>
00042 #include <pthread.h>
00043 
00044 // ######################################################################
00045 GPSlistener::~GPSlistener()
00046 {  }
00047 
00048 // ######################################################################
00049 GPSdata::GPSdata()
00050 { fixtype = 0; }
00051 
00052 // ######################################################################
00053 void GPSdata::toString(char *str, const int siz) const
00054 {
00055   snprintf(str, siz, "Fix=%d/%d*%04d/%02d/%02d*%02d:%02d:%02d "
00056            "Lat=%.6f Lon=%.6f Alt=%.2f Galt=%.2f Sp=%.2f Heading=%.2f "
00057            "Mvar=%.2f Dil=%.1f/%.1f/%.1f Err=%.1f/%.1f/%.1f",
00058            fixtype, nsat, fixye, fixmo, fixda, fixho, fixmi,
00059            fixse, latitude, longitude, altitude, galtitude, speed, heading,
00060            magvar, pdil, hdil, vdil, epe, hpe, vpe);
00061 }
00062 
00063 // ######################################################################
00064 bool GPSdata::fromString(const char *str)
00065 {
00066   int ifixtype, insat, ifixho, ifixmi, ifixse, ifixda, ifixmo;
00067   if (sscanf(str, "Fix=%d/%d*%d/%d/%d*%d:%d:%d "
00068              "Lat=%lf Lon=%lf Alt=%f Galt=%f Sp=%f Heading=%f "
00069              "Mvar=%f Dil=%f/%f/%f Err=%f/%f/%f",
00070              &ifixtype, &insat, &fixye, &ifixmo, &ifixda, &ifixho, &ifixmi,
00071              &ifixse, &latitude, &longitude, &altitude, &galtitude, &speed,
00072              &heading, &magvar, &pdil, &hdil, &vdil, &epe, &hpe, &vpe) != 20)
00073     {
00074       LERROR("Conversion failed -- MARKING AS BAD");
00075       fixtype = 0;
00076       return false;
00077     }
00078   fixtype = byte(ifixtype); nsat = byte(insat); fixmo = byte(ifixmo);
00079   fixda = byte(ifixda); fixho = byte(ifixho); fixmi = byte(ifixmi);
00080   fixse = byte(ifixse);
00081   return true;
00082 }
00083 
00084 // ######################################################################
00085 void *gps_run(void *c)
00086 {
00087   GPS *cc = (GPS *)c;
00088   cc->run();
00089   return NULL;
00090 }
00091 
00092 // ######################################################################
00093 GPS::GPS(OptionManager& mgr, const std::string& descrName,
00094          const std::string& tagName) :
00095   ModelComponent(mgr, descrName, tagName),
00096   itsSerial(new Serial(mgr, "GPS Serial Port", "GPSserial")),
00097   itsData(), itsKeepgoing(false), itsGotnew(false), itsListener(NULL)
00098 {
00099   // set a default config for our serial port:
00100   itsSerial->configure("/dev/ttyS0", 4800, "8N1", false, false, 20000);
00101 
00102   // adopt our serial port as a subcomponent:
00103   addSubComponent(itsSerial);
00104 
00105   // initialize our mutex:
00106   pthread_mutex_init(&itsLock, NULL);
00107 }
00108 
00109 // ######################################################################
00110 void GPS::setListener(rutz::shared_ptr<GPSlistener>& listener)
00111 { itsListener = listener; }
00112 
00113 // ######################################################################
00114 GPS::~GPS()
00115 {
00116   pthread_mutex_destroy(&itsLock);
00117 }
00118 
00119 // ######################################################################
00120 void GPS::start2()
00121 {
00122   // since we are in start2(), we know our serial is up already. So
00123   // here we just need to get our thread going:
00124   itsKeepgoing = true;
00125   pthread_create(&itsRunner, NULL, &gps_run, (void *)this);
00126 }
00127 
00128 // ######################################################################
00129 void GPS::stop1()
00130 {
00131   itsKeepgoing = false;
00132   usleep(300000); // make sure thread has exited
00133 }
00134 
00135 // ######################################################################
00136 bool GPS::getData(GPSdata& data)
00137 {
00138   pthread_mutex_lock(&itsLock);
00139   data = itsData; bool ret = itsGotnew; itsGotnew = false;
00140   pthread_mutex_unlock(&itsLock);
00141   return ret;
00142 }
00143 
00144 // ######################################################################
00145 void GPS::run()
00146 {
00147   char buf[100]; // NMEA 0183 limits sentences to 80 chars
00148   int i = 0, retry = 10, idx[50];
00149   float heading = 0.0f; // will decide btw GPS and compass based on speed
00150   bool gotstart = false;
00151 
00152   while(itsKeepgoing)
00153     {
00154       //if (retry < 0) LINFO("Too many serial errors");
00155 
00156       // let's receive the data, byte per byte:
00157       int ret = itsSerial->read(&buf[i], 1);
00158       if (ret == 0)
00159         { LDEBUG("Timeout on read() -- WAITING MORE");
00160         --retry; gotstart = false; continue; }
00161       if (ret != 1)
00162         { LDEBUG("read() error -- DROP"); buf[0] = 'X';
00163         --retry; gotstart = false; continue; }
00164 
00165       // read went well
00166       ++i; retry = 10;
00167 
00168       // buffer overflow?
00169       if (i >= 100)
00170         { LERROR("Serial buffer overflow -- TRASHING");
00171         i = 0; gotstart = false; continue; }
00172 
00173       // complete sentence received?
00174       if (buf[i-1] != '\n') continue;
00175 
00176       // data is ready for decoding:
00177       i -= 2; buf[i] = '\0';
00178       LDEBUG("Received: %s", buf);
00179 
00180       // check the checksum:
00181       if (i < 4) { LERROR("Short sentence -- DROP");
00182       i = 0; gotstart = false; continue; }
00183       byte chksum = 0; for (int j = 1; j < i - 3; j ++) chksum ^= buf[j];
00184       if (chksum != strtol(&buf[i-2], NULL, 16))
00185         { LERROR("Wrong checksum -- DROP"); i=0; gotstart = false; continue; }
00186       i -= 3; buf[i] = '\0';
00187 
00188       // start the decoding by splitting it into an array of strings:
00189       int n = 0; idx[n++] = 0;
00190       for (int j = 0; j < i; j ++)
00191         if (buf[j] == ',') { idx[n++] = j+1; buf[j] = '\0'; }
00192 
00193       // let's now fill up our data structure based on the sentence:
00194       bool triggerlistener = false;
00195       pthread_mutex_lock(&itsLock);
00196       if (strcmp(buf, "$GPGGA") == 0 && n == 15)
00197         {
00198           itsData.fixho = byte(str2d(&buf[idx[1]], 2));
00199           itsData.fixmi = byte(str2d(&buf[idx[1]+2], 2));
00200           itsData.fixse = byte(str2d(&buf[idx[1]+4], 2));
00201 
00202           itsData.latitude = str2d(&buf[idx[2]], 2) +
00203             str2d(&buf[idx[2]+2], 7) / 60.0;
00204           if (buf[idx[3]] == 'S') itsData.latitude = -itsData.latitude;
00205 
00206           itsData.longitude = str2d(&buf[idx[4]], 3) +
00207             str2d(&buf[idx[4]+3], 7) / 60.0;
00208           if (buf[idx[5]] == 'W') itsData.longitude = -itsData.longitude;
00209 
00210           itsData.nsat = byte(atoi(&buf[idx[7]]));
00211 
00212           if (buf[idx[10]] == 'M')
00213             itsData.galtitude = atof(&buf[idx[9]]);
00214 
00215           // we got our first sentence for a new fix:
00216           gotstart = true;
00217         }
00218 
00219       if (strcmp(buf, "$GPRMC") == 0 && n == 13)
00220         {
00221           itsData.speed = atof(&buf[idx[7]]) / 1.85200f; // knot to km/h
00222           heading = atof(&buf[idx[8]]);
00223           itsData.magvar = strtod(&buf[idx[10]], NULL);
00224           if (buf[idx[11]] == 'E') itsData.magvar = - itsData.magvar;
00225           itsData.fixda = byte(str2d(&buf[idx[9]], 2));
00226           itsData.fixmo = byte(str2d(&buf[idx[9]+2], 2));
00227           itsData.fixye = 2000 + byte(str2d(&buf[idx[9]+4], 2));
00228         }
00229 
00230       if (strcmp(buf, "$GPGSA") == 0 && n == 18)
00231         {
00232           itsData.fixtype = byte(atoi(&buf[idx[2]]) - 1);
00233           itsData.pdil = atof(&buf[idx[15]]);
00234           itsData.hdil = atof(&buf[idx[16]]);
00235           itsData.vdil = atof(&buf[idx[17]]);
00236         }
00237 
00238       if (strcmp(buf, "$PGRME") == 0 && n == 7)
00239         {
00240           if (buf[idx[2]] == 'M') itsData.hpe = atof(&buf[idx[1]]);
00241           if (buf[idx[4]] == 'M') itsData.vpe = atof(&buf[idx[3]]);
00242           if (buf[idx[6]] == 'M') itsData.epe = atof(&buf[idx[5]]);
00243         }
00244 
00245       if (strcmp(buf, "$PGRMZ") == 0 && n == 3)
00246         {
00247           if (buf[idx[2]] == 'f')
00248             itsData.altitude = atof(&buf[idx[1]]) * 0.3048;
00249         }
00250 
00251       if (strcmp(buf, "$HCHDG") == 0 && n == 6)
00252         {
00253           // this sentence comes after the GPRMC one which provides
00254           // GPS-derived true heading, valid when we are moving. If we
00255           // are not moving, we will use compass magnetic heading (and
00256           // convert it to true heading) instead:
00257           if (itsData.speed < 5.0)
00258             itsData.heading = atof(&buf[idx[1]]) - itsData.magvar;
00259           else
00260             itsData.heading = heading;
00261 
00262           // this is our last sentence, so once we have it we indicate
00263           // we have completed receiving a new fix:
00264           if (gotstart) { itsGotnew = true; triggerlistener = true; }
00265           gotstart = false;
00266         }
00267       itsGotnew = true; triggerlistener = true;
00268       pthread_mutex_unlock(&itsLock);
00269 
00270       // trigger our listener if a completed new block has arrived:
00271       if (triggerlistener && itsListener.get())
00272       {
00273         itsListener->newData(itsData);
00274       }
00275 
00276       // ready for next sentence:
00277       i = 0;
00278     }
00279 }
00280 
00281 // ######################################################################
00282 double GPS::str2d(const char *s, const int nchar) const
00283 {
00284   char tmp[nchar + 1]; tmp[nchar] = '\0';
00285   for (int i = 0; i < nchar; i ++) tmp[i] = s[i];
00286   return strtod(tmp, NULL);
00287 }
00288 
00289 // ######################################################################
00290 /* So things look consistent in everyone's emacs... */
00291 /* Local Variables: */
00292 /* indent-tabs-mode: nil */
00293 /* End: */
Generated on Sun May 8 08:40:37 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3