summaryrefslogtreecommitdiff
path: root/src/venus_gps.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/venus_gps.c')
-rw-r--r--src/venus_gps.c420
1 files changed, 270 insertions, 150 deletions
diff --git a/src/venus_gps.c b/src/venus_gps.c
index 4a02096..1f317a0 100644
--- a/src/venus_gps.c
+++ b/src/venus_gps.c
@@ -21,6 +21,8 @@
*
*/
+#define _GNU_SOURCE
+
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
@@ -36,6 +38,7 @@
#include <getopt.h>
#include <time.h>
#include <signal.h>
+#include <string.h>
#include "log.h"
#include "cbuffer.h"
@@ -44,23 +47,18 @@
#include "venus_api.h"
static sig_atomic_t timer_expired;
-static void sigalarm_handler(int arg) {
+static void sigalarm_handler(int arg)
+{
timer_expired = 1;
}
static sig_atomic_t sigpipe = 0;
-static void sigpipe_handler(int arg) {
+static void sigpipe_handler(int arg)
+{
sigpipe = 1;
}
-enum {
- MODE_SERVER = 0,
- MODE_CLIENT = 1,
-};
-
static char *device = "/dev/ttyS3";
-static char remote_host[256];
-static int port = 5445;
static speed_t baud_rate = B9600;
static uint8_t gga = 1;
@@ -71,17 +69,20 @@ static uint8_t rmc = 1;
static uint8_t vtg = 1;
static uint8_t zda = 1;
+#if CONFIG_CAN_RESET
/*
* XXX: I don't know if these work, yet. I haven't tried them out.
*
*/
-static uint8_t start_mode = (uint8_t) -1;
+static uint8_t start_mode = (uint8_t) - 1;
static int16_t latitude = 45;
static int16_t longitude = -93;
static int16_t altitude = 256;
+#endif
-static int tcp_exchange_data(int sd, int tty) {
+static int exchange_data(int fd0, int fd1)
+{
int i;
int err;
struct pollfd fds[2];
@@ -92,57 +93,59 @@ static int tcp_exchange_data(int sd, int tty) {
memset(fds, 0, sizeof(fds));
- for(i = 0; i < nfds; i++) {
+ for (i = 0; i < nfds; i++) {
memset(&fds[i], 0, sizeof(fds[i]));
bufs[i].read_index = bufs[i].write_index = 0;
}
- fds[0].fd = tty;
- fds[1].fd = sd;
+ fds[0].fd = fd0;
+ fds[1].fd = fd1;
- while(1) {
- for(i = 0; i < nfds; i++) {
+ while (1) {
+ for (i = 0; i < nfds; i++) {
fds[i].events = 0;
}
- for(i = 0; i < nfds; i++) {
- if(buffer_free(&bufs[i])) {
+ for (i = 0; i < nfds; i++) {
+ if (buffer_free(&bufs[i])) {
fds[i].events |= POLLIN;
}
- if(buffer_used(&bufs[i])) {
+ if (buffer_used(&bufs[i])) {
fds[DIR_REV(i)].events |= POLLOUT;
}
}
err = poll(fds, nfds, -1);
- if(err <= 0) {
- log_error("poll returned %d errno: %d", err, errno);
+ if (err <= 0) {
+ log_error("poll returned %d %m", err);
break;
}
log_debug("poll returned %d", err);
- for(i = 0; i < nfds; i++) {
- if(fds[i].revents) {
- log_debug("fds[%d]: %d revents: %04X POLLIN: %04X POLLOUT: %04X POLLHUP: %04X", i, fds[i].fd, fds[i].revents, POLLIN, POLLOUT, POLLHUP);
+ for (i = 0; i < nfds; i++) {
+ if (fds[i].revents) {
+ log_debug("fds[%d]: %d revents: %04X", i, fds[i].fd, fds[i].revents);
- if(fds[i].revents & POLLHUP) {
+ if (fds[i].revents & POLLHUP) {
log_error("fds[%d]: %d POLLHUP", i, fds[i].fd);
return 0;
- } else if(fds[i].revents & POLLOUT) {
- log_debug("fds[%d]: %d POLLOUT buffer_used: %lu", i, fds[i].fd, (unsigned long) buffer_used(&bufs[DIR_REV(i)]));
+ } else if (fds[i].revents & POLLOUT) {
+ log_debug("fds[%d]: %d POLLOUT buffer_used: %lu",
+ i, fds[i].fd, (unsigned long) buffer_used(&bufs[DIR_REV(i)]));
err = buffered_write(fds[i].fd, &bufs[DIR_REV(i)]);
- if(err < 0) {
- log_error("fds[%d]: %d safe_write failed: errno: %d", i, fds[i].fd, errno);
+ if (err < 0) {
+ log_error("fds[%d]: %d safe_write failed: %m", i, fds[i].fd);
return -1;
}
- } else if(fds[i].revents & POLLIN) {
- log_debug("fds[%d]: %d POLLIN buffer_free: %lu", i, fds[i].fd, (unsigned long) buffer_free(&bufs[i]));
+ } else if (fds[i].revents & POLLIN) {
+ log_debug("fds[%d]: %d POLLIN buffer_free: %lu",
+ i, fds[i].fd, (unsigned long) buffer_free(&bufs[i]));
err = buffered_read(fds[i].fd, &bufs[i]);
- if(err <= 0) {
- log_debug("fds[%d]: %d safe_read failed: errno: %d", i, fds[i].fd, errno);
+ if (err <= 0) {
+ log_debug("fds[%d]: %d safe_read failed: %m", i, fds[i].fd);
return -1;
}
}
@@ -153,20 +156,21 @@ static int tcp_exchange_data(int sd, int tty) {
return -1;
}
-static int udp_send_msgs(int sd, int tty) {
+static int udp_send_msgs(int sd, int tty)
+{
char buf[NMEA_SENTENCE_MAX];
int count;
int err;
- while(1) {
+ while (1) {
count = read_nmea_sentence(tty, buf, sizeof(buf));
- if(count <= 0) {
+ if (count <= 0) {
log_error("read_nmea_sentence failed: quiting");
return -1;
}
err = send(sd, buf, count, 0);
- if(err < 0) {
- log_debug("send failed: %d", errno);
+ if (err < 0) {
+ log_debug("send failed: %m");
return -1;
}
}
@@ -174,22 +178,17 @@ static int udp_send_msgs(int sd, int tty) {
return -1;
}
-static int udp_server() {
- log_error("UDP Server mode is not implemented");
- exit(1);
- return -1;
-}
-
-static int udp_client() {
+static int gps_to_udp_client(const char *host, int port)
+{
int tty;
int sd;
- log_notice("Venus GPS UDP Client connecting to %s:%d", remote_host, port);
+ log_notice("Venus GPS UDP Client connecting to %s:%d", host, port);
- while(1) {
- sd = inet_conn_str(remote_host, port, SOCK_DGRAM);
- if(sd < 0) {
- log_error("inet_conn_str failed: %d", errno);
+ while (1) {
+ sd = inet_conn_str(host, port, SOCK_DGRAM);
+ if (sd < 0) {
+ log_error("inet_conn_str failed: %m");
sleep(60);
} else {
log_debug("connection opened");
@@ -208,16 +207,17 @@ static int udp_client() {
return -1;
}
-static int tcp_client() {
+static int gps_to_tcp_client(const char *host, int port)
+{
int tty;
int sd;
- log_notice("Venus GPS TCP Client connecting to %s:%d", remote_host, port);
+ log_notice("Venus GPS TCP Client connecting to %s:%d", host, port);
- while(1) {
- sd = inet_conn_str(remote_host, port, SOCK_STREAM);
- if(sd < 0) {
- log_error("inet_conn_str failed: %d", errno);
+ while (1) {
+ sd = inet_conn_str(host, port, SOCK_STREAM);
+ if (sd < 0) {
+ log_error("inet_conn_str failed: %m");
sleep(60);
} else {
log_debug("connection opened");
@@ -227,7 +227,7 @@ static int tcp_client() {
set_nonblocking(tty);
set_nonblocking(sd);
- tcp_exchange_data(sd, tty);
+ exchange_data(sd, tty);
close(sd);
close(tty);
@@ -237,7 +237,8 @@ static int tcp_client() {
return -1;
}
-static int tcp_server() {
+static int gps_to_tcp_server(int port)
+{
int err;
int tty;
int lsd;
@@ -250,8 +251,8 @@ static int tcp_server() {
struct sockaddr_in cli_addr;
lsd = socket(PF_INET, SOCK_STREAM, 0);
- if(lsd == -1) {
- log_error("socket failed: %d", errno);
+ if (lsd == -1) {
+ log_error("socket failed: %m");
exit(1);
}
@@ -261,33 +262,33 @@ static int tcp_server() {
serv_addr.sin_port = htons(port);
err = setsockopt(lsd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
- if(err == -1) {
- log_error("setsockopt failed: %d", errno);
+ if (err == -1) {
+ log_error("setsockopt failed: %m");
exit(1);
}
serv_addr_len = sizeof(serv_addr);
-
+
err = bind(lsd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
- if(err == -1) {
- log_error("bind failed: %d", errno);
+ if (err == -1) {
+ log_error("bind failed: %m");
exit(1);
}
err = listen(lsd, 1);
- if(err == -1) {
- log_error("listen failed: %d", errno);
+ if (err == -1) {
+ log_error("listen failed: %m");
exit(1);
}
log_notice("Venus GPS TCP Server listening on port %d", port);
- while(1) {
+ while (1) {
cli_addr_len = sizeof(cli_addr);
- sd = accept(lsd, (struct sockaddr *) &cli_addr, (socklen_t *) &cli_addr_len);
- if(sd == -1) {
- log_error("accept failed: %d", errno);
+ sd = accept(lsd, (struct sockaddr *) &cli_addr, (socklen_t *) & cli_addr_len);
+ if (sd == -1) {
+ log_error("accept failed: %m");
} else {
log_debug("accepted");
@@ -296,7 +297,7 @@ static int tcp_server() {
set_nonblocking(tty);
set_nonblocking(sd);
- tcp_exchange_data(sd, tty);
+ exchange_data(sd, tty);
close(sd);
close(tty);
@@ -308,7 +309,88 @@ static int tcp_server() {
return -1;
}
-static void print_version(const char *name) {
+static int gps_to_file(const char *file)
+{
+ int tty;
+ int fd;
+
+ log_notice("Venus GPS to file %s", file);
+
+ if (!strcmp(file, "-")) {
+ fd = fileno(stdout);
+ } else {
+ fd = open(file, O_WRONLY);
+ if (fd < 0) {
+ log_error("failed to open %s: %m", file);
+ return -1;
+ }
+ }
+
+ set_nonblocking(fd);
+
+ while (1) {
+ tty = venus_open(device, baud_rate);
+
+ set_nonblocking(tty);
+
+ exchange_data(fd, tty);
+
+ close(tty);
+ }
+
+ return -1;
+}
+
+int gps_to_serial(const char *port)
+{
+ int tty;
+ int fd;
+ struct termios tio;
+
+ log_notice("Venus GPS to serial %s", port);
+
+ fd = open(port, O_RDWR | O_NOCTTY);
+ if (fd < 0) {
+ log_error("failed to open %s: %m", port);
+ return -1;
+ }
+
+ tcgetattr(fd, &tio);
+ cfmakeraw(&tio);
+ cfsetspeed(&tio, B115200);
+
+ tio.c_cflag &= ~CSIZE;
+ tio.c_cflag |= CS8;
+
+ tio.c_cflag &= ~(PARENB | PARODD);
+ tio.c_cflag |= 0;
+
+ tio.c_cflag &= ~CSTOPB;
+ tio.c_cflag |= 0;
+
+ tio.c_cflag &= ~CRTSCTS;
+ tio.c_cflag |= 0;
+
+ tcsetattr(fd, TCSANOW, &tio);
+ tcflush(fd, TCIOFLUSH);
+
+ set_nonblocking(fd);
+
+ while (1) {
+ tty = venus_open(device, baud_rate);
+
+ set_nonblocking(tty);
+
+ exchange_data(fd, tty);
+
+ close(tty);
+ }
+
+ return -1;
+}
+
+static void print_version(const char *name)
+{
printf("%s (" PACKAGE ") " VERSION " (" __DATE__ " " __TIME__ ")\n", name);
printf("Copyright (C) 2010 by Multi-Tech Systems\n");
printf(
@@ -317,15 +399,17 @@ static void print_version(const char *name) {
"This program has absolutely no warranty.\n");
}
-static void usage(FILE *out) {
+static void usage(FILE * out)
+{
fprintf(out, "usage: venus-gps [ OPTIONS ... ]\n");
fprintf(out, "where OPTIONS := { \n");
fprintf(out, " --daemonize |\n");
fprintf(out, " -d --device <device> (default: /dev/ttyS3) |\n");
- fprintf(out, " -p --port <port> (default: 5445) |\n");
- fprintf(out, " --protocol { tcp (default) | udp } |\n");
- fprintf(out, " --mode { client | server (default) } |\n");
- fprintf(out, " --remote-host <host> |\n");
+ fprintf(out, " --file <file> |\n");
+ fprintf(out, " --tcp-server |\n");
+ fprintf(out, " --tcp-client <host> |\n");
+ fprintf(out, " --udp-client <host> |\n");
+ fprintf(out, " --serial <device> |\n");
#if CONFIG_CAN_DEFAULT
fprintf(out, " -f --factory-defaults |\n");
#endif
@@ -351,9 +435,9 @@ static void usage(FILE *out) {
}
#define OPT_DEVICE 'd'
+#define OPT_BAUD_RATE 'b'
#define OPT_PORT 'p'
#define OPT_FACTORY_DEFAULTS 'f'
-#define OPT_BAUD_RATE 'b'
enum {
OPT_VERSION = 128,
@@ -369,21 +453,25 @@ enum {
OPT_LATITUDE,
OPT_LONGITUDE,
OPT_ALTITUDE,
- OPT_REMOTE_HOST,
OPT_DAEMONIZE,
- OPT_PROTOCOL,
- OPT_MODE,
+ OPT_FILE,
+ OPT_TCP_SERVER,
+ OPT_TCP_CLIENT,
+ OPT_UDP_CLIENT,
+ OPT_SERIAL,
};
static char *short_options = "b:d:p:f";
static struct option long_options[] = {
- {"protocol", 1, 0, OPT_PROTOCOL},
{"daemonize", 0, 0, OPT_DAEMONIZE},
{"device", 1, 0, OPT_DEVICE},
- {"mode", 1, 0, OPT_MODE},
- {"remote-host", 1, 0, OPT_REMOTE_HOST},
- {"port", 1, 0, OPT_PORT},
{"baud-rate", 1, 0, OPT_BAUD_RATE},
+ {"port", 1, 0, OPT_PORT},
+ {"file", 1, 0, OPT_FILE},
+ {"tcp-server", 0, 0, OPT_TCP_SERVER},
+ {"tcp-client", 1, 0, OPT_TCP_CLIENT},
+ {"udp-client", 1, 0, OPT_UDP_CLIENT},
+ {"serial", 1, 0, OPT_SERIAL},
#if CONFIG_CAN_DEFAULT
{"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS},
#endif
@@ -407,14 +495,27 @@ static struct option long_options[] = {
{0, 0, 0, 0},
};
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[])
+{
int i;
+ int tmp;
+ char *cp;
int option_index;
int tty;
+#if CONFIG_CAN_DEFAULT
int factory_defaults = 0;
+#endif
int daemonize = 0;
- int proto = IPPROTO_TCP;
- int mode = MODE_SERVER;
+ int port = 5445;
+
+ char *file = "-";
+ char tcp_server_enabled = 0;
+ int tcp_server_port = port;
+ char *tcp_client_host = NULL;
+ int tcp_client_port = port;
+ char *udp_client_host = NULL;
+ int udp_client_port = port;
+ char *serial_port = NULL;
struct sigaction sa;
@@ -428,56 +529,83 @@ int main(int argc, char *argv[]) {
sa.sa_flags = 0;
sigaction(SIGPIPE, &sa, NULL);
- while((i = getopt_long(argc, argv, short_options, long_options, &option_index)) >= 0) {
- switch(i) {
+ while ((i = getopt_long(argc, argv, short_options, long_options, &option_index)) >= 0) {
+ switch (i) {
case 0:
break;
- case OPT_PROTOCOL:
- if(!strcasecmp("tcp", optarg)) {
- proto = IPPROTO_TCP;
- } else if(!strcasecmp("udp", optarg)) {
- proto = IPPROTO_UDP;
- } else {
- log_error("invalid transport protocol");
- usage(stderr);
- exit(1);
- }
- break;
-
case OPT_DAEMONIZE:
daemonize = 1;
break;
- case OPT_MODE:
- if(!strcasecmp("server", optarg)) {
- mode = MODE_SERVER;
- } else if(!strcasecmp("client", optarg)) {
- mode = MODE_CLIENT;
- } else {
- log_error("invalid mode");
- usage(stderr);
- exit(1);
- }
- break;
-
case OPT_DEVICE:
device = optarg;
break;
case OPT_PORT:
port = atoi(optarg);
- if(port <= 0 || port >= (1 << 16)) {
+ if (port <= 0 || port >= (1 << 16)) {
log_error("invalid port");
usage(stderr);
exit(1);
}
break;
+ case OPT_FILE:
+ file = optarg;
+ break;
+
+ case OPT_SERIAL:
+ serial_port = optarg;
+ break;
+
+ case OPT_TCP_SERVER:
+ tcp_server_enabled = 1;
+ tcp_server_port = port;
+ break;
+
+ case OPT_TCP_CLIENT:
+ cp = strchr(optarg, ':');
+ if (cp) {
+ tcp_client_host = strndup(optarg, cp - optarg);
+
+ cp++;
+
+ tcp_client_port = atoi(cp);
+ if (tcp_client_port <= 0 || tcp_client_port >= (1 << 16)) {
+ log_error("invalid port");
+ usage(stderr);
+ exit(1);
+ }
+ } else {
+ tcp_client_host = strdup(optarg);
+ tcp_client_port = port;
+ }
+ break;
+
+ case OPT_UDP_CLIENT:
+ cp = strchr(optarg, ':');
+ if (cp) {
+ udp_client_host = strndup(optarg, cp - optarg);
+
+ cp++;
+
+ udp_client_port = atoi(cp);
+ if (udp_client_port <= 0 || udp_client_port >= (1 << 16)) {
+ log_error("invalid port");
+ usage(stderr);
+ exit(1);
+ }
+ } else {
+ udp_client_host = strdup(optarg);
+ udp_client_port = port;
+ }
+ break;
+
case OPT_BAUD_RATE:
baud_rate = atoi(optarg);
baud_rate = value_to_baud(baud_rate);
- if(baud_rate == (speed_t) -1) {
+ if (baud_rate == (speed_t) - 1) {
usage(stderr);
exit(1);
}
@@ -537,10 +665,6 @@ int main(int argc, char *argv[]) {
break;
#endif
- case OPT_REMOTE_HOST:
- snprintf(remote_host, sizeof(remote_host), "%s", optarg);
- break;
-
case OPT_VERSION:
print_version("venus-gps");
exit(0);
@@ -557,19 +681,18 @@ int main(int argc, char *argv[]) {
}
}
- if(optind < argc) {
+ if (optind < argc) {
usage(stderr);
exit(1);
}
- if(daemonize) {
+ if (daemonize) {
#if CONFIG_USE_SYSLOG
- daemon(0, 0);
+ tmp = daemon(0, 0);
#else
- daemon(0, 1);
+ tmp = daemon(0, 1);
#endif
}
-
#if CONFIG_USE_SYSLOG
openlog("VenusGPS", LOG_NDELAY, LOG_FACILITY);
# if DEBUG
@@ -580,24 +703,28 @@ int main(int argc, char *argv[]) {
#endif
tty = venus_open(device, baud_rate);
- if(tty < 0) {
- log_error("failed to open tty: errno: %d", errno);
+ if (tty < 0) {
+ log_error("failed to open tty: %m");
exit(1);
}
venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
venus_query_sw_version(tty, SW_TYPE_RESERVED);
- if(factory_defaults) {
+#if CONFIG_CAN_DEFAULT
+ if (factory_defaults) {
venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
log_notice("setting factory defaults");
exit(0);
}
- if(start_mode != (typeof(start_mode)) -1) {
+#endif
+#if CONFIG_CAN_RESET
+ if (start_mode != (typeof(start_mode)) - 1) {
venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude);
log_notice("issuing system restart");
exit(0);
}
+#endif
venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);
@@ -605,26 +732,19 @@ int main(int argc, char *argv[]) {
close(tty);
- if(proto == IPPROTO_TCP) {
- if(mode == MODE_SERVER) {
- tcp_server();
- } else {
- if(!*remote_host) {
- log_notice("remote-host must be specified");
- exit(1);
- }
- tcp_client();
- }
- } else if(proto == IPPROTO_UDP) {
- if(mode == MODE_SERVER) {
- udp_server();
- } else {
- if(!*remote_host) {
- log_notice("remote-host must be specified");
- exit(1);
- }
- udp_client();
- }
+ /*
+ * FIXME: Support more than one mode at a time
+ */
+ if (tcp_server_enabled) {
+ gps_to_tcp_server(tcp_server_port);
+ } else if (tcp_client_host) {
+ gps_to_tcp_client(tcp_client_host, tcp_client_port);
+ } else if (udp_client_host) {
+ gps_to_udp_client(udp_client_host, udp_client_port);
+ } else if (serial_port) {
+ gps_to_serial(serial_port);
+ } else if (file) {
+ gps_to_file(file);
}
return 0;
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-07 15:31:26 +0000