ArGPS.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program 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.

     This program 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 this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/


#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArGPS.h"

#include "Aria.h"
#include <iostream>



/* 
 * How to add support for new GPS data:
 * ------------------------------------
 *
 *  1. Add a variable to store the data to the ArGPS declaration in ArGPS.h,
 *     including a flag that indicates whether that data is valid (the "haveXXX"
 *     booleans). Initialize that flag in the ArGPS constructor definition below.
 *
 *  2. Create a handler method and functor for the NMEA message that provides
 *     the data. Initialize the functor in the constructor below.
 *
 *  3. Add the functor to the handler map myHandlers in the constructor below. 
 *
 *  4. Implement the handler method to examine the field (don't forget that 
 *     NMEA does not require that all fields be given).
 *
 *  5. Add a line to the initialization commands above which are required to 
 *     start some GPS systems to start sending data.
 *
 * Some possible new NMEA message types to add are GPGST (RAIM residuals and
 * error information), GPMSS (beacon signal info), PTNLDG (Trimble proprietary
 * DGPS status information), and GPZDA (time of day information).
 *
 *
 *
 * How to add support for new GPS types:
 * -------------------------------------
 *
 * If your GPS device uses NMEA and does not require any special initialization
 * commands, then it will probably work if you use the right BAUD rate.
 *
 * If your GPS device is special, then you may need to define a subclass
 * of ArGPS. You can override connect(), setDeviceType(), and/or read() to do 
 * special things. See ArNovatelGPS as an example. Then add support to
 * it to ArGPSConnector: add a new member of the GPSType enum, a check for
 * it in parseArgs(), mention it in logArgs(), and create your ArGPS subclass
 * in createGP().
 *
 * You can find out the NMEA messages ArGPS wants by accessing "myHandlers",
 * of type HandlersMap (a std::map).
 * 
 */




AREXPORT ArGPS::ArGPS() :

  // Data
  myHavePosition(false),
  myHaveSpeed(false),
  myGPSPositionTimestamp(0), 
  myFixType(NoFix), 
  myNumSatellitesTracked(0),
  myHaveAltitude(false),
  myHaveDGPSStation(false),
  myHavePositionError(false),
  myHaveVerticalPositionError(false),
  myHaveCompassHeading(false),
  myHaveHDOP(false), 
  myHaveVDOP(false), 
  myHavePDOP(false),

  // objects
  myDevice(NULL),

  // handler functors
  myGPRMCHandler(this, &ArGPS::handleGPRMC),
  myGPGGAHandler(this, &ArGPS::handleGPGGA),
  myPGRMEHandler(this, &ArGPS::handlePGRME),
  myPGRMZHandler(this, &ArGPS::handlePGRMZ),
  myHCHDGHandler(this, &ArGPS::handleHCHDG),
  myGPGSAHandler(this, &ArGPS::handleGPGSA),

  // parse state
  MaxNumFields(50),
  MaxFieldSize(128),
  ignoreChecksum(false),
  checksumBufOffset(0),
  inChecksum(false),
  inMessage(false),
  currentChecksum(0),
  gotCR(false)
{
  myHandlers["GPRMC"] = &myGPRMCHandler;
  myHandlers["GPGGA"] = &myGPGGAHandler;
  myHandlers["PGRME"] = &myPGRMEHandler;
  myHandlers["PGRMZ"] = &myPGRMZHandler;
  myHandlers["HCHDG"] = &myHCHDGHandler;
  myHandlers["GPGSA"] = &myGPGSAHandler;
}

AREXPORT ArGPS::~ArGPS()
{
  memset(checksumBuf, 0, 3);
}






AREXPORT bool ArGPS::connect()
{
  if (!myDevice)
  {
    ArLog::log(ArLog::Terse, "GPS Error: Cannot connect, device connection invalid.");
    return false;
  }

  if (myDevice->getStatus() != ArDeviceConnection::STATUS_OPEN) 
  {
    ArLog::log(ArLog::Terse, "GPS Error: Cannot connect, device connection not open.");
    return false;
  }


  return true;
}



AREXPORT bool ArGPS::blockingConnect(unsigned long connectTimeout)
{
  ArTime start;
  start.setToNow();
  if (!connect()) return false;
  while ((unsigned long)start.mSecSince() <= connectTimeout)
  {
    if (read() & ReadUpdated)  // read until data is sucessfully parsed 
      return true;
    ArUtil::sleep(100);
    // Maybe try connect() again every few seconds?
  }
  return false;
}



void ArGPS::nextField()
{
  currentMessage.push_back(currentField);
  currentField = "";
  if (currentMessage.size() > MaxNumFields)
    endMessage();
}

void ArGPS::endMessage()
{
  inMessage = false;
  inChecksum = false;
  currentField = "";
  gotCR = false;
}

void ArGPS::beginChecksum()
{
  checksumBufOffset = 0;
  inChecksum = true;
}

void ArGPS::beginMessage()
{
  currentMessage.clear();
  inChecksum = false;
  inMessage = true;
  currentField = "";
  gotCR = false;
  currentChecksum = 0;
}


AREXPORT int ArGPS::read()
{
  if (!myDevice) return ReadError;
  char buf[256]; // move into LexState struct?

  int result = 0;

  // keep reading data, returning when there's no data left or there's an error.
  while(true)
  {
    int n = myDevice->read(buf, sizeof(buf));

    if (n < 0) 
    {
      return result|ReadError;
    }

    if (n == 0) 
    {
      return result|ReadFinished;
    }

    //std::cerr << "\t[ArGPS: parsing chunk \"" << buf << "\"]\n";

    for (int i = 0; i < n; i++)
    {
      // Check for message start
      if (buf[i] == '$')
      {
        beginMessage();
        continue;
      }

      // Otherwise, we must be in a sentece to do anything
      if (!inMessage)
        continue;

      // Reached the CRLF at the end?
      if (buf[i] == '\r') 
      {
        gotCR = true;
        continue;
      }
      if (buf[i] == '\n') 
      {
        if (gotCR) 
        {
          endMessage();
          HandlerMap::iterator h = myHandlers.find(currentMessage[0]);
          if (h != myHandlers.end()) 
          {
            //printf("\t[Calling handler for %s...]\n", currentMessage[0].c_str());
            h->second->invoke(&currentMessage);
            result |= ReadUpdated;
          }
        }

        // a syntax error, abort the message and start looking for the next one.
        endMessage();
        continue;
      }

      // Are we in the final checksum field?
      if (inChecksum)
      {
        checksumBuf[checksumBufOffset++] = buf[i];
        if (checksumBufOffset > 1)   // two bytes of checksum
        {
          int checksumRec = (int) strtol(checksumBuf, NULL, 16);
          //printf("\t[End checksum. Message provided checksum \"%s\", => %x. Calculated checksum is %x (%d).]\n", checksumBuf, checksumRec, currentChecksum, currentChecksum);
          if (checksumRec != currentChecksum) {
            ArLog::log(ArLog::Normal, "GPS: Warning: Skipping message with incorrect checksum.");
            // abort the message and start looking for the next one.
            endMessage();
          }
        }
        continue;
      }


      // Got to the checksum?
      if (buf[i] == '*')
      {
        nextField();
        if (!ignoreChecksum)
          beginChecksum();
        continue;
      }

      // Every other byte in a message (between $ and *) XORs to form the
      // checksum:
      currentChecksum ^= buf[i];

      // Time to start a new field?
      if (buf[i] == ',')
      {
        nextField();
        continue;
      }


      // Else, we must be in the middle of a field
      // TODO we could use strchr to look ahead in the buf 
      // for the end of the field (',' or '*') or end of the buf, and copy more
      // than one byte at a time.
      currentField += buf[i];
      if (currentField.size() > MaxFieldSize)
      {
        endMessage();
        continue;
      }
    }
  }
  return result;
}




// Key navigation data (position, etc.)
void ArGPS::handleGPRMC(MessageVector* message)
{
  // Good data?:
  if (message->size() < 3 || (*message)[2] != "A") return;

  if (!readFloatFromStringVec(message, 3, &myLatitude, &gpsDegminToDegrees)) return;

  if (message->size() < 5) return;
  if ((*message)[4] == "S") myLatitude *= -1;

  if (!readFloatFromStringVec(message, 5, &myLongitude, &gpsDegminToDegrees)) return;

  if (message->size() < 7) return;
  if ((*message)[6] == "W") myLongitude *= -1;

  // Only set these if we got both position components:
  myHavePosition = true;
  myGPSPositionTimestamp = atoi((*message)[1].c_str());

  myHaveSpeed = readFloatFromStringVec(message, 7, &mySpeed, &knotsToMPS);

}

// Fix type, number of satellites tracked, DOP and also maybe altitude
void ArGPS::handleGPGGA(MessageVector* message)
{
  if (message->size() < 7) return;
  switch(atoi((*message)[6].c_str()))
  {
    case 0:
      myFixType = BadFix;
      break;
    case 1: 
      myFixType = GPSFix;
      break;
    case 2:
      myFixType = DGPSFix;
      break;
    case 3:
      myFixType = PPSFix;
      break;
    case 4:
      myFixType = RTKinFix;
      break;
    case 5:
      myFixType = FloatRTKinFix;
      break;
    case 6:
      myFixType = DeadReckFix;
      break;
    case 7: 
      myFixType = ManualFix;
      break;
    case 8:
      myFixType = SimulatedFix;
      break;
    default:
      myFixType = UnknownFixType;
  }
  
  readUShortFromStringVec(message, 7, &myNumSatellitesTracked);
  myHaveHDOP = readFloatFromStringVec(message, 8, &myHDOP); // note redundant with GPGSA
  myHaveAltitude = readFloatFromStringVec(message, 9, &myAltitude); // might be redundante with PGRMZ
  myHaveDGPSStation = readUShortFromStringVec(message, 14, &myDGPSStationID);
}


// Error estimation in ground distance units (actually a proprietary message)
void ArGPS::handlePGRME(MessageVector* message)
{
  // TODO: check units; keep as DOP or convert to something else?
  // Use GPGSA instead of PGRME?
  myHavePositionError = readFloatFromStringVec(message, 1, &myPositionError);
  myHaveVerticalPositionError = readFloatFromStringVec(message, 3, &myVerticalPositionError);
}

// Altitude (actually a Garmin proprietary message)
void ArGPS::handlePGRMZ(MessageVector* message)
{
  // This is redundant with GPGGA and often a different value (plus the
  // conversion...) Favor this over that one, or separate into two values?
  // Yes, if this is specifically from an altimiter and the value in GGA is
  // from the satellite positions.
  if (myHaveAltitude && message->size() >= 3 && strcasecmp((*message)[2].c_str(), "f") == 0)
    myAltitude = feetToMeters(myAltitude);
}

// Compass heading (actually a proprietary message)
void ArGPS::handleHCHDG(MessageVector* message)
{
  myHaveCompassHeading = readFloatFromStringVec(message, 1, &myCompassHeading);
}

// GPS DOP and satellite IDs
void ArGPS::handleGPGSA(MessageVector* message)
{
  // This message alse has satellite IDs, not sure if that information is
  // useful though.
  
  myHavePDOP = readFloatFromStringVec(message, 15, &myPDOP);
  myHaveHDOP = readFloatFromStringVec(message, 16, &myHDOP);
  myHaveVDOP = readFloatFromStringVec(message, 17, &myVDOP);
}

AREXPORT const char* ArGPS::getFixTypeName() const 
{
  switch (getFixType())
  {
    case NoFix: return "None";
    case BadFix: return "Bad";
    case GPSFix: return "GPS";
    case DGPSFix: return "DGPS";
    case PPSFix: return "PPS";
    case RTKinFix: return "RT Kinematic";
    case FloatRTKinFix: return "Float. RT Kinematic";
    case DeadReckFix: return "Dead Reckoning";
    case ManualFix: return "Manual";
    case SimulatedFix: return "Simulated";
    default: return "Unknown";
  }
}

AREXPORT void ArGPS::logData() const
{
  ArLog::log(ArLog::Normal, "GPS Fix=%s Num. Satellites=%d", getFixTypeName(), getNumSatellitesTracked());
  
  if (havePosition())
  {
    ArLog::log(ArLog::Normal, "GPS Latitude=%0.4fdeg Longitude=%0.4fdeg Timestamp=%d", getLatitude(), getLongitude(), getGPSPositionTimestamp());
    // for  fun... ArLog::log(ArLog::Normal, "GPS Maps: <http://www.topozone.com/map.asp?lat=%f&lon=%f&datum=nad83&u=5>  <http://maps.google.com/maps?q=%f,+%f>", getLatitude(), getLongitude(), getLatitude(), getLongitude());
  }
  
  if (haveSpeed())
    ArLog::log(ArLog::Normal, "GPS Speed=%0.4fm/s (%0.4fmi/h)", getSpeed(), mpsToMph(getSpeed()));

  if (haveAltitude())
    ArLog::log(ArLog::Normal, "GPS Altitude=%0.4fm (%0.4fft)", getAltitude(), metersToFeet(getAltitude()));

  if (haveCompassHeading())
    ArLog::log(ArLog::Normal, "GPS Compass Heading=%0.4fdeg", getCompassHeading());

  if (havePositionError())
    ArLog::log(ArLog::Normal, "GPS Position Error Estimate=%0.4fm", getPositionError());

  if (haveVerticalPositionError())
    ArLog::log(ArLog::Normal, "GPS Vertical Position Error Estimate=%0.4fm", getVerticalPositionError());

  if (havePDOP())
    ArLog::log(ArLog::Normal, "GPS PDOP=%0.4f", getPDOP());
  if (haveHDOP())
    ArLog::log(ArLog::Normal, "GPS HDOP=%0.4f", getHDOP());
  if (haveVDOP())
    ArLog::log(ArLog::Normal, "GPS VDOP=%0.4f", getVDOP());

  if (haveDGPSStation())
    ArLog::log(ArLog::Normal, "GPS DGPS Station ID=%d", getDGPSStationID());

}

AREXPORT void ArGPS::printData() const
{
  printf("GPS: ");
  if (!havePosition())
  {
    printf("Pos:-");
  }
  else
  {
    printf("Pos:% 2.6f,% 2.6f", getLatitude(), getLongitude());
  }

  if (!haveSpeed())
    printf("   Spd:-");
  else
    printf("   Spd:%4.4fm/s (%3.4fmi/h)", getSpeed(), mpsToMph(getSpeed()));

  if (!haveAltitude())
    printf("   Alt:-");
  else
    printf("   Alt:%4.2fm (%4.2fft)", getAltitude(), metersToFeet(getAltitude()));

  if (!haveCompassHeading())
    printf("   Head:-");
  else
    printf("   Head:%3.1fdeg", getCompassHeading());

  printf("   NSats:%2d", getNumSatellitesTracked());

  if (!havePositionError())
    printf("   ErrEst:-");
  else
    printf("   ErrEst:%2.4fm", getPositionError());

  if (haveHDOP())
    printf("   HDOP:%1.2f", getHDOP());
  else
    printf("   HDP:-");

  printf("   Fix:%-10s", getFixTypeName());
  printf("   (%d)", getGPSPositionTimestamp());
}


double ArGPS::gpsDegminToDegrees(double degmin) 
{
  double degrees;
  double minutes = modf(degmin / 100.0, &degrees) * 100.0;
  return degrees + (minutes / 60.0);
}


double ArGPS::knotsToMPS(double knots) 
{
  return(knots * 0.514444444);
}


bool ArGPS::readFloatFromString(std::string& str, double* target, double (*convf)(double))
{
  if (str.length() == 0) return false;
  if (convf)
    *target = (*convf)(atof(str.c_str()));
  else
    *target = atof(str.c_str());
  return true;
}

bool ArGPS::readUShortFromString(std::string& str, unsigned short* target, unsigned short (*convf)(unsigned short))
{
  if (str.length() == 0) return false;
  if (convf)
    *target = (*convf)(atof(str.c_str()));
  else
    *target = (unsigned short) atoi(str.c_str());
  return true;
}


bool ArGPS::readFloatFromStringVec(std::vector<std::string>* vec, size_t i, double* target, double (*convf)(double))
{
  if (vec->size() < (i+1)) return false;
  return readFloatFromString((*vec)[i], target, convf);
}

bool ArGPS::readUShortFromStringVec(std::vector<std::string>* vec, size_t i, unsigned short* target, unsigned short (*convf)(unsigned short))
{
  if (vec->size() < (i+1)) return false;
  return readUShortFromString((*vec)[i], target, convf);
}

---