garminnmea.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000-2003  Brian Gerkey [e-mail]
 *                           Andrew Howard [e-mail]
 *
 *  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
 *
 */

/*
 * $Id: garminnmea.cc,v 1.24 2006/02/23 02:21:49 gerkey Exp $
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_garminnmea garminnmea
 * @brief Garmin GPS unit
 
%Device driver for the Garmin geko 201 handheld GPS unit.  Interacts with
the unit by speaking NMEA over a serial line.  As such, this driver may
work with other Garmin units, and (likely with some modification) other
NMEA-compliant GPS units.
 
The driver may also attempt to read DGPS RTCM corrections from a
multi-cast network address, and forward these corrections to the
GPS unit.  The @ref util_dgps_server utility may be used to gather
and broadcast the DGPS RTCM corrections.
 
NMEA and proprietary Garmin codes can be found at
http://home.mira.net/~gnb/gps/nmea.html

@par Compile-time dependencies

- none

@par Requires

- none

@par Provides

- @ref interface_gps

@par Configuration requests

- none

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
  - Serial port where the GPS unit is connected
- baud (integer)
  - Default: 4800
  - Speed of serial connection to the GPS unit
- dgps_enable (integer)
  - Default: 1
  - Enable/disable listening for DGPS corrections via UDP multicast
    (use @ref util_dgps_server to send the corrections)
- dgps_group (string)
  - Default: "225.0.0.43"
  - Multicast group on which to listen for DGPS corrections</td></tr>
- dgps_port (integer)
  - Default: 7778
  - UDP port on which to listen for DGPS corrections

@par Example

@verbatim
driver
(
  name "garminnmea"
  provides ["gps:0"]
  port "/dev/ttyS1"
)
@endverbatim

@authors Brian Gerkey, Andrew Howard

*/
/** @} */
#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif


#include <fcntl.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>  // for htons(3)
#include <arpa/inet.h> // inet_addr
#include <errno.h>


#include <libplayercore/playercore.h>
#include <replace/replace.h>

#define DEFAULT_GPS_PORT "/dev/ttyS0"
#define DEFAULT_DGPS_GROUP "225.0.0.43"
#define DEFAULT_DGPS_PORT 7778


// time that we'll wait to get the first round of data from the unit.
// currently 1s
#define GPS_STARTUP_CYCLE_USEC 100000
#define GPS_STARTUP_CYCLES 10

// these are the standard NMEA sentences that come 
// out of the geko 201
#define NMEA_GPRMB "GPRMB"
#define NMEA_GPRMC "GPRMC"
#define NMEA_GPGGA "GPGGA"
#define NMEA_GPGSA "GPGSA"
#define NMEA_GPGSV "GPGSV"
#define NMEA_GPGLL "GPGLL"
#define NMEA_GPBOD "GPBOD"
#define NMEA_GPRTE "GPRTE"

// these are the proprietary NMEA sentences that 
// come out of the geko 201
#define NMEA_PGRME "PGRME"
#define NMEA_PGRMZ "PGRMZ"
#define NMEA_PSLIB "PSLIB"

// spec says 82, but that done mean squat. Make it big
#define NMEA_MAX_SENTENCE_LEN 83

#define NMEA_START_CHAR '$'
#define NMEA_END_CHAR '\n'
#define NMEA_CHKSUM_CHAR '*'

// WGS84 Parameters
#define WGS84_A		6378137.0		// major axis
#define WGS84_B		6356752.31424518	// minor axis
#define WGS84_F		0.0033528107		// ellipsoid flattening
#define WGS84_E		0.0818191908		// first eccentricity
#define WGS84_EP	0.0820944379		// second eccentricity

// UTM Parameters
#define UTM_K0		0.9996			// scale factor
#define UTM_FE		500000.0		// false easting
#define UTM_FN_N	0.0			// false northing on north hemisphere
#define UTM_FN_S	10000000.0		// false northing on south hemisphere
#define UTM_E2		(WGS84_E*WGS84_E)	// e^2
#define UTM_E4		(UTM_E2*UTM_E2)		// e^4
#define UTM_E6		(UTM_E4*UTM_E2)		// e^6
#define UTM_EP2		(UTM_E2/(1-UTM_E2))	// e'^2


class GarminNMEA:public Driver 
{
  private:

    // string name of serial port to use
    const char* gps_serial_port;

    // GPS baud rate
    int gps_baud;

    // file descriptor for the gps unit
    int gps_fd;  

    // Enable DGPS corrections?
    int dgps_enable;
  
    // Port number for DGPS RTCM corrections
    const char *dgps_group;
    int dgps_port;

    // file descriptor for the DGPS UDP socket
    int dgps_fd;
    bool gps_fd_blocking;
  
    char nmea_buf[NMEA_MAX_SENTENCE_LEN+1];
    size_t nmea_buf_len;

    // Status
    int read_count;

    // Filtered GPS geodetic coords; for outlier rejection
    double filter_a, filter_thresh;
    double filter_lat, filter_lon;
    bool filter_good;
  
    // Current GPS data packet
    player_gps_data_t data;

    int SetupSerial();
    void ShutdownSerial();
  
    int SetupSocket();
    void ShutdownSocket();

    int ReadSentence(char* buf, size_t len);
    int WriteSentence(const char *buf, size_t len);
    int ReadSocket(char *buf, size_t len);

    int FillBuffer();
    int ParseSentence(const char* buf);
    int ParseGPGGA(const char *buf);
    int ParseGPRMC(const char *buf);
    int ParsePGRME(const char *buf);
    char* GetNextField(char* field, size_t len, const char* ptr);

    // utility functions to convert geodetic to UTM position
    void UTM(double lat, double lon, double *x, double *y);


  public:
    GarminNMEA( ConfigFile* cf, int section);

    virtual int Setup();
    virtual int Shutdown();
    virtual void Main();
};


///////////////////////////////////////////////////////////////////////////
// initialization function
Driver* GarminNMEA_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new GarminNMEA( cf, section)));
}


///////////////////////////////////////////////////////////////////////////
// a driver registration function
void 
GarminNMEA_Register(DriverTable* table)
{
  table->AddDriver("garminnmea",  GarminNMEA_Init);
}

///////////////////////////////////////////////////////////////////////////
GarminNMEA::GarminNMEA( ConfigFile* cf, int section) 
: Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_GPS_CODE)
{
  memset(&data,0,sizeof(data));

  gps_fd = -1;

  gps_serial_port = cf->ReadString(section, "port", DEFAULT_GPS_PORT);
  gps_baud = cf->ReadInt(section, "baud", 4800);

  read_count = 0;
  
  filter_a = 0.80;
  filter_thresh = 1.0;
  filter_lat = 0;
  filter_lon = 0;

  dgps_enable = cf->ReadInt(section, "dgps_enable", 1);
  dgps_group = cf->ReadString(section, "dgps_group", DEFAULT_DGPS_GROUP);
  dgps_port = cf->ReadInt(section, "dgps_port", DEFAULT_DGPS_PORT);

  return;
}


///////////////////////////////////////////////////////////////////////////
int
GarminNMEA::Setup()
{
  // Set up the serial port to talk to the GPS unit
  if (SetupSerial() != 0)
    return -1;

  // Set up the UDP port to get DGPS corrections
  if (SetupSocket() != 0)
    return -1;
  
  puts("Done.");
  
  // start the thread to talk with the GPS unit
  StartThread();

  return(0);
}


///////////////////////////////////////////////////////////////////////////
int
GarminNMEA::Shutdown()
{
  StopThread();

  ShutdownSocket();
  ShutdownSerial();

  return(0);
}


///////////////////////////////////////////////////////////////////////////
// Initialize the serial port
int
GarminNMEA::SetupSerial()
{
  struct termios term;
  int flags;
  int attempt;
  int maxattempts=10;

  printf("GPS connection initializing (%s)...", gps_serial_port);
  fflush(stdout);

  // open it.  non-blocking at first, in case there's no gps unit.
  if((gps_fd = open(gps_serial_port, 
                    O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0)
  {
    PLAYER_ERROR1("open(): %s\n", strerror(errno));
    return(-1);
  }  

  gps_fd_blocking = false;
 
  if(tcflush(gps_fd, TCIFLUSH ) < 0 )
  {
    PLAYER_ERROR1("tcflush(): %s\n", strerror(errno));
    close(gps_fd);
    gps_fd = -1;
    return(-1);
  }
  if(tcgetattr(gps_fd, &term) < 0 )
  {
    PLAYER_ERROR1("tcgetattr(): %s\n", strerror(errno));
    close(gps_fd);
    gps_fd = -1;
    return(-1);
  }
  
  cfmakeraw(&term);

  if (gps_baud == 9600)
  {
    cfsetispeed(&term, B9600);
    cfsetospeed(&term, B9600);
  }
  else if (gps_baud == 19200)
  {
    cfsetispeed(&term, B19200);
    cfsetospeed(&term, B19200);
  }
  else if (gps_baud == 38400)
  {
    cfsetispeed(&term, B38400);
    cfsetospeed(&term, B38400);
  }
  else
  {
    cfsetispeed(&term, B4800);
    cfsetospeed(&term, B4800);
  }
  
  if(tcsetattr(gps_fd, TCSAFLUSH, &term) < 0 )
  {
    PLAYER_ERROR1("tcsetattr(): %s\n", strerror(errno));
    close(gps_fd);
    gps_fd = -1;
    return(-1);
  }

  printf("filling buffer\n");
  fflush(stdout);
  
  memset(nmea_buf,0,sizeof(nmea_buf));
  nmea_buf_len=0;
  /* try to read some data, just to make sure we actually have a gps unit */
  for(attempt=0;attempt<maxattempts;attempt++)
  {
    if(!FillBuffer())
      break;
  }
  if(attempt==maxattempts)
  {
    PLAYER_ERROR1("Couldn't connect to GPS unit, most likely because the \n"
                  "unit is not connected or is connected not to %s\n", 
                    gps_serial_port);
    close(gps_fd);
    gps_fd = -1;
    return(-1);
  }

  printf("done filling buffer\n");
  fflush(stdout);

  /* ok, we got data, so now set NONBLOCK, and continue */
  if((flags = fcntl(gps_fd, F_GETFL)) < 0)
  {
    PLAYER_ERROR1("fcntl(): %s\n", strerror(errno));
    close(gps_fd);
    gps_fd = -1;
    return(1);
  }
  if(fcntl(gps_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
  {
    PLAYER_ERROR1("fcntl(): %s\n", strerror(errno));
    close(gps_fd);
    gps_fd = -1;
    return(1);
  }

  gps_fd_blocking = true;

  return 0;
}


///////////////////////////////////////////////////////////////////////////
// Shutdown the serial port
void
GarminNMEA::ShutdownSerial()
{
  close(gps_fd);
  gps_fd=-1;
  return;
}


///////////////////////////////////////////////////////////////////////////
// Initialise the UDP socket for recieving DGPS corrections
int
GarminNMEA::SetupSocket()
{
  sockaddr_in addr;
  struct ip_mreq mreq;

  if (!this->dgps_enable)
    return 0;
  
  // Set up the read socket
  this->dgps_fd = socket(PF_INET, SOCK_DGRAM, 0);
  if (this->dgps_fd == -1)
  {
    PLAYER_ERROR1("error initializing socket : %s", strerror(errno));
    return 1;
  }

  // Set socket options to allow sharing of port
  u_int share = 1;
  if (setsockopt(this->dgps_fd, SOL_SOCKET, SO_REUSEADDR,
                 (const char*)&share, sizeof(share)) < 0)
  {
    PLAYER_ERROR1("error initializing socket : %s", strerror(errno));
    return 1;
  }
    
  // Bind socket to port
  memset(&addr, 0, sizeof(addr));
  addr.sin_family = AF_INET;
  addr.sin_addr.s_addr = htonl(INADDR_ANY);
  addr.sin_port = htons(this->dgps_port);
  if (bind(this->dgps_fd, (sockaddr*) &addr, sizeof(addr)) < 0)
  {
    PLAYER_ERROR1("error initializing socket : %s", strerror(errno));
    return 1;
  }

  // Join the multi-cast group
  mreq.imr_multiaddr.s_addr = inet_addr(this->dgps_group);
  mreq.imr_interface.s_addr = htonl(INADDR_ANY);
  if (setsockopt(this->dgps_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) != 0)
  {
    PLAYER_ERROR1("error joining multicast group : %s", strerror(errno));
    return 1;
  }

  return 0;
}


/*
 * Shutdown the DGPS socket
 */
void
GarminNMEA::ShutdownSocket()
{
  if (!this->dgps_enable)
    return;

  close(this->dgps_fd);

  return;
}


/*
 * Driver thread runs here.  We have to poll on both the serial port
 * and the UDP socket.
 */
void
GarminNMEA::Main()
{
  int len;
  char buf[NMEA_MAX_SENTENCE_LEN];
  char rtcm_buf[1024];
  struct pollfd fds[2];
  int fd_count;

  fd_count = 0;
  
  fds[fd_count].fd = this->gps_fd;
  fds[fd_count].events = POLLIN | POLLPRI | POLLERR | POLLHUP;
  fd_count++;

  if (this->dgps_enable)
  {
    fds[fd_count].fd = this->dgps_fd;
    fds[fd_count].events = POLLIN | POLLPRI | POLLERR | POLLHUP;
    fd_count++;
  }

  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  for(;;)
  {
    pthread_testcancel();

    if (poll(fds, fd_count, 100) < 0)
    {
      PLAYER_ERROR1("poll returned [%s]", strerror(errno));
      continue;
    }

    // Read incoming data from the GPS
    if (fds[0].revents)
    {
      if(ReadSentence(buf,sizeof(buf)))
      {
        PLAYER_ERROR("while reading from GPS unit; bailing");
        pthread_exit(NULL);
      }
      ParseSentence((const char*)buf);
    }

    // Read incoming DGPS corrections from the socket
    if (this->dgps_enable)
    {
      if (fds[1].revents)
      {
        len = ReadSocket(rtcm_buf, sizeof(rtcm_buf));
        if (len < 0)
          pthread_exit(NULL);

        printf("got udp packet of length %d\n", len);
        fflush(stdout);
      
        // Write the DGPS sentence to the GPS unit
        if (WriteSentence(rtcm_buf, len) != 0)
          pthread_exit(NULL);
      }
    }
  }

  return;
}


/*
 * Find a complete NMEA sentence and copy it into 'buf', which should be of
 * length 'len'.  This function will call FillBuffer() as necessary to get
 * enough data into 'nmea_buf' to form a complete sentence. 'buf' will be
 * NULL-terminated.
 *
 */
int
GarminNMEA::ReadSentence(char* buf, size_t len)
{
  char* ptr;
  char* ptr2;
  size_t sentlen;
  char tmp[8];
  int chksum;
  int oursum; 

  //printf("reading sentence\n");