wrsw_hal: serial port GPS message parser for UTC/TAI input

5ec76ea1 · Tomasz Wlostowski · ee089b42 · 5ec76ea1 · 5ec76ea1 · 5ec76ea1
Commit 5ec76ea1 authored Jun 08, 2012 by Tomasz Wlostowski
8 changed files
--- a/userspace/wrsw_hal/Makefile
+++ b/userspace/wrsw_hal/Makefile

 CC = $(CROSS_COMPILE)gcc

-OBJS = hal_exports.o hal_main.o hal_ports.o hal_config.o rt_client.o hal_timing.o
+OBJS = hal_exports.o hal_main.o hal_ports.o hal_config.o rt_client.o hal_timing.o gps_resync/serial_linux.o gps_resync/nmea.o gps_resync/gps_resync.o

 BINARY = wrsw_hal

@@ -16,7 +16,7 @@ CFLAGS = -g -Wall -DDEBUG -DRT_CLIENT\

 LDFLAGS = -L$(WR_INSTALL_ROOT)/lib -L../3rdparty/lib -L$(WR_LIB) \
 	 -L../libswitchhw -L../mini-rpc \
-	 -lminipc -llua -lm -ldl -lswitchhw
+	 -lminipc -llua -lm -ldl -lswitchhw 

 all: $(BINARY)


--- a/userspace/wrsw_hal/gps_resync/gps_resync.c
+++ b/userspace/wrsw_hal/gps_resync/gps_resync.c
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <time.h>
+#include <unistd.h>
+
+
+#include "serial.h"
+#include "nmea.h"
+
+#include <hw/switch_hw.h>
+
+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 ;
+}
+
+static int nmea_read_tai(const char *dev_name, int64_t *t_out)
+{
+		char buf[1024];
+  	nmea_gprmc_t gprmc;
+
+		serial_open((char *)dev_name, 115200);
+		read_nmea_msg(buf, 1024);
+		serial_close();
+	
+    if(nmea_parse_gprmc(buf, strlen(buf), &gprmc) < 0)
+    	return -1;
+
+
+		TRACE(TRACE_INFO, "NMEA time: %d/%d/%d %02d:%02d:%02d.%02d\n", gprmc.utc.year + 1900,gprmc.utc.mon+1,gprmc.utc.day,gprmc.utc.hour, gprmc.utc.min, gprmc.utc.sec,gprmc.utc.hsec);
+
+		*t_out = nmea_time_to_tai(gprmc.utc);
+
+		return 0;
+}
+
+void nmea_resync_ppsgen(const char *dev_name)
+{
+	uint32_t utc, nsec;
+	int64_t tai;
+	
+	nmea_read_tai("/dev/ttyS2", &tai);
+	shw_pps_gen_read_time(	&utc, &nsec);
+
+	shw_pps_gen_adjust_utc(tai - utc);
+
+	while(!shw_pps_gen_busy()) usleep(100000);
+}
+
--- a/userspace/wrsw_hal/gps_resync/gps_resync.h
+++ b/userspace/wrsw_hal/gps_resync/gps_resync.h
+#ifndef __NMEA_RESYNC_H
+#define __NMEA_RESYNC_H
+
+#include <time.h>
+
+int nmea_get_unix_seconds(const char *dev_name, time_t *t_out);
+void nmea_resync_ppsgen(const char *dev_name);
+
+#endif
--- a/userspace/wrsw_hal/gps_resync/nmea.c
+++ b/userspace/wrsw_hal/gps_resync/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 standart printf) with CRC tail (*CRC)
+ */
+static 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_gprmc_t *pack)
+{
+    int nsen;
+    char time_buff[NMEA_TIMEPARSE_BUF];
+
+    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;
+}
+
+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 */
+}
--- a/userspace/wrsw_hal/gps_resync/nmea.h
+++ b/userspace/wrsw_hal/gps_resync/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;
+
+int nmea_parse_gprmc(const char *buff, int buff_sz, nmea_gprmc_t *pack);
+int64_t nmea_time_to_tai(nmea_time_t t);
+
+#endif /* __NMEA_H__ */
--- a/userspace/wrsw_hal/gps_resync/serial.h
+++ b/userspace/wrsw_hal/gps_resync/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 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();
+int serial_data_avail();
+
+void sys_delay(int msecs);
+unsigned int sys_get_clock_usec();
+
+#endif
--- a/userspace/wrsw_hal/gps_resync/serial_linux.c
+++ b/userspace/wrsw_hal/gps_resync/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()
+{
+    close(serial_fd);
+}
+
+int serial_write(char *data, int len)
+{
+//    printf("WS: '");
+    int i;
+//    for(i=0;i<len;i++) printf("%c", data[i]);
+//    printf("'\n");
+
+    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)
+{
+//    printf("WB: '%c'\n", b);
+    write (serial_fd,&b,1);
+}
+
+unsigned char serial_read_byte()
+{
+    unsigned char b;
+    serial_read(&b,1);
+//    fprintf(stderr,"%02x ", b);
+    return b;
+
+}
+
+int serial_data_avail()
+{
+    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()
+{
+    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()
+{
+  while(serial_data_avail()) serial_read_byte();
+}
\ No newline at end of file
--- a/userspace/wrsw_hal/hal_timing.c
+++ b/userspace/wrsw_hal/hal_timing.c
@@ -12,6 +12,8 @@
 #include "rt_ipc.h"
 #include "hal_exports.h"

+#include "gps_resync/gps_resync.h"
+
 static int timing_mode;

 #define LOCK_TIMEOUT_EXT 60000
@@ -21,7 +23,8 @@ int hal_init_timing()
 {
 	char str[128];
 	timeout_t lock_tmo;
-
+	int use_utc = 0;
+	
 	if(rts_connect() < 0)
 	{
 		TRACE(TRACE_ERROR, "Failed to establish communication with the RT subsystem.");
@@ -62,22 +65,36 @@ int hal_init_timing()
 			break;
 	}

-	while(!tmo_expired(&lock_tmo))
+	while(1)
 	{
 		struct rts_pll_state pstate;

+		if(tmo_expired(&lock_tmo))
+		{
+			TRACE(TRACE_ERROR, "Can't lock the PLL. If running in the GrandMaster mode, are you sure the 1-PPS and 10 MHz reference clock signals are properly connected?");
+			return -1;
+		}
+
 		if(rts_get_state(&pstate) < 0)
 			return -1;

 		if(pstate.flags & RTS_DMTD_LOCKED)
-			return 0;
+			break;

 		usleep(100000);
 	}

+
+	if(hal_config_get_int("timing.use_nmea", &use_utc) < 0)
+		use_utc = 0;
+	
+	if(timing_mode == HAL_TIMING_MODE_GRAND_MASTER && use_utc)
+	{
+		TRACE(TRACE_INFO, "re-syncing to UTC from serial port");
+		nmea_resync_ppsgen("/dev/ttyS2");
+	}
 			
-	TRACE(TRACE_ERROR, "Can't lock the PLL. If running in the GrandMaster mode, are you sure the 1-PPS and 10 MHz reference clock signals are properly connected?");
-	return -1;
+	return 0;
 }

 int hal_get_timing_mode()