1 files changed, 487 insertions, 0 deletions
diff --git a/src/venus_api.c b/src/venus_api.c
new file mode 100644
index 0000000..52a4a96
--- /dev/null
+++ b/src/venus_api.c
@@ -0,0 +1,487 @@
+/*
+ * SkyTraq Venus 5 GPS API
+ *
+ * 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
+ *
+ */
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <time.h>
+#include <asm/byteorder.h>
+#include <netdb.h>
+
+#include "log.h"
+#include "cbuffer.h"
+#include "utils.h"
+#include "venus_api.h"
+
+static const struct baud_map __baud_map[] = {
+	{B4800, 4800},
+	{B9600, 9600},
+	{B19200, 19200},
+	{B38400, 38400},
+	{B57600, 57600},
+	{B115200, 115200},
+};
+
+static const struct baud_map __venus_baud_map[] = {
+	{B4800, VENUS_4800},
+	{B9600, VENUS_9600},
+	{B19200, VENUS_19200},
+	{B38400, VENUS_38400},
+	{B57600, VENUS_57600},
+	{B115200, VENUS_115200},
+};
+
+speed_t value_to_baud(speed_t value) {
+	int n = sizeof(__baud_map) / sizeof(struct baud_map);
+	int i;
+
+	for(i = 0; i < n; ++i) {
+		if(__baud_map[i].value == value) {
+			return __baud_map[i].baud;
+		}
+	}
+
+	log_warning("baud rate not valid: %lu", (unsigned long) value);
+
+	return (speed_t) -1;
+}
+
+speed_t baud_to_venus(speed_t baud) {
+	int n = sizeof(__venus_baud_map) / sizeof(struct baud_map);
+	int i;
+
+	for(i = 0; i < n; ++i) {
+		if(__venus_baud_map[i].baud == baud) {
+			return __venus_baud_map[i].value;
+		}
+	}
+
+	log_warning("baud rate not valid: %lu", (unsigned long) baud);
+
+	return (speed_t) -1;
+}
+
+static uint8_t seq_start[] = {0xA0, 0xA1};
+static uint8_t seq_end[] = {0x0D, 0x0A};
+
+void free_venus_msg_data(struct venus_msg *msg) {
+	if(!msg) {
+		return;
+	}
+
+	if(msg->data) {
+		free(msg->data);
+	}
+	msg->data = NULL;
+}
+
+uint8_t checksum(void *data, uint16_t len) {
+	int i;
+	uint8_t cs = 0;
+
+	for(i = 0; i < len; i++) {
+		cs ^= ((char *) data)[i];
+	}
+
+	return cs;
+}
+
+int venus_write_msg(int fd, struct venus_msg *msg) {
+	int err;
+	uint8_t cs = 0;
+
+	log_debug("id: %d", msg->data[0]);
+
+	err = full_write(fd, seq_start, sizeof(seq_start));
+	if(err != sizeof(seq_start)) {
+		log_error("failed to write seq_start: %d %d", err, errno);
+	}
+	msg->len = __cpu_to_be16(msg->len);
+	err = full_write(fd, &msg->len, 2);
+	if(err != 2) {
+		log_error("failed to write len: %d %d", err, errno);
+	}
+	msg->len = __be16_to_cpu(msg->len);
+	err = full_write(fd, msg->data, msg->len);
+	if(err != msg->len) {
+		log_error("failed to write data: %d %d", err, errno);
+	}
+	cs = checksum(msg->data, msg->len);
+	err = full_write(fd, &cs, 1);
+	if(err != 1) {
+		log_error("failed to write checksum: %d %d", err, errno);
+	}
+	err = full_write(fd, seq_end, sizeof(seq_end));
+	if(err != sizeof(seq_end)) {
+		log_error("failed to write seq_end: %d %d", err, errno);
+	}
+
+	return 0;
+}
+
+int venus_read_msg(int fd, struct venus_msg *msg) {
+	uint8_t cs;
+	uint8_t *data;
+	uint16_t len;
+	uint8_t seq[2];
+	int attempts = 0;
+
+again:
+	while(1) {
+		safe_readn(fd, seq, 2);
+		//log_error("start seq %c %c", seq[0], seq[1]);
+		if(!memcmp(seq, seq_start, 2)) {
+			break;
+		}
+		attempts++;
+		if(attempts > 1024) {
+			log_error("max attempts reached");
+			return -1;
+		}
+	}
+
+	safe_readn(fd, &len, 2);
+	len = __be16_to_cpu(len);
+	data = malloc(len);
+	safe_readn(fd, data, len);
+	safe_readn(fd, &cs, 1);
+	if(checksum(data, len) != cs) {
+		log_error("checksum mismatch");
+		return -1;
+	}
+	safe_readn(fd, seq, 2);
+	if(memcmp(seq, seq_end, 2)) {
+		log_error("expected seq end");
+		return -1;
+	}
+
+	if(len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) {
+		log_debug("read ACK ID_NONE");
+		free(data);
+		goto again;
+	}
+
+	msg->data = data;
+	msg->len = len;
+
+#if DEBUG
+	{
+	int i;
+	printf("READ MSG: ");
+	for(i = 0; i < msg->len; i++) {
+		printf("%02X ", msg->data[i]);
+	}
+	printf("\n");
+	}
+#endif
+
+	return 0;
+}
+
+ssize_t read_nmea_sentence(int fd, void *buf, size_t count) {
+	int err;
+	char c;
+	ssize_t total = 0;
+	char *cp = buf;
+
+	int start = 0;
+
+	while(1) {
+		err = safe_read(fd, &c, 1);
+		if(err <= 0) {
+			log_error("read from gps failed: %d", errno);
+			return -1;
+		}
+		if(!start) {
+			if(c != '$') {
+				continue;
+			}
+			start = 1;
+		}
+
+		if(total < count) {
+			*cp++ = c;
+			total++;
+		}
+
+		if(c == '\n') {
+			break;
+		}
+	}
+
+	return total;
+}
+
+int venus_system_restart(
+	int fd,
+	uint8_t mode,
+	time_t utc,
+	int16_t latitude,
+	int16_t longitude,
+	int16_t altitude)
+{
+	int err;
+	uint8_t data[15];
+	struct venus_msg msg;
+
+	struct tm broken;
+	struct tm *tmp;
+
+	tmp = gmtime_r(&utc, &broken);
+	if (tmp == NULL) {
+		log_error("gmtime_r: %d", errno);
+		return -1;
+	}
+
+	data[0] = ID_SYSTEM_RESTART;
+	data[1] = mode;
+
+	uint16_t year = __cpu_to_le16(((uint16_t) broken.tm_year) + 1900);
+	memcpy(data + 2, &year, 2);
+	data[4] = (uint8_t) (broken.tm_mon + 1);
+	data[5] = (uint8_t) broken.tm_mday;
+	data[6] = (uint8_t) broken.tm_hour;
+	data[7] = (uint8_t) broken.tm_min;
+	data[8] = (uint8_t) broken.tm_sec;
+
+	latitude = (int16_t) __cpu_to_le16((uint16_t) latitude);
+	memcpy(data + 9, &latitude, 2);
+
+	longitude = (int16_t) __cpu_to_le16((uint16_t) longitude);
+	memcpy(data + 11, &longitude, 2);
+
+	altitude = (int16_t) __cpu_to_le16((uint16_t) altitude);
+	memcpy(data + 13, &altitude, 2);
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_factory_defaults(int fd, uint8_t type) {
+	int err;
+	uint8_t data[2];
+	struct venus_msg msg;
+
+	data[0] = ID_FACTORY_DEFAULTS;
+	data[1] = type;
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) {
+	int err;
+	uint8_t data[4];
+	struct venus_msg msg;
+
+	data[0] = ID_CONF_SERIAL;
+	data[1] = com;
+	data[2] = baud;
+	data[3] = attr;
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_conf_format(int fd, uint8_t type, uint8_t attr) {
+	int err;
+	uint8_t data[3];
+	struct venus_msg msg;
+
+	data[0] = ID_CONF_FORMAT;
+	data[1] = type;
+	data[2] = attr;
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_query_sw_version(int fd, uint8_t type) {
+	int err;
+	uint8_t data[2];
+	struct venus_msg msg;
+
+	data[0] = ID_QUERY_SW_VERSION;
+	data[1] = type;
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_conf_nmea(
+	int fd,
+	uint8_t gga,
+	uint8_t gsa,
+	uint8_t gsv,
+	uint8_t gll,
+	uint8_t rmc,
+	uint8_t vtg,
+	uint8_t zda,
+	uint8_t attr)
+{
+	int err;
+	uint8_t data[9];
+	struct venus_msg msg;
+
+	data[0] = ID_CONF_NMEA;
+	data[1] = gga;
+	data[2] = gsa;
+	data[3] = gsv;
+	data[4] = gll;
+	data[5] = rmc;
+	data[6] = vtg;
+	data[7] = zda;
+	data[8] = attr;
+
+	msg.data = data;
+	msg.len = sizeof(data);
+	err = venus_write_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_write_msg: %d", err);
+		return -1;
+	}
+
+	err = venus_read_msg(fd, &msg);
+	if(err < 0) {
+		log_error("venus_read_msg: %d", err);
+		return -1;
+	}
+	free_venus_msg_data(&msg);
+
+	return 0;
+}
+
+int venus_tty_configure(int fd, speed_t baud_rate) {
+	struct termios tio;
+	tcgetattr(fd, &tio);
+	cfmakeraw(&tio);
+	cfsetspeed(&tio, baud_rate);
+	tcsetattr(fd, TCSANOW, &tio);
+
+	return 0;
+}
+
+int venus_open(const char *dev, speed_t baud_rate) {
+	int tty;
+
+	tty = open(dev, O_RDWR | O_NOCTTY);
+	if(tty < 0) {
+		log_error("failed to open %s: errno: %d", dev, errno);
+		exit(1);
+	}
+
+	venus_tty_configure(tty, baud_rate);
+	tcflush(tty, TCIOFLUSH);
+
+	return tty;
+}
+
+
+
+