[FEATURE: #335] userspace/tools/nmea: add sources for reading GPS NMEA interface

userspace/tools/nmea/nmea.c

+/*
+ *
+ * NMEA library
+ * URL: http://nmea.sourceforge.net
+ * Author: Tim (xtimor@gmail.com)
+ * Licence: http://www.gnu.org/licenses/lgpl.html
+ *
+ * Modified for use in wrsw_hal by Tomasz Wlostowski/CERN
+ */
+
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <ctype.h>
+#include <string.h>
+#include <limits.h>
+
+#include "nmea.h"
+
+#define NMEA_TOKS_COMPARE   (1)
+#define NMEA_TOKS_PERCENT   (2)
+#define NMEA_TOKS_WIDTH     (3)
+#define NMEA_TOKS_TYPE      (4)
+
+#define NMEA_CONVSTR_BUF	128
+#define NMEA_TIMEPARSE_BUF	128
+
+/**
+ * \brief Calculate control sum of binary buffer
+ */
+static int nmea_calc_crc(const char *buff, int buff_sz)
+{
+    int chsum = 0,
+	it;
+
+    for(it = 0; it < buff_sz; ++it)
+	chsum ^= (int)buff[it];
+
+    return chsum;
+}
+
+/**
+ * \brief Convert string to number
+ */
+static int nmea_atoi(const char *str, int str_sz, int radix)
+{
+    char *tmp_ptr;
+    char buff[NMEA_CONVSTR_BUF];
+    int res = 0;
+
+    if (str_sz < NMEA_CONVSTR_BUF) {
+	memcpy(&buff[0], str, str_sz);
+	buff[str_sz] = '\0';
+	res = strtol(&buff[0], &tmp_ptr, radix);
+    }
+
+    return res;
+}
+
+/**
+ * \brief Convert string to fraction number
+ */
+static double nmea_atof(const char *str, int str_sz)
+{
+    char *tmp_ptr;
+    char buff[NMEA_CONVSTR_BUF];
+    double res = 0;
+
+    if (str_sz < NMEA_CONVSTR_BUF) {
+	memcpy(&buff[0], str, str_sz);
+	buff[str_sz] = '\0';
+	res = strtod(&buff[0], &tmp_ptr);
+    }
+
+    return res;
+}
+
+/**
+ * \brief Formating string (like standard printf) with CRC tail (*CRC)
+ */
+int nmea_printf(char *buff, int buff_sz, const char *format, ...)
+{
+    int retval, add = 0;
+    va_list arg_ptr;
+
+    if (buff_sz <= 0)
+	return 0;
+
+    va_start(arg_ptr, format);
+
+    retval = vsnprintf(buff, buff_sz, format, arg_ptr);
+
+    if (retval > 0) {
+	add = snprintf(
+	    buff + retval, buff_sz - retval, "*%02x\r\n",
+	    nmea_calc_crc(buff + 1, retval - 1));
+    }
+
+    retval += add;
+
+    if (retval < 0 || retval > buff_sz) {
+	memset(buff, ' ', buff_sz);
+	retval = buff_sz;
+    }
+
+    va_end(arg_ptr);
+
+    return retval;
+}
+
+/**
+ * \brief Analyse string (specificate for NMEA sentences)
+ */
+static int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
+{
+    const char *beg_tok;
+    const char *end_buf = buff + buff_sz;
+
+    va_list arg_ptr;
+    int tok_type = NMEA_TOKS_COMPARE;
+    int width = 0;
+    const char *beg_fmt = 0;
+    int snum = 0, unum = 0;
+
+    int tok_count = 0;
+    void *parg_target;
+
+    va_start(arg_ptr, format);
+
+    for(; *format && buff < end_buf; ++format) {
+	switch(tok_type) {
+	case NMEA_TOKS_COMPARE:
+	    if ('%' == *format)
+		tok_type = NMEA_TOKS_PERCENT;
+	    else if (*buff++ != *format)
+		goto fail;
+	    break;
+	case NMEA_TOKS_PERCENT:
+	    width = 0;
+	    beg_fmt = format;
+	    tok_type = NMEA_TOKS_WIDTH;
+	case NMEA_TOKS_WIDTH:
+	    if (isdigit(*format))
+		break;
+	    {
+		tok_type = NMEA_TOKS_TYPE;
+		if (format > beg_fmt)
+		    width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
+	    }
+	case NMEA_TOKS_TYPE:
+	    beg_tok = buff;
+
+	    if (!width && ('c' == *format || 'C' == *format) && *buff != format[1])
+		width = 1;
+
+	    if (width) {
+		if (buff + width <= end_buf)
+		    buff += width;
+		else
+		    goto fail;
+	    } else {
+		if (!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
+		    buff = end_buf;
+	    }
+
+	    if (buff > end_buf)
+		goto fail;
+
+	    tok_type = NMEA_TOKS_COMPARE;
+	    tok_count++;
+
+	    parg_target = 0; width = (int)(buff - beg_tok);
+
+	    switch(*format) {
+	    case 'c':
+	    case 'C':
+		parg_target = (void *)va_arg(arg_ptr, char *);
+		if (width && 0 != (parg_target))
+		    *((char *)parg_target) = *beg_tok;
+		break;
+	    case 's':
+	    case 'S':
+		parg_target = (void *)va_arg(arg_ptr, char *);
+		if (width && 0 != (parg_target)) {
+		    memcpy(parg_target, beg_tok, width);
+		    ((char *)parg_target)[width] = '\0';
+		}
+		break;
+	    case 'f':
+	    case 'g':
+	    case 'G':
+	    case 'e':
+	    case 'E':
+		parg_target = (void *)va_arg(arg_ptr, double *);
+		if (width && 0 != (parg_target))
+		    *((double *)parg_target) = nmea_atof(beg_tok, width);
+		break;
+	    };
+
+	    if (parg_target)
+		break;
+	    if (0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
+		break;
+	    if (!width)
+		break;
+
+	    switch(*format) {
+	    case 'd':
+	    case 'i':
+		snum = nmea_atoi(beg_tok, width, 10);
+		memcpy(parg_target, &snum, sizeof(int));
+		break;
+	    case 'u':
+		unum = nmea_atoi(beg_tok, width, 10);
+		memcpy(parg_target, &unum, sizeof(unsigned int));
+		break;
+	    case 'x':
+	    case 'X':
+		unum = nmea_atoi(beg_tok, width, 16);
+		memcpy(parg_target, &unum, sizeof(unsigned int));
+		break;
+	    case 'o':
+		unum = nmea_atoi(beg_tok, width, 8);
+		memcpy(parg_target, &unum, sizeof(unsigned int));
+		break;
+	    default:
+		goto fail;
+	    };
+
+	    break;
+	};
+    }
+
+fail:
+
+    va_end(arg_ptr);
+
+    return tok_count;
+}
+
+static int _nmea_parse_time(const char *buff, int buff_sz, nmea_time_t *res)
+{
+    int success = 0;
+
+    switch(buff_sz) {
+    case sizeof("hhmmss") - 1:
+	success = (3 == nmea_scanf(buff, buff_sz,
+	    "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
+	    ));
+	break;
+    case sizeof("hhmmss.s") - 1:
+    case sizeof("hhmmss.ss") - 1:
+    case sizeof("hhmmss.sss") - 1:
+	success = (4 == nmea_scanf(buff, buff_sz,
+	    "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
+	    ));
+	break;
+    default:
+	success = 0;
+	break;
+    }
+
+    return (success?0:-1);
+}
+
+/**
+ * \brief Parse RMC packet from buffer.
+ * @param buff a constant character pointer of packet buffer.
+ * @param buff_sz buffer size.
+ * @param pack a pointer of packet which will filled by function.
+ * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
+ */
+
+int nmea_parse_gprmc(const char *buff, int buff_sz, nmea_time_t *utc)
+{
+    int nsen;
+    char time_buff[NMEA_TIMEPARSE_BUF];
+
+    nmea_gprmc_t *pack = (nmea_gprmc_t *)utc;
+
+    assert(buff && pack);
+
+    memset(pack, 0, sizeof(nmea_gprmc_t));
+
+    nsen = nmea_scanf(buff, buff_sz,
+	"$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
+	&(time_buff[0]),
+	&(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
+	&(pack->speed), &(pack->direction),
+	&(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
+	&(pack->declination), &(pack->declin_ew), &(pack->mode));
+
+    if (nsen != 13 && nsen != 14)
+    {
+	return 0;
+    }
+
+    if (0 != _nmea_parse_time(&time_buff[0],
+			     (int)strlen(&time_buff[0]),
+			     &(pack->utc))
+      ) {
+	return 0;
+    }
+
+    if (pack->utc.year < 90)
+	pack->utc.year += 100;
+    pack->utc.mon -= 1;
+
+    return 1;
+}
+
+/**
+ * \brief Parse ZDA packet from buffer.
+ * @param buff a constant character pointer of packet buffer.
+ * @param buff_sz buffer size.
+ * @param pack a pointer of packet which will filled by function.
+ * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
+ */
+
+int nmea_parse_gpzda(const char *buff, int buff_sz, nmea_time_t *utc)
+{
+    int nsen;
+    char time_buff[NMEA_TIMEPARSE_BUF];
+
+    nmea_gpzda_t *pack = (nmea_gpzda_t *)utc;
+
+    assert(buff && pack);
+
+    memset(pack, 0, sizeof(nmea_gpzda_t));
+
+    nsen = nmea_scanf(buff, buff_sz,
+		      "$GPZDA,%s,%2d,%2d,%4d,%2c,%2d*",
+		      &(time_buff[0]), &(pack->day), &(pack->month),
+		      &(pack->year), &(pack->lzd), &(pack->lzmd));
+
+    if (nsen != 6) {
+	return 0;
+    }
+
+    if (0 != _nmea_parse_time(&time_buff[0],
+			      (int)strlen(&time_buff[0]),
+			      &(pack->utc))
+       ) {
+	return 0;
+    }
+
+    //reformat inline with rmc
+    pack->utc.year = pack->year-1900;
+    if (pack->utc.year < 90)
+	pack->utc.year += 100;
+
+    pack->utc.mon = pack->month-1;
+    pack->utc.day = pack->day;
+
+    return 1;
+}
+
+int64_t nmea_time_to_tai(nmea_time_t t)
+{
+	short month, year;
+	int64_t result;
+	const int m_to_d[12] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
+
+	month = t.mon;
+	year = t.year + month / 12 + 1900;
+	month %= 12;
+	if (month < 0) {
+		year -= 1;
+		month += 12;
+	}
+	result = (year - 1970) * 365 + (year - 1969) / 4 + m_to_d[month];
+	result = (year - 1970) * 365 + m_to_d[month];
+
+	if (month <= 1)
+		year -= 1;
+
+	result += (year - 1968) / 4;
+	result -= (year - 1900) / 100;
+	result += (year - 1600) / 400;
+	result += t.day;
+	result -= 1;
+	result *= 24;
+	result += t.hour;
+	result *= 60;
+	result += t.min;
+	result *= 60;
+	result += t.sec;
+
+	return(result + 19LL); /* TAI = GPS + 19 s */
+}

userspace/tools/nmea/nmea.h

+/*
+ *
+ * NMEA library
+ * URL: http://nmea.sourceforge.net
+ * Author: Tim (xtimor@gmail.com)
+ * Licence: http://www.gnu.org/licenses/lgpl.html
+ * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $
+ *
+ * Stripped-down version for use in wrsw_hal by Tomasz Wlostowski/CERN.
+ */
+
+/*! \file */
+
+#ifndef __NMEA_H__
+#define __NMEA_H__
+
+typedef struct 
+{
+    int     year;       /**< Years since 1900 */
+    int     mon;        /**< Months since January - [0,11] */
+    int     day;        /**< Day of the month - [1,31] */
+    int     hour;       /**< Hours since midnight - [0,23] */
+    int     min;        /**< Minutes after the hour - [0,59] */
+    int     sec;        /**< Seconds after the minute - [0,59] */
+    int     hsec;       /**< Hundredth part of second - [0,99] */
+
+} nmea_time_t;
+
+/**
+ * RMC packet information structure (Recommended Minimum sentence C)
+ */
+typedef struct
+{
+    nmea_time_t utc;    /**< UTC of position */
+    char    status;     /**< Status (A = active or V = void) */
+    double  lat;        /**< Latitude in NDEG - [degree][min].[sec/60] */
+    char    ns;         /**< [N]orth or [S]outh */
+    double  lon;        /**< Longitude in NDEG - [degree][min].[sec/60] */
+    char    ew;         /**< [E]ast or [W]est */
+    double  speed;      /**< Speed over the ground in knots */
+    double  direction;  /**< Track angle in degrees True */
+    double  declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
+    char    declin_ew;  /**< [E]ast or [W]est */
+    char    mode;       /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */
+
+} nmea_gprmc_t;
+
+/**
+ * ZDA packet information structure
+ */
+typedef struct
+{
+    nmea_time_t utc;    /**< UTC of position */
+    int day;      /**< day, 01 to 31*/
+    int month;    /**< month, 01 to 12*/
+    int year;     /**< year */
+    int lzd;      /**< local zone description, 00 to +/- 13 hours */
+    int lzmd;     /**< local zone minutes description */
+} nmea_gpzda_t;
+
+int nmea_parse_gprmc(const char *buff, int buff_sz, nmea_time_t *utc);
+int nmea_parse_gpzda(const char *buff, int buff_sz, nmea_time_t *utc);
+int64_t nmea_time_to_tai(nmea_time_t t);
+
+#endif /* __NMEA_H__ */

userspace/tools/nmea/serial.h

+/*      SAM7Sisp - alternatywny bootloader dla mikrokontrolerw AT91SAM7
+
+        (c) Tomasz Wlostowski 2006
+
+        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.
+*/
+
+
+#ifndef __SERIAL_H
+#define __SERIAL_H
+
+int serial_open(char *dev_name, int speed);
+void serial_close(void);
+void serial_set_dtr(int s);
+int serial_read(char *data, int len);
+int serial_write(char *data, int len);
+void serial_write_byte(unsigned char b);
+unsigned char serial_read_byte(void);
+int serial_data_avail(void);
+
+void sys_delay(int msecs);
+unsigned int sys_get_clock_usec(void);
+
+#endif

userspace/tools/nmea/serial_linux.c

+/*      SAM7Sisp - alternatywny bootloader dla mikrokontrolerw AT91SAM7
+
+        (c) Tomasz Wlostowski 2006
+
+        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.
+*/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <asm/ioctls.h>
+#include <string.h>
+#include <termios.h>
+#include <fcntl.h>
+
+static int serial_fd;
+
+void serial_set_dtr(int s) // pin 4
+{
+    unsigned int v;
+    if (serial_fd < 0) return;
+    ioctl(serial_fd, TIOCMGET, &v);
+    if (s) v |= TIOCM_DTR; else v &= ~TIOCM_DTR;
+    ioctl(serial_fd, TIOCMSET, &v);
+}
+
+int serial_open(char *dev_name, int speed)
+{
+    struct termios t;
+    int fd;
+    int spd;
+
+    switch(speed)
+    {
+	case 115200: spd = B115200; break;
+	case 57600:  spd = B57600;  break;
+	case 38400:  spd = B38400;  break;
+	case 19200:  spd = B19200;  break;
+	case 9600:   spd = B9600;   break;
+	default: return -2;
+    }
+
+    fd = open(dev_name, O_RDWR);
+
+    if (fd<0)
+	return -1;
+
+    tcgetattr (fd, &t);
+    t.c_iflag = IGNBRK | IGNPAR;
+    t.c_oflag = t.c_lflag = t.c_line = 0;
+    t.c_cflag = CS8 | CREAD |  CLOCAL | HUPCL | spd;
+    tcsetattr (fd, TCSAFLUSH, &t);
+
+    serial_fd = fd;
+    return 0;
+}
+
+void serial_close(void)
+{
+    close(serial_fd);
+}
+
+int serial_write(char *data, int len)
+{
+#ifdef DEBUG
+    int i;
+
+    printf("WS: '");
+    for (i=0; i<len; i++)
+	printf("%c", data[i]);
+    printf("'\n");
+#endif /* DEBUG */
+
+    return write(serial_fd, data, len);
+}
+
+
+int serial_read(char *data, int len)
+{
+    int nbytes = 0;
+
+    while (len) {
+	if (read(serial_fd, data, 1) == 1) {
+	    len--; data++; nbytes++;
+	}
+    }
+
+    return nbytes;
+};
+
+void serial_write_byte(unsigned char b)
+{
+#ifdef DEBUG
+    printf("WB: '%c'\n", b);
+#endif /* DEBUG */
+
+    write (serial_fd, &b, 1);
+}
+
+char serial_read_byte(void)
+{
+    char b;
+
+    serial_read(&b, 1);
+#ifdef DEBUG
+    printf("%02x ", b);
+#endif /* DEBUG */
+
+    return b;
+
+}
+
+int serial_data_avail(void)
+{
+    fd_set set;
+    struct timeval tv;
+
+    FD_ZERO(&set);
+    FD_SET(serial_fd, &set);
+
+    tv.tv_sec = 0;
+    tv.tv_usec = 0;
+
+    return select(serial_fd+1, &set, NULL, NULL, &tv) > 0;
+}
+
+unsigned int sys_get_clock_usec(void)
+{
+    struct timezone tz = {0, 0};
+    struct timeval tv;
+
+    gettimeofday(&tv, &tz);
+
+    return tv.tv_usec + tv.tv_sec * 1000000;
+}
+
+void sys_delay(int msecs)
+{
+    usleep(msecs * 1000);
+}
+
+void serial_purge(void)
+{
+    while (serial_data_avail())
+	serial_read_byte();
+}

userspace/tools/nmea/wr_nmea.c

+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+#include <libwr/wrs-msg.h>
+
+
+#include "serial.h"
+#include "wr_nmea.h"
+
+
+static nmea_gpzda_t nmea_gpzda;
+static nmea_gprmc_t nmea_gprmc;
+
+int nmea_init(struct wr_nmea *nmea, char *dev, int baud, char *fmt)
+{
+	nmea->dev = dev;
+	nmea->baud = baud;
+
+	if (strcmp(fmt, "GPZDA") == 0){
+		nmea->fmt = fmt;
+		nmea->parse = nmea_parse_gpzda;
+		nmea->utc = &(nmea_gpzda.utc);
+	} else if (strcmp(fmt, "GPRMC") == 0){
+		nmea->fmt = fmt;
+		nmea->parse = nmea_parse_gprmc;
+		nmea->utc = &(nmea_gprmc.utc);
+	} else {
+		pr_error("%s unsupported format\n", __func__);
+	}
+	return 0;
+}
+
+static void read_nmea_msg(char *msgbuf, int len)
+{
+	int i = 0;
+	char c;
+
+	while ((c = serial_read_byte()) != '$') {};
+		*msgbuf++ = c;
+	while ((c = serial_read_byte()) != '\r' && (i++) < len - 1)
+		*msgbuf++ = c;
+	*msgbuf++ = '\r';
+	*msgbuf++ = '\n';
+
+	*msgbuf++ = 0 ;
+}
+
+void read_nmea_msg_type(char *msgbuf, int len, const char *msg_type)
+{
+	do {
+		read_nmea_msg(msgbuf, len);
+	} while (strncmp(&msgbuf[1], msg_type, 5) != 0); //ignore starting "$"
+}
+
+int nmea_read_tai(struct wr_nmea *nmea, int64_t *t_out)
+{
+    char buf[1024];
+
+    serial_open(nmea->dev, nmea->baud);
+    read_nmea_msg_type(buf, 1024, nmea->fmt);
+    serial_close();
+
+    if(nmea->parse(buf, strlen(buf), (nmea->utc)) < 0)
+	return -1;
+
+    pr_info("NMEA time: %d/%d/%d %02d:%02d:%02d.%02d\n",
+	    nmea->utc->year+1900,
+	    nmea->utc->mon+1,
+	    nmea->utc->day,
+	    nmea->utc->hour,
+	    nmea->utc->min,
+	    nmea->utc->sec,
+	    nmea->utc->hsec);
+
+    *t_out = nmea_time_to_tai(*(nmea->utc));
+
+    return 0;
+}

userspace/tools/nmea/wr_nmea.h

+#ifndef __WR_NMEA_H
+#define __WR_NMEA_H
+
+#include <time.h>
+#include "nmea.h"
+
+struct wr_nmea {
+    nmea_time_t *utc;
+    char *dev;
+    int baud;
+    char *fmt;
+    int (*parse)(const char *buff, int buff_sz, nmea_time_t *pack);
+};
+
+int nmea_init(struct wr_nmea *nmea, char *dev, int baud, char *fmt);
+int nmea_read_tai(struct wr_nmea *nmea, int64_t *t_out);
+#endif