From 1c31c99edd9de9dcb403750b04041eccc751ac5e Mon Sep 17 00:00:00 2001 From: John Crispin Date: Tue, 29 Jul 2014 05:39:38 +0100 Subject: [PATCH] Import --- CMakeLists.txt | 22 ++++ log.h | 33 ++++++ main.c | 115 +++++++++++++++++++ nmea.c | 344 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ nmea.h | 30 +++++ 5 files changed, 544 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 log.h create mode 100644 main.c create mode 100644 nmea.c create mode 100644 nmea.h diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..326e649 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 2.6) + +PROJECT(ugps C) +ADD_DEFINITIONS(-Os -ggdb -Wall -Werror --std=gnu99 -Wmissing-declarations) + +SET(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS "") + +SET(SOURCES main.c nmea) + +SET(LIBS ubox ubus) + +IF(DEBUG) + ADD_DEFINITIONS(-DDEBUG -g3) +ENDIF() + +ADD_EXECUTABLE(ugps ${SOURCES}) + +TARGET_LINK_LIBRARIES(ugps ${LIBS}) + +INSTALL(TARGETS ugps + RUNTIME DESTINATION sbin +) diff --git a/log.h b/log.h new file mode 100644 index 0000000..b8ae621 --- /dev/null +++ b/log.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2013 Felix Fietkau + * Copyright (C) 2013 John Crispin + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License version 2.1 + * as published by the Free Software Foundation + * + * 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. + */ + +#ifndef __LOG_H +#define __LOG_H + +#include +#include + +#define LOG(fmt, ...) do { \ + syslog(LOG_INFO, fmt, ## __VA_ARGS__); \ + fprintf(stderr, "ugps: "fmt, ## __VA_ARGS__); \ + } while (0) + +#define ERROR(fmt, ...) do { \ + syslog(LOG_ERR, fmt, ## __VA_ARGS__); \ + fprintf(stderr, "ugps: "fmt, ## __VA_ARGS__); \ + } while (0) + +extern unsigned int debug; + +#endif diff --git a/main.c b/main.c new file mode 100644 index 0000000..841a2b6 --- /dev/null +++ b/main.c @@ -0,0 +1,115 @@ +/* + * 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. + * + * Copyright (C) 2014 John Crispin + */ + +#include +#include +#include +#include + +#include +#include + +#include "log.h" +#include "nmea.h" + +static struct ustream_fd stream; +static struct ubus_auto_conn conn; +static struct blob_buf b; +struct timespec stamp = { 0 }; + +void +gps_timestamp(void) +{ + clock_gettime(CLOCK_MONOTONIC, &stamp); +} + +static int +gps_info(struct ubus_context *ctx, struct ubus_object *obj, + struct ubus_request_data *req, const char *method, + struct blob_attr *msg) +{ + struct timespec now; + + clock_gettime(CLOCK_MONOTONIC, &now); + + blob_buf_init(&b, 0); + + if (!stamp.tv_sec) { + blobmsg_add_u8(&b, "signal", 0); + } else { + blobmsg_add_u32(&b, "age", now.tv_sec - stamp.tv_sec); + blobmsg_add_string(&b, "lattitude", lattitude); + blobmsg_add_string(&b, "longitude", longitude); + blobmsg_add_string(&b, "elivation", elivation); + blobmsg_add_string(&b, "course", course); + blobmsg_add_string(&b, "speed", speed); + } + ubus_send_reply(ctx, req, b.head); + + return UBUS_STATUS_OK; +} + +static const struct ubus_method gps_methods[] = { + UBUS_METHOD_NOARG("info", gps_info), +}; + +static struct ubus_object_type gps_object_type = + UBUS_OBJECT_TYPE("gps", gps_methods); + +static struct ubus_object gps_object = { + .name = "gps", + .type = &gps_object_type, + .methods = gps_methods, + .n_methods = ARRAY_SIZE(gps_methods), +}; + +static void +ubus_connect_handler(struct ubus_context *ctx) +{ + int ret; + + ret = ubus_add_object(ctx, &gps_object); + if (ret) + fprintf(stderr, "Failed to add object: %s\n", ubus_strerror(ret)); +} + +static int +usage(void) +{ + LOG("ugps \n"); + return -1; +} + +int +main(int argc, char ** argv) +{ + + signal(SIGPIPE, SIG_IGN); + + if (argc != 2) + return usage(); + + uloop_init(); + conn.cb = ubus_connect_handler; + ubus_auto_connect(&conn); + nmea_open(argv[1], &stream, B4800); + uloop_run(); + uloop_done(); + + return 0; +} diff --git a/nmea.c b/nmea.c new file mode 100644 index 0000000..2478f9f --- /dev/null +++ b/nmea.c @@ -0,0 +1,344 @@ +/* + * 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. + * + * Copyright (C) 2014 John Crispin + */ + +#define _BSD_SOURCE +#define _XOPEN_SOURCE +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "log.h" +#include "nmea.h" + +#define MAX_NMEA_PARAM 20 +#define MAX_TIME_OFFSET 2 +#define MAX_BAD_TIME 3 + +struct nmea_param { + char *str; + int num; +} nmea_params[MAX_NMEA_PARAM]; + +static int nmea_bad_time; +char longitude[32] = { 0 }, lattitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 }; +int gps_valid = 0; + +static void +nmea_txt_cb(void) +{ + char *ids[] = { "ERROR", "WARNING", "NOTICE", }; + + if (nmea_params[3].num < 0 || nmea_params[3].num > 2) + nmea_params[3].num = 0; + + LOG("%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str); +} + +static void +nmea_rmc_cb(void) +{ + struct tm tm; + char tmp[256]; + + if (*nmea_params[2].str != 'A') { + gps_valid = 0; + fprintf(stderr, "waiting for valid signal\n"); + return; + } + + gps_valid = 1; + memset(&tm, 0, sizeof(tm)); + tm.tm_isdst = 1; + + if (!strptime(nmea_params[1].str, "%H%M%S", &tm)) + ERROR("failed to parse time\n"); + else if (!strptime(nmea_params[9].str, "%d%m%y", &tm)) + ERROR("failed to parse date\n"); + else { + /* is there a libc api for the tz adjustment ? */ + struct timeval tv = { mktime(&tm), 0 }; + struct timeval cur; + + strftime(tmp, 256, "%D %02H:%02M:%02S", &tm); + LOG("date: %s UTC\n", tmp); + + tv.tv_sec -= timezone; + if (daylight) + tv.tv_sec += 3600; + + gettimeofday(&cur, NULL); + + if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) { + if (++nmea_bad_time > MAX_BAD_TIME) { + LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp); + settimeofday(&tv, NULL); + } + } else { + nmea_bad_time = 0; + } + } + + if (strlen(nmea_params[3].str) != 9 || strlen(nmea_params[5].str) != 10) { + ERROR("lat/lng have invalid string length\n"); + } else { + int latd, latm, lats; + int lngd, lngm, lngs; + float flats, flngs; + LOG("position: %s, %s\n", + nmea_params[3].str, nmea_params[5].str); + latm = atoi(&nmea_params[3].str[2]); + nmea_params[3].str[2] = '\0'; + latd = atoi(nmea_params[3].str); + lats = atoi(&nmea_params[3].str[5]); + if (*nmea_params[4].str != 'N') + latm *= -1; + + lngm = atoi(&nmea_params[5].str[3]); + nmea_params[5].str[3] = '\0'; + lngd = atoi(nmea_params[5].str); + lngs = atoi(&nmea_params[5].str[6]); + if (*nmea_params[6].str != 'E') + lngm *= -1; + + flats = lats; + flats *= 60; + flats /= 10000; + + flngs = lngs; + flngs *= 60; + flngs /= 10000; + +#define ms_to_deg(x, y) (((x * 10000) + y) / 60) + + LOG("position: %d°%d.%04d, %d°%d.%04d\n", + latd, latm, lats, lngd, lngm, lngs); + LOG("position: %d°%d'%.1f\" %d°%d'%.1f\"\n", + latd, latm, flats, lngd, lngm, flngs); + + snprintf(lattitude, sizeof(lattitude), "%d.%d", latd, ms_to_deg(latm, lats)); + snprintf(longitude, sizeof(longitude), "%d.%d", lngd, ms_to_deg(lngm, lngs)); + LOG("position: %s %s\n", lattitude, longitude); + gps_timestamp(); + } +} + +static void +nmea_gga_cb(void) +{ + if (!gps_valid) + return; + strncpy(elivation, nmea_params[9].str, sizeof(elivation)); + LOG("height: %s\n", elivation); +} + +static void +nmea_vtg_cb(void) +{ + if (!gps_valid) + return; + strncpy(course, nmea_params[1].str, sizeof(course)); + strncpy(speed, nmea_params[6].str, sizeof(speed)); + LOG("course: %s\n", course); + LOG("speed: %s\n", speed); +} + +static struct nmea_msg { + char *msg; + int cnt; + void (*handler) (void); +} nmea_msgs[] = { + { + .msg = "TXT", + .cnt = 5, + .handler = nmea_txt_cb, + }, { + .msg = "RMC", + .cnt = 11, + .handler = nmea_rmc_cb, + }, { + .msg = "GGA", + .cnt = 14, + .handler = nmea_gga_cb, + }, { + .msg = "VTG", + .cnt = 9, + .handler = nmea_vtg_cb, + }, +}; + +static int +nmea_verify_checksum(char *s) +{ + char *csum = strrchr(s, '*'); + int isum, c = 0; + + if (!csum) + return -1; + + *csum = '\0'; + csum++; + isum = strtol(csum, NULL, 16); + + while(*s) + c ^= *s++; + + if (isum != c) + return -1; + + return 0; +} + +static int +nmea_tokenize(char *msg) +{ + int cnt = 0; + char *tok = strtok(msg, ","); + + while (tok && cnt < MAX_NMEA_PARAM) { + nmea_params[cnt].str = tok; + nmea_params[cnt].num = atoi(tok); + cnt++; + tok = strtok(NULL, ","); + } + + return cnt; +} + +static void +nmea_process(char *a) +{ + char *csum; + int cnt, i; + + if (strncmp(a, "$GP", 3)) + return; + + a++; + csum = strrchr(a, '*'); + if (!csum) + return; + + if (nmea_verify_checksum(a)) { + ERROR("nmea message has invlid checksum\n"); + return; + } + + cnt = nmea_tokenize(&a[2]); + if (cnt < 0) { + ERROR("failed to tokenize %s\n", a);\ + return; + } + + for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) { + if (strcmp(nmea_params[0].str, nmea_msgs[i].msg)) + continue; + if (nmea_msgs[i].cnt <= cnt) + nmea_msgs[i].handler(); + else + ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt); + return; + } +} + +static int +nmea_consume(struct ustream *s, char **a) +{ + char *eol = strstr(*a, "\n"); + + if (!eol) + return -1; + + *eol++ = '\0'; + + nmea_process(*a); + + ustream_consume(s, eol - *a); + *a = eol; + + return 0; +} + +static void +nmea_msg_cb(struct ustream *s, int bytes) +{ + int len; + char *a = ustream_get_read_buf(s, &len); + + while (!nmea_consume(s, &a)) + ; +} + +static void nmea_notify_cb(struct ustream *s) +{ + if (!s->eof) + return; + + ERROR("tty error, shutting down\n"); + exit(-1); +} + +int +nmea_open(char *dev, struct ustream_fd *s, speed_t speed) +{ + struct termios tio; + int tty; + + tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (tty < 0) { + ERROR("%s: device open failed\n", dev); + return -1; + } + + tcgetattr(tty, &tio); + tio.c_cflag |= CREAD; + tio.c_cflag |= CS8; + tio.c_iflag |= IGNPAR; + tio.c_lflag &= ~(ICANON); + tio.c_lflag &= ~(ECHO); + tio.c_lflag &= ~(ECHOE); + tio.c_lflag &= ~(ISIG); + tio.c_cc[VMIN] = 1; + tio.c_cc[VTIME] = 0; + cfsetispeed(&tio, speed); + cfsetospeed(&tio, speed); + tcsetattr(tty, TCSANOW, &tio); + + s->stream.string_data = true; + s->stream.notify_read = nmea_msg_cb; + s->stream.notify_state = nmea_notify_cb; + + ustream_fd_init(s, tty); + + tcflush(tty, TCIFLUSH); + + return 0; +} diff --git a/nmea.h b/nmea.h new file mode 100644 index 0000000..fea9879 --- /dev/null +++ b/nmea.h @@ -0,0 +1,30 @@ +/* + * 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. + * + * Copyright (C) 2014 John Crispin + */ + +#ifndef __TTY_H_ +#define __TTY_H_ + +#include + +#include + +extern char longitude[32], lattitude[32], course[16], speed[16], elivation[16]; +extern int nmea_open(char *dev, struct ustream_fd *s, speed_t speed); +extern void gps_timestamp(void); + +#endif -- 2.11.0