void main(void)
{
real4 glat, glong, gspeed;
real2 gangle;
char b1[110], gvalid;
setbaud(BAUD9600);
PORTA = 0;
myprint("\nComplete LATLONG GPS data (binary) v1.0\n\n");
gps_sync();
while(1) {
gps_read(b1);
gvalid = b1[18];
glat.c[3] = b1[52];
glat.c[2] = b1[53];
glat.c[1] = b1[54];
glat.c[0] = b1[55];
glong.c[3] = b1[56];
glong.c[2] = b1[57];
glong.c[1] = b1[58];
glong.c[0] = b1[59];
gspeed.c[3] = b1[66];
gspeed.c[2] = b1[67];
gspeed.c[1] = b1[68];
gspeed.c[0] = b1[69];
gangle.c[1] = b1[70];
gangle.c[0] = b1[71];
PORTA = 0;
print_latlong(&glat, &glong, &gspeed, &gangle, gvalid);
PIOC = 0;
PORTB=glat.c[0];
}
}
/*@-mustfreefresh -compdestroy@*/
static int shm_mainloop(void)
{
int status;
if ((status = gps_open(GPSD_SHARED_MEMORY, NULL, &gpsdata)) != 0) {
(void)fprintf(stderr,
"%s: shm open failed with status %d.\n",
progname, status);
return 1;
}
print_gpx_header();
for (;;) {
status = gps_read(&gpsdata);
if (status == -1)
break;
if (status > 0)
conditionally_log_fix(&gpsdata);
}
(void)gps_close(&gpsdata);
return 0;
}
//Will read data from stream. Passed a stream pointer.
void read_data(void *ptr){
gps_data_t *gpsdata = (gps_data_t)ptr;
while(){
if(!gps_waiting(gps_ptr, 500000)){
Cleanup(GPS_TIMEOUT);
}
else{
errno = 0;
if(gps_read(gpsdata) == -1){
//print failure
Cleanup(errno == 0 ? GPS_GONE : GPS_ERROR);
}
else{
if(check_errors(gps_data)){
print_data(gps_data);
}
}
}
}
}
bool mainLoop(struct gps_data_t *gps_data, int sockfd)
{
int rc;
while (1) {
/* wait for 2 seconds to receive data */
if (gps_waiting (gps_data, 2000000)) {
/* read data */
if ((rc = gps_read(gps_data)) == -1) {
return false;
} else {
/* Display data from the GPS receiver. */
if ((gps_data->status == STATUS_FIX) &&
(gps_data->fix.mode == MODE_2D || gps_data->fix.mode == MODE_3D) &&
!isnan(gps_data->fix.latitude) &&
!isnan(gps_data->fix.longitude)) {
if (isPrevValid) {
distance += earth_distance(prev.latitude,
prev.longitude,
gps_data->fix.latitude,
gps_data->fix.longitude);
}
logDataOut(&gps_data->fix);
prev = gps_data->fix;
isPrevValid = 1;
} else {
printf("no GPS data available\n");
}
}
}
}
return true;
}
/*------------------------------------
-- Method: Thread::exec
-- Date: November 12 2013
-- Designer: Sam Youssef
-- Programmer: Sam Youssef
--
-- INTERFACE:
-- int Thread::exec(void)
-- RETURNS:
-- int. On termination.
--
-- NOTES:
-- Enters loop which polls gps device. Reads gps data into gpsd structure.
-- On success, emits a signal which is captured by GpsWindow. In turn, GpsWindow
-- displays the data.
--
-- Is aborted when Thread class receives the appropriate disconnect signal
-- from GpsWindow, either on program close or via the user clicking
-- disconnect.
--------------------------------------*/
int Thread::exec()
{
bool ab = abort = false;
while( !ab )
{
mutex.lock();
ab = abort;
mutex.unlock();
if( ! gps_waiting(gpsd, 2000000) ) {
fprintf(stderr, "\tmygps: GPS timeout\n");
break;
}
if(gps_read(gpsd) == -1) {
fprintf(stderr, "\targs:socket error 4\n");
break;
}
emit(read_true());
}
return 1;
}
static int gps_query(struct gps_data_t *gpsdata, const char *fmt, ... )
/* query a gpsd instance for new data */
{
char buf[BUFSIZ];
va_list ap;
int ret;
va_start(ap, fmt);
(void)vsnprintf(buf, sizeof(buf)-2, fmt, ap);
va_end(ap);
if (buf[strlen(buf)-1] != '\n')
(void)strlcat(buf, "\n", BUFSIZ);
if (write(gpsdata->gps_fd, buf, strlen(buf)) <= 0) {
gpsd_report(LOG_ERROR, "gps_query(), write failed\n");
return -1;
}
gpsd_report(LOG_PROG, "gps_query(), wrote, %s\n", buf);
ret = gps_read(gpsdata);
if (ERROR_IS & gpsdata->set) {
gpsd_report(LOG_ERROR, "gps_query() error '%s'\n", gpsdata->error);
}
return ret;
}
static bool gps_query(struct gps_data_t *gpsdata,
gps_mask_t expect,
const int timeout,
const char *fmt, ... )
/* ship a command and wait on an expected response type */
{
static fd_set rfds;
char buf[BUFSIZ];
va_list ap;
time_t starttime;
struct timespec tv;
sigset_t oldset, blockset;
(void)sigemptyset(&blockset);
(void)sigaddset(&blockset, SIGHUP);
(void)sigaddset(&blockset, SIGINT);
(void)sigaddset(&blockset, SIGTERM);
(void)sigaddset(&blockset, SIGQUIT);
(void)sigprocmask(SIG_BLOCK, &blockset, &oldset);
va_start(ap, fmt);
(void)vsnprintf(buf, sizeof(buf)-2, fmt, ap);
va_end(ap);
if (buf[strlen(buf)-1] != '\n')
(void)strlcat(buf, "\n", sizeof(buf));
if (write(gpsdata->gps_fd, buf, strlen(buf)) <= 0) {
gpsd_log(&context.errout, LOG_ERROR, "gps_query(), write failed\n");
return false;
}
gpsd_log(&context.errout, LOG_PROG, "gps_query(), wrote, %s\n", buf);
FD_ZERO(&rfds);
starttime = time(NULL);
for (;;) {
FD_CLR(gpsdata->gps_fd, &rfds);
gpsd_log(&context.errout, LOG_PROG, "waiting...\n");
tv.tv_sec = 2;
tv.tv_nsec = 0;
if (pselect(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &tv, &oldset) == -1) {
if (errno == EINTR || !FD_ISSET(gpsdata->gps_fd, &rfds))
continue;
gpsd_log(&context.errout, LOG_ERROR, "select %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
gpsd_log(&context.errout, LOG_PROG, "reading...\n");
(void)gps_read(gpsdata);
if (ERROR_SET & gpsdata->set) {
gpsd_log(&context.errout, LOG_ERROR, "error '%s'\n", gpsdata->error);
return false;
}
if ((expect == NON_ERROR) || (expect & gpsdata->set) != 0)
return true;
else if (timeout > 0 && (time(NULL) - starttime > timeout)) {
gpsd_log(&context.errout, LOG_ERROR,
"timed out after %d seconds\n",
timeout);
return false;
}
}
return false;
}
void one_gps_cycle(struct gps_data_t *gps_data) {
int err;
if ((err = gps_waiting(gps_data, ONE_SECOND)) == 0) {
if (errno == 0) {
puts("timeout");
return;
} else {
fprintf(stderr, gps_errstr(err));
exit(1);
}
}
if ((err = gps_read(gps_data)) < 0) {
perror("gps_read");
exit(1);
}
print_timestamp(gps_data->online);
switch (gps_data->status) {
case STATUS_NO_FIX:
puts("No fix");
break;
case STATUS_FIX:
puts("Have fix without DGPS");
print_gps_fix(&gps_data->fix);
break;
case STATUS_DGPS_FIX:
puts("Have fix with DGPS");
print_gps_fix(&gps_data->fix);
break;
default:
printf("status = %d\n", gps_data->status);
}
if (gps_data->set & ERROR_SET)
printf("ERROR_SET error: %s\n", gps_data->error);
if (gps_data->set & LOGMESSAGE_SET)
puts("LOGMESSAGE_SET");
if (gps_data->set & POLICY_SET)
puts("POLICY_SET");
if (gps_data->set & VERSION_SET)
printf("VERSION_SET \"%s\" \"%s\" %d.%d \"%s\"\n",
gps_data->version.release,
gps_data->version.rev,
gps_data->version.proto_major,
gps_data->version.proto_minor,
gps_data->version.remote);
if (gps_data->set & GST_SET) {
puts("GST_SET (pseudorange errors)");
printf(" utctime %g\n", gps_data->gst.utctime);
printf(" rms_deviation %g\n", gps_data->gst.rms_deviation);
printf(" smajor_deviation %g\n", gps_data->gst.smajor_deviation);
printf(" sminor_deviation %g\n", gps_data->gst.sminor_deviation);
printf(" smajor_orientation %g\n", gps_data->gst.smajor_orientation);
printf(" lat_err_deviation %g\n", gps_data->gst.lat_err_deviation);
printf(" lon_err_deviation %g\n", gps_data->gst.lon_err_deviation);
printf(" alt_err_deviation %g\n", gps_data->gst.alt_err_deviation);
}
if (gps_data->set & SUBFRAME_SET)
puts("SUBFRAME_SET");
if (gps_data->set & PACKET_SET)
puts("PACKET_SET");
if (gps_data->set & AIS_SET)
puts("AIS_SET");
if (gps_data->set & RTCM3_SET)
puts("RTCM3_SET");
if (gps_data->set & RTCM2_SET)
puts("RTCM2_SET");
if (gps_data->set & DEVICEID_SET)
puts("DEVICEID_SET");
if (gps_data->set & DEVICELIST_SET) {
printf("DEVICELIST_SET\n ");
print_timestamp(gps_data->devices.time);
for (int i = 0; i < gps_data->devices.ndevices; i++)
print_devconfig(&gps_data->devices.list[i]);
}
if (gps_data->set & DEVICE_SET)
puts("DEVICE_SET");
if (gps_data->set & CLIMBERR_SET)
puts("CLIMBERR_SET");
if (gps_data->set & TRACKERR_SET)
puts("TRACKERR_SET");
if (gps_data->set & SPEEDERR_SET)
puts("SPEEDERR_SET");
if (gps_data->set & SATELLITE_SET)
puts("SATELLITE_SET");
if (gps_data->set & ATTITUDE_SET) {
puts("ATTITUDE_SET");
print_attitude(&gps_data->attitude);
}
if (gps_data->set & VERR_SET)
puts("VERR_SET");
if (gps_data->set & HERR_SET)
puts("HERR_SET");
if (gps_data->set & DOP_SET)
puts("DOP_SET");
if (gps_data->set & MODE_SET)
puts("MODE_SET");
if (gps_data->set & STATUS_SET)
puts("STATUS_SET");
if (gps_data->set & CLIMB_SET)
puts("CLIMB_SET");
if (gps_data->set & TRACK_SET)
puts("TRACK_SET");
if (gps_data->set & SPEED_SET)
puts("SPEED_SET");
if (gps_data->set & ALTITUDE_SET)
puts("ALTITUDE_SET");
if (gps_data->set & LATLON_SET)
puts("LATLON_SET");
if (gps_data->set & TIMERR_SET)
puts("TIMERR_SET");
if (gps_data->set & TIME_SET)
puts("TIME_SET");
if (gps_data->set & ONLINE_SET) {
printf("ONLINE_SET ");
print_timestamp(gps_data->online);
}
//printf("0x%08x\n", (unsigned)gps_data->set);
putchar('\n');
}
int main(int argc, char *argv[])
{
int option, rc;
struct sockaddr_in localAddr, servAddr;
struct hostent *h;
int n;
for(n=0;n<CLIMB;n++) climb[n]=0.0;
switch (gpsd_units())
{
case imperial:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
break;
case nautical:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
break;
case metric:
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
break;
default:
/* leave the default alone */
break;
}
/* Process the options. Print help if requested. */
while ((option = getopt(argc, argv, "Vhl:su:")) != -1) {
switch (option) {
case 'V':
(void)fprintf(stderr, "lcdgs revision " REVISION "\n");
exit(EXIT_SUCCESS);
case 'h':
default:
usage(argv[0]);
break;
case 'l':
switch ( optarg[0] ) {
case 'd':
deg_type = deg_dd;
continue;
case 'm':
deg_type = deg_ddmm;
continue;
case 's':
deg_type = deg_ddmmss;
continue;
default:
(void)fprintf(stderr, "Unknown -l argument: %s\n", optarg);
}
break;
case 's':
sleep(10);
continue;
case 'u':
switch ( optarg[0] ) {
case 'i':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
continue;
case 'n':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
continue;
case 'm':
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
continue;
default:
(void)fprintf(stderr, "Unknown -u argument: %s\n", optarg);
}
}
}
/* Grok the server, port, and device. */
if (optind < argc) {
gpsd_source_spec(argv[optind], &source);
} else
gpsd_source_spec(NULL, &source);
/* Daemonize... */
if (daemon(0, 0) != 0)
(void)fprintf(stderr,
"lcdgps: demonization failed: %s\n",
strerror(errno));
/* Open the stream to gpsd. */
if (gps_open(source.server, source.port, &gpsdata) != 0) {
(void)fprintf( stderr,
"lcdgps: no gpsd running or network error: %d, %s\n",
errno, gps_errstr(errno));
exit(EXIT_FAILURE);
}
/* Connect to LCDd */
h = gethostbyname(LCDDHOST);
if (h==NULL) {
printf("%s: unknown host '%s'\n",argv[0],LCDDHOST);
exit(EXIT_FAILURE);
}
servAddr.sin_family = h->h_addrtype;
memcpy((char *) &servAddr.sin_addr.s_addr, h->h_addr_list[0], h->h_length);
servAddr.sin_port = htons(LCDDPORT);
/* create socket */
sd = socket(AF_INET, SOCK_STREAM, 0);
if (BAD_SOCKET(sd)) {
perror("cannot open socket ");
exit(EXIT_FAILURE);
}
/* bind any port number */
localAddr.sin_family = AF_INET;
localAddr.sin_addr.s_addr = htonl(INADDR_ANY);
localAddr.sin_port = htons(0);
/* coverity[uninit_use_in_call] */
rc = bind(sd, (struct sockaddr *) &localAddr, sizeof(localAddr));
if (rc == -1) {
printf("%s: cannot bind port TCP %u\n",argv[0],LCDDPORT);
perror("error ");
exit(EXIT_FAILURE);
}
/* connect to server */
/* coverity[uninit_use_in_call] */
rc = connect(sd, (struct sockaddr *) &servAddr, sizeof(servAddr));
if (rc == -1) {
perror("cannot connect ");
exit(EXIT_FAILURE);
}
/* Do the initial field label setup. */
reset_lcd();
/* Here's where updates go. */
unsigned int flags = WATCH_ENABLE;
if (source.device != NULL)
flags |= WATCH_DEVICE;
(void)gps_stream(&gpsdata, flags, source.device);
for (;;) { /* heart of the client */
if (!gps_waiting(&gpsdata, 50000000)) {
fprintf( stderr, "lcdgps: error while waiting\n");
exit(EXIT_FAILURE);
} else {
(void)gps_read(&gpsdata);
update_lcd(&gpsdata);
}
}
}
int main(int argc, char *argv[])
{
int option;
unsigned int flags = WATCH_ENABLE;
/*@ -observertrans @*/
switch (gpsd_units()) {
case imperial:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
break;
case nautical:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
break;
case metric:
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
break;
default:
/* leave the default alone */
break;
}
/*@ +observertrans @*/
/* Process the options. Print help if requested. */
while ((option = getopt(argc, argv, "hVl:smu:D:")) != -1) {
switch (option) {
#ifdef CLIENTDEBUG_ENABLE
case 'D':
debug = atoi(optarg);
gps_enable_debug(debug, stderr);
break;
#endif /* CLIENTDEBUG_ENABLE */
case 'm':
magnetic_flag = true;
break;
case 's':
silent_flag = true;
break;
case 'u':
/*@ -observertrans @*/
switch (optarg[0]) {
case 'i':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
continue;
case 'n':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
continue;
case 'm':
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
continue;
default:
(void)fprintf(stderr, "Unknown -u argument: %s\n", optarg);
}
break;
/*@ +observertrans @*/
case 'V':
(void)fprintf(stderr, "cgps: %s (revision %s)\n",
VERSION, REVISION);
exit(EXIT_SUCCESS);
case 'l':
switch (optarg[0]) {
case 'd':
deg_type = deg_dd;
continue;
case 'm':
deg_type = deg_ddmm;
continue;
case 's':
deg_type = deg_ddmmss;
continue;
default:
(void)fprintf(stderr, "Unknown -l argument: %s\n", optarg);
/*@ -casebreak @*/
}
break;
case 'h':
default:
usage(argv[0]);
break;
}
}
/* Grok the server, port, and device. */
if (optind < argc) {
gpsd_source_spec(argv[optind], &source);
} else
gpsd_source_spec(NULL, &source);
/* Open the stream to gpsd. */
if (gps_open(source.server, source.port, &gpsdata) != 0) {
(void)fprintf(stderr,
"cgps: no gpsd running or network error: %d, %s\n",
errno, gps_errstr(errno));
exit(EXIT_FAILURE);
}
/* note: we're assuming BSD-style reliable signals here */
(void)signal(SIGINT, die);
(void)signal(SIGHUP, die);
(void)signal(SIGWINCH, resize);
windowsetup();
status_timer = time(NULL);
if (source.device != NULL)
flags |= WATCH_DEVICE;
(void)gps_stream(&gpsdata, flags, source.device);
/* heart of the client */
for (;;) {
int c;
if (!gps_waiting(&gpsdata, 5000000)) {
die(GPS_TIMEOUT);
} else {
errno = 0;
if (gps_read(&gpsdata) == -1) {
fprintf(stderr, "cgps: socket error 4\n");
die(errno == 0 ? GPS_GONE : GPS_ERROR);
} else {
/* Here's where updates go now that things are established. */
#ifdef TRUENORTH
if (compass_flag)
update_compass_panel(&gpsdata);
else
#endif /* TRUENORTH */
update_gps_panel(&gpsdata);
}
}
/* Check for user input. */
c = wgetch(datawin);
switch (c) {
/* Quit */
case 'q':
die(CGPS_QUIT);
break;
/* Toggle spewage of raw gpsd data. */
case 's':
silent_flag = !silent_flag;
break;
/* Clear the spewage area. */
case 'c':
(void)werase(messages);
break;
default:
break;
}
}
}
static bool gps_query(/*@out@*/struct gps_data_t *gpsdata,
gps_mask_t expect,
const int timeout,
const char *fmt, ... )
/* ship a command and wait on an expected response type */
{
static fd_set rfds;
char buf[BUFSIZ];
va_list ap;
time_t starttime;
#ifdef COMPAT_SELECT
struct timeval tv;
#else
struct timespec tv;
sigset_t oldset, blockset;
(void)sigemptyset(&blockset);
(void)sigaddset(&blockset, SIGHUP);
(void)sigaddset(&blockset, SIGINT);
(void)sigaddset(&blockset, SIGTERM);
(void)sigaddset(&blockset, SIGQUIT);
(void)sigprocmask(SIG_BLOCK, &blockset, &oldset);
#endif /* COMPAT_SELECT */
va_start(ap, fmt);
(void)vsnprintf(buf, sizeof(buf)-2, fmt, ap);
va_end(ap);
if (buf[strlen(buf)-1] != '\n')
(void)strlcat(buf, "\n", BUFSIZ);
/*@-usedef@*/
if (write(gpsdata->gps_fd, buf, strlen(buf)) <= 0) {
gpsd_report(&context.errout, LOG_ERROR, "gps_query(), write failed\n");
return false;
}
/*@+usedef@*/
gpsd_report(&context.errout, LOG_PROG, "gps_query(), wrote, %s\n", buf);
FD_ZERO(&rfds);
starttime = time(NULL);
for (;;) {
FD_CLR(gpsdata->gps_fd, &rfds);
gpsd_report(&context.errout, LOG_PROG, "waiting...\n");
/*@ -usedef -type -nullpass -compdef @*/
tv.tv_sec = 2;
#ifdef COMPAT_SELECT
tv.tv_usec = 0;
if (select(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &tv) == -1) {
#else
tv.tv_nsec = 0;
if (pselect(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &tv, &oldset) == -1) {
#endif
if (errno == EINTR || !FD_ISSET(gpsdata->gps_fd, &rfds))
continue;
gpsd_report(&context.errout, LOG_ERROR, "select %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
/*@ +usedef +type +nullpass +compdef @*/
gpsd_report(&context.errout, LOG_PROG, "reading...\n");
(void)gps_read(gpsdata);
if (ERROR_SET & gpsdata->set) {
gpsd_report(&context.errout, LOG_ERROR, "error '%s'\n", gpsdata->error);
return false;
}
/*@ +ignorequals @*/
if ((expect == NON_ERROR) || (expect & gpsdata->set) != 0)
return true;
else if (timeout > 0 && (time(NULL) - starttime > timeout)) {
gpsd_report(&context.errout, LOG_ERROR,
"timed out after %d seconds\n",
timeout);
return false;
}
/*@ -ignorequals @*/
}
return false;
}
static void onsig(int sig)
{
if (sig == SIGALRM) {
gpsd_report(&context.errout, LOG_ERROR, "packet recognition timed out.\n");
exit(EXIT_FAILURE);
} else {
gpsd_report(&context.errout, LOG_ERROR, "killed by signal %d\n", sig);
exit(EXIT_SUCCESS);
}
}
static char /*@observer@*/ *gpsd_id( /*@in@ */ struct gps_device_t *session)
/* full ID of the device for reports, including subtype */
{
static char buf[128];
if ((session == NULL) || (session->device_type == NULL) ||
(session->device_type->type_name == NULL))
return "unknown,";
(void)strlcpy(buf, session->device_type->type_name, sizeof(buf));
if (session->subtype[0] != '\0') {
(void)strlcat(buf, " ", sizeof(buf));
(void)strlcat(buf, session->subtype, sizeof(buf));
}
return (buf);
}
static void ctlhook(struct gps_device_t *device UNUSED, gps_mask_t changed UNUSED)
/* recognize when we've achieved sync */
{
static int packet_counter = 0;
/*
* If it's NMEA, go back around enough times for the type probes to
* reveal any secret identity (like SiRF or UBX) the chip might have.
* If it's not, getting more packets might fetch subtype information.
*/
if (packet_counter++ >= REDIRECT_SNIFF)
{
hunting = false;
(void) alarm(0);
}
}
int main(int argc, char **argv)
{
int option, status;
char *device = NULL, *devtype = NULL;
char *speed = NULL, *control = NULL, *rate = NULL;
bool to_binary = false, to_nmea = false, reset = false;
bool lowlevel=false, echo=false;
struct gps_data_t gpsdata;
const struct gps_type_t *forcetype = NULL;
const struct gps_type_t **dp;
#ifdef CONTROLSEND_ENABLE
char cooked[BUFSIZ];
ssize_t cooklen = 0;
#endif /* RECONFIGURE_ENABLE */
context.errout.label = "gpsctl";
#define USAGE "usage: gpsctl [-l] [-b | -n | -r] [-D n] [-s speed] [-c rate] [-T timeout] [-V] [-t devtype] [-x control] [-e] <device>\n"
while ((option = getopt(argc, argv, "bec:fhlnrs:t:x:D:T:V")) != -1) {
switch (option) {
case 'b': /* switch to vendor binary mode */
to_binary = true;
break;
case 'c':
#ifdef RECONFIGURE_ENABLE
rate = optarg;
#else
gpsd_report(&context.errout, LOG_ERROR,
"cycle-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 'x': /* ship specified control string */
#ifdef CONTROLSEND_ENABLE
control = optarg;
lowlevel = true;
if ((cooklen = hex_escapes(cooked, control)) <= 0) {
gpsd_report(&context.errout, LOG_ERROR,
"invalid escape string (error %d)\n", (int)cooklen);
exit(EXIT_FAILURE);
}
#else
gpsd_report(&context.errout, LOG_ERROR,
"control_send capability has been conditioned out.\n");
#endif /* CONTROLSEND_ENABLE */
break;
case 'e': /* echo specified control string with wrapper */
lowlevel = true;
echo = true;
break;
case 'f': /* force direct access to the device */
lowlevel = true;
break;
case 'l': /* list known device types */
for (dp = gpsd_drivers; *dp; dp++) {
#ifdef RECONFIGURE_ENABLE
if ((*dp)->mode_switcher != NULL)
(void)fputs("-[bn]\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->speed_switcher != NULL)
(void)fputs("-s\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->rate_switcher != NULL)
(void)fputs("-c\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* RECONFIGURE_ENABLE */
#ifdef CONTROLSEND_ENABLE
if ((*dp)->control_send != NULL)
(void)fputs("-x\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* CONTROLSEND_ENABLE */
(void)puts((*dp)->type_name);
}
exit(EXIT_SUCCESS);
case 'n': /* switch to NMEA mode */
#ifdef RECONFIGURE_ENABLE
to_nmea = true;
#else
gpsd_report(&context.errout, LOG_ERROR,
"speed-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 'r': /* force-switch to default mode */
#ifdef RECONFIGURE_ENABLE
reset = true;
lowlevel = false; /* so we'll abort if the daemon is running */
#else
gpsd_report(&context.errout, LOG_ERROR,
"reset capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 's': /* change output baud rate */
#ifdef RECONFIGURE_ENABLE
speed = optarg;
#else
gpsd_report(&context.errout, LOG_ERROR,
"speed-change capability has been conditioned out.\n");
#endif /* RECONFIGURE_ENABLE */
break;
case 't': /* force the device type */
devtype = optarg;
break;
case 'T': /* set the timeout on packet recognition */
timeout = (unsigned)atoi(optarg);
explicit_timeout = true;
break;
case 'D': /* set debugging level */
debuglevel = atoi(optarg);
#ifdef CLIENTDEBUG_ENABLE
gps_enable_debug(debuglevel, stderr);
#endif /* CLIENTDEBUG_ENABLE */
break;
case 'V':
(void)fprintf(stderr, "version %s (revision %s)\n",
VERSION, REVISION);
exit(EXIT_SUCCESS);
case 'h':
default:
fprintf(stderr, USAGE);
break;
}
}
if (optind < argc)
device = argv[optind];
if (devtype != NULL) {
int matchcount = 0;
for (dp = gpsd_drivers; *dp; dp++) {
if (strstr((*dp)->type_name, devtype) != NULL) {
forcetype = *dp;
matchcount++;
}
}
if (matchcount == 0)
gpsd_report(&context.errout, LOG_ERROR,
"no driver type name matches '%s'.\n", devtype);
else if (matchcount == 1) {
assert(forcetype != NULL);
gpsd_report(&context.errout, LOG_PROG,
"%s driver selected.\n", forcetype->type_name);
} else {
forcetype = NULL;
gpsd_report(&context.errout, LOG_ERROR,
"%d driver type names match '%s'.\n",
matchcount, devtype);
}
}
if (((int)to_nmea + (int)to_binary + (int)reset) > 1) {
gpsd_report(&context.errout, LOG_ERROR, "make up your mind, would you?\n");
exit(EXIT_SUCCESS);
}
(void) signal(SIGINT, onsig);
(void) signal(SIGTERM, onsig);
(void) signal(SIGQUIT, onsig);
/*@-nullpass@*/ /* someday, add null annotation to the gpsopen() params */
if (!lowlevel) {
/* Try to open the stream to gpsd. */
if (gps_open(NULL, NULL, &gpsdata) != 0) {
gpsd_report(&context.errout, LOG_ERROR,
"no gpsd running or network error: %s.\n",
gps_errstr(errno));
lowlevel = true;
}
}
/*@-nullpass@*/
if (!lowlevel) {
int i, devcount;
if (!explicit_timeout)
timeout = HIGH_LEVEL_TIMEOUT;
/* what devices have we available? */
if (!gps_query(&gpsdata, DEVICELIST_SET, (int)timeout, "?DEVICES;\n")) {
gpsd_report(&context.errout, LOG_ERROR, "no DEVICES response received.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
if (gpsdata.devices.ndevices == 0) {
gpsd_report(&context.errout, LOG_ERROR, "no devices connected.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
} else if (gpsdata.devices.ndevices > 1 && device == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"multiple devices and no device specified.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
gpsd_report(&context.errout, LOG_PROG,
"%d device(s) found.\n",gpsdata.devices.ndevices);
/* try to mine the devicelist return for the data we want */
if (gpsdata.devices.ndevices == 1 && device == NULL) {
device = gpsdata.dev.path;
i = 0;
} else {
assert(device != NULL);
for (i = 0; i < gpsdata.devices.ndevices; i++)
if (strcmp(device, gpsdata.devices.list[i].path) == 0) {
goto devicelist_entry_matches;
}
gpsd_report(&context.errout, LOG_ERROR,
"specified device not found in device list.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
devicelist_entry_matches:;
}
(void)memcpy(&gpsdata.dev,
&gpsdata.devices.list[i],
sizeof(struct devconfig_t));
devcount = gpsdata.devices.ndevices;
/* if the device has not identified, watch it until it does so */
if (gpsdata.dev.driver[0] == '\0') {
if (gps_stream(&gpsdata, WATCH_ENABLE|WATCH_JSON, NULL) == -1) {
gpsd_report(&context.errout, LOG_ERROR, "stream set failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
while (devcount > 0) {
errno = 0;
if (gps_read(&gpsdata) == -1) {
gpsd_report(&context.errout, LOG_ERROR, "data read failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
}
if (gpsdata.set & DEVICE_SET) {
--devcount;
assert(gpsdata.dev.path[0]!='\0' && gpsdata.dev.driver[0]!='\0');
if (strcmp(gpsdata.dev.path, device) == 0) {
goto matching_device_seen;
}
}
}
gpsd_report(&context.errout, LOG_ERROR, "data read failed.\n");
(void)gps_close(&gpsdata);
exit(EXIT_FAILURE);
matching_device_seen:;
}
/* sanity check */
if (gpsdata.dev.driver[0] == '\0') {
gpsd_report(&context.errout, LOG_SHOUT, "%s can't be identified.\n",
gpsdata.dev.path);
(void)gps_close(&gpsdata);
exit(EXIT_SUCCESS);
}
/* if no control operation was specified, just ID the device */
if (speed==NULL && rate == NULL && !to_nmea && !to_binary && !reset) {
(void)printf("%s identified as a %s",
gpsdata.dev.path, gpsdata.dev.driver);
if (gpsdata.dev.subtype[0] != '\0') {
(void)fputc(' ', stdout);
(void)fputs(gpsdata.dev.subtype, stdout);
}
if (gpsdata.dev.baudrate > 0)
(void)printf(" at %u baud", gpsdata.dev.baudrate);
(void)fputc('.', stdout);
(void)fputc('\n', stdout);
}
status = 0;
#ifdef RECONFIGURE_ENABLE
if (reset)
{
gpsd_report(&context.errout, LOG_PROG,
"cannot reset with gpsd running.\n");
exit(EXIT_SUCCESS);
}
/*@-boolops@*/
/*
* We used to wait on DEVICE_SET here. That doesn't work
* anymore because when the demon generates its response it
* sets the mode bit in the response from the current packet
* type, which may not have changed (probably will not have
* changed) even though the command to switch modes has been
* sent and will shortly take effect.
*/
if (to_nmea) {
if (!gps_query(&gpsdata, NON_ERROR, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"native\":0}\r\n",
device)) {
gpsd_report(&context.errout, LOG_ERROR,
"%s mode change to NMEA failed\n",
gpsdata.dev.path);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s mode change succeeded\n", gpsdata.dev.path);
}
else if (to_binary) {
if (!gps_query(&gpsdata, NON_ERROR, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"native\":1}\r\n",
device)) {
gpsd_report(&context.errout, LOG_ERROR,
"%s mode change to native mode failed\n",
gpsdata.dev.path);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s mode change succeeded\n",
gpsdata.dev.path);
}
/*@+boolops@*/
if (speed != NULL) {
char parity = 'N';
char stopbits = '1';
if (strchr(speed, ':') == NULL)
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"bps\":%s}\r\n",
device, speed);
else {
char *modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"No support for that word length.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", stopbits) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"Stop bits must be 1 or 2.\n");
status = 1;
}
}
if (status == 0)
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"bps\":%s,\"parity\":\"%c\",\"stopbits\":%c}\r\n",
device, speed, parity, stopbits);
}
if (atoi(speed) != (int)gpsdata.dev.baudrate) {
gpsd_report(&context.errout, LOG_ERROR,
"%s driver won't support %s%c%c\n",
gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
} else
gpsd_report(&context.errout, LOG_PROG,
"%s change to %s%c%c succeeded\n",
gpsdata.dev.path,
speed, parity, stopbits);
}
if (rate != NULL) {
(void)gps_query(&gpsdata,
DEVICE_SET, (int)timeout,
"?DEVICE={\"path\":\"%s\",\"cycle\":%s}\n",
device, rate);
}
#endif /* RECONFIGURE_ENABLE */
(void)gps_close(&gpsdata);
exit(status);
#ifdef RECONFIGURE_ENABLE
} else if (reset) {
/* hard reset will go through lower-level operations */
const int speeds[] = {2400, 4800, 9600, 19200, 38400, 57600, 115200};
static struct gps_device_t session; /* zero this too */
int i;
if (device == NULL || forcetype == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"device and type must be specified for the reset operation.\n");
exit(EXIT_FAILURE);
}
/*@ -mustfreeonly -immediatetrans @*/
gps_context_init(&context, "gpsctl");
context.errout.debug = debuglevel;
session.context = &context;
gpsd_tty_init(&session);
(void)strlcpy(session.gpsdata.dev.path, device, sizeof(session.gpsdata.dev.path));
session.device_type = forcetype;
(void)gpsd_open(&session);
(void)gpsd_set_raw(&session);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
for(i = 0; i < (int)(sizeof(speeds) / sizeof(speeds[0])); i++) {
(void)gpsd_set_speed(&session, speeds[i], 'N', 1);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
}
gpsd_set_speed(&session, 4800, 'N', 1);
for (i = 0; i < 3; i++)
if (session.device_type->mode_switcher)
session.device_type->mode_switcher(&session, MODE_NMEA);
gpsd_wrap(&session);
exit(EXIT_SUCCESS);
/*@ +mustfreeonly +immediatetrans @*/
#endif /* RECONFIGURE_ENABLE */
} else {
/* access to the daemon failed, use the low-level facilities */
static struct gps_device_t session; /* zero this too */
fd_set all_fds;
fd_set rfds;
/*
* Unless the user explicitly requested it, always run to end of
* hunt rather than timing out. Otherwise we can easily get messages
* that spuriously look like failure at high baud rates.
*/
/*@ -mustfreeonly -immediatetrans @*/
gps_context_init(&context, "gpsctl");
context.errout.debug = debuglevel;
session.context = &context; /* in case gps_init isn't called */
if (echo)
context.readonly = true;
if (timeout > 0) {
(void) alarm(timeout);
(void) signal(SIGALRM, onsig);
}
/*
* Unless the user has forced a type and only wants to see the
* string (not send it) we now need to try to open the device
* and find out what is actually there.
*/
if (!(forcetype != NULL && echo)) {
int maxfd = 0;
if (device == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"device must be specified for low-level access.\n");
exit(EXIT_FAILURE);
}
gpsd_init(&session, &context, device);
if (gpsd_activate(&session, O_PROBEONLY) < 0) {
gpsd_report(&context.errout, LOG_ERROR,
"initial GPS device %s open failed\n",
device);
exit(EXIT_FAILURE);
}
gpsd_report(&context.errout, LOG_INF,
"device %s activated\n", session.gpsdata.dev.path);
/*@i1@*/FD_SET(session.gpsdata.gps_fd, &all_fds);
if (session.gpsdata.gps_fd > maxfd)
maxfd = session.gpsdata.gps_fd;
/* initialize the GPS context's time fields */
gpsd_time_init(&context, time(NULL));
/*@-compdef@*/
/* grab packets until we time out, get sync, or fail sync */
for (hunting = true; hunting; )
{
fd_set efds;
switch(gpsd_await_data(&rfds, &efds, maxfd, &all_fds, &context.errout))
{
case AWAIT_GOT_INPUT:
break;
case AWAIT_NOT_READY:
/* no recovery from bad fd is possible */
if (FD_ISSET(session.gpsdata.gps_fd, &efds))
exit(EXIT_FAILURE);
continue;
case AWAIT_FAILED:
exit(EXIT_FAILURE);
}
switch(gpsd_multipoll(FD_ISSET(session.gpsdata.gps_fd, &rfds),
&session, ctlhook, 0))
{
case DEVICE_READY:
FD_SET(session.gpsdata.gps_fd, &all_fds);
break;
case DEVICE_UNREADY:
FD_CLR(session.gpsdata.gps_fd, &all_fds);
break;
case DEVICE_ERROR:
/* this is where a failure to sync lands */
gpsd_report(&context.errout, LOG_WARN,
"device error, bailing out.\n");
exit(EXIT_FAILURE);
case DEVICE_EOF:
gpsd_report(&context.errout, LOG_WARN,
"device signed off, bailing out.\n");
exit(EXIT_SUCCESS);
default:
break;
}
}
/*@+compdef@*/
gpsd_report(&context.errout, LOG_PROG,
"%s looks like a %s at %d.\n",
device, gpsd_id(&session),
session.gpsdata.dev.baudrate);
if (forcetype!=NULL && strcmp("NMEA0183", session.device_type->type_name) !=0 && strcmp(forcetype->type_name, session.device_type->type_name)!=0) {
gpsd_report(&context.errout, LOG_ERROR,
"'%s' doesn't match non-generic type '%s' of selected device.\n",
forcetype->type_name,
session.device_type->type_name);
}
}
(void)printf("%s identified as a %s at %u baud.\n",
device, gpsd_id(&session),
session.gpsdata.dev.baudrate);
/* if no control operation was specified, we're done */
if (speed==NULL && !to_nmea && !to_binary && control==NULL)
exit(EXIT_SUCCESS);
/* maybe user wants to see the packet rather than send it */
if (echo)
session.gpsdata.gps_fd = fileno(stdout);
/* control op specified; maybe we forced the type */
if (forcetype != NULL)
(void)gpsd_switch_driver(&session, forcetype->type_name);
/* now perform the actual control function */
status = 0;
#ifdef RECONFIGURE_ENABLE
/*@ -nullderef @*/
if (to_nmea || to_binary) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->mode_switcher == NULL) {
gpsd_report(&context.errout, LOG_SHOUT,
"%s devices have no mode switch.\n",
session.device_type->type_name);
status = 1;
} else {
int target_mode = to_nmea ? MODE_NMEA : MODE_BINARY;
gpsd_report(&context.errout, LOG_SHOUT,
"switching to mode %s.\n",
to_nmea ? "NMEA" : "BINARY");
session.device_type->mode_switcher(&session, target_mode);
settle(&session);
}
context.readonly = write_enable;
}
if (speed) {
char parity = echo ? 'N': session.gpsdata.dev.parity;
int stopbits = echo ? 1 : session.gpsdata.dev.stopbits;
char *modespec;
modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"No support for that word lengths.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", parity) == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"Stop bits must be 1 or 2.\n");
status = 1;
}
stopbits = (int)(stopbits-'0');
}
if (status == 0) {
if (session.device_type->speed_switcher == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no speed switch.\n",
session.device_type->type_name);
status = 1;
}
else if (session.device_type->speed_switcher(&session,
(speed_t)atoi(speed),
parity,
stopbits)) {
settle(&session);
gpsd_report(&context.errout, LOG_PROG,
"%s change to %s%c%d succeeded\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
} else {
gpsd_report(&context.errout, LOG_ERROR,
"%s driver won't support %s%c%d.\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
}
}
}
if (rate) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->rate_switcher == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no rate switcher.\n",
session.device_type->type_name);
status = 1;
} else {
double rate_dbl = strtod(rate, NULL);
if (!session.device_type->rate_switcher(&session, rate_dbl)) {
gpsd_report(&context.errout, LOG_ERROR, "rate switch failed.\n");
status = 1;
}
settle(&session);
}
context.readonly = write_enable;
}
#endif /* RECONFIGURE_ENABLE */
#ifdef CONTROLSEND_ENABLE
/*@ -compdef @*/
if (control) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->control_send == NULL) {
gpsd_report(&context.errout, LOG_ERROR,
"%s devices have no control sender.\n",
session.device_type->type_name);
status = 1;
} else {
if (session.device_type->control_send(&session,
cooked,
(size_t)cooklen) == -1) {
gpsd_report(&context.errout, LOG_ERROR,
"control transmission failed.\n");
status = 1;
}
settle(&session);
}
context.readonly = write_enable;
}
/*@ +compdef @*/
#endif /* CONTROLSEND_ENABLE */
exit(status);
/*@ +nullderef @*/
/*@ +mustfreeonly +immediatetrans @*/
}
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
syslog(LOG_INFO, "initializing core");
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("core") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("core.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
exit(EXIT_FAILURE);
}
syslog(LOG_CRIT, "logger opened");
sleep(1); /* give scl some time to establish
a link between publisher and subscriber */
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "| core startup |");
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "initializing system");
/* set-up real-time scheduling: */
struct sched_param sp;
sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
sched_setscheduler(getpid(), SCHED_FIFO, &sp);
if (mlockall(MCL_CURRENT | MCL_FUTURE))
{
LOG(LL_ERROR, "mlockall() failed");
exit(EXIT_FAILURE);
}
/* initialize hardware/drivers: */
omap_i2c_bus_init();
baro_altimeter_init();
ultra_altimeter_init();
ahrs_init();
motors_init();
voltage_reader_start();
//gps_init();
LOG(LL_INFO, "initializing model/controller");
model_init();
ctrl_init();
/* initialize command interface */
LOG(LL_INFO, "initializing cmd interface");
cmd_init();
/* prepare main loop: */
for (int i = 0; i < NUM_AVG; i++)
{
output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f);
}
LOG(LL_INFO, "system up and running");
struct timespec ts_curr;
struct timespec ts_prev;
struct timespec ts_diff;
clock_gettime(CLOCK_REALTIME, &ts_curr);
/* run model and controller: */
while (1)
{
/* calculate dt: */
ts_prev = ts_curr;
clock_gettime(CLOCK_REALTIME, &ts_curr);
TIMESPEC_SUB(ts_diff, ts_curr, ts_prev);
float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC;
/* read sensor values into model input structure: */
model_input_t model_input;
model_input.dt = dt;
ahrs_read(&model_input.ahrs_data);
gps_read(&model_input.gps_data);
model_input.ultra_z = ultra_altimeter_read();
model_input.baro_z = baro_altimeter_read();
/* execute model step: */
model_state_t model_state;
model_step(&model_state, &model_input);
/* execute controller step: */
mixer_in_t mixer_in;
ctrl_step(&mixer_in, dt, &model_state);
/* set up mixer input: */
mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch);
mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll);
mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw);
mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas);
/* write data to motor mixer: */
EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in));
}
}
static gpointer _gpsdClientRead(gpointer dummy)
// start gpsd client - loop forever
// return if _ais_list is NULL or 10 failed attempt to connect
{
int nWait = 0;
g_print("s52ais:_gpsdClientRead(): start looping ..\n");
__builtin_bzero(&_gpsdata, sizeof(_gpsdata));
//*
while (0 != gps_open(GPSD_HOST, GPSD_PORT, &_gpsdata)) { // android (gpsd 2.96)
g_print("s52ais:_gpsdClientRead(): no gpsd running or network error, wait 1 sec: %d, %s\n", errno, gps_errstr(errno));
// try to connect to GPSD server, bailout after 10 failed attempt
g_static_mutex_lock(&_ais_list_mutex);
if ((NULL==_ais_list) || (10 <= ++nWait)) {
g_print("s52ais:_gpsdClientRead() no AIS list (main exited) or no GPSD server.. terminate _gpsClientRead thread\n");
g_static_mutex_unlock(&_ais_list_mutex);
return NULL;
}
g_static_mutex_unlock(&_ais_list_mutex);
g_usleep(1000 * 1000); // 1.0 sec
}
if (-1 == gps_stream(&_gpsdata, WATCH_ENABLE|WATCH_NEWSTYLE, NULL)) {
g_print("s52ais:_gpsdClientRead():gps_stream() failed .. exiting\n");
return NULL;
}
//*/
// debug
//gps_enable_debug(3, stderr);
//gps_enable_debug(8, stderr);
// heart of the client
for (;;) {
g_static_mutex_lock(&_ais_list_mutex);
if (NULL == _ais_list) {
g_print("s52ais:_gpsdClientRead() no AIS list .. main exited .. terminate gpsRead thread\n");
goto exit;
}
g_static_mutex_unlock(&_ais_list_mutex);
if (FALSE == gps_waiting(&_gpsdata, 500*1000)) { // wait 0.5 sec (500*1000 uSec)
//g_print("s52ais:_gpsdClientRead():gps_waiting() timed out\n");
g_static_mutex_lock(&_ais_list_mutex);
_updateTimeTag();
g_static_mutex_unlock(&_ais_list_mutex);
} else {
errno = 0;
int ret = gps_read(&_gpsdata);
if (0 < ret) {
// no error
//g_print("s52ais:_gpsdClientRead():gps_read() ..\n");
// handle AIS data
g_static_mutex_lock(&_ais_list_mutex);
_updateAISdata(&_gpsdata);
g_static_mutex_unlock(&_ais_list_mutex);
continue;
}
if (0 == ret) {
g_print("s52ais:_gpsdClientRead():gps_read(): NO DATA .. [ret=0, errno=%i]\n", errno);
continue;
} else {
g_print("s52ais:_gpsdClientRead():gps_read(): socket error 4 .. GPSD died [ret=%i, errno=%i]\n", ret, errno);
goto exit;
}
}
}
exit:
// exit thread
g_static_mutex_unlock(&_ais_list_mutex);
gps_stream(&_gpsdata, WATCH_DISABLE, NULL);
gps_close(&_gpsdata);
return dummy;
}
int dwgps_read (double *plat, double *plon, float *pspeed, float *pcourse, float *palt)
{
#if __WIN32__
text_color_set(DW_COLOR_ERROR);
dw_printf ("Internal error, dwgps_read, shouldn't be here.\n");
return (-1);
#elif ENABLE_GPS
int err;
if (init_status != INIT_SUCCESS) {
text_color_set(DW_COLOR_ERROR);
dw_printf ("Internal error, dwgps_read without successful init.\n");
return (-1);
}
#if USE_GPS_SHM
/*
* Shared memory version.
*/
err = gps_read (&gpsdata);
#if DEBUG
dw_printf ("gps_read returns %d bytes\n", err);
#endif
#else
/*
* Socket version.
*/
// Wait for up to 1000 milliseconds.
// This should only happen in the beaconing thread so
// I'm not worried about other functions hanging.
if (gps_waiting(&gpsdata, 1000)) {
err = gps_read (&gpsdata);
}
else {
gps_stream(&gpsdata, WATCH_ENABLE | WATCH_JSON, NULL);
sleep (1);
}
#endif
if (err > 0) {
/* Data is available. */
if (gpsdata.status >= STATUS_FIX && gpsdata.fix.mode >= MODE_2D) {
*plat = gpsdata.fix.latitude;
*plon = gpsdata.fix.longitude;
*pcourse = gpsdata.fix.track;
*pspeed = MPS_TO_KNOTS * gpsdata.fix.speed; /* libgps uses meters/sec */
if (gpsdata.fix.mode >= MODE_3D) {
*palt = gpsdata.fix.altitude;
return (3);
}
return (2);
}
/* No fix. Probably temporary condition. */
return (0);
}
else if (err == 0) {
/* No data available at the present time. */
return (0);
}
else {
/* More serious error. */
return (-1);
}
#else
text_color_set(DW_COLOR_ERROR);
dw_printf ("Internal error, dwgps_read, shouldn't be here.\n");
return (-1);
#endif
} /* end dwgps_read */
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";
}
int main(int argc, char *argv[])
{
if (argc<0) {
error(" hostname error\n");
}
int socket_fd, port_no, i,j,s;
uint8_t arr[60]; //buffer to store payload
memset(arr, 0, 60);
uint8_t msgNum;
srand(time(NULL));
msgNum=(uint8_t)(rand()%128);
printf("The random value picked is %u\n",msgNum);
struct timespec time1,start,stop,diff;
int64_t t1,t2;
struct BasicSafetyMessage *BSM_head = (struct BasicSafetyMessage *)arr; //pointer to BSM
printf("size of BSM is %lu\n", sizeof(struct BasicSafetyMessage));
struct gps_data_t gpsdata;
j = gps_open("localhost", "3333", &gpsdata); // can be any port number
printf("open status is %d\n", j);
(void)gps_stream(&gpsdata, WATCH_ENABLE | WATCH_JSON, NULL);
printf("overall status is %d\n", gpsdata.status);
socket_fd = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_ALL)); //ethernet header automatically constructed
if (socket_fd < 0) {
error("Error opening socket");
}
else
{
printf("opened socket\n");
}
//to determine interface number of the interface to be used
struct ifreq ifr;
size_t if_name_len = strlen("wlan0");
if (if_name_len<sizeof(ifr.ifr_name)) {
memcpy(ifr.ifr_name, "wlan0", if_name_len);
ifr.ifr_name[if_name_len] = 0;
}
else {
error("interface name is too long\n");
}
if (ioctl(socket_fd, SIOCGIFINDEX, &ifr) == -1) {
error("error in ioctl block\n");
}
int ifindex = ifr.ifr_ifindex; //store interface number in the variable 'ifindex'
const unsigned char ether_broadcast_addr[] = { 0xff,0xff,0xff,0xff,0xff,0xff }; //destination address- broadcast address in this case
struct sockaddr_ll addr = { 0 };
addr.sll_family = AF_PACKET;
addr.sll_ifindex = ifindex;
addr.sll_halen = ETHER_ADDR_LEN;
addr.sll_protocol = htons(ETH_P_ALL);
memcpy(addr.sll_addr, ether_broadcast_addr, ETHER_ADDR_LEN);
int broadcast = 1;
if (setsockopt(socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) == -1) { //setting the 'broadcast' option on socket
error("Error in setting the socket option for broadcast");
}
else
{
printf("set option broadcast\n");
}
//fill structure of BSM
getFixedData(&BSM_head);
int tot_len = sizeof(struct BasicSafetyMessage);
FILE *fp;
fp = fopen("location.txt","a");
fprintf(fp,"Timestamp(microsecs) SeqNum Latitude Longitude \n");
int c;
for (c=0;c<1800;c++) //to keep broadcasting continuously
{
int num_bytes;
if (gps_read(&gpsdata) == -1){
printf("error inside read\n");
}
// If we have data, gpsdata.status becomes greater than 0
else if(gpsdata.status > 0){
getGpsSensorData(&BSM_head,&gpsdata,&msgNum);
}
getAnotherSensorData();
clock_gettime(CLOCK_REALTIME, &time1);
//timestamp in microseconds
int64_t micros = time1.tv_sec * 1000000;
// Adding full microseconds
micros += time1.tv_nsec/1000;
// rounding up
if (time1.tv_nsec % 1000 >= 500) {
++micros;
}
fprintf(fp,"\t");
fprintf(fp,"%u",micros);
fprintf(fp,"\t\t");
fprintf(fp,"%u",BSM_head->blob1.msgCnt);
fprintf(fp,"\t\t");
fprintf(fp,"%lf",BSM_head->blob1.lat);
fprintf(fp,"\t\t");
fprintf(fp,"%lf",BSM_head->blob1.longi);
fprintf(fp,"\n");
if(c!=0){
clock_gettime(CLOCK_REALTIME, &stop);
//timestamp in nanoseconds
t2 = stop.tv_sec * 1000000000;
// Adding full nanoseconds
t2 += stop.tv_nsec;
printf("\n t2 is %u\n",t2);
diff.tv_sec = 0;
diff.tv_nsec = (100000000-(t2-t1));
printf("difference is %u in secs and %u nanosecs\n",diff.tv_sec,diff.tv_nsec);
nanosleep(&diff,NULL);
printf("after sleep\n");
}
num_bytes = sendto(socket_fd, BSM_head, tot_len, 0, (struct sockaddr*)&addr, sizeof(addr));//sendto() to broadcast the BSM packet
clock_gettime(CLOCK_REALTIME, &start);
//timestamp in nanoseconds
t1 = start.tv_sec * 1000000000;
// Adding full nanoseconds
t1 += start.tv_nsec;
printf("\n t1 is %u\n",t1);
if (num_bytes <0) {
error("Error in sending the packet\n");
}
else
{
printf("Sent %d bytes\n", num_bytes);
}
/*
printf("The contents of the buffer are:\n");
for(s=0;s<num_bytes;s=s+2){
printf("%02x %02x\n",arr[s+1],arr[s]);
}
*/
//sleep(1);
}
fclose(fp);
close(socket_fd);
(void)gps_stream(&gpsdata, WATCH_DISABLE, NULL);
(void)gps_close(&gpsdata);
return 0;
}
int main(int argc, char **argv)
{
int option, status;
char *device = NULL, *devtype = NULL;
char *speed = NULL, *control = NULL, *rate = NULL;
bool to_binary = false, to_nmea = false, reset = false;
bool lowlevel=false, echo=false;
struct gps_data_t gpsdata;
const struct gps_type_t *forcetype = NULL;
const struct gps_type_t **dp;
unsigned int timeout = 4;
#ifdef ALLOW_CONTROLSEND
char cooked[BUFSIZ];
ssize_t cooklen = 0;
#endif /* ALLOW_RECONFIGURE */
#define USAGE "usage: gpsctl [-l] [-b | -n | -r] [-D n] [-s speed] [-c rate] [-T timeout] [-V] [-t devtype] [-x control] [-e] <device>\n"
while ((option = getopt(argc, argv, "bec:fhlnrs:t:x:D:T:V")) != -1) {
switch (option) {
case 'b': /* switch to vendor binary mode */
to_binary = true;
break;
case 'c':
#ifdef ALLOW_RECONFIGURE
rate = optarg;
#else
gpsd_report(LOG_ERROR, "cycle-change capability has been conditioned out.\n");
#endif /* ALLOW_RECONFIGURE */
break;
case 'x': /* ship specified control string */
#ifdef ALLOW_CONTROLSEND
control = optarg;
lowlevel = true;
if ((cooklen = hex_escapes(cooked, control)) <= 0) {
gpsd_report(LOG_ERROR,
"invalid escape string (error %d)\n", (int)cooklen);
exit(1);
}
#else
gpsd_report(LOG_ERROR, "control_send capability has been conditioned out.\n");
#endif /* ALLOW_CONTROLSEND */
break;
case 'e': /* echo specified control string with wrapper */
lowlevel = true;
echo = true;
break;
case 'f': /* force direct access to the device */
lowlevel = true;
break;
case 'l': /* list known device types */
for (dp = gpsd_drivers; *dp; dp++) {
#ifdef ALLOW_RECONFIGURE
if ((*dp)->mode_switcher != NULL)
(void)fputs("-[bn]\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->speed_switcher != NULL)
(void)fputs("-s\t", stdout);
else
(void)fputc('\t', stdout);
if ((*dp)->rate_switcher != NULL)
(void)fputs("-c\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* ALLOW_RECONFIGURE */
#ifdef ALLOW_CONTROLSEND
if ((*dp)->control_send != NULL)
(void)fputs("-x\t", stdout);
else
(void)fputc('\t', stdout);
#endif /* ALLOW_CONTROLSEND */
(void)puts((*dp)->type_name);
}
exit(0);
case 'n': /* switch to NMEA mode */
#ifdef ALLOW_RECONFIGURE
to_nmea = true;
#else
gpsd_report(LOG_ERROR, "speed-change capability has been conditioned out.\n");
#endif /* ALLOW_RECONFIGURE */
break;
case 'r': /* force-switch to default mode */
#ifdef ALLOW_RECONFIGURE
reset = true;
lowlevel = false; /* so we'll abort if the daemon is running */
#else
gpsd_report(LOG_ERROR, "reset capability has been conditioned out.\n");
#endif /* ALLOW_RECONFIGURE */
break;
case 's': /* change output baud rate */
#ifdef ALLOW_RECONFIGURE
speed = optarg;
#else
gpsd_report(LOG_ERROR, "speed-change capability has been conditioned out.\n");
#endif /* ALLOW_RECONFIGURE */
break;
case 't': /* force the device type */
devtype = optarg;
break;
case 'T': /* set the timeout on packet recognition */
timeout = (unsigned)atoi(optarg);
break;
case 'D': /* set debugging level */
debuglevel = atoi(optarg);
gpsd_hexdump_level = debuglevel;
#ifdef CLIENTDEBUG_ENABLE
gps_enable_debug(debuglevel, stderr);
#endif /* CLIENTDEBUG_ENABLE */
break;
case 'V':
(void)fprintf(stderr, "gpsctl: version %s (revision %s)\n",
VERSION, REVISION);
break;
case 'h':
default:
fprintf(stderr, USAGE);
break;
}
}
if (optind < argc)
device = argv[optind];
if (devtype != NULL) {
int matchcount = 0;
for (dp = gpsd_drivers; *dp; dp++) {
if (strstr((*dp)->type_name, devtype) != NULL) {
forcetype = *dp;
matchcount++;
}
}
if (matchcount == 0)
gpsd_report(LOG_ERROR, "no driver type name matches '%s'.\n", devtype);
else if (matchcount == 1) {
assert(forcetype != NULL);
gpsd_report(LOG_PROG, "%s driver selected.\n", forcetype->type_name);
} else {
forcetype = NULL;
gpsd_report(LOG_ERROR, "%d driver type names match '%s'.\n",
matchcount, devtype);
}
}
if ((int)to_nmea + (int)to_binary + (int)reset > 1) {
gpsd_report(LOG_ERROR, "make up your mind, would you?\n");
exit(0);
}
(void) signal(SIGINT, onsig);
(void) signal(SIGTERM, onsig);
(void) signal(SIGQUIT, onsig);
/*@-nullpass@*/ /* someday, add null annotation to the gpsopen_r() params */
if (!lowlevel) {
/* Try to open the stream to gpsd. */
if (gps_open_r(NULL, NULL, &gpsdata) != 0) {
gpsd_report(LOG_ERROR, "no gpsd running or network error: %s.\n",
netlib_errstr(errno));
lowlevel = true;
}
}
/*@-nullpass@*/
/* ^ someday, add out annotation to the gpspoll() param and remove */
if (!lowlevel) {
/* OK, there's a daemon instance running. Do things the easy way */
struct devconfig_t *devlistp;
(void)gps_read(&gpsdata);
if ((gpsdata.set & DEVICELIST_SET) != 0) {
gpsd_report(LOG_ERROR, "no VERSION response received; update your gpsd.\n");
(void)gps_close(&gpsdata);
exit(1);
}
(void)gps_query(&gpsdata, "?DEVICES;\n");
if ((gpsdata.set & DEVICELIST_SET) == 0) {
gpsd_report(LOG_ERROR, "no DEVICES response received.\n");
(void)gps_close(&gpsdata);
exit(1);
}
if (gpsdata.devices.ndevices == 0) {
gpsd_report(LOG_ERROR, "no devices connected.\n");
(void)gps_close(&gpsdata);
exit(1);
} else if (gpsdata.devices.ndevices > 1 && device == NULL) {
gpsd_report(LOG_ERROR,
"multiple devices and no device specified.\n");
(void)gps_close(&gpsdata);
exit(1);
}
gpsd_report(LOG_PROG,"%d device(s) found.\n",gpsdata.devices.ndevices);
if (gpsdata.devices.ndevices == 1) {
devlistp = &gpsdata.devices.list[0];
device = devlistp->path;
} else {
int i;
assert(device != NULL);
for (i = 0; i < gpsdata.devices.ndevices; i++)
if (strcmp(device, gpsdata.devices.list[i].path) == 0)
goto foundit;
gpsd_report(LOG_ERROR, "specified device not found.\n");
(void)gps_close(&gpsdata);
exit(1);
foundit:
devlistp = &gpsdata.devices.list[i];
}
/* if no control operation was specified, just ID the device */
if (speed==NULL && rate == NULL && !to_nmea && !to_binary && !reset) {
gpsd_report(LOG_SHOUT, "%s identified as %s at %d\n",
devlistp->path, devlistp->driver, devlistp->baudrate);
exit(0);
}
status = 0;
#ifdef ALLOW_RECONFIGURE
if (reset)
{
gpsd_report(LOG_PROG, "cannot reset with gpsd running.\n");
exit(0);
}
/*@-boolops@*/
if (to_nmea) {
(void)gps_query(&gpsdata, "?DEVICE={\"path\":\"%s\",\"native\":0}\r\n", device);
if ((gpsdata.set & ERROR_SET) || (gpsdata.dev.driver_mode != MODE_NMEA)) {
gpsd_report(LOG_ERROR, "%s mode change to NMEA failed\n", gpsdata.dev.path);
status = 1;
} else
gpsd_report(LOG_PROG, "%s mode change succeeded\n", gpsdata.dev.path);
}
else if (to_binary) {
(void)gps_query(&gpsdata, "?DEVICE={\"path\":\"%s\",\"native\":1}\r\n", device);
if ((gpsdata.set & ERROR_SET) || (gpsdata.dev.driver_mode != MODE_BINARY)) {
gpsd_report(LOG_ERROR, "%s mode change to native mode failed\n", gpsdata.dev.path);
status = 1;
} else
gpsd_report(LOG_PROG, "%s mode change succeeded\n", gpsdata.dev.path);
}
/*@+boolops@*/
if (speed != NULL) {
char parity = 'N';
char stopbits = '1';
if (strchr(speed, ':') == NULL)
(void)gps_query(&gpsdata,
"?DEVICE={\"path\":\"%s\",\"bps\":%s}\r\n",
device, speed);
else {
char *modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(LOG_ERROR, "No support for that word lengths.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(LOG_ERROR, "What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", stopbits) == NULL) {
gpsd_report(LOG_ERROR, "Stop bits must be 1 or 2.\n");
status = 1;
}
}
if (status == 0)
(void)gps_query(&gpsdata,
"?DEVICE={\"path\":\"%s\",\"bps\":%s,\"parity\":\"%c\",\"stopbits\":%c}\r\n",
device, speed, parity, stopbits);
}
if (atoi(speed) != (int)gpsdata.dev.baudrate) {
gpsd_report(LOG_ERROR, "%s driver won't support %s%c%c\n",
gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
} else
gpsd_report(LOG_PROG, "%s change to %s%c%c succeeded\n",
gpsdata.dev.path,
speed, parity, stopbits);
}
if (rate != NULL) {
(void)gps_query(&gpsdata,
"?DEVICE={\"path\":\"%s\",\"cycle\":%s}\n",
device, rate);
}
#endif /* ALLOW_RECONFIGURE */
(void)gps_close(&gpsdata);
exit(status);
#ifdef ALLOW_RECONFIGURE
} else if (reset) {
/* hard reset will go through lower-level operations */
const int speeds[] = {2400, 4800, 9600, 19200, 38400, 57600, 115200};
static struct gps_context_t context; /* start it zeroed */
static struct gps_device_t session; /* zero this too */
int i;
if (device == NULL || forcetype == NULL) {
gpsd_report(LOG_ERROR, "device and type must be specified for the reset operation.\n");
exit(1);
}
/*@ -mustfreeonly -immediatetrans @*/
session.context = &context;
gpsd_tty_init(&session);
(void)strlcpy(session.gpsdata.dev.path, device, sizeof(session.gpsdata.dev.path));
session.device_type = forcetype;
(void)gpsd_open(&session);
(void)gpsd_set_raw(&session);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
for(i = 0; i < (int)(sizeof(speeds) / sizeof(speeds[0])); i++) {
(void)gpsd_set_speed(&session, speeds[i], 'N', 1);
(void)session.device_type->speed_switcher(&session, 4800, 'N', 1);
(void)tcdrain(session.gpsdata.gps_fd);
}
gpsd_set_speed(&session, 4800, 'N', 1);
for (i = 0; i < 3; i++)
if (session.device_type->mode_switcher)
session.device_type->mode_switcher(&session, MODE_NMEA);
gpsd_wrap(&session);
exit(0);
/*@ +mustfreeonly +immediatetrans @*/
#endif /* ALLOW_RECONFIGURE */
} else {
/* access to the daemon failed, use the low-level facilities */
static struct gps_context_t context; /* start it zeroed */
static struct gps_device_t session; /* zero this too */
/*@ -mustfreeonly -immediatetrans @*/
session.context = &context; /* in case gps_init isn't called */
if (echo)
context.readonly = true;
(void) alarm(timeout);
(void) signal(SIGALRM, onsig);
/*
* Unless the user has forced a type and only wants to see the
* string (not send it) we now need to try to open the device
* and find out what is actually there.
*/
if (!(forcetype != NULL && echo)) {
int seq;
if (device == NULL) {
gpsd_report(LOG_ERROR, "device must be specified for low-level access.\n");
exit(1);
}
gpsd_init(&session, &context, device);
gpsd_report(LOG_PROG, "initialization passed.\n");
if (gpsd_activate(&session) == -1) {
gpsd_report(LOG_ERROR,
"activation of device %s failed, errno=%d\n",
device, errno);
exit(2);
}
/* hunt for packet type and serial parameters */
for (seq = 0; session.device_type == NULL; seq++) {
if (get_packet(&session) == ERROR_SET) {
gpsd_report(LOG_ERROR,
"autodetection failed.\n");
exit(2);
} else {
gpsd_report(LOG_IO,
"autodetection after %d reads.\n", seq);
(void) alarm(0);
break;
}
}
gpsd_report(LOG_PROG, "%s looks like a %s at %d.\n",
device, gpsd_id(&session), session.gpsdata.dev.baudrate);
if (forcetype!=NULL && strcmp("Generic NMEA", session.device_type->type_name) !=0 && strcmp(forcetype->type_name, session.device_type->type_name)!=0) {
gpsd_report(LOG_ERROR, "'%s' doesn't match non-generic type '%s' of selected device.\n", forcetype->type_name, session.device_type->type_name);
}
/*
* If we've identified this as an NMEA device, we have to eat
* packets for a while to see if one of our probes elicits an
* ID response telling us that it's really a SiRF or
* something. If so, the libgpsd(3) layer will automatically
* redispatch to the correct driver type.
*/
if (strcmp(session.device_type->type_name, "Generic NMEA") == 0) {
int dummy;
for (dummy = 0; dummy < REDIRECT_SNIFF; dummy++) {
if ((get_packet(&session) & DEVICEID_SET)!=0)
break;
}
}
gpsd_report(LOG_SHOUT, "%s identified as a %s at %d.\n",
device, gpsd_id(&session), session.gpsdata.dev.baudrate);
}
/* if no control operation was specified, we're done */
if (speed==NULL && !to_nmea && !to_binary && control==NULL)
exit(0);
/* maybe user wants to see the packet rather than send it */
if (echo)
session.gpsdata.gps_fd = fileno(stdout);
/* control op specified; maybe we forced the type */
if (forcetype != NULL)
(void)gpsd_switch_driver(&session, forcetype->type_name);
/* now perform the actual control function */
status = 0;
#ifdef ALLOW_RECONFIGURE
/*@ -nullderef @*/
if (to_nmea || to_binary) {
if (session.device_type->mode_switcher == NULL) {
gpsd_report(LOG_SHOUT,
"%s devices have no mode switch.\n",
session.device_type->type_name);
status = 1;
} else {
int target_mode = to_nmea ? MODE_NMEA : MODE_BINARY;
int target_type = to_nmea ? NMEA_PACKET : session.device_type->packet_type;
gpsd_report(LOG_SHOUT,
"switching to mode %s.\n",
to_nmea ? "NMEA" : "BINARY");
session.device_type->mode_switcher(&session, target_mode);
/*
* Hunt for packet type again (mode might have
* changed). We've found by experiment that you can't
* close the connection to the device after a mode
* change but before you see a packet of the right
* type come back from it - otherwise you can hit a
* timing window where the mode-change control message
* gets ignored or flushed.
*/
if (!echo) {
/* suppresses probing for subtypes */
context.readonly = true;
(void)sleep(1);
(void) alarm(timeout);
for (;;) {
if (get_packet(&session) == ERROR_SET) {
continue;
} else if (session.packet.type == target_type) {
(void)alarm(0);
break;
}
}
context.readonly = false;
}
/*@ -nullpass @*/
gpsd_report(LOG_SHOUT, "after mode change, %s looks like a %s at %d.\n",
device, gpsd_id(&session), session.gpsdata.dev.baudrate);
/*@ +nullpass @*/
}
}
if (speed) {
char parity = echo ? 'N': session.gpsdata.dev.parity;
int stopbits = echo ? 1 : session.gpsdata.dev.stopbits;
char *modespec;
modespec = strchr(speed, ':');
/*@ +charint @*/
status = 0;
if (modespec!=NULL) {
*modespec = '\0';
if (strchr("78", *++modespec) == NULL) {
gpsd_report(LOG_ERROR, "No support for that word lengths.\n");
status = 1;
}
parity = *++modespec;
if (strchr("NOE", parity) == NULL) {
gpsd_report(LOG_ERROR, "What parity is '%c'?\n", parity);
status = 1;
}
stopbits = *++modespec;
if (strchr("12", parity) == NULL) {
gpsd_report(LOG_ERROR, "Stop bits must be 1 or 2.\n");
status = 1;
}
stopbits = (int)(stopbits-'0');
}
if (status == 0) {
if (session.device_type->speed_switcher == NULL) {
gpsd_report(LOG_ERROR,
"%s devices have no speed switch.\n",
session.device_type->type_name);
status = 1;
}
else if (session.device_type->speed_switcher(&session,
(speed_t)atoi(speed),
parity,
stopbits)) {
/*
* See the 'deep black magic' comment in
* gpsd.c:set_serial() Probably not needed here,
* but it can't hurt.
*/
(void)tcdrain(session.gpsdata.gps_fd);
(void)usleep(50000);
gpsd_report(LOG_PROG, "%s change to %s%c%d succeeded\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
} else {
gpsd_report(LOG_ERROR, "%s driver won't support %s%c%d.\n",
session.gpsdata.dev.path,
speed, parity, stopbits);
status = 1;
}
}
}
if (rate) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->rate_switcher == NULL) {
gpsd_report(LOG_ERROR,
"%s devices have no rate switcher.\n",
session.device_type->type_name);
status = 1;
} else {
double rate_dbl = strtod(rate, NULL);
if (!session.device_type->rate_switcher(&session, rate_dbl)) {
gpsd_report(LOG_ERROR, "rate switch failed.\n");
status = 1;
}
}
context.readonly = write_enable;
}
#endif /* ALLOW_RECONFIGURE */
#ifdef ALLOW_CONTROLSEND
/*@ -compdef @*/
if (control) {
bool write_enable = context.readonly;
context.readonly = false;
if (session.device_type->control_send == NULL) {
gpsd_report(LOG_ERROR,
"%s devices have no control sender.\n",
session.device_type->type_name);
status = 1;
} else {
if (session.device_type->control_send(&session,
cooked,
(size_t)cooklen) == -1) {
gpsd_report(LOG_ERROR, "control transmission failed.\n");
status = 1;
}
}
context.readonly = write_enable;
}
/*@ +compdef @*/
#endif /* ALLOW_CONTROLSEND */
if (forcetype == NULL || !echo) {
/*
* Give the device time to settle before closing it. Alas, this is
* voodoo programming; we don't know it will have any effect, but
* GPSes are notoriously prone to timing-dependent errors.
*/
(void)usleep(300000);
gpsd_wrap(&session);
}
exit(status);
/*@ +nullderef @*/
/*@ +mustfreeonly +immediatetrans @*/
}
}
void gps_poll() {
while (gps_can_read()) {
int ch = gps_read();
switch (decoder_state) {
case GPS_HEADER1:
if (ch == 0xa0)
decoder_state = GPS_HEADER2;
else {
debug("Got garbage char "); debug_int(ch); debug(" from GPS\r\n");
}
break;
case GPS_HEADER2:
if (ch == 0xa2)
decoder_state = GPS_LENGTH1;
else {
debug("Got garbage char "); debug_int(ch); debug(" from GPS (expecting 2nd header byte)\r\n");
decoder_state = GPS_HEADER1;
}
break;
case GPS_LENGTH1:
gps_payload_len = (ch & 0x7f) << 8;
decoder_state = GPS_LENGTH2;
break;
case GPS_LENGTH2:
gps_payload_len += ch;
gps_payload_remain = gps_payload_len;
if (gps_payload_len > GPS_BUFFER_SIZE - 1) {
debug_int(gps_payload_len); debug(" byte payload too big\r\n");
gps_ignore_payload = 1;
} else {
gps_message_valid = 1;
gps_ignore_payload = 0;
gps_payload_checksum = 0;
gps_payload_ptr = gps_payload;
}
decoder_state = GPS_PAYLOAD;
break;
case GPS_PAYLOAD:
if (!gps_ignore_payload) {
*(gps_payload_ptr++) = ch;
gps_payload_checksum += ch;
}
if (--gps_payload_remain == 0)
decoder_state = GPS_CHECKSUM1;
break;
case GPS_CHECKSUM1:
msg_checksum = ch << 8;
decoder_state = GPS_CHECKSUM2;
break;
case GPS_CHECKSUM2:
msg_checksum += ch;
if (!gps_ignore_payload) {
gps_payload_checksum &= 0x7FFF;
if (gps_payload_checksum != msg_checksum) {
debug("Received checksum "); debug_int(msg_checksum);
debug(" != calculated "); debug_int(gps_payload_checksum);
debug(", discarding message\r\n");
gps_message_valid = 0;
}
}
decoder_state = GPS_TRAILER1;
break;
case GPS_TRAILER1:
if (ch == 0xb0)
decoder_state = GPS_TRAILER2;
else {
debug("GPS got garbage "); debug_int(ch); debug(" (expecting 1st trailer)\r\n");
decoder_state = GPS_HEADER1;
}
break;
case GPS_TRAILER2:
if (ch == 0xb3) {
if (gps_message_valid)
gps_handle_message();
} else {
debug("GPS Got garbage "); debug_int(ch); debug(" (expecting 2nd trailer\r\n");
}
decoder_state = GPS_HEADER1;
break;
default:
debug("GPS decoder in unknown state?\r\n");
decoder_state = GPS_HEADER1;
}
}
}
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;
}
/**
* 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 main( int argc, char **argv )
{
// Enforce proper command line arguments.
if( argc != 2 )
{
fprintf( stderr, "Usage: %s <hostname>\n", argv[0] );
exit( EXIT_FAILURE );
}
// Connect to remote host.
int sockfd = createConnection( argv[1], PORT_NUMBER );
if( sockfd < 0 )
{
fprintf( stderr, "Couldn't connect to %s.\n", argv[1] );
exit( EXIT_FAILURE );
}
else
{
printf( "Connected to %s\n.", argv[0] );
}
// Connect to gpsd.
if( gps_open( "localhost", DEFAULT_GPSD_PORT, &gpsData ) != 0 )
{
fprintf( stderr, "Couldn't connect to gpsd, errno = %d, %s.\n", errno, gps_errstr( errno ) );
exit( EXIT_FAILURE );
}
else
{
printf( "Connected to gpsd.\n" );
}
// Register for updates from gpsd.
gps_stream( &gpsData, WATCH_ENABLE | WATCH_JSON, NULL );
printf( "Waiting for gps lock." );
for(;;)
{
if( !gps_waiting( &gpsData, 5000000 ) )
{
fprintf( stderr, "GPS fix timed out.\n" );
exit( EXIT_FAILURE );
}
else
{
if( gps_read( &gpsData ) == -1 )
{
fprintf( stderr, "gps_read() error, errno = %d\n", errno );
}
else
{
if( isnan( gpsData.fix.latitude ) || isnan( gpsData.fix.longitude ) )
{
fprintf( stderr, "Bad GPS fix.\n" );
}
else
{
printf( "Latitude: %f\n", gpsData.fix.latitude );
printf( "Longitude: %f\n", gpsData.fix.longitude );
}
}
}
}
close( sockfd );
return 0;
}
int main(){
int gps_recv;
struct gps_data_t gps_data;
struct timeval tv;
struct tm *tm_info;
FILE *gps_file_ptr;
int millisec;
char file_path[] = "/home/odroid/Canon_camera/Canon_gps_log.csv";
/* Open GPS device (GPSD) */
gps_recv = gps_open("localhost", "2947", &gps_data);
if (gps_recv == -1) {
printf("code: %d, reason %s\n", gps_recv, gps_errstr(gps_recv) );
}
/* Start streaming GPS data */
gps_stream(&gps_data, WATCH_ENABLE, NULL);
char lat_marker;
char lng_marker;
double latitude;
double longitude;
double latitude_min;
double longitude_min;
gps_file_ptr = fopen (file_path, "a");
int i;
for (i=1;i<=50;i++) {
if (gps_waiting(&gps_data, GPS_WAIT)) {
/* Then GPS data is available, so read the data */
gps_recv = gps_read(&gps_data);
/* printf("%d\n", gps_recv); */
/* Check for error in reading data */
if (gps_recv == -1) {
printf("Error reading GPS. code: %d, reason %s\n", gps_recv, gps_errstr(gps_recv) );
}
else {
/* No problems in reading data, check for GPS fix*/
/* printf("No errors %d -- fix: %d\n", MODE_3D, gps_data.fix.mode); */
if ( (gps_data.status == STATUS_FIX) && (gps_data.fix.mode == MODE_2D || gps_data.fix.mode == MODE_3D) && !isnan(gps_data.fix.latitude) && !isnan(gps_data.fix.longitude) ) {
latitude = fabs(gps_data.fix.latitude);
longitude = fabs(gps_data.fix.longitude);
latitude_min = (latitude - (int) latitude)*60;
longitude_min = (longitude - (int) longitude)*60;
if (gps_data.fix.latitude >= 0 ) { lat_marker = 'N'; }
else { lat_marker = 'S'; }
if (gps_data.fix.longitude >= 0 ) { lng_marker = 'E'; }
else { lng_marker = 'W'; }
printf("%f %c, %f %c \n", fabs(gps_data.fix.latitude), lat_marker, fabs(gps_data.fix.longitude), lng_marker);
/* Write timestamp in YYYY-MM-DD_HH-MM-SS-sss */
gettimeofday(&tv, NULL);
char time_buffer[26];
millisec = (long) (tv.tv_usec/1000.0 + 0.5); // Round to nearest millisec
tm_info = localtime(&tv.tv_sec);
strftime(time_buffer, 26, "%Y-%m-%d_%H-%M-%S-", tm_info);
fprintf(gps_file_ptr, "%s", time_buffer);
fprintf(gps_file_ptr, "%03d,", millisec);
/* Write latititude and longitude in DD-MM-SS.sss */
fprintf(gps_file_ptr, "%d-%d-%f,%c,", (int) latitude, (int) latitude_min, (latitude_min - (int) latitude_min )*60, lat_marker);
fprintf(gps_file_ptr, "%d-%d-%f,%c,", (int) longitude, (int) longitude_min, (longitude_min - (int) longitude_min )*60, lng_marker);
/* Write Altitude, Heading */
if (gps_data.fix.mode == MODE_3D) {
fprintf(gps_file_ptr, "%f,%f\n", gps_data.fix.altitude, gps_data.fix.track);
}
else {
/* if not 3D fix, altitude is not defined */
fprintf(gps_file_ptr, "%d,%f\n", 0, gps_data.fix.track);
}
/* fprintf(gps_file_ptr, "%f%c,%f%c\n", fabs(gps_data.fix.latitude), lat_marker, fabs(gps_data.fix.longitude), lng_marker); */
}
}
}
else {
printf("-\n");
/* printf("\n ----- Did not wait long enough ----- \n");*/
}
}
/* Close logging file */
fclose(gps_file_ptr);
/* Stop streaming data and close GPS connection */
gps_stream(&gps_data, WATCH_DISABLE, NULL);
gps_close(&gps_data);
printf("GPS -- %d\n", gps_recv);
return 0;
}
void task_GPS(void) {
user_debug_msg(DBG_MSG, STR_TASK_GPS ": Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
while (1) {
gps_update();
//Display current latitude, longitude, velocity and heading
#if 1
sprintf(str_tmp, STR_TASK_GPS ": %09.2f,%9.4f,%10.4f,%6.1f,%6.1f,%4.1f,%7.3f,%7.2f,%7.2f,%u,%u,%u,%2X,%u,%u,%u,%u",
gps_read().time,gps_read().latitude,gps_read().longitude,gps_read().altitude,gps_read().geoid,
gps_read().hdop,gps_read().speed,gps_read().heading,gps_read().mag_var,gps_read().num_sat,
gps_read().stationID,gps_read().date,gps_read().fixflag,
gps_read().rmc_update,gps_read().gga_update,gps_read().rmc_count,gps_read().gga_count);
user_debug_msg(DBG_MSG, str_tmp);
#endif
// Note that this is not in keeping with all the other TAPS -- this should be driven by TAP_delay, etc.
OS_Delay(500);
}
}
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;
}
int
gps_recv(gps_handle gps, int to, u_char *buf, int * cnt)
{
int dle_seen;
int etx_seen;
int sum;
int len;
int rlen = -1;
u_char *ptr;
int stat;
/* sync to the first DLE to come down the pike. */
while (1) {
do {
stat = gps_read(gps, buf, to);
} while ((stat == 1) && (*buf != dle));
/* We have a timeout or a frame (or possibly the middle or
end of a packet). If a timeout return a -1, otherwise
prepare to receive the rest of the frame */
switch (stat) {
case -1:
gps_printf(gps, 2, "%s: sync error\n", __func__);
return -1;
case 0:
gps_printf(gps, 2, "%s: timeout\n", __func__);
return 0;
case 1:
break;
}
/* start receiving characters into buf. An end of buffer or
a DLE ETX sequence will terminate the reception. Each
read is given a READ_TO second timeout -- if we time out
assume the gps died and return an error. */
ptr = buf;
dle_seen = 0;
etx_seen = 0;
sum = 0;
len = 0;
do {
stat = gps_read(gps, ptr, READ_TO);
if (stat != 1) {
gps_printf(gps, 2, "%s: frame error\n", __func__);
return -1;
}
if (dle_seen) {
if (*ptr == etx) {
etx_seen = 1;
break;
}
dle_seen = 0;
} else {
if (*ptr == dle) {
dle_seen = 1;
continue;
}
}
if ((rlen == -1) && (len == 1)) {
/* this is the length byte, add it to the
checksum and save it, but do not keep it
in the buffer */
sum += *ptr;
rlen = *ptr;
} else {
sum += *ptr++;
len += 1;
}
} while (len < *cnt);
if (etx_seen) {
/* subtract one from the length as we don't count the
checksum. */
len -= 1;
/* warn if the length is not the expected value.
Add in the packet type to the expected length. */
rlen += 1;
if (rlen != len)
gps_printf(gps, 1, "%s: bad frame len, "
"%d expected, %d received\n",
__func__, rlen, len);
if ((sum & 0xff) == 0) {
/* good checksum, update len rcvd and return */
*cnt = len;
if (gps_debug(gps) >= 4)
gps_display('{', buf, len);
return 1;
} else {
/* bad checksum -- try again */
if (gps_debug(gps) >= 4)
gps_display('!', buf, len);
return -1;
}
} else {
/* frame too large, return error */
gps_printf(gps, 1,
"%s: frame too large for %d byte buffer\n",
__func__, *cnt);
return -1;
}
}
}
/** \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;
}
/******************************************************************************
**** ****
** **
task_monitor()
This task proves the proper functioning of the passthrough feature of the
GPSRM 1. Passthrough connects the OEM615V's COM1 port to one of three pairs
of IO lines on the CSK bus: IO.[5,4], IO.[17,16] or IO.[33,32]. Which pair
is implemented depends on the GPSRM 1's Assembly Revision (ASSY REV).
task_gps() initially configures the GPSRM 1 with passthrough enabled, but
without any logging active in the OEM615V.
task_monitor() (re-)starts once commanded to by task_gps() ... this happens
after all the basic tests (I2C working, can power OEM615V on and off) are
concluded, the OEM615V is on and not in reset, and is ready to talk via COM1.
task_monitor() then commands the OEM615V to start logging, and then scans
the resulting NMEA strings from the OEM615V for valid GPS fix and GPS data.
** **
**** ****
******************************************************************************/
void task_monitor(void) {
// Initially we're stopped, waiting for task_gps to configure the GPSRM1 and OEM615V.
user_debug_msg(STR_TASK_MONITOR "Stopped.");
OS_Stop();
// Eventually task_gps() starts this task.
user_debug_msg(STR_TASK_MONITOR "Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
// Now that we're ready for NMEA messages from the OEM615V, tell it to
// start logging them at 1Hz on its COM1.
// We need to send this out to all three possible paths into the GPSRM 1's
// passthrough port.
// Don't forget to terminate the string with CRLF!
csk_uart1_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart2_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart3_puts("LOG COM1 GPGGA ONTIME 1\r\n");
// Additionally, tell the OEM615V to output a pulsetrain (2ms @ 125kHz) via the VARF output
csk_uart1_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart2_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart3_puts("FREQUENCYOUT ENABLE 200 800\r\n");
// Wait a few seconds for the NMEA buffers to fill ...
OS_Delay(200);
OS_Delay(200);
// We remain here forever, parsing NMEA strings from the OEM615 for GPS status
// and data. While this is happening (and after a GPS fix has been achieved),
// it's a good time to test disconnecting and reconnecting the GPS antenna from
// the OEM615V, to see that its GPS Position Valid LED goes out when the
// antenna is disconnected.
while(1) {
// Update gps values from incoming NMEA strings.
gps_update();
// Display current sats in view, HDOP, altitude, latitude & longitude if we have a fix.
if((gps_read().fixflag&gps_fix)||(gps_read().fixflag&diffgps_fix)) {
sprintf(strTmp, STR_TASK_MONITOR " GMT Sat HDOP Alt. Latitude Longitude\r\n" \
"\t\t\t\t-----------------------------------------------\r\n" \
"\t\t\t\t%s %02u %3.1f %6.1fm %8.4f %8.4f\r\n",
gps_NMEA_GMT_time_hhmmss(), gps_read().num_sat, gps_read().hdop, gps_read().altitude, gps_read().latitude, gps_read().longitude);
} /* if() */
// O/wise indicate that we do not have a fix.
else {
sprintf(strTmp, STR_TASK_MONITOR "No valid GPS position -- check antenna.\r\n");
} /* else() */
user_debug_msg(strTmp);
// Repeat every 5s.
OS_Delay(250);
OS_Delay(250);
} /* while() */
} /* task_monitor() */
static gboolean gpsd_data_cb(GIOChannel *src, GIOCondition condition, gpointer data) {
int ret;
char pbuf[256];
time_t now;
struct tm *ts;
char buf[80];
ret = gps_read(&gpsdata);
if (!isnan(gpsdata.fix.latitude)) {
lat = gpsdata.fix.latitude;
lon = gpsdata.fix.longitude;
}
if (!isnan(gpsdata.fix.speed)) speed=gpsdata.fix.speed*2.24;
if (!isnan(gpsdata.fix.track)) track=gpsdata.fix.track;
if ((beacon_time==0) || (secs_since_beacon<0)) {
printf("init beacon time\n");
beacon_time=gtime;
}
if(!isnan(gpsdata.fix.time)) gtime = gpsdata.fix.time;
if (gtime>next_time) {
printf("3 min point\n");
next_time = gtime+180;
}
#if 0
printf("\n===============================================\n");
printf("ssb: %f\ntime until beacon: %f\nbeacon_rate: %f\n", secs_since_beacon, beacon_rate-secs_since_beacon, beacon_rate);
printf("track: %f\noldtrack: %f\n", track, old_track);
printf("hcsb: %f\ntt: %f\nmtt: %f\n", hcsb, turn_threshold, min_turn_time);
#endif
// calculate smart beacon
secs_since_beacon = gtime-beacon_time;
if (speed < slow_speed) {
//printf("low speed = %f\n", speed);
beacon_rate = slow_rate;
} else {
if (speed > high_speed) {
//printf("high speed = %f\n", speed);
beacon_rate = fast_rate;
} else {
beacon_rate = fast_rate * high_speed/speed;
}
if (old_track == -1) old_track = track;
hcsb = fabs(track - old_track);
if (hcsb > 180) hcsb = 360 - hcsb;
turn_threshold = min_turn_angle + turn_slope / speed;
if (hcsb > turn_threshold && secs_since_beacon > min_turn_time) {
secs_since_beacon = beacon_rate;
}
}
if (secs_since_beacon >= beacon_rate) {
//printf("***************************************** BEACON\n");
printf("smart beacon\n");
now = (time_t) gtime;
ts = gmtime(&now);
strftime(buf, sizeof(buf), "%d%H%Mz", ts);
// char buf[256]="MM0YEQ-11>APZGJC:@112149z5551.30N/00430.60W$118/032/moving\r\n";
float latmin, lonmin;
latmin = (lat - trunc(lat))*60;
lonmin = -(lon - trunc(lon))*60;
printf("%f %f\n", lat, lon);
sprintf(pbuf, "MM0YEQ-10>APZGJC:@%s%02.0f%05.2fN/%03.0f%05.2fW$%03.0f/%03.0f/message\n", buf, trunc(lat),latmin,-trunc(lon),lonmin, track, speed);
if(&ctx !=0) {
printf("sending\n");
aprsis_write(&ctx, pbuf, strlen(pbuf));
}
printf("pbuf=%s\n", pbuf);
beacon_time = gtime;
old_track=track;
}
return TRUE;
}
