Initial commit of files

daemons/gpsd/GpsDaemon.cpp

@@ -0,0 +1,384 @@
+//
+// 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;
+}