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);
}
}
