BeoGPS.C

00001 /*!@file Robots2/Beobot2/Hardware/BeoGPS.C Ice Module for GPS           */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/Hardware/BeoGPS.C
00034 // $ $Id: BeoGPS.C 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 //////////////////////////////////////////////////////////////////////////
00037 
00038 #include "Robots/Beobot2/Hardware/BeoGPS.H"
00039 #include "Ice/BeobotEvents.ice.H"
00040 
00041 // const ModelOptionCateg MOC_BeoGPS = {
00042 //         MOC_SORTPRI_3, "Beobot GPS Related Options" };
00043 
00044 // const ModelOptionDef OPT_SerialDev =
00045 // { MODOPT_ARG(std::string), "SerialDev", &MOC_BeoGPS, OPTEXP_CORE,
00046 //         "The serial device file",
00047 //         "serial-dev", '\0', "/dev/ttyUSBX", "/dev/ttyUSB1"};
00048 
00049 // #####################################################################
00050 BeoGPS::BeoGPS(OptionManager& mgr,
00051                const std::string& descrName, const std::string& tagName) :
00052   RobotBrainComponent(mgr, descrName, tagName),
00053   itsSerial(new Serial(mgr)),
00054   itsOfs(new OutputFrameSeries(mgr)),
00055   itsDisplayUpdateRate(.05),
00056   //  itsSerialDev(&OPT_SerialDev, this, 0),
00057   itsPosition(0.0,0.0,0.0),
00058   itsInitLat(-1.0),
00059   itsInitLon(-1.0),
00060   itsDispImage(512,512,ZEROS),
00061   itsCurrMessageID(0)
00062 {
00063   addSubComponent(itsSerial);
00064   addSubComponent(itsOfs);
00065 }
00066 
00067 // ######################################################################
00068 BeoGPS::~BeoGPS()
00069 {
00070 
00071 }
00072 
00073 // ######################################################################
00074 void BeoGPS::start1()
00075 {
00076   itsSerial->configure("/dev/ttyUSB0", 115200);
00077 
00078 //   itsSerial->configure
00079 //     (itsSerialDev.getVal().c_str(), 115200, "8N1", false, false, 0);
00080 
00081   // Check to ensure that our ttyUSB devices are set up correctly
00082   // by polling their IDs
00083 //   LINFO("Checking for Sensor board");
00084 //   unsigned char cmd = 0;
00085 //   std::vector<unsigned char> propIDvec = itsSerial->readFrame(cmd, 1, .5);
00086 
00087 //   std::string propID(propIDvec.begin(), propIDvec.end());
00088 
00089 //   if(propID == "")
00090 //     LFATAL("ERROR! Unrecognized device on %s",
00091 //            itsSerialDev.getVal().c_str());
00092 //   else if(propID != "sensorboard")
00093 //     LFATAL("ERROR! Incorrect device on %s: %s",
00094 //            itsSerialDev.getVal().c_str(), propID.c_str());
00095 
00096 //   LINFO("%s found on %s", propID.c_str(), itsSerialDev.getVal().c_str());
00097 }
00098 
00099 // ######################################################################
00100 void BeoGPS::registerTopics()
00101 {
00102   registerPublisher("GPSMessageTopic");
00103 }
00104 
00105 // ######################################################################
00106 void BeoGPS::evolve()
00107 {
00108   // Request a set of GPS readings from the sensor board
00109   //std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 1, .2);
00110   unsigned char cmd = 97;
00111   itsSerial->write(&cmd, 1);
00112 
00113   unsigned char buffer[256];
00114   itsSerial->read(&buffer,20);
00115 
00116   std::vector<unsigned char> frame(buffer+2, buffer+12);
00117 
00118   // If we recieved the ex./pected 20 bytes back
00119   // (4 bytes for each of the 5 gpss) then let's parse them.
00120   // The resulting values are the number of clock ticks
00121   // at 80Mhz that it took for the gps pulse to leave the gps,
00122   // bounce off of the nearest object, and return
00123   if(frame.size() == 10)
00124     {
00125       int latDD = frame[0];
00126       int latMM = frame[1];
00127 
00128       int latMMMM = ((0x0FF & frame[2])  << 8)  |
00129                                 ((0x0FF & frame[3])  << 0);
00130 
00131       int lonDD = frame[4];
00132       int lonMM = frame[5];
00133 
00134       int lonMMMM = ((0x0FF & frame[6])  << 8)  |
00135                                 ((0x0FF & frame[7])  << 0);
00136 
00137       int precision = frame[8];
00138       int satNum = frame[9];
00139 
00140       double lat = latDD + (latMM * 60.0 + latMMMM * 60.0 / 10000.0) / 3600.0;
00141       double lon = lonDD + (lonMM * 60.0 + lonMMMM * 60.0 / 10000.0) / 3600.0;
00142 
00143 
00144                         if(latDD == 34 && lonDD == 118 )
00145                         {
00146                                 if(itsInitLat == -1.0 )
00147                                 {
00148 
00149                                         LINFO("Set init GPS point  Lat: %f Lon %f",lat,lon);
00150                                         itsInitLat = lat;
00151                                         itsInitLon = lon;
00152                                 }
00153                                 updatePosition(lat,lon);
00154 
00155                                 BeobotEvents::GPSMessagePtr msg = new BeobotEvents::GPSMessage;
00156                                 msg->latitude = lat;
00157                                 msg->longitude = lon;
00158                                 msg->precision = precision;
00159                                 msg->satNum = satNum;
00160 
00161                                 msg->RequestID = itsCurrMessageID;
00162 
00163                                 //LINFO("initLat %f initLon%f",itsInitLat,itsInitLon);
00164                                 LINFO("[%6d]Publishing GPS report Lat: %f Lon %f Precision %d SatNum %d",
00165                                                 itsCurrMessageID, lat, lon,precision,satNum);
00166                                 this->publish("GPSMessageTopic", msg);
00167 
00168 
00169 
00170                                 itsCurrMessageID++;
00171                         }
00172                         else
00173                                 LINFO("Waiting GPS... Lat: %f Lon %f Precision %d SatNum %d",  lat, lon,precision,satNum);
00174 
00175                                 itsData.lat = lat;
00176                                 itsData.lon = lon;
00177                                 itsData.precision = precision;
00178                                 itsData.satNum = satNum;
00179 
00180                                 // Plot the location
00181                                 plotGPS();
00182 }
00183   else
00184     {
00185       LERROR("Invalid Frame Size Received from GPS!");
00186     }
00187   usleep(100000);
00188 }
00189 void BeoGPS::updatePosition(double lat,double lon)
00190 {
00191   //Find Y movement
00192   double R = 6378.7 ; //Earth Radians (KM)
00193   double theta = DEG2RAD(itsInitLon - lon);
00194   double d1 = sin(DEG2RAD(itsInitLat)) * sin(DEG2RAD(itsInitLat));
00195   double d2 = cos(DEG2RAD(itsInitLat)) * cos(DEG2RAD(itsInitLat)) * cos(theta);
00196   double y = acos(d1 + d2) * R * 1000.0;//meter
00197 
00198   //Find X movement
00199   d1 = sin(DEG2RAD(itsInitLat)) * sin(DEG2RAD(lat));
00200   d2 = cos(DEG2RAD(itsInitLat)) * cos(DEG2RAD(lat));
00201   double x = acos(d1 + d2) * R * 1000.0;//meter
00202 
00203   if(x != 0.0 && y != 0.0)
00204     {
00205       itsPosition.x = (itsInitLat > lat) ? x : -x;
00206       itsPosition.y = (itsInitLon > lon) ? y : -y;
00207     }
00208 
00209   LINFO("Publishing GPS report Lat: %f Lon: %f  X: %f Y: %f",
00210         lat, lon, itsPosition.x, itsPosition.y);
00211 }
00212 
00213 // ######################################################################
00214 void BeoGPS::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00215                            const Ice::Current&)
00216 { }
00217 
00218 // ######################################################################
00219 void BeoGPS::plotGPS()
00220 {
00221    if(itsDisplayTimer.getSecs() > itsDisplayUpdateRate)
00222      {
00223        itsDisplayTimer.reset();
00224 
00225        //Draw The Map
00226        double mapScale = 1.0;
00227        Point2D<int> drawPos
00228          (int(itsPosition.x*mapScale + itsDispImage.getWidth()/2),
00229           int(itsPosition.y*mapScale + itsDispImage.getHeight()/2));
00230        if(itsDispImage.coordsOk(drawPos))
00231          itsDispImage.setVal(drawPos, PixRGB<byte>(0,255,0));
00232 
00233 
00234        //itsDispImage.setVal(Point2D<int>(100,100), PixRGB<byte>(0,255,255));
00235 
00236        Image<PixRGB<byte> > itsDrawitsDispImage(itsDispImage);
00237        drawCircle(itsDrawitsDispImage, drawPos, 7, PixRGB<byte>(255,255,255));
00238 
00239                          char buffer[128];
00240                          sprintf(buffer, "X=%2.2f Y=%2.2f Lon: %1.7f Lat: %1.7f PCSN: %-3d SatNum: %-3d",
00241                                          itsPosition.x,itsPosition.y,
00242                                          itsData.lat,itsData.lon,
00243                                          itsData.precision,itsData.satNum);
00244                          writeText(itsDispImage, Point2D<int>(0,0), buffer, PixRGB<byte>(255,255,255),
00245                                          PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00246 
00247        itsOfs->writeRGB(itsDispImage, "GPSData", FrameInfo("GPSData",SRC_POS));
00248        itsOfs->updateNext();
00249      }
00250 }
00251 
00252 // ######################################################################
00253 /* So things look consistent in everyone's emacs... */
00254 /* Local Variables: */
00255 /* indent-tabs-mode: nil */
00256 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3