Cnomicon/daemons/gpsd/GpsDaemon.cpp

//
// GPS Daemon
// Neal Probert
//

#include    <stdio.h>
#include    <unistd.h>
#include    <stdlib.h>

#include    <fcntl.h>
#include    <string.h>
#include    <time.h>
#include    <sys/timeb.h>
#include    <sys/types.h>
#include    <sys/stat.h>
#include    <sys/socket.h>
#include    <sys/types.h>

#include "UriParse.h"
#include "GpsDaemon.h"

#include "NmeaParser.h"
#include "ApplanixParser.h"
#include "FastraxParser.h"
#include "GarminParser.h"
#include "SirfParser.h"
#include "UbloxParser.h"

#include "netlib.h"

//****************************************************************************
// defines
//***************************************************************************/

//****************************************************************************
// macros
//***************************************************************************/

//****************************************************************************
// structs & typedefs
//***************************************************************************/

//****************************************************************************
// global constants
//***************************************************************************/

//****************************************************************************
// global variables
//***************************************************************************/

//****************************************************************************
// static constants
//***************************************************************************/

//****************************************************************************
// static variables
//***************************************************************************/

//****************************************************************************
// static functions
//***************************************************************************/

//****************************************************************************
// C++ functions
//***************************************************************************/

GpsDaemon::GpsDaemon()
{
  log_open( GPSD_NAME );
  
  clockset = 0;
  msgcount = 0;
  
  dgps_corr = true;
  
  SourceIn = CorrIn = -1;
  
  strcpy( Uri, "" );
  nClients = 0;
  
  SetClientTimeout(false);    // don't reset timeout on client activity
}

GpsDaemon::~GpsDaemon()
{
  Gps.GpsClose();
  Log.LogClose();
}

/* NMEA **********************************************************************/

void GpsDaemon::PositionListener( const GpsPoint &Pos, const ErrorInfo &Err )
{
}

void GpsDaemon::TimeListener( const struct timeval &tv )
{
  // good fix and time
  if ( clockset && tv.tv_sec )
  {
    if ( getuid() == 0 )
      settimeofday( &tv, NULL );
    clockset = 0;
  }
}

/* TCP ***********************************************************************/

int GpsDaemon::ServerTimeout(void)
{
  // timeout (a bad thing)
  DgpsReconnect();
  return 0;
}

int GpsDaemon::ServerSocket(void)
{
  char buf[2048];
  int n = 0;
  
  
  if ( IsSet( CorrIn ) )
  {
    // corrections
    if ( dgps_corr )
      // from RTCM server (once a minute)
      n = Dgps.DgpsRead( buf, sizeof(buf) );
    else
      // from NTRIP server (once a minute)
      n = Ntrip.NtripRead( (unsigned char *)buf, sizeof(buf) );
    
    // xmit
    if ( n > 0 )
      Gps.GpsWrite( buf, n );
    else if ( n == 0 )
    {
      if ( dgps_corr )
        log_error( "Remote RTCM server closed!" );
      else
        log_error( "Remote NTRIP server closed!" );
      DgpsReconnect();                
    }
    ResetServerTimeout();
  }
  else if ( IsSet( SourceIn ) )
  {
    if ( gps_source )
      // packet from GPS device
      n = Gps.GpsRead( buf, sizeof(buf) );
    else
      // remote gpsd/glty
      n = Gpsd.Recv( buf, sizeof(buf) );
    
    if ( n > 0 && Parser && Parser->GpsParse(buf, n) > 0 )
    {
      msgcount++;
      
      // 'cast
      ClientCast( buf, n );
    }
  }
  return n;
}

int GpsDaemon::ClientIn( int sock, const char *buf, int n )
{
  // allows for NMEA playback via netcat
  if ( *buf == '$' )
  {
    // broadcast it
    Bcast.SendTo( buf, n );
    
    // send to clients
    TcpServer::ClientCast( buf, n );        
  }
  else
  {
    // parse for commands?
    
    // just log it
    log_write( buf );
  }
  
  return n;
}

int GpsDaemon::ClientUp( int sock )
{
  if ( nClients++ == 0 )
    ;
  return 1;
}

void GpsDaemon::ClientDown( int sock )
{
  // disconnect source
  if ( --nClients <= 0 )
    nClients = 0;
}

void GpsDaemon::ClientCast( const char *buf, int n )
{
  // broadcast it
  Bcast.SendTo( buf, n );
  
  // send to clients
  TcpServer::ClientCast( buf, n );
  
  // log NMEA data with time stamp
  Log.LogWrite( buf, n );
}

/* GPS ***********************************************************************/

int GpsDaemon::GpsOpen( const char *device, int baud )
{
  UriParse parsed;
  parsed.setRate( baud );
  parsed.setUri( device );
  
  const char *proto = parsed.getProto();    
  
  SourceIn = -1;
  
  if ( !proto )
    proto = "nmea";
  
  // connect to gpsd?
  if ( strcmp( proto, "gpsd" ) == 0 )
  {
    // does r mode for NMEA
    SourceIn = Gpsd.GpsdConnect( parsed.getHost() );
    proto = "nmea";
  }
  else if ( strcmp( proto, "glty" ) == 0 )
  {
    // put in NMEA mode
    SourceIn = Gpsd.GpsdConnect( parsed.getHost() );
    Gpsd.Send( "$PGLTY,NMEA\n", 12 );
    proto = "pvii";
  }
  else
  {
    // open device
    SourceIn = Gps.GpsOpen( parsed.getPath(), parsed.getRate() );
    gps_source = true;
  }
  
  // check
  if ( SourceIn < 0 )
    return SourceIn;
  
  bool binary = false;
  
  // attach parser (should probably use a table)
  if ( strcasecmp( proto, "nmea") == 0 )
  {
    // ascii
    NmeaParser *parser = new NmeaParser();
    Parser = (GpsParser *)parser;
  }
  else if ( strcasecmp( proto, "applanix") == 0 )
  {
    // binary
    binary = true;
    ApplanixParser *parser = new ApplanixParser();
    Parser = (GpsParser *)parser;
  }
  else if ( strcasecmp( proto, "fastrax") == 0 )
  {
    // ascii
    FastraxParser *parser = new FastraxParser();
    Parser = (GpsParser *)parser;
  }
  else if ( strcasecmp( proto, "garmin") == 0 )
  {
    // ascii/binary
    GarminParser *parser = new GarminParser();        
    Parser = (GpsParser *)parser;
  }
  else if ( strcasecmp( proto, "sirf") == 0 )
  {
    // binary
    SirfParser *parser = new SirfParser();
    Parser = (GpsParser *)parser;
  }
  else if ( strcasecmp( proto, "ublox") == 0 )
  {
    // ascii/binary
    UbloxParser *parser = new UbloxParser();        
    Parser = (GpsParser *)parser;
  }
  else
  {
    // ascii
    NmeaParser *parser = new NmeaParser();
    Parser = (GpsParser *)parser;        
  }
  
  // set mode
  if ( binary )
    Gps.SetCanonical( false, 0, 0 );
  
  // subscribe
  Parser->Subscribe( (GpsListener*)this );
  
  // add listener
  AddFd( SourceIn );
  
  return SourceIn;
}

int GpsDaemon::DgpsConnect( const char *uri )
{
  if ( !uri )
    return -1;
  
  // already connected?
  if ( CorrIn >= 0 )
    return CorrIn;
  
  UriParse parsed;
  parsed.setService( "2101" );
  parsed.setUri( uri );
  
  const char *proto = parsed.getProto();
  
  if ( strcmp( proto, "dgps" ) == 0 || strcmp( proto, "dgpsip" ) == 0
    || strcmp( proto, "rtcmd" ) == 0 )
  {
    // RTCM server (dgpsip)
    CorrIn = Dgps.DgpsConnect( parsed.getHost(), parsed.getService() );
    dgps_corr = true;
  }
  else if ( strcmp( proto, "ntrip" ) == 0 )
  {
    // NTRIP caster
    CorrIn = Ntrip.NtripConnect( uri );
    dgps_corr = false;
  }
  
  // add listener
  if ( CorrIn >= 0 )
  {
    strcpy( Uri, uri );
    AddFd( CorrIn );
  }
  
  return CorrIn;
}

void GpsDaemon::DgpsDisconnect( void )
{
  if ( CorrIn >= 0 )
    ;
  else
    return;
  Dgps.DgpsClose();
  Ntrip.NtripClose();
  DelFd( CorrIn );
  CorrIn = -1;
}

void GpsDaemon::DgpsReconnect( void )
{
  if ( nClients == 0 || strlen(Uri) == 0 )
    return;
  log_printf( "Remote RTCM/NTRIP timed out, reconnecting" );
  Dgps.DgpsClose();
  Ntrip.NtripClose();
  DelFd( CorrIn );
  CorrIn = -1;
  
  DgpsConnect( Uri );
}

int GpsDaemon::GpsBroadcast( const char *host )
{
  const char *port = GPSD_SERVICE;
  
  int sock = Bcast.Caster( host, port );
  
  return sock;
}