示例#1
0
void ugear_event( void ) {
    if (UgearBuffer()){
        ReadUgearBuffer();
    }
    if (ugear_msg_received){
        parse_ugear_msg();
        ugear_msg_received = FALSE;
        if (gps_pos_available){
            //gps_downlink();
            gps_verbose_downlink = !launch;
            UseGpsPosNoSend(estimator_update_state_gps);
            gps_msg_received_counter = gps_msg_received_counter+1;
            #ifdef GX2
            if (gps_msg_received_counter == 1){
                gps_send();
                gps_msg_received_counter = 0;
            }
            #endif
            #ifdef XSENSDL
            if (gps_msg_received_counter == 25){
                gps_send();
                gps_msg_received_counter = 0;
            }
            #endif
            gps_pos_available = FALSE;
        }
    }
}
示例#2
0
文件: libgps_sock.c 项目: yazug/gpsd
int gps_sock_stream(struct gps_data_t *gpsdata, unsigned int flags, void *d)
/* ask gpsd to stream reports at you, hiding the command details */
{
    char buf[GPS_JSON_COMMAND_MAX];

    if ((flags & (WATCH_JSON | WATCH_NMEA | WATCH_RAW)) == 0) {
	flags |= WATCH_JSON;
    }
    if ((flags & WATCH_DISABLE) != 0) {
	(void)strlcpy(buf, "?WATCH={\"enable\":false,", sizeof(buf));
	if (flags & WATCH_JSON)
	    (void)strlcat(buf, "\"json\":false,", sizeof(buf));
	if (flags & WATCH_NMEA)
	    (void)strlcat(buf, "\"nmea\":false,", sizeof(buf));
	if (flags & WATCH_RAW)
	    (void)strlcat(buf, "\"raw\":1,", sizeof(buf));
	if (flags & WATCH_RARE)
	    (void)strlcat(buf, "\"raw\":0,", sizeof(buf));
	if (flags & WATCH_SCALED)
	    (void)strlcat(buf, "\"scaled\":false,", sizeof(buf));
	if (flags & WATCH_TIMING)
	    (void)strlcat(buf, "\"timing\":false,", sizeof(buf));
	if (flags & WATCH_SPLIT24)
	    (void)strlcat(buf, "\"split24\":false,", sizeof(buf));
	if (flags & WATCH_PPS)
	    (void)strlcat(buf, "\"pps\":false,", sizeof(buf));
	str_rstrip_char(buf, ',');
	(void)strlcat(buf, "};", sizeof(buf));
	libgps_debug_trace((DEBUG_CALLS, "gps_stream() disable command: %s\n", buf));
	return gps_send(gpsdata, buf);
    } else {			/* if ((flags & WATCH_ENABLE) != 0) */
	(void)strlcpy(buf, "?WATCH={\"enable\":true,", sizeof(buf));
	if (flags & WATCH_JSON)
	    (void)strlcat(buf, "\"json\":true,", sizeof(buf));
	if (flags & WATCH_NMEA)
	    (void)strlcat(buf, "\"nmea\":true,", sizeof(buf));
	if (flags & WATCH_RARE)
	    (void)strlcat(buf, "\"raw\":1,", sizeof(buf));
	if (flags & WATCH_RAW)
	    (void)strlcat(buf, "\"raw\":2,", sizeof(buf));
	if (flags & WATCH_SCALED)
	    (void)strlcat(buf, "\"scaled\":true,", sizeof(buf));
	if (flags & WATCH_TIMING)
	    (void)strlcat(buf, "\"timing\":true,", sizeof(buf));
	if (flags & WATCH_SPLIT24)
	    (void)strlcat(buf, "\"split24\":true,", sizeof(buf));
	if (flags & WATCH_PPS)
	    (void)strlcat(buf, "\"pps\":true,", sizeof(buf));
	if (flags & WATCH_DEVICE)
	    str_appendf(buf, sizeof(buf), "\"device\":\"%s\",", (char *)d);
	str_rstrip_char(buf, ',');
	(void)strlcat(buf, "};", sizeof(buf));
	libgps_debug_trace((DEBUG_CALLS, "gps_stream() enable command: %s\n", buf));
	return gps_send(gpsdata, buf);
    }
}
示例#3
0
struct gps_data_t* gpsmm::send(const char *request)
{
    if (gps_send(gps_state(),request)==-1) {
	return NULL;
    }
    else {
	return backup();
    }
}
示例#4
0
/*
 * Send a nak for the given packet type. Return 1 if the packet
 * sent OK, othewise -1.
 */
int
gps_send_nak(gps_handle gps, u_char type)
{
	u_char buf[4];

	buf[0] = nak;
	buf[1] = type;
	buf[2] = 0;
	return gps_send(gps, buf, 3);
}
示例#5
0
int main(int argc, char *argv[])
{
    struct gps_data_t collect;
    struct fixsource_t source;
    char buf[BUFSIZ];
    int option;
    bool batchmode = false;
    bool forwardmode = false;
    char *fmsg = NULL;
#ifdef CLIENTDEBUG_ENABLE
    int debug = 0;
#endif

    (void)signal(SIGSEGV, onsig);
    (void)signal(SIGBUS, onsig);

    while ((option = getopt(argc, argv, "bf:hsD:?")) != -1) {
	switch (option) {
	case 'b':
	    batchmode = true;
	    break;
	case 'f':
	    forwardmode = true;
	    fmsg = optarg;
	    break;
	case 's':
	    (void)
		printf
		("Sizes: fix=%zd gpsdata=%zd rtcm2=%zd rtcm3=%zd ais=%zd compass=%zd raw=%zd devices=%zd policy=%zd version=%zd, noise=%zd\n",
		 sizeof(struct gps_fix_t),
		 sizeof(struct gps_data_t), sizeof(struct rtcm2_t),
		 sizeof(struct rtcm3_t), sizeof(struct ais_t),
		 sizeof(struct attitude_t), sizeof(struct rawdata_t),
		 sizeof(collect.devices), sizeof(struct policy_t),
		 sizeof(struct version_t), sizeof(struct gst_t));
	    exit(EXIT_SUCCESS);
#ifdef CLIENTDEBUG_ENABLE
	case 'D':
	    debug = atoi(optarg);
	    break;
#endif
	case '?':
	case 'h':
	default:
	    (void)fputs("usage: test_libgps [-b] [-f fwdmsg] [-D lvl] [-s] [server[:port:[device]]]\n", stderr);
	    exit(EXIT_FAILURE);
	}
    }

    /* Grok the server, port, and device. */
    if (optind < argc) {
	gpsd_source_spec(argv[optind], &source);
    } else
	gpsd_source_spec(NULL, &source);

#ifdef CLIENTDEBUG_ENABLE
    gps_enable_debug(debug, stdout);
#endif
    if (batchmode) {
#ifdef SOCKET_EXPORT_ENABLE
	while (fgets(buf, sizeof(buf), stdin) != NULL) {
	    if (buf[0] == '{' || isalpha(buf[0])) {
		gps_unpack(buf, &gpsdata);
		libgps_dump_state(&gpsdata);
	    }
	}
#endif
    } else if (gps_open(source.server, source.port, &collect) != 0) {
	(void)fprintf(stderr,
		      "test_libgps: no gpsd running or network error: %d, %s\n",
		      errno, gps_errstr(errno));
	exit(EXIT_FAILURE);
    } else if (forwardmode) {
	(void)gps_send(&collect, fmsg);
	(void)gps_read(&collect);
#ifdef SOCKET_EXPORT_ENABLE
	libgps_dump_state(&collect);
#endif
	(void)gps_close(&collect);
    } else {
	int tty = isatty(0);

	if (tty)
	    (void)fputs("This is the gpsd exerciser.\n", stdout);
	for (;;) {
	    if (tty)
		(void)fputs("> ", stdout);
	    if (fgets(buf, sizeof(buf), stdin) == NULL) {
		if (tty)
		    putchar('\n');
		break;
	    }
	    collect.set = 0;
	    (void)gps_send(&collect, buf);
	    (void)gps_read(&collect);
#ifdef SOCKET_EXPORT_ENABLE
	    libgps_dump_state(&collect);
#endif
	}
	(void)gps_close(&collect);
    }

    return 0;
}
示例#6
0
int main(int argc, char *argv[])
{
    struct gps_data_t collect;
    char buf[BUFSIZ];
    int option;
    bool batchmode = false;
    int debug = 0;

    (void)signal(SIGSEGV, onsig);
    (void)signal(SIGBUS, onsig);

    while ((option = getopt(argc, argv, "bhsD:?")) != -1) {
	switch (option) {
	case 'b':
	    batchmode = true;
	    break;
	case 's':
	    (void)
		printf
		("Sizes: fix=%zd gpsdata=%zd rtcm2=%zd rtcm3=%zd ais=%zd compass=%zd raw=%zd devices=%zd policy=%zd version=%zd, noise=%zd\n",
		 sizeof(struct gps_fix_t),
		 sizeof(struct gps_data_t), sizeof(struct rtcm2_t),
		 sizeof(struct rtcm3_t), sizeof(struct ais_t),
		 sizeof(struct attitude_t), sizeof(struct rawdata_t),
		 sizeof(collect.devices), sizeof(struct policy_t),
		 sizeof(struct version_t), sizeof(struct gst_t));
	    exit(EXIT_SUCCESS);
	case 'D':
	    debug = atoi(optarg);
	    break;
	case '?':
	case 'h':
	default:
	    (void)fputs("usage: test_libgps [-b] [-D lvl] [-s]\n", stderr);
	    exit(EXIT_FAILURE);
	}
    }

    gps_enable_debug(debug, stdout);
    if (batchmode) {
	while (fgets(buf, sizeof(buf), stdin) != NULL) {
	    if (buf[0] == '{' || isalpha(buf[0])) {
		gps_unpack(buf, &gpsdata);
		libgps_dump_state(&gpsdata);
	    }
	}
    } else if (gps_open(NULL, 0, &collect) <= 0) {
	(void)fputs("Daemon is not running.\n", stdout);
	exit(EXIT_FAILURE);
    } else if (optind < argc) {
	(void)strlcpy(buf, argv[optind], BUFSIZ);
	(void)strlcat(buf, "\n", BUFSIZ);
	(void)gps_send(&collect, buf);
	(void)gps_read(&collect);
	libgps_dump_state(&collect);
	(void)gps_close(&collect);
    } else {
	int tty = isatty(0);

	if (tty)
	    (void)fputs("This is the gpsd exerciser.\n", stdout);
	for (;;) {
	    if (tty)
		(void)fputs("> ", stdout);
	    if (fgets(buf, sizeof(buf), stdin) == NULL) {
		if (tty)
		    putchar('\n');
		break;
	    }
	    collect.set = 0;
	    (void)gps_send(&collect, buf);
	    (void)gps_read(&collect);
	    libgps_dump_state(&collect);
	}
	(void)gps_close(&collect);
    }

    return 0;
}
示例#7
0
/**
 * Thread reading from gpsd.
 */
static void * cgps_thread (void * pData)
{
  struct gps_data_t gpsd_conn;
  unsigned int err_count;
  cgps_thread_running = CGPS_TRUE;

  while (CGPS_TRUE)
  {
    pthread_mutex_lock (&cgps_thread_lock);
    if (cgps_thread_shutdown == CGPS_TRUE)
    {
      goto quit;
    }
    pthread_mutex_unlock (&cgps_thread_lock);

    err_count = 0;

#if GPSD_API_MAJOR_VERSION > 4
    int status = gps_open (cgps_config_data.host, cgps_config_data.port, &gpsd_conn);
#else
    int status = gps_open_r (cgps_config_data.host, cgps_config_data.port, &gpsd_conn);
#endif
    if (status < 0)
    {
      WARNING ("gps plugin: connecting to %s:%s failed: %s",
               cgps_config_data.host, cgps_config_data.port, gps_errstr (status));

      // Here we make a pause until a new tentative to connect, we check also if
      // the thread does not need to stop.
      if (cgps_thread_pause(cgps_config_data.pause_connect) == CGPS_FALSE)
      {
        goto quit;
      }

      continue;
    }

    gps_stream (&gpsd_conn, WATCH_ENABLE | WATCH_JSON | WATCH_NEWSTYLE, NULL);
    gps_send (&gpsd_conn, CGPS_CONFIG);

    while (CGPS_TRUE)
    {
      pthread_mutex_lock (&cgps_thread_lock);
      if (cgps_thread_shutdown == CGPS_TRUE)
      {
        goto stop;
      }
      pthread_mutex_unlock (&cgps_thread_lock);

#if GPSD_API_MAJOR_VERSION > 4
      long timeout_us = CDTIME_T_TO_US (cgps_config_data.timeout);
      if (!gps_waiting (&gpsd_conn, (int) timeout_us ))
#else
      if (!gps_waiting (&gpsd_conn))
#endif
      {
        continue;
      }

      if (gps_read (&gpsd_conn) == -1)
      {
        WARNING ("gps plugin: incorrect data! (err_count: %d)", err_count);
        err_count++;

        if (err_count > CGPS_MAX_ERROR)
        {
          // Server is not responding ...
          if (gps_send (&gpsd_conn, CGPS_CONFIG) == -1)
          {
            WARNING ("gps plugin: gpsd seems to be down, reconnecting");
            gps_close (&gpsd_conn);
            break;
          }
          // Server is responding ...
          else
          {
            err_count = 0;
          }
        }

        continue;
      }

      pthread_mutex_lock (&cgps_data_lock);

      // Number of sats in view:
      cgps_data.sats_used = (gauge_t) gpsd_conn.satellites_used;
      cgps_data.sats_visible = (gauge_t) gpsd_conn.satellites_visible;

      // dilution of precision:
      cgps_data.vdop = NAN;
      cgps_data.hdop = NAN;
      if (cgps_data.sats_used > 0)
      {
        cgps_data.hdop = gpsd_conn.dop.hdop;
        cgps_data.vdop = gpsd_conn.dop.vdop;
      }

      DEBUG ("gps plugin: %.0f sats used (of %.0f visible), hdop = %.3f, vdop = %.3f",
             cgps_data.sats_used, cgps_data.sats_visible, cgps_data.hdop, cgps_data.vdop);

      pthread_mutex_unlock (&cgps_data_lock);
    }
  }

stop:
  DEBUG ("gps plugin: thread closing gpsd connection ... ");
  gps_stream (&gpsd_conn, WATCH_DISABLE, NULL);
  gps_close (&gpsd_conn);
quit:
  DEBUG ("gps plugin: thread shutting down ... ");
  cgps_thread_running = CGPS_FALSE;
  pthread_mutex_unlock (&cgps_thread_lock);
  pthread_exit (NULL);
}
示例#8
0
文件: libgps_sock.c 项目: biiont/gpsd
int gps_stream(struct gps_data_t *gpsdata, unsigned int flags,
	       /*@null@*/ void *d)
/* ask gpsd to stream reports at you, hiding the command details */
{
    char buf[GPS_JSON_COMMAND_MAX];

    if ((flags & (WATCH_JSON | WATCH_OLDSTYLE | WATCH_NMEA | WATCH_RAW)) == 0) {
	flags |= WATCH_JSON;
    }
    if ((flags & WATCH_DISABLE) != 0) {
	if ((flags & WATCH_OLDSTYLE) != 0) {
	    (void)strlcpy(buf, "w-", sizeof(buf));
	    if ((flags & WATCH_NMEA) != 0)
		(void)strlcat(buf, "r-", sizeof(buf));
	} else {
	    (void)strlcpy(buf, "?WATCH={\"enable\":false,", sizeof(buf));
	    if (flags & WATCH_JSON)
		(void)strlcat(buf, "\"json\":false,", sizeof(buf));
	    if (flags & WATCH_NMEA)
		(void)strlcat(buf, "\"nmea\":false,", sizeof(buf));
	    if (flags & WATCH_RAW)
		(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
	    if (flags & WATCH_RARE)
		(void)strlcat(buf, "\"raw\":0,", sizeof(buf));
	    if (flags & WATCH_SCALED)
		(void)strlcat(buf, "\"scaled\":false,", sizeof(buf));
	    if (flags & WATCH_TIMING)
		(void)strlcat(buf, "\"timing\":false,", sizeof(buf));
	    if (buf[strlen(buf) - 1] == ',')
		buf[strlen(buf) - 1] = '\0';
	    (void)strlcat(buf, "};", sizeof(buf));
	}
	libgps_debug_trace((DEBUG_CALLS, "gps_stream() disable command: %s\n", buf));
	return gps_send(gpsdata, buf);
    } else {			/* if ((flags & WATCH_ENABLE) != 0) */

	if ((flags & WATCH_OLDSTYLE) != 0) {
	    (void)strlcpy(buf, "w+x", sizeof(buf));
	    if ((flags & WATCH_NMEA) != 0)
		(void)strlcat(buf, "r+", sizeof(buf));
	} else {
	    (void)strlcpy(buf, "?WATCH={\"enable\":true,", sizeof(buf));
	    if (flags & WATCH_JSON)
		(void)strlcat(buf, "\"json\":true,", sizeof(buf));
	    if (flags & WATCH_NMEA)
		(void)strlcat(buf, "\"nmea\":true,", sizeof(buf));
	    if (flags & WATCH_RARE)
		(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
	    if (flags & WATCH_RAW)
		(void)strlcat(buf, "\"raw\":2,", sizeof(buf));
	    if (flags & WATCH_SCALED)
		(void)strlcat(buf, "\"scaled\":true,", sizeof(buf));
	    if (flags & WATCH_TIMING)
		(void)strlcat(buf, "\"timing\":true,", sizeof(buf));
	    /*@-nullpass@*//* shouldn't be needed, splint has a bug */
	    if (flags & WATCH_DEVICE)
		(void)snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf),
			       "\"device\":\"%s\",", (char *)d);
	    /*@+nullpass@*/
	    if (buf[strlen(buf) - 1] == ',')
		buf[strlen(buf) - 1] = '\0';
	    (void)strlcat(buf, "};", sizeof(buf));
	}
	libgps_debug_trace((DEBUG_CALLS, "gps_stream() enable command: %s\n", buf));
	return gps_send(gpsdata, buf);
    }
}
/*
  possibly send a new GPS UBLOX packet
 */
void sitl_update_gps(double latitude, double longitude, float altitude,
		     double speedN, double speedE, bool have_lock)
{
	struct ubx_nav_posllh {
		uint32_t	time; // GPS msToW
		int32_t		longitude;
		int32_t		latitude;
		int32_t		altitude_ellipsoid;
		int32_t		altitude_msl;
		uint32_t	horizontal_accuracy;
		uint32_t	vertical_accuracy;
	} pos;
	struct ubx_nav_status {
		uint32_t	time;				// GPS msToW
		uint8_t		fix_type;
		uint8_t		fix_status;
		uint8_t		differential_status;
		uint8_t		res;
		uint32_t	time_to_first_fix;
		uint32_t	uptime;				// milliseconds
	} status;
	struct ubx_nav_velned {
		uint32_t	time;				// GPS msToW
		int32_t		ned_north;
		int32_t		ned_east;
		int32_t		ned_down;
		uint32_t	speed_3d;
		uint32_t	speed_2d;
		int32_t		heading_2d;
		uint32_t	speed_accuracy;
		uint32_t	heading_accuracy;
	} velned;
        const uint8_t MSG_POSLLH = 0x2;
	const uint8_t MSG_STATUS = 0x3;
	const uint8_t MSG_VELNED = 0x12;
	double lon_scale;

	// 4Hz
	if (millis() - gps_state.last_update < 250) {
		return;
	}
	gps_state.last_update = millis();

	pos.time = millis(); // FIX
	pos.longitude = longitude * 1.0e7;
	pos.latitude  = latitude * 1.0e7;
	pos.altitude_ellipsoid = altitude*1000.0;
	pos.altitude_msl = altitude*1000.0;
	pos.horizontal_accuracy = 5;
	pos.vertical_accuracy = 10;

	status.time = pos.time;
	status.fix_type = have_lock?3:0;
	status.fix_status = have_lock?1:0;
	status.differential_status = 0;
	status.res = 0;
	status.time_to_first_fix = 0;
	status.uptime = millis();

	velned.time = pos.time;
	velned.ned_north = 100.0 * speedN;
	velned.ned_east  = 100.0 * speedE;
	velned.ned_down  = 0;
#define sqr(x) ((x)*(x))
	velned.speed_2d = sqrt(sqr(speedN)+sqr(speedE)) * 100;
	velned.speed_3d = velned.speed_2d;
        lon_scale = cos(ToRad(latitude));
	velned.heading_2d = ToDeg(atan2(lon_scale*speedE, speedN)) * 100000.0;
	velned.speed_accuracy = 2;
	velned.heading_accuracy = 4;

	if (gps_state.gps_fd == 0) {
		return;
	}

	gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
	gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status));
	gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
}