static gboolean gps_periodic(gpointer data __attribute__ ((unused))) { int ret = gps_waiting(gpsdata); if (ret == -1) { perror("socket error\n"); exit(2); } else if (ret) gps_poll(gpsdata); return 1; }
static void handle_input(XtPointer client_data, int *source, XtInputId *id) { if (gps_poll(gpsdata) < 0) { XtRemoveInput(gps_input); (void)gps_close(gpsdata); XtRemoveTimeOut(timeout); XmTextFieldSetString(text_10, "No GPS data available"); (void)err_dialog(toplevel, "No GPS data available.\n\n" "Check the connection to gpsd and if gpsd is running"); gps_lost = true; gps_timeout = XtAppAddTimeOut(app, 3000, handle_gps, app); } }
static gboolean gpsd_io(GIOChannel * iochan, GIOCondition condition, gpointer t) { struct gpsd_priv *priv = t; // debug(10, "enter condition=%d\n", condition); if (condition == G_IO_IN) { if (priv->gps) { polled_priv = priv; if (gps_poll(priv->gps) !=0) { goto err; } } return TRUE; } err: gpsd_close(priv); gpsd_start_reconnect(priv); return FALSE; }
/** \brief Update the qth data by whatever method is appropriate. * \param qth the qth data structure to update * \param qth the time at which the qth is to be computed. this may be ignored by gps updates. */ gboolean qth_data_update(qth_t * qth, gdouble t) { gboolean retval = FALSE; #ifdef HAS_LIBGPS guint num_loops = 0; #endif switch (qth->type) { case QTH_STATIC_TYPE: /* never changes */ break; case QTH_GPSD_TYPE: if (((t - qth->gpsd_update) > 30.0 / 86400.0) && (t - qth->gpsd_connected > 30.0 / 86400.0)) { /* if needed restart the gpsd interface */ qth_data_update_stop(qth); qth_data_update_init(qth); qth->gpsd_connected = t; } if (qth->gps_data != NULL) { #ifdef HAS_LIBGPS switch (GPSD_API_MAJOR_VERSION) { case 4: #if GPSD_API_MAJOR_VERSION==4 while (gps_waiting(qth->gps_data) == TRUE) { /* this is a watchdog in case there is a problem with the gpsd code. */ /* if the server was up and has failed then gps_waiting in 2.92 confirmed */ /* will return 1 (supposedly fixed in later versions.) */ /* if we do not do this as a while loop, the gpsd packets can backup */ /* and no longer be in sync with the gps receiver */ num_loops++; if (num_loops > 1000) { retval = FALSE; break; } if (gps_poll(qth->gps_data) == 0) { /* handling packet_set inline with http://gpsd.berlios.de/client-howto.html */ if (qth->gps_data->set & PACKET_SET) { if (qth->gps_data->fix.mode >= MODE_2D) { if (qth->lat != qth->gps_data->fix.latitude) { qth->lat = qth->gps_data->fix.latitude; retval = TRUE; } if (qth->lon != qth->gps_data->fix.longitude) { qth->lon = qth->gps_data->fix.longitude; retval = TRUE; } } if (qth->gps_data->fix.mode == MODE_3D) { if (qth->alt != qth->gps_data->fix.altitude) { qth->alt = qth->gps_data->fix.altitude; retval = TRUE; } } else { if (qth->alt != 0) { qth->alt = 0; retval = TRUE; } } } } } #endif break; case 5: #if GPSD_API_MAJOR_VERSION==5 while (gps_waiting(qth->gps_data, 0) == 1) { /* see comment from above */ /* hopefully not needed but does not hurt anything. */ num_loops++; if (num_loops > 1000) { retval = FALSE; break; } if (gps_read(qth->gps_data) == 0) { /* handling packet_set inline with http://gpsd.berlios.de/client-howto.html */ if (qth->gps_data->set & PACKET_SET) { if (qth->gps_data->fix.mode >= MODE_2D) { if (qth->lat != qth->gps_data->fix.latitude) { qth->lat = qth->gps_data->fix.latitude; retval = TRUE; } if (qth->lon != qth->gps_data->fix.longitude) { qth->lon = qth->gps_data->fix.longitude; retval = TRUE; } } if (qth->gps_data->fix.mode == MODE_3D) { if (qth->alt != qth->gps_data->fix.altitude) { qth->alt = qth->gps_data->fix.altitude; retval = TRUE; } } else { if (qth->alt != 0) { qth->alt = 0; retval = TRUE; } } } } } #endif break; default: break; } #endif if (retval == TRUE) { qth->gpsd_update = t; } } break; default: break; } /* check that data is valid */ qth_validate(qth); /* update qra */ if (longlat2locator(qth->lon, qth->lat, qth->qra, 2) != RIG_OK) { sat_log_log(SAT_LOG_LEVEL_ERROR, _("%s: Could not set QRA for %s at %f, %f."), __FUNCTION__, qth->name, qth->lon, qth->lat); } return retval; }
void CGPSDThread::run() { #if GPSD_API_MAJOR_VERSION >= 5 gpsdata = new gps_data_t(); if(gpsdata) { gps_open( "localhost", DEFAULT_GPSD_PORT, gpsdata ); } #else gpsdata = gps_open( "localhost", DEFAULT_GPSD_PORT ); #endif if( !gpsdata ) { // qDebug() << "gps_open failed."; return; } // if // qDebug() << "connected to gpsd."; gps_stream( gpsdata, WATCH_NEWSTYLE, NULL ); fd_set fds; while( true ) { // sleep until either GPSD or our controlling thread has data for us. FD_ZERO(&fds); FD_SET(gpsdata->gps_fd, &fds); FD_SET(pipe_fd, &fds); int nfds = (pipe_fd > gpsdata->gps_fd ? pipe_fd : gpsdata->gps_fd) + 1; int data = select(nfds, &fds, NULL, NULL, NULL); if( data == -1 ) { break; } // if else if( data ) { if( FD_ISSET( pipe_fd, &fds ) ) { // qDebug() << "stop command received"; char s; read( pipe_fd, &s, 1 ); break; } // if else if( FD_ISSET( gpsdata->gps_fd, &fds ) ) { #if GPSD_API_MAJOR_VERSION >= 5 gps_read( gpsdata ); #else gps_poll( gpsdata ); #endif if( !decodeData() ) break; } // else if } // else if } // while gps_close( gpsdata ); #if GPSD_API_MAJOR_VERSION >= 5 delete gpsdata; #endif // qDebug() << "thread done"; }
float estimator_base::spin() { /* wait for sensor update of 2 file descriptor for 10 ms */ int poll_ret = poll(fds, 1, 50); if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (poll_error_counter < 10 || poll_error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ printf("[controller] ERROR return value from poll(): %d\n", poll_ret); } poll_error_counter++; return 0; } else { parameter_update_poll(); sensor_combined_poll(); gps_poll(); struct input_s input; input.gyro_x = _sensor_combined.gyro_rad_s[0]; input.gyro_y = _sensor_combined.gyro_rad_s[1]; input.gyro_z = _sensor_combined.gyro_rad_s[2]; input.accel_x = _sensor_combined.accelerometer_m_s2[0]; input.accel_y = _sensor_combined.accelerometer_m_s2[1]; input.accel_z = _sensor_combined.accelerometer_m_s2[2]; input.static_pres = (_init_static - _sensor_combined.baro_pres_mbar)*100; // 1 mbar == 100 pa input.diff_pres = (_sensor_combined.differential_pressure_pa < 0 ? 0 : _sensor_combined.differential_pressure_pa); if(_gps_init && _gps_new) { input.gps_new = true; float conversion = (3.14159/(180*1e7)); //from 1/10th of a micro degree to radian input.gps_n = EARTH_RADIUS * (float)(_gps.lat - _init_lat) * conversion; input.gps_e = EARTH_RADIUS * cosf((float)_init_lat * conversion) * (float)(_gps.lon - _init_lon) * conversion; input.gps_h = (_gps.alt - _init_alt) / 1e3f; input.gps_Vg = _gps.vel_m_s; input.gps_course = _gps.cog_rad; } else { input.gps_new = false; } if(_time_to_run == 1) { if(true)//_gps_init) // don't estimate unless you have gps { hrt_abstime curr_time = hrt_absolute_time(); input.Ts = (prev_time_ != 0) ? (curr_time - prev_time_) * 0.000001f : 0.0f; prev_time_ = curr_time; struct output_s output; estimate(_params, input, output); vehicle_state_publish(output); } _time_to_run = -1; } _time_to_run++; return input.gps_n; } }
int main(int argc, char **argv){ int ch; fd_set fds; int casoc = 0; progname = argv[0]; while ((ch = getopt(argc, argv, "hVi:j:s:p:")) != -1){ switch (ch) { case 'i': sl = (unsigned int)atoi(optarg); if (sl < 1) sl = 1; if (sl >= 3600) fprintf(stderr, "WARNING: polling interval is an hour or more!\n"); break; case 'j': casoc = (unsigned int)atoi(optarg); casoc = casoc ? 1 : 0; break; case 's': host = optarg; break; case 'p': port = optarg; break; case 'V': (void)fprintf(stderr, "SVN ID: $Id: cgpxlogger.c 4406 2007-07-27 16:18:18Z ckuethe $ \n"); exit(0); default: usage(); /* NOTREACHED */ } } gpsdata = gps_open(host, port); if (!gpsdata) { char *err_str; switch (errno) { case NL_NOSERVICE: err_str = "can't get service entry"; break; case NL_NOHOST: err_str = "can't get host entry"; break; case NL_NOPROTO: err_str = "can't get protocol entry"; break; case NL_NOSOCK: err_str = "can't create socket"; break; case NL_NOSOCKOPT: err_str = "error SETSOCKOPT SO_REUSEADDR"; break; case NL_NOCONNECT: err_str = "can't connect to host"; break; default: err_str = "Unknown"; break; } fprintf(stderr, "cgpxlogger: no gpsd running or network error: %d, %s\n", errno, err_str); exit(1); } signal(SIGINT, bye); signal(SIGTERM, bye); signal(SIGQUIT, bye); signal(SIGHUP, bye); header(); if (casoc) gps_query(gpsdata, "j1\n"); gps_set_raw_hook(gpsdata, process); for(;;){ int data; struct timeval tv; FD_ZERO(&fds); FD_SET(gpsdata->gps_fd, &fds); gps_query(gpsdata, pollstr); tv.tv_usec = 250000; tv.tv_sec = 0; data = select(gpsdata->gps_fd + 1, &fds, NULL, NULL, &tv); if (data < 0) { fprintf(stderr,"%s\n", strerror(errno)); footer(); exit(2); } else if (data) gps_poll(gpsdata); sleep(sl); } }