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.c631
1 files changed, 631 insertions, 0 deletions
diff --git a/src/venus_gps.c b/src/venus_gps.c
new file mode 100644
index 0000000..4a02096
--- /dev/null
+++ b/src/venus_gps.c
@@ -0,0 +1,631 @@
+/*
+ * Venus GPS Controller Example
+ *
+ * Copyright (C) 2010 by Multi-Tech Systems
+ *
+ * Author: James Maki <jmaki@multitech.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdarg.h>
+#include <poll.h>
+#include <getopt.h>
+#include <time.h>
+#include <signal.h>
+
+#include "log.h"
+#include "cbuffer.h"
+#include "utils.h"
+#include "venus_gps.h"
+#include "venus_api.h"
+
+static sig_atomic_t timer_expired;
+static void sigalarm_handler(int arg) {
+ timer_expired = 1;
+}
+
+static sig_atomic_t sigpipe = 0;
+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;
+static uint8_t gsa = 1;
+static uint8_t gsv = 1;
+static uint8_t gll = 1;
+static uint8_t rmc = 1;
+static uint8_t vtg = 1;
+static uint8_t zda = 1;
+
+/*
+ * XXX: I don't know if these work, yet. I haven't tried them out.
+ *
+ */
+static uint8_t start_mode = (uint8_t) -1;
+static int16_t latitude = 45;
+static int16_t longitude = -93;
+static int16_t altitude = 256;
+
+
+static int tcp_exchange_data(int sd, int tty) {
+ int i;
+ int err;
+ struct pollfd fds[2];
+ nfds_t nfds = 2;
+ struct cbuffer bufs[2];
+
+ log_debug("starting");
+
+ memset(fds, 0, sizeof(fds));
+
+ 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;
+
+ while(1) {
+ for(i = 0; i < nfds; i++) {
+ fds[i].events = 0;
+ }
+
+ for(i = 0; i < nfds; i++) {
+ if(buffer_free(&bufs[i])) {
+ fds[i].events |= POLLIN;
+ }
+ 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);
+ 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);
+
+ 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)]));
+
+ 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);
+ 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]));
+
+ 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);
+ return -1;
+ }
+ }
+ }
+ }
+ }
+
+ return -1;
+}
+
+static int udp_send_msgs(int sd, int tty) {
+ char buf[NMEA_SENTENCE_MAX];
+ int count;
+ int err;
+
+ while(1) {
+ count = read_nmea_sentence(tty, buf, sizeof(buf));
+ 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);
+ return -1;
+ }
+ }
+
+ return -1;
+}
+
+static int udp_server() {
+ log_error("UDP Server mode is not implemented");
+ exit(1);
+ return -1;
+}
+
+static int udp_client() {
+ int tty;
+ int sd;
+
+ log_notice("Venus GPS UDP Client connecting to %s:%d", remote_host, port);
+
+ while(1) {
+ sd = inet_conn_str(remote_host, port, SOCK_DGRAM);
+ if(sd < 0) {
+ log_error("inet_conn_str failed: %d", errno);
+ sleep(60);
+ } else {
+ log_debug("connection opened");
+
+ tty = venus_open(device, baud_rate);
+
+ set_nonblocking(sd);
+
+ udp_send_msgs(sd, tty);
+
+ close(sd);
+ close(tty);
+ }
+ }
+
+ return -1;
+}
+
+static int tcp_client() {
+ int tty;
+ int sd;
+
+ log_notice("Venus GPS TCP Client connecting to %s:%d", remote_host, port);
+
+ while(1) {
+ sd = inet_conn_str(remote_host, port, SOCK_STREAM);
+ if(sd < 0) {
+ log_error("inet_conn_str failed: %d", errno);
+ sleep(60);
+ } else {
+ log_debug("connection opened");
+
+ tty = venus_open(device, baud_rate);
+
+ set_nonblocking(tty);
+ set_nonblocking(sd);
+
+ tcp_exchange_data(sd, tty);
+
+ close(sd);
+ close(tty);
+ }
+ }
+
+ return -1;
+}
+
+static int tcp_server() {
+ int err;
+ int tty;
+ int lsd;
+ int sd;
+ int cli_addr_len;
+ int serv_addr_len;
+ int reuse = 1;
+
+ struct sockaddr_in serv_addr;
+ struct sockaddr_in cli_addr;
+
+ lsd = socket(PF_INET, SOCK_STREAM, 0);
+ if(lsd == -1) {
+ log_error("socket failed: %d", errno);
+ exit(1);
+ }
+
+ memset(&serv_addr, 0, sizeof(serv_addr));
+ serv_addr.sin_family = AF_INET;
+ serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+ 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);
+ 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);
+ exit(1);
+ }
+
+ err = listen(lsd, 1);
+ if(err == -1) {
+ log_error("listen failed: %d", errno);
+ exit(1);
+ }
+
+ log_notice("Venus GPS TCP Server listening on port %d", port);
+
+ 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);
+ } else {
+ log_debug("accepted");
+
+ tty = venus_open(device, baud_rate);
+
+ set_nonblocking(tty);
+ set_nonblocking(sd);
+
+ tcp_exchange_data(sd, tty);
+
+ close(sd);
+ close(tty);
+ }
+ }
+
+ close(lsd);
+
+ 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(
+"This program is free software; you may redistribute it under the terms of\n"
+"the GNU General Public License version 2 or (at your option) any later version.\n"
+"This program has absolutely no warranty.\n");
+}
+
+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");
+#if CONFIG_CAN_DEFAULT
+ fprintf(out, " -f --factory-defaults |\n");
+#endif
+#if CONFIG_CAN_RESET
+ fprintf(out, "\n");
+ fprintf(out, " --start-mode 0-5 (default: 0) |\n");
+ fprintf(out, " --latitude VAL (default: ) |\n");
+ fprintf(out, " --longitude VAL (default: ) |\n");
+ fprintf(out, " --altitude VAL (default: ) |\n");
+#endif
+ fprintf(out, "\n");
+ fprintf(out, " --gga 0-255 (default: 1) |\n");
+ fprintf(out, " --gsa 0-255 (default: 1) |\n");
+ fprintf(out, " --gsv 0-255 (default: 1) |\n");
+ fprintf(out, " --gll 0-255 (default: 1) |\n");
+ fprintf(out, " --rmc 0-255 (default: 1) |\n");
+#if CONFIG_HAVE_ZDA
+ fprintf(out, " --zda 0-255 (default: 1) |\n");
+#endif
+ fprintf(out, " --vtg 0-255 (default: 1)\n");
+ fprintf(out, " }\n");
+ fprintf(out, "\n");
+}
+
+#define OPT_DEVICE 'd'
+#define OPT_PORT 'p'
+#define OPT_FACTORY_DEFAULTS 'f'
+#define OPT_BAUD_RATE 'b'
+
+enum {
+ OPT_VERSION = 128,
+ OPT_HELP,
+ OPT_GGA,
+ OPT_GSA,
+ OPT_GSV,
+ OPT_GLL,
+ OPT_RMC,
+ OPT_VTG,
+ OPT_ZDA,
+ OPT_START_MODE,
+ OPT_LATITUDE,
+ OPT_LONGITUDE,
+ OPT_ALTITUDE,
+ OPT_REMOTE_HOST,
+ OPT_DAEMONIZE,
+ OPT_PROTOCOL,
+ OPT_MODE,
+};
+
+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},
+#if CONFIG_CAN_DEFAULT
+ {"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS},
+#endif
+ {"gga", 1, 0, OPT_GGA},
+ {"gsa", 1, 0, OPT_GSA},
+ {"gsv", 1, 0, OPT_GSV},
+ {"gll", 1, 0, OPT_GLL},
+ {"rmc", 1, 0, OPT_RMC},
+ {"vtg", 1, 0, OPT_VTG},
+#if CONFIG_HAVE_ZDA
+ {"zda", 1, 0, OPT_ZDA},
+#endif
+#if CONFIG_CAN_RESET
+ {"start-mode", 1, 0, OPT_START_MODE},
+ {"latitude", 1, 0, OPT_LATITUDE},
+ {"longitude", 1, 0, OPT_LONGITUDE},
+ {"altitude", 1, 0, OPT_ALTITUDE},
+#endif
+ {"version", 0, NULL, OPT_VERSION},
+ {"help", 0, NULL, OPT_HELP},
+ {0, 0, 0, 0},
+};
+
+int main(int argc, char *argv[]) {
+ int i;
+ int option_index;
+ int tty;
+ int factory_defaults = 0;
+ int daemonize = 0;
+ int proto = IPPROTO_TCP;
+ int mode = MODE_SERVER;
+
+ struct sigaction sa;
+
+ sa.sa_handler = sigalarm_handler;
+ sigemptyset(&sa.sa_mask);
+ sa.sa_flags = 0;
+ sigaction(SIGALRM, &sa, NULL);
+
+ sa.sa_handler = sigpipe_handler;
+ sigemptyset(&sa.sa_mask);
+ sa.sa_flags = 0;
+ sigaction(SIGPIPE, &sa, NULL);
+
+ 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)) {
+ log_error("invalid port");
+ usage(stderr);
+ exit(1);
+ }
+ break;
+
+ case OPT_BAUD_RATE:
+ baud_rate = atoi(optarg);
+ baud_rate = value_to_baud(baud_rate);
+ if(baud_rate == (speed_t) -1) {
+ usage(stderr);
+ exit(1);
+ }
+ break;
+
+ case OPT_GGA:
+ gga = atoi(optarg);
+ break;
+
+ case OPT_GSA:
+ gsa = atoi(optarg);
+ break;
+
+ case OPT_GSV:
+ gsv = atoi(optarg);
+ break;
+
+ case OPT_GLL:
+ gll = atoi(optarg);
+ break;
+
+ case OPT_RMC:
+ rmc = atoi(optarg);
+ break;
+
+ case OPT_VTG:
+ vtg = atoi(optarg);
+ break;
+
+#if CONFIG_HAVE_ZDA
+ case OPT_ZDA:
+ zda = atoi(optarg);
+ break;
+#endif
+
+#if CONFIG_CAN_RESET
+ case OPT_START_MODE:
+ start_mode = (uint8_t) atoi(optarg);
+ break;
+
+ case OPT_LATITUDE:
+ latitude = (int16_t) atoi(optarg);
+ break;
+
+ case OPT_LONGITUDE:
+ longitude = (int16_t) atoi(optarg);
+ break;
+
+ case OPT_ALTITUDE:
+ altitude = (int16_t) atoi(optarg);
+ break;
+#endif
+
+#if CONFIG_CAN_DEFAULT
+ case OPT_FACTORY_DEFAULTS:
+ factory_defaults = 1;
+ break;
+#endif
+
+ case OPT_REMOTE_HOST:
+ snprintf(remote_host, sizeof(remote_host), "%s", optarg);
+ break;
+
+ case OPT_VERSION:
+ print_version("venus-gps");
+ exit(0);
+ break;
+
+ case OPT_HELP:
+ usage(stdout);
+ exit(0);
+ break;
+
+ default:
+ usage(stderr);
+ exit(1);
+ }
+ }
+
+ if(optind < argc) {
+ usage(stderr);
+ exit(1);
+ }
+
+ if(daemonize) {
+#if CONFIG_USE_SYSLOG
+ daemon(0, 0);
+#else
+ daemon(0, 1);
+#endif
+ }
+
+#if CONFIG_USE_SYSLOG
+ openlog("VenusGPS", LOG_NDELAY, LOG_FACILITY);
+# if DEBUG
+ setlogmask(LOG_UPTO(LOG_DEBUG));
+# else
+ setlogmask(LOG_UPTO(LOG_INFO));
+# endif
+#endif
+
+ tty = venus_open(device, baud_rate);
+ if(tty < 0) {
+ log_error("failed to open tty: errno: %d", errno);
+ exit(1);
+ }
+
+ venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
+ venus_query_sw_version(tty, SW_TYPE_RESERVED);
+
+ if(factory_defaults) {
+ venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
+ log_notice("setting factory defaults");
+ exit(0);
+ }
+ 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);
+ }
+
+ venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);
+
+ venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);
+
+ 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();
+ }
+ }
+
+ return 0;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-15 14:04:48 +0000