BeoGPS.C

/*!@file Robots2/Beobot2/Hardware/BeoGPS.C Ice Module for GPS           */
// //////////////////////////////////////////////////////////////////// //
// The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
// University of Southern California (USC) and the iLab at USC.         //
// See http://iLab.usc.edu for information about this project.          //
// //////////////////////////////////////////////////////////////////// //
// Major portions of the iLab Neuromorphic Vision Toolkit are protected //
// under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
// in Visual Environments, and Applications'' by Christof Koch and      //
// Laurent Itti, California Institute of Technology, 2001 (patent       //
// pending; application number 09/912,225 filed July 23, 2001; see      //
// http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
// //////////////////////////////////////////////////////////////////// //
// This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
// redistribute it and/or modify it under the terms of the GNU General  //
// Public License as published by the Free Software Foundation; either  //
// version 2 of the License, or (at your option) any later version.     //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
// that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
// PURPOSE.  See the GNU General Public License for more details.       //
//                                                                      //
// You should have received a copy of the GNU General Public License    //
// along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
// to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
// Boston, MA 02111-1307 USA.                                           //
// //////////////////////////////////////////////////////////////////// //
//
// Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
// $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/Hardware/BeoGPS.C
// $ $Id: BeoGPS.C 12962 2010-03-06 02:13:53Z irock $
//
//////////////////////////////////////////////////////////////////////////

#include "Robots/Beobot2/Hardware/BeoGPS.H"
#include "Ice/BeobotEvents.ice.H"

// const ModelOptionCateg MOC_BeoGPS = {
//         MOC_SORTPRI_3, "Beobot GPS Related Options" };

// const ModelOptionDef OPT_SerialDev =
// { MODOPT_ARG(std::string), "SerialDev", &MOC_BeoGPS, OPTEXP_CORE,
//         "The serial device file",
//         "serial-dev", '\0', "/dev/ttyUSBX", "/dev/ttyUSB1"};

// #####################################################################
BeoGPS::BeoGPS(OptionManager& mgr,
               const std::string& descrName, const std::string& tagName) :
  RobotBrainComponent(mgr, descrName, tagName),
  itsSerial(new Serial(mgr)),
  itsOfs(new OutputFrameSeries(mgr)),
  itsDisplayUpdateRate(.05),
  //  itsSerialDev(&OPT_SerialDev, this, 0),
  itsPosition(0.0,0.0,0.0),
  itsInitLat(-1.0),
  itsInitLon(-1.0),
  itsDispImage(512,512,ZEROS),
  itsCurrMessageID(0)
{
  addSubComponent(itsSerial);
  addSubComponent(itsOfs);
}

// ######################################################################
BeoGPS::~BeoGPS()
{

}

// ######################################################################
void BeoGPS::start1()
{
  itsSerial->configure("/dev/ttyUSB0", 115200);

//   itsSerial->configure
//     (itsSerialDev.getVal().c_str(), 115200, "8N1", false, false, 0);

  // Check to ensure that our ttyUSB devices are set up correctly
  // by polling their IDs
//   LINFO("Checking for Sensor board");
//   unsigned char cmd = 0;
//   std::vector<unsigned char> propIDvec = itsSerial->readFrame(cmd, 1, .5);

//   std::string propID(propIDvec.begin(), propIDvec.end());

//   if(propID == "")
//     LFATAL("ERROR! Unrecognized device on %s",
//            itsSerialDev.getVal().c_str());
//   else if(propID != "sensorboard")
//     LFATAL("ERROR! Incorrect device on %s: %s",
//            itsSerialDev.getVal().c_str(), propID.c_str());

//   LINFO("%s found on %s", propID.c_str(), itsSerialDev.getVal().c_str());
}

// ######################################################################
void BeoGPS::registerTopics()
{
  registerPublisher("GPSMessageTopic");
}

// ######################################################################
void BeoGPS::evolve()
{
  // Request a set of GPS readings from the sensor board
  //std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 1, .2);
  unsigned char cmd = 97;
  itsSerial->write(&cmd, 1);

  unsigned char buffer[256];
  itsSerial->read(&buffer,20);

  std::vector<unsigned char> frame(buffer+2, buffer+12);

  // If we recieved the ex./pected 20 bytes back
  // (4 bytes for each of the 5 gpss) then let's parse them.
  // The resulting values are the number of clock ticks
  // at 80Mhz that it took for the gps pulse to leave the gps,
  // bounce off of the nearest object, and return
  if(frame.size() == 10)
    {
      int latDD = frame[0];
      int latMM = frame[1];

      int latMMMM = ((0x0FF & frame[2])  << 8)  |
                                ((0x0FF & frame[3])  << 0);

      int lonDD = frame[4];
      int lonMM = frame[5];

      int lonMMMM = ((0x0FF & frame[6])  << 8)  |
                                ((0x0FF & frame[7])  << 0);

      int precision = frame[8];
      int satNum = frame[9];

      double lat = latDD + (latMM * 60.0 + latMMMM * 60.0 / 10000.0) / 3600.0;
      double lon = lonDD + (lonMM * 60.0 + lonMMMM * 60.0 / 10000.0) / 3600.0;


                        if(latDD == 34 && lonDD == 118 )
                        {
                                if(itsInitLat == -1.0 )
                                {

                                        LINFO("Set init GPS point  Lat: %f Lon %f",lat,lon);
                                        itsInitLat = lat;
                                        itsInitLon = lon;
                                }
                                updatePosition(lat,lon);

                                BeobotEvents::GPSMessagePtr msg = new BeobotEvents::GPSMessage;
                                msg->latitude = lat;
                                msg->longitude = lon;
                                msg->precision = precision;
                                msg->satNum = satNum;

                                msg->RequestID = itsCurrMessageID;

                                //LINFO("initLat %f initLon%f",itsInitLat,itsInitLon);
                                LINFO("[%6d]Publishing GPS report Lat: %f Lon %f Precision %d SatNum %d",
                                                itsCurrMessageID, lat, lon,precision,satNum);
                                this->publish("GPSMessageTopic", msg);



                                itsCurrMessageID++;
                        }
                        else
                                LINFO("Waiting GPS... Lat: %f Lon %f Precision %d SatNum %d",  lat, lon,precision,satNum);

                                itsData.lat = lat;
                                itsData.lon = lon;
                                itsData.precision = precision;
                                itsData.satNum = satNum;

                                // Plot the location
                                plotGPS();
}
  else
    {
      LERROR("Invalid Frame Size Received from GPS!");
    }
  usleep(100000);
}
void BeoGPS::updatePosition(double lat,double lon)
{
  //Find Y movement
  double R = 6378.7 ; //Earth Radians (KM)
  double theta = DEG2RAD(itsInitLon - lon);
  double d1 = sin(DEG2RAD(itsInitLat)) * sin(DEG2RAD(itsInitLat));
  double d2 = cos(DEG2RAD(itsInitLat)) * cos(DEG2RAD(itsInitLat)) * cos(theta);
  double y = acos(d1 + d2) * R * 1000.0;//meter

  //Find X movement
  d1 = sin(DEG2RAD(itsInitLat)) * sin(DEG2RAD(lat));
  d2 = cos(DEG2RAD(itsInitLat)) * cos(DEG2RAD(lat));
  double x = acos(d1 + d2) * R * 1000.0;//meter

  if(x != 0.0 && y != 0.0)
    {
      itsPosition.x = (itsInitLat > lat) ? x : -x;
      itsPosition.y = (itsInitLon > lon) ? y : -y;
    }

  LINFO("Publishing GPS report Lat: %f Lon: %f  X: %f Y: %f",
        lat, lon, itsPosition.x, itsPosition.y);
}

// ######################################################################
void BeoGPS::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
                           const Ice::Current&)
{ }

// ######################################################################
void BeoGPS::plotGPS()
{
   if(itsDisplayTimer.getSecs() > itsDisplayUpdateRate)
     {
       itsDisplayTimer.reset();

       //Draw The Map
       double mapScale = 1.0;
       Point2D<int> drawPos
         (int(itsPosition.x*mapScale + itsDispImage.getWidth()/2),
          int(itsPosition.y*mapScale + itsDispImage.getHeight()/2));
       if(itsDispImage.coordsOk(drawPos))
         itsDispImage.setVal(drawPos, PixRGB<byte>(0,255,0));


       //itsDispImage.setVal(Point2D<int>(100,100), PixRGB<byte>(0,255,255));

       Image<PixRGB<byte> > itsDrawitsDispImage(itsDispImage);
       drawCircle(itsDrawitsDispImage, drawPos, 7, PixRGB<byte>(255,255,255));

                         char buffer[128];
                         sprintf(buffer, "X=%2.2f Y=%2.2f Lon: %1.7f Lat: %1.7f PCSN: %-3d SatNum: %-3d",
                                         itsPosition.x,itsPosition.y,
                                         itsData.lat,itsData.lon,
                                         itsData.precision,itsData.satNum);
                         writeText(itsDispImage, Point2D<int>(0,0), buffer, PixRGB<byte>(255,255,255),
                                         PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));

       itsOfs->writeRGB(itsDispImage, "GPSData", FrameInfo("GPSData",SRC_POS));
       itsOfs->updateNext();
     }
}

// ######################################################################
/* So things look consistent in everyone's emacs... */
/* Local Variables: */
/* indent-tabs-mode: nil */
/* End: */