GPS/GPS.cpp

Go to the documentation of this file.

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

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
//#include <stdio.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <math.h>
#include <fcntl.h>

using std::cerr;
using std::cout;
using std::endl;


double GPS::invalid;

GPS::GPS(void) {
  initVars();
}

GPS::GPS(const char* serial_dev, int nowait) 
{
  initVars();

  if (nowait) {
        gps_fd = open(serial_dev, O_RDWR | O_NOCTTY | O_NONBLOCK);   
  } else {
        gps_fd = open(serial_dev, O_RDWR | O_NOCTTY);   
  }

  if (gps_fd < 0)
  {     
    std::cerr << "opening serial port '" << serial_dev << "' failed : " << strerror(errno) << std::endl;    
                exit(EXIT_FAILURE);
  }

  // set new port settings for 9600 baud, no parity, use hardware flow control
  struct termios newtio;
  bzero(&newtio, sizeof(newtio));
  newtio.c_cflag = CS8 | B9600 | CRTSCTS | CLOCAL | CREAD;
  newtio.c_iflag = IGNPAR; //ignore framing and parity errors, or cariage returns
  newtio.c_oflag = 0;    //no output processing
  newtio.c_lflag = ICANON;
  newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
  newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
  newtio.c_cc[VERASE]   = 0;     /* del */
  newtio.c_cc[VKILL]    = 0;     /* @ */
  newtio.c_cc[VEOF]     = 0;     /* Ctrl-d */
  newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
  newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
  newtio.c_cc[VSWTC]    = 0;     /* '\0' */
  newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
  newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
  newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
  newtio.c_cc[VEOL]     = 0;     /* '\0' */
  newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
  newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
  newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
  newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
  newtio.c_cc[VEOL2]    = 0;     /* '\0' */

  //flush both the input and output, before changing serial port attibutes
  tcflush(gps_fd, TCIOFLUSH); 

  int result = tcsetattr(gps_fd,TCSANOW,&newtio);
  if (result == -1) {
    perror("GPS::GPS : tcsetattr failed");
    exit(EXIT_FAILURE);
  }
}

int 
GPS::init(float period, int dynamicMode) {
    //even though GPS may be in blocking mode, 
    //initialization should be in non-blocking mode
    int flags;
    int result = fcntl(gps_fd, F_GETFL, &flags);
        if (result == -1) {
            perror("GPS::init() error with fcntl F_GETFL");
            return -1;
        }
        result = fcntl(gps_fd, F_SETFL, flags | O_NONBLOCK);    
        if (result == -1) {
            perror("GPS::init() error with fcntl F_SETFL");
            return -1;
    }

        //clear out any crap in the serial port buffer before starting
        do {    
            result = read(gps_fd, msgbuf, sizeof(msgbuf)-1);
            if (result == 0) {
                    if (errno == EAGAIN) {
                          break;
                    } else {
                          perror ("GPS::init() error with read");
                          return -1;
                        }                   
                }
    } while (result > 0);

        //reset settings on serial port 
        writeLine("$PASHS,RST\r\n");     
        if (checkAck()) {
                cerr << "GPS::init() error - could not reset GPS - trying once more" << endl;
                sleep(3);
                writeLine("$PASHS,RST\r\n");    
                if (checkAck()) {
                  cerr << "GPS::init() error - could not reset GPS failing" << endl;
                  return -1;
                }
        }       

        // set initial altitude - based on what we have seen before
        writeLine("$PASHS,ALT,219.60\r\n");
        if (checkAck()) {
                cerr << "GPS::init() error - could not run set initial altitude on GPS" << endl;
                return -1;
        }

        char msgtmp[40];
        
        // set dynamic mode for GPS unit
        snprintf(msgtmp, sizeof(msgtmp),"$PASHS,DYN,%d\r\n",dynamicMode);
        writeLine(msgtmp);
        if (checkAck()) {
                cerr << "GPS::init() error - could set dynamic mode on GPS" << endl;
                cerr << "GPS::init() - with command : " << msgtmp;
                return -1;
        }       
        
        // have GPS unit provide data every <period> seconds
        snprintf(msgtmp, sizeof(msgtmp),"$PASHS,NME,POS,A,ON,%f\r\n",period);
        writeLine(msgtmp);
        if (checkAck()) {
                cerr << "GPS::init() error - could not run NME,POS on GPS" << endl;
                cerr << "GPS::init() - with command : " << msgtmp;
                return -1;
        }

        // TODO
        // change BAUD rate?
        
        // have GPS unit provide data UTM coordinates instead of latitude/longitude
        // need to use parseUTM instead of parsePOS
        //writeLine("$PASHS,NME,UTM,A,ON,5\r\n");
        //checkAck();

        // put serial port back into whatever mode it was in before
        result = fcntl(gps_fd, F_SETFL, flags); 
        if (result == -1) {
            perror("GPS::init() error with fcntl F_SETFL (2)");
            return -1;
    }
        
        //cout << "GPS initialization done" << endl;
        return 0;
}

void 
GPS::initVars(void) 
{
        // initialize class variables
        invalid = nan("");
        msgbuf[0] = '\0';
        //reading = 0;  
        using_file = 0;
        gps_fd = -1;
        msglen = 0;
        has_positions = has_velocities = 0;
        latitude = longitude = altitude = invalid;
        x = y = z = invalid;
        xvel = yvel = zvel = invalid;
        hVelocity = vVelocity = direction = invalid;
        pdop = hdop = vdop = tdop = invalid;
}


GPS::~GPS(void)
{
  if (gps_fd != -1) 
    close(gps_fd);
}



// Will read and buffer entire line of input.  It will allow about 10 seconds
// for a complete line of input to be read.  The fuction will return 1 if there
// was a complete line of input that was read, otherwise it will return 0;

int 
GPS::readLine(void) 
{
  int result;

  for(int i=1; i<10; ++i) {
    // read from serial port
    result = read(gps_fd, msgbuf, sizeof(msgbuf)-1);
    if (result == 0) {
      std::cerr << "GPS::readLine : serial port closed?" << std::endl;
      //exit(EXIT_FAILURE);
                        return -1;
    } 
    else if ((result == -1) && (errno != EAGAIN)) {
      std::cerr << "GPS::readLine error - " << strerror(errno) << std::endl;
      //exit(EXIT_FAILURE);
                        return -1;
    } 
    else if (result > 0) {
      msglen = result;
      std::cout << "GPS responded in " << i << " seconds\n";
      if (msgbuf[msglen-1] != '\n') {
                                std::cerr << "GPS::Readline read didn't return a complete line" << std::endl;
                                //exit(EXIT_FAILURE);
                                return -1;
      }
      msgbuf[msglen] = '\0';      
      return 0;
    }   

    // wait for a little bit before trying again
    sleep(1);

  }

  return -1;
}



// Writes msg to serial port, makes sure complete message is put on serial port
// Before returning

int 
GPS::writeLine(const char* msg) 
{
  int len = strlen(msg);
  
  //std::cout << "GPS::writeLine, writing len= " << len 
        //    << " msg='" << msg << "' to serial fd=" << gps_fd << std::endl;

  int result = write(gps_fd,msg,len);
  if (result == -1) {
    std::cerr << "GPS::writeLine error writing to gps serial port : " << strerror(errno) << std::endl;
                return -1;
    //exit(EXIT_FAILURE);
  } 
  else if (result != len) {
    std::cerr << "GPS::writeLine error, only wrote " << result << " bytes of " << len << std::endl;
                return -1;
    //exit(EXIT_FAILURE);
  }

  return 0;
}




// Will check to see if the next responce message from the GPS unit is an 
// acknowledgement message 

int 
GPS::checkAck(void) {
  if ( readLine() ) {
    std::cerr << "GPS::checkAck : No repsonse from GPS unit" << std::endl;
                return -1;
  }

  if (strcmp(msgbuf, "$PASHR,ACK*3D\r\n") == 0) {
    return 0;
  }
  else if (strcmp(msgbuf, "$PASHR,NAK*30\r\n") == 0) {
    std::cerr << "GPS::checkAck : recieved NAK message '" << msgbuf << ",\n";
                return -1;
    //exit(EXIT_FAILURE);
  }
  else {
    std::cerr << "GPS::checkAck : recieved unknown message '" << msgbuf << ",\n";
                return -1;
    //exit(EXIT_FAILURE);
  } 

  return -1;
}


// This function won't block waiting for data from the serial port.  
// This function will buffer the GPS serial data until it has a complete record 
//  that it can parse.  
// This function will return a 1 when it has new, data or a 0 otherwise

int 
GPS::Read(void) {
        int result;

        // read any new data from GPS system into buffer
        result = read(gps_fd, msgbuf, sizeof(msgbuf));  
        if (result == 0) {
                std::cerr << "GPS::Read : serial port closed?" << std::endl;
                //exit(EXIT_FAILURE);
                return -1;
        } 
        else if (result == -1) {
                if (errno == EAGAIN) {
                        return 0;
                }
                else {
                        std::cerr << "GPS::Read error - " << strerror(errno) << std::endl;
                        return -1;
                        //exit(EXIT_FAILURE);
                }
        }       
  
        if (result >= (int) sizeof(msgbuf)) {
                std::cerr << "GPS::ReadNoWait : message is longer than buffer" << std::endl;
                exit(EXIT_FAILURE);
        }

        msglen = result;
        msgbuf[msglen] = '\0';
        //std::cout << "POS = '" << msgbuf << "'\n";  

        //make sure the message is complete
        if ( (msgbuf[msglen-2] != '\r') ||
                 (msgbuf[msglen-1] != '\n') ) {
                std::cerr << "GPS::ReadNoWait : missing EOL chars in '" << msgbuf << "'\n";
                //exit(EXIT_FAILURE);
                return -1;
        }

        if (dispatchMsg(msgbuf) < 0) {
                cerr << "GPS::Read error parsing message '" << msgbuf << "'\n";
                return -1;
        }
        else
                return 1;
}


// dispatchMsg()
// Determine what message type GPS is using and calls correct parsing
// fucntion based on the type (parsePOS/parseUTM)

int
GPS::dispatchMsg(const char* msg) {
        if (memcmp(msg,"$PASHR,POS",10) == 0) {         
                return parsePOS(msg);
        }
        else if (memcmp(msgbuf,"$PASHR,UTM",10) == 0) {
                return parseUTM(msg);
        }
        else {
                cerr << "GPS::dispatchMsg - Unknown message '" << msg << '\'' << endl;
                return -1;
        }
}



// Parse POS GPS string into values.  
// Returns 0 if successfull or negative value otherwise
// pashr : response header = '$PASHR'
// pos   : response type = 'POS'     
// d1    : rtsc   - 1 if using RTSC differential correction, 0 if not
// d2    : numsat - number of satellites used in computing position
// m1    : time   - UTC time hhmmss.tt (00 to 235959.50)
// m2    : latitude   - ddmm.mmmmm
// c1    : latsector  - 'N' or 'S' (north or south)
// m3    : longitude  - dddmm.mmmmm 
// c2    : longsector - 'E' or 'W'
// f1    : altitude   - +aaaaa.aa  (in meters)
// f2    : siteid     = '????'  not used (what is it supposed to be?)
// f3    : direction  - ttt.tt referenced from true north (0.00 - 359.99)
// f4    : horz veloc - 000.00 to 999.99 (in knots)
// f5    : vert veloc - -999.99 to +999.99 (in meters/second)
// f6    : pdop       - 0.00 to 99.0
// f7    : hdop       - 
// f8    : vdop       -
// f9    : tdop       - 
// s1    : version    - 4 character ascii
// hh    : checksum   - 2 character hex

int 
GPS::parsePOS(const char* msg) 
{  
  // enumerate the different fields in the array
  enum {pashr=0, pos, d1, d2, m1, m2, c1, m3, c2, f1, f2, f3, f4, f5, f6, f7, f8, f9, s1, hh};

  // find different fields of message by looking for ','
  const char *fieldarr[20], *field=msg, *lastfield=msg; 
  fieldarr[0] = msg;
  for(int i=1; i<=18; ++i) {
    field = strchr(field, ','); 
    if (field == NULL) {
      std::cerr << "GPS::read missing field from '" << lastfield << " iter=" << i << std::endl
                << "  in '" << msg << "'\n"; 
      return -1;
    }
    ++field; // move past ',' 
    fieldarr[i] = field;
    lastfield = field;
  }      

  field = strchr(field, '*'); 
  if (*field != '*') {
    std::cerr << "GPS::parsePOS missing checksum in '" << field << "'\n"
              << "  in '" << msg << ",\n"; 
    return -1;
  }
  fieldarr[19] = field+1;    
    
  // TODO : make sure checksum is correct
 
  // make sure response header matches
  if ( memcmp(fieldarr[pashr], "$PASHR,POS", 10) != 0 ) {
    //if ( ( memcmp(fieldarr[pashr], "$PASHR", 6) != 0 ) || 
    //   ( memcmp(fieldarr[pos], "POS", 3) != 0 ) ) {
    std::cerr << "GPS::parsePOS incorrect message header '" << fieldarr[pashr] << ',' << fieldarr[pos] << "'\n";
    return -1;
  }

  // make sure firmware version makes sense 
  if ( memcmp(fieldarr[s1],"DC00",4) != 0 ) {
    std::cerr << "GPS::parsePOS incorrect firmware version '" << fieldarr[s1] << "'\n";
    return -1;
  }

  errno = 0;
  is_corrected = strtol(fieldarr[d1],NULL,10);
  numsat = strtol(fieldarr[d2],NULL,10);           
  if (errno != 0) {
    std::cerr << "GPS::parsePOS - error converting decimal values : " << strerror(errno) << std::endl;
    return -1;
  }          

  // convert longitude latitude and altitude into x,y,z position WRT and zero-reference
  if ( (*fieldarr[m2] != ',') &&
       (*fieldarr[m3] != ',') &&
       (*fieldarr[c1] != ',') &&
       (*fieldarr[c2] != ',') &&
       (*fieldarr[f1] != ',') ) {
    has_positions = 1;

    NorS = *fieldarr[c1];
    EorW = *fieldarr[c2];
     
    if ( ((NorS != 'N') && (NorS != 'S'))  ||
         ((EorW != 'E') && (EorW != 'W')) ) {
      std::cerr << "GPS::parsePOS - bad GPS region (N S E W) : in " 
                << *fieldarr[c1] << *fieldarr[c2] << std::endl;
      return -1;
    }                   


    errno=0;
    //convert latitude into radians
    {
      char numbuf[3] = {*fieldarr[m2], *(fieldarr[m2]+1), '\0'};
      latitude = minutes2radians * strtod(fieldarr[m2]+2,NULL);
      latitude += degrees2radians * strtod(numbuf,NULL);    
    }
    //convert longitude into radians
    {
      char numbuf[4] = {*fieldarr[m3], *(fieldarr[m3]+1), *(fieldarr[m3]+2), '\0'};
      longitude = minutes2radians * strtod(fieldarr[m3]+3,NULL);      
      longitude += degrees2radians * strtod(numbuf,NULL);
    }

    z = altitude = strtod(fieldarr[m1],NULL);

    if (errno != 0) {
      std::cerr << "GPS::parsePOS - error converting longitude/latitude values : " << strerror(errno) << std::endl;
      return -1;
    }          


    x = (latitude-zeroLatitude) * earthRadius;
    y = (longitude-zeroLongitude) * cos(latitude) * earthRadius;
    
  } 
  else {
    // no positions availible
    has_positions = 0;
    latitude=longitude=invalid;
    x=y=z=invalid;
  }

  // convert direction and speeds 
  if ( (*fieldarr[f3] != ',') && 
       (*fieldarr[f4] != ',') && 
       (*fieldarr[f5] != ',') ) {
    has_velocities = 1;
    errno = 0;

    direction = degrees2radians * strtod(fieldarr[f3],NULL);
    hVelocity = knots2mps * strtod(fieldarr[f4],NULL);
    vVelocity = strtod(fieldarr[f5],NULL);
    if (errno != 0) {
      std::cerr << "GPS::parsePOS - error converting velocity values to floats : " << strerror(errno) << std::endl;
      return -1;
    }      
        xvel = cos(direction) * hVelocity;
        yvel = sin(direction) * hVelocity;
  } else {
    has_velocities = 0;
    direction = invalid;
    hVelocity = invalid;
    vVelocity = invalid;
        xvel = invalid;
        yvel = invalid;
  }


  // get *dop values
  errno = 0;
  pdop = strtod(fieldarr[f6],NULL);
  hdop = strtod(fieldarr[f7],NULL);
  vdop = strtod(fieldarr[f8],NULL);
  tdop = strtod(fieldarr[f9],NULL);
  if (errno != 0) {
    std::cerr << "GPS::parsePOS - error converting *dop values to floats : " << strerror(errno) << std::endl;
    return -1;
  }

  return 0;
}



// Parse UTM GPS string into values.  
// Returns 0 if successfull or negative value otherwise
//  THIS FUNCTION NEEDS TO BE DONE
//

int 
GPS::parseUTM(const char* msg) 
{  
  // enumerate the different fields in the array
  enum {pashr=0, pos, d1, d2, m1, m2, c1, m3, c2, f1, f2, f3, f4, f5, f6, f7, f8, f9, s1, hh};

  // find different fields of message by looking for ','
  const char *fieldarr[20], *field=msg, *lastfield=msg; 
  fieldarr[0] = msg;
  for(int i=1; i<=18; ++i) {
    field = strchr(field, ','); 
    if (field == NULL) {
      std::cerr << "GPS::read missing field from '" << lastfield << " iter=" << i << std::endl
                << "  in '" << msg << "'\n"; 
      return -1;
    }
    ++field; // move past ',' 
    fieldarr[i] = field;
    lastfield = field;
  }      

  field = strchr(field, '*'); 
  if (*field != '*') {
    std::cerr << "GPS::parsePOS missing checksum in '" << field << "'\n"
              << "  in '" << msg << ",\n"; 
    return -1;
  }
  fieldarr[19] = field+1;    
    
  // TODO : make sure checksum is correct
 
  // make sure response header matches
  if ( memcmp(fieldarr[pashr], "$PASHR,POS", 10) != 0 ) {
    //if ( ( memcmp(fieldarr[pashr], "$PASHR", 6) != 0 ) || 
    //   ( memcmp(fieldarr[pos], "POS", 3) != 0 ) ) {
    std::cerr << "GPS::parsePOS incorrect message header '" << fieldarr[pashr] << ',' << fieldarr[pos] << "'\n";
    return -1;
  }

  // make sure firmware version makes sense 
  if ( memcmp(fieldarr[s1],"DC00",4) != 0 ) {
    std::cerr << "GPS::parsePOS incorrect firmware version '" << fieldarr[s1] << "'\n";
    return -1;
  }

  errno = 0;
  is_corrected = strtol(fieldarr[d1],NULL,10);
  numsat = strtol(fieldarr[d2],NULL,10);           
  if (errno != 0) {
    std::cerr << "GPS::parsePOS - error converting decimal values : " << strerror(errno) << std::endl;
    return -1;
  }          

  // convert longitude latitude and altitude into x,y,z position WRT and zero-reference
  if ( (*fieldarr[m2] != ',') &&
       (*fieldarr[m3] != ',') &&
       (*fieldarr[c1] != ',') &&
       (*fieldarr[c2] != ',') &&
       (*fieldarr[f1] != ',') ) {
    has_positions = 1;

    NorS = *fieldarr[c1];
    EorW = *fieldarr[c2];
     
    if ( ((NorS != 'N') && (NorS != 'S'))  ||
         ((EorW != 'E') && (EorW != 'W')) ) {
      std::cerr << "GPS::parsePOS - bad GPS region (N S E W) : in " 
                << *fieldarr[c1] << *fieldarr[c2] << std::endl;
      return -1;
    }                   


    errno=0;
    //convert latitude into radians
    {
      char numbuf[3] = {*fieldarr[m2], *(fieldarr[m2]+1), '\0'};
      latitude = minutes2radians * strtod(fieldarr[m2]+2,NULL);
      latitude += degrees2radians * strtod(numbuf,NULL);    
    }
    //convert longitude into radians
    {
      char numbuf[4] = {*fieldarr[m3], *(fieldarr[m3]+1), *(fieldarr[m3]+2), '\0'};
      longitude = minutes2radians * strtod(fieldarr[m3]+3,NULL);      
      longitude += degrees2radians * strtod(numbuf,NULL);
    }

    z = altitude = strtod(fieldarr[m1],NULL);

    if (errno != 0) {
      std::cerr << "GPS::parsePOS - error converting longitude/latitude values : " << strerror(errno) << std::endl;
      return -1;
    }          


    x = (latitude-zeroLatitude) * earthRadius;
    y = (longitude-zeroLongitude) * cos(latitude) * earthRadius;
    
  } 
  else {
    // no positions availible
    has_positions = 0;
    latitude=longitude=invalid;
    x=y=z=invalid;
  }

  // convert direction and speeds 
  if ( (*fieldarr[f3] != ',') && 
       (*fieldarr[f4] != ',') && 
       (*fieldarr[f5] != ',') ) {
    has_velocities = 1;
    errno = 0;

    direction = degrees2radians * strtod(fieldarr[f3],NULL);
    hVelocity = knots2mps * strtod(fieldarr[f4],NULL);
    vVelocity = strtod(fieldarr[f5],NULL);
    if (errno != 0) {
      std::cerr << "GPS::parsePOS - error converting velocity values to floats : " << strerror(errno) << std::endl;
      return -1;
    }      
  } else {
    has_velocities = 0;
    direction = invalid;
    hVelocity = invalid;
    vVelocity = invalid;
  }


  // get *dop values
  errno = 0;
  pdop = strtod(fieldarr[f6],NULL);
  hdop = strtod(fieldarr[f7],NULL);
  vdop = strtod(fieldarr[f8],NULL);
  tdop = strtod(fieldarr[f9],NULL);
  if (errno != 0) {
    std::cerr << "GPS::parsePOS - error converting *dop values to floats : " << strerror(errno) << std::endl;
    return -1;
  }

  return 0;
}



// Prints out GPS position and velocity data contained in class

void
GPS::printData(void) {
  std::cout 
    << numsat << " Satellites, " << std::endl;

  if (has_positions) 
    std::cout 
      << "lat = " << latitude << " rads, long = " << longitude << " rads, " << NorS << EorW << std::endl
      << " [x,y,z] pos = [" << x << ',' << y << ',' << z << ']' << std::endl; 
  
  if (has_velocities) 
    std::cout 
      << "dir = " << direction << ", hVel = " << hVelocity << ", vVel = " << vVelocity << std::endl
      << " [x,y,z] vel = [" << xvel << ',' << yvel << ',' << zvel << ']' << std::endl; 

  std::cout << "DOP : P=" << pdop << " H=" << hdop << " V=" << vdop << " T=" << tdop << std::endl;

}

---