drivers.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 927 行 · 第 1/3 页

 * This is useful to supplement the heading provided by another GPS
 * unit. A GPS heading is unreliable at slow speed or no speed.
 *
 **************************************************************************/

enum {
#include "packet_states.h"
};

static void tnt_add_checksum(char *sentence)
{
    unsigned char sum = '\0';
    char c, *p = sentence;

    if (*p == '@') {
	p++;
    } else {
	gpsd_report(LOG_ERROR, "Bad TNT sentence: '%s'\n", sentence);
    }
    while ( ((c = *p) != '*') && (c != '\0')) {
	sum ^= c;
	p++;
    }
    *p++ = '*';
    /*@i@*/snprintf(p, 4, "%02X\r\n", sum);
}

static int tnt_send(int fd, const char *fmt, ... )
{
    int status;
    char buf[BUFSIZ];
    va_list ap;

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
    va_end(ap);
    strlcat(buf, "*", BUFSIZ);
    tnt_add_checksum(buf);
    status = (int)write(fd, buf, strlen(buf));
    tcdrain(fd);
    if (status == (int)strlen(buf)) {
	gpsd_report(LOG_IO, "=> GPS: %s\n", buf);
	return status;
    } else {
	gpsd_report(LOG_WARN, "=> GPS: %s FAILED\n", buf);
	return -1;
    }
}

#define TNT_SNIFF_RETRIES       100
/*
 * The True North compass won't start talking
 * unless you ask it to. So to identify it we
 * need to query for its ID string.
 */
static int tnt_packet_sniff(struct gps_device_t *session)
{
    unsigned int n, count = 0;

    gpsd_report(LOG_RAW, "tnt_packet_sniff begins\n");
    for (n = 0; n < TNT_SNIFF_RETRIES; n++)
    {
      count = 0;
      (void)tnt_send(session->gpsdata.gps_fd, "@X?");
      if (ioctl(session->gpsdata.gps_fd, FIONREAD, &count) < 0)
	  return BAD_PACKET;
      if (count == 0) {
	  //int delay = 10000000000.0 / session->gpsdata.baudrate;
	  //gpsd_report(LOG_RAW, "usleep(%d)\n", delay);
	  //usleep(delay);
	  gpsd_report(LOG_RAW, "sleep(1)\n");
	  (void)sleep(1);
      } else if (generic_get(session) >= 0) {
	if((session->packet.type == NMEA_PACKET)&&(session->packet.state == NMEA_RECOGNIZED))
	{
	  gpsd_report(LOG_RAW, "tnt_packet_sniff returns %d\n",session->packet.type);
	  return session->packet.type;
	}
      }
    }

    gpsd_report(LOG_RAW, "tnt_packet_sniff found no packet\n");
    return BAD_PACKET;
}

static void tnt_probe_subtype(struct gps_device_t *session, unsigned int seq UNUSED)
{
  // Send codes to start the flow of data
  //tnt_send(session->gpsdata.gps_fd, "@BA?"); // Query current rate
  //tnt_send(session->gpsdata.gps_fd, "@BA=8"); // Start HTM packet at 1Hz
  /*
   * Sending this twice seems to make it more reliable!!
   * I think it gets the input on the unit synced up.
   */
  (void)tnt_send(session->gpsdata.gps_fd, "@BA=15"); // Start HTM packet at 1200 per minute
  (void)tnt_send(session->gpsdata.gps_fd, "@BA=15"); // Start HTM packet at 1200 per minute
}

static bool tnt_probe(struct gps_device_t *session)
{
  unsigned int *ip;
#ifdef FIXED_PORT_SPEED
    /* just the one fixed port speed... */
    static unsigned int rates[] = {FIXED_PORT_SPEED};
#else /* FIXED_PORT_SPEED not defined */
  /* The supported baud rates */
  static unsigned int rates[] = {38400, 19200, 2400, 4800, 9600 };
#endif /* FIXED_PORT_SPEED defined */

  gpsd_report(LOG_PROG, "Probing TrueNorth Compass\n");

  /*
   * Only block until we get at least one character, whatever the
   * third arg of read(2) says.
   */
  /*@ ignore @*/
  memset(session->ttyset.c_cc,0,sizeof(session->ttyset.c_cc));
  session->ttyset.c_cc[VMIN] = 1;
  /*@ end @*/

  session->ttyset.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
  session->ttyset.c_cflag |= CREAD | CLOCAL;
  session->ttyset.c_iflag = session->ttyset.c_oflag = session->ttyset.c_lflag = (tcflag_t) 0;

  session->baudindex = 0;
  for (ip = rates; ip < rates + sizeof(rates)/sizeof(rates[0]); ip++)
      if (ip == rates || *ip != rates[0])
      {
	  gpsd_report(LOG_PROG, "hunting at speed %d\n", *ip);
	  gpsd_set_speed(session, *ip, 'N',1);
	  if (tnt_packet_sniff(session) != BAD_PACKET)
	      return true;
      }
  return false;
}

struct gps_type_t trueNorth = {
    .type_name      = "True North",	/* full name of type */
    .trigger	    = " TNT1500",
    .channels       = 0,		/* not an actual GPS at all */
    .probe_wakeup   = NULL,		/* this will become a real method */
    .probe_detect   = tnt_probe,	/* probe by sending ID query */
    .probe_subtype  = tnt_probe_subtype,/* probe for True North Digital Compass */
#ifdef ALLOW_RECONFIGURE
    .configurator   = NULL,		/* no setting changes */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = generic_get,	/* how to get a packet */
    .parse_packet   = nmea_parse_input,	/* how to interpret a packet */
    .rtcm_writer    = NULL,		/* Don't send */
    .speed_switcher = NULL,		/* no speed switcher */
    .mode_switcher  = NULL,		/* no mode switcher */
    .rate_switcher  = NULL,		/* no wrapup */
    .cycle_chars    = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert	    = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup	    = NULL,		/* no wrapup */
    .cycle	    = 20,		/* updates per second */
};
#endif
#ifdef RTCM104_ENABLE
/**************************************************************************
 *
 * RTCM-104, used for broadcasting DGPS corrections and by DGPS radios
 *
 **************************************************************************/

static gps_mask_t rtcm104_analyze(struct gps_device_t *session)
{
    rtcm_unpack(&session->gpsdata.rtcm, (char *)session->packet.isgps.buf);
    gpsd_report(LOG_RAW, "RTCM packet type 0x%02x length %d words: %s\n",
		session->gpsdata.rtcm.type,
		session->gpsdata.rtcm.length+2,
		gpsd_hexdump(session->packet.isgps.buf, (session->gpsdata.rtcm.length+2)*sizeof(isgps30bits_t)));
    return RTCM_SET;
}

static struct gps_type_t rtcm104 = {
    .type_name     = "RTCM104",		/* full name of type */
    .trigger       = NULL,		/* no recognition string */
    .channels      = 0,			/* not used */
    .probe_wakeup  = NULL,		/* no wakeup to be done before hunt */
    .probe_detect  = NULL,		/* no probe */
    .probe_subtype = NULL,		/* no subtypes */
#ifdef ALLOW_RECONFIGURE
    .configurator  = NULL,		/* no configurator */
#endif /* ALLOW_RECONFIGURE */
    .get_packet    = generic_get,	/* how to get a packet */
    .parse_packet  = rtcm104_analyze,	/*  */
    .rtcm_writer   = NULL,		/* don't send RTCM data,  */
    .speed_switcher= NULL,		/* no speed switcher */
    .mode_switcher = NULL,		/* no mode switcher */
    .rate_switcher = NULL,		/* no sample-rate switcher */
    .cycle_chars   = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert	   = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup	   = NULL,		/* no wrapup code */
    .cycle	   = 1,			/* updates every second */
};
#endif /* RTCM104_ENABLE */

#ifdef GARMINTXT_ENABLE
/**************************************************************************
 *
 * Garmin Simple Text protocol
 *
 **************************************************************************/

static gps_mask_t garmintxt_parse_input(struct gps_device_t *session)
{
    //gpsd_report(LOG_PROG, "Garmin Simple Text packet\n");
    return garmintxt_parse(session);
}


static struct gps_type_t garmintxt = {
    .type_name     = "Garmin Simple Text",		/* full name of type */
    .trigger       = NULL,		/* no recognition string */
    .channels      = 0,			/* not used */
    .probe_wakeup  = NULL,		/* no wakeup to be done before hunt */
    .probe_detect  = NULL,		/* no probe */
    .probe_subtype = NULL,		/* no subtypes */
#ifdef ALLOW_RECONFIGURE
    .configurator  = NULL,		/* no configurator */
#endif /* ALLOW_RECONFIGURE */
    .get_packet    = generic_get,	/* how to get a packet */
    .parse_packet  = garmintxt_parse_input,	/*  */
    .rtcm_writer   = NULL,		/* don't send RTCM data,  */
    .speed_switcher= NULL,		/* no speed switcher */
    .mode_switcher = NULL,		/* no mode switcher */
    .rate_switcher = NULL,		/* no sample-rate switcher */
    .cycle_chars   = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert	   = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup	   = NULL,		/* no wrapup code */
    .cycle	   = 1,			/* updates every second */
};
#endif /* GARMINTXT_ENABLE */

extern struct gps_type_t garmin_usb_binary, garmin_ser_binary;
extern struct gps_type_t sirf_binary, tsip_binary;
extern struct gps_type_t evermore_binary, italk_binary;
extern struct gps_type_t navcom_binary;

/*@ -nullassign @*/
/* the point of this rigamarole is to not have to export a table size */
static struct gps_type_t *gpsd_driver_array[] = {
#ifdef NMEA_ENABLE
    &nmea,
#ifdef ASHTECH_ENABLE
    &ashtech,
#endif /* ASHTECHV18_ENABLE */
#ifdef FV18_ENABLE
    &fv18,
#endif /* FV18_ENABLE */
#ifdef GPSCLOCK_ENABLE
    &gpsclock,
#endif /* GPSCLOCK_ENABLE */
#ifdef GARMIN_ENABLE
    &garmin,
#endif /* GARMIN_ENABLE */
#ifdef TRIPMATE_ENABLE
    &tripmate,
#endif /* TRIPMATE_ENABLE */
#ifdef EARTHMATE_ENABLE
    &earthmate,
#endif /* EARTHMATE_ENABLE */
#endif /* NMEA_ENABLE */
#ifdef ZODIAC_ENABLE
    &zodiac_binary,
#endif /* ZODIAC_ENABLE */
#ifdef NAVCOM_ENABLE
    &navcom_binary,
#endif /* NAVCOM_ENABLE */
#ifdef UBX_ENABLE
    &ubx_binary,
#endif /* UBX_ENABLE */
#ifdef GARMIN_ENABLE
    &garmin_usb_binary,
    &garmin_ser_binary,
#endif /* GARMIN_ENABLE */
#ifdef SIRF_ENABLE
    &sirf_binary,
#endif /* SIRF_ENABLE */
#ifdef TSIP_ENABLE
    &tsip_binary,
#endif /* TSIP_ENABLE */
#ifdef TNT_ENABLE
    &trueNorth,
#endif /* TSIP_ENABLE */
#ifdef EVERMORE_ENABLE
    &evermore_binary,
#endif /* EVERMORE_ENABLE */
#ifdef ITRAX_ENABLE
    &italk_binary,
#endif /* ITRAX_ENABLE */
#ifdef RTCM104_ENABLE
    &rtcm104,
#endif /* RTCM104_ENABLE */
#ifdef GARMINTXT_ENABLE
    &garmintxt,
#endif /* GARMINTXT_ENABLE */
    NULL,
};
/*@ +nullassign @*/
struct gps_type_t **gpsd_drivers = &gpsd_driver_array[0];