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; } } }
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); } }
struct gps_data_t* gpsmm::send(const char *request) { if (gps_send(gps_state(),request)==-1) { return NULL; } else { return backup(); } }
/* * 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); }
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; }
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; }
/** * 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); }
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)); }