diff --git a/gpsutils/Kconfig b/gpsutils/Kconfig new file mode 100644 index 000000000..9797cd6f3 --- /dev/null +++ b/gpsutils/Kconfig @@ -0,0 +1,10 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +menu "GPS Utilities" + +source "$APPSDIR/gpsutils/minmea/Kconfig" + +endmenu # GPS Utilities diff --git a/gpsutils/Make.defs b/gpsutils/Make.defs new file mode 100644 index 000000000..bdd232f1d --- /dev/null +++ b/gpsutils/Make.defs @@ -0,0 +1,37 @@ +############################################################################ +# apps/gpsutils/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2016 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include $(wildcard gpsutils/*/Make.defs) diff --git a/gpsutils/Makefile b/gpsutils/Makefile new file mode 100644 index 000000000..f34706ee0 --- /dev/null +++ b/gpsutils/Makefile @@ -0,0 +1,36 @@ +############################################################################ +# apps/gpsutils/Makefile +# +# Copyright (C) 2016 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include $(APPDIR)/Directory.mk diff --git a/gpsutils/README.txt b/gpsutils/README.txt new file mode 100644 index 000000000..548f83895 --- /dev/null +++ b/gpsutils/README.txt @@ -0,0 +1,12 @@ +External Libraries +^^^^^^^^^^^^^^^^^^ + + The apps/gpsutils directory is used to include libraries from external + projects that are not part of NuttX Applications, but are useful for NuttX + developers and users. + +gpsutils/minmea +^^^^^^^^^^^^^^^^^^ + + MINMEA is a NMEA parser developed by Kosma Moczek. Kosma is also a NuttX + contributor. diff --git a/gpsutils/minmea/.gitignore b/gpsutils/minmea/.gitignore new file mode 100644 index 000000000..fa1ec7579 --- /dev/null +++ b/gpsutils/minmea/.gitignore @@ -0,0 +1,11 @@ +/Make.dep +/.depend +/.built +/*.asm +/*.obj +/*.rel +/*.lst +/*.sym +/*.adb +/*.lib +/*.src diff --git a/gpsutils/minmea/COPYING b/gpsutils/minmea/COPYING new file mode 100644 index 000000000..012ddd952 --- /dev/null +++ b/gpsutils/minmea/COPYING @@ -0,0 +1,14 @@ + DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE + Version 2, December 2004 + + Copyright (C) 2004 Sam Hocevar + + Everyone is permitted to copy and distribute verbatim or modified + copies of this license document, and changing it is allowed as long + as the name is changed. + + DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. You just DO WHAT THE FUCK YOU WANT TO. + diff --git a/gpsutils/minmea/Kconfig b/gpsutils/minmea/Kconfig new file mode 100644 index 000000000..c762f8ebd --- /dev/null +++ b/gpsutils/minmea/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config GPSUTILS_MINMEA_LIB + bool "MINMEA NMEA Library" + default n + ---help--- + Enable support for the MINMEA NMEA library. + + NOTE: This library depends on having some version of math.h + at include/nuttx. There are some different ways to accomplish + this. See the discussion in the top-level nuttx/README.txt file. + +if GPSUTILS_MINMEA_LIB +endif diff --git a/gpsutils/minmea/Make.defs b/gpsutils/minmea/Make.defs new file mode 100644 index 000000000..12e95d8c2 --- /dev/null +++ b/gpsutils/minmea/Make.defs @@ -0,0 +1,39 @@ +############################################################################ +# apps/gpsutils/minmea/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2016 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +ifeq ($(CONFIG_GPSUTILS_MINMEA_LIB),y) +CONFIGURED_APPS += gpsutils/minmea +endif diff --git a/gpsutils/minmea/Makefile b/gpsutils/minmea/Makefile new file mode 100644 index 000000000..d522f536f --- /dev/null +++ b/gpsutils/minmea/Makefile @@ -0,0 +1,99 @@ +############################################################################ +# apps/gpsutils/minmea/Makefile +# +# Copyright (C) 2016 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NSH Library + +ASRCS = +CSRCS = minmea.c + +CFLAGS += -std=c99 + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\libapps$(LIBEXT) +else +ifeq ($(WINTOOL),y) + BIN = ..\\libapps$(LIBEXT) +else + BIN = ../libapps$(LIBEXT) +endif +endif + +ROOTDEPPATH = --dep-path . +VPATH = + +# Build targets + +all: .built +.PHONY: context .depend depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + $(call ARCHIVE, $(BIN), $(OBJS)) + @touch .built + +install: + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, .built) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/gpsutils/minmea/README.md b/gpsutils/minmea/README.md new file mode 100644 index 000000000..299ce3caa --- /dev/null +++ b/gpsutils/minmea/README.md @@ -0,0 +1,145 @@ +# minmea, a lightweight GPS NMEA 0183 parser library + +[![Build Status](https://travis-ci.org/cloudyourcar/minmea.svg?branch=master)](https://travis-ci.org/cloudyourcar/minmea) + +Minmea is a minimalistic GPS parser library written in pure C intended for +resource-constrained platforms, especially microcontrollers and other embedded +systems. + +## Features + +* Written in ISO C99. +* No dynamic memory allocation. +* No floating point usage in the core library. +* Supports both fixed and floating point values. +* One source file and one header - can't get any simpler. +* Easily extendable to support new sentences. +* Complete with a test suite and static analysis. + +## Supported sentences + +* ``RMC`` (Recommended Minimum: position, velocity, time) +* ``GGA`` (Fix Data) +* ``GSA`` (DOP and active satellites) +* ``GLL`` (Geographic Position: Latitude/Longitude) +* ``GST`` (Pseudorange Noise Statistics) +* ``GSV`` (Satellites in view) + +Adding support for more sentences is trivial; see ``minmea.c`` source. + +## Fractional number format + +Internally, minmea stores fractional numbers as pairs of two integers: ``{value, scale}``. +For example, a value of ``"-123.456"`` would be parsed as ``{-123456, 1000}``. As this +format is quite unwieldy, minmea provides the following convenience functions for converting +to either fixed-point or floating-point format: + +* ``minmea_rescale({-123456, 1000}, 10) => -1235`` +* ``minmea_float({-123456, 1000}) => -123.456`` + +The compound type ``struct minmea_float`` uses ``int_least32_t`` internally. Therefore, +the coordinate precision is guaranteed to be at least ``[+-]DDDMM.MMMMM`` (five decimal digits) +or ±20cm LSB at the equator. + +## Coordinate format + +NMEA uses the clunky ``DDMM.MMMM`` format which, honestly, is not good in the internet era. +Internally, minmea stores it as a fractional number (see above); for practical uses, +the value should be probably converted to the DD.DDDDD floating point format using the +following function: + +* ``minmea_tocoord({-375165, 100}) => -37.860832`` + +The library doesn't perform this conversion automatically for the following reasons: + +* The conversion is not reversible. +* It requires floating point hardware. +* The user might want to perform this conversion later on or retain the original values. + +## Example + +```c +char line[MINMEA_MAX_LENGTH]; +while (fgets(line, sizeof(line), stdin) != NULL) { + switch (minmea_sentence_id(line)) { + case MINMEA_SENTENCE_RMC: { + struct minmea_sentence_rmc frame; + if (minmea_parse_rmc(&frame, line)) { + printf("$RMC: raw coordinates and speed: (%d/%d,%d/%d) %d/%d\n", + frame.latitude.value, frame.latitude.scale, + frame.longitude.value, frame.longitude.scale, + frame.speed.value, frame.speed.scale); + printf("$RMC fixed-point coordinates and speed scaled to three decimal places: (%d,%d) %d\n", + minmea_rescale(&frame.latitude, 1000), + minmea_rescale(&frame.longitude, 1000), + minmea_rescale(&frame.speed, 1000)); + printf("$RMC floating point degree coordinates and speed: (%f,%f) %f\n", + minmea_tocoord(&frame.latitude), + minmea_tocoord(&frame.longitude), + minmea_tofloat(&frame.speed)); + } + } break; + + case MINMEA_SENTENCE_GGA: { + struct minmea_sentence_gga frame; + if (minmea_parse_gga(&frame, line)) { + printf("$GGA: fix quality: %d\n", frame.fix_quality); + } + } break; + + case MINMEA_SENTENCE_GSV: { + struct minmea_sentence_gsv frame; + if (minmea_parse_gsv(&frame, line)) { + printf("$GSV: message %d of %d\n", frame.msg_nr, frame.total_msgs); + printf("$GSV: sattelites in view: %d\n", frame.total_sats); + for (int i = 0; i < 4; i++) + printf("$GSV: sat nr %d, elevation: %d, azimuth: %d, snr: %d dbm\n", + frame.sats[i].nr, + frame.sats[i].elevation, + frame.sats[i].azimuth, + frame.sats[i].snr); + } + } break; + } +} +``` + +## Integration with your project + +Simply add ``minmea.[ch]`` to your project, ``#include "minmea.h"`` and you're +good to go. + +## Running unit tests + +Building and running the tests requires the following: + +* Check Framework (http://check.sourceforge.net/). +* Clang Static Analyzer (http://clang-analyzer.llvm.org/). + +If you have both in your ``$PATH``, running the tests should be as simple as +typing ``make``. + +## Limitations + +* Only a handful of frames is supported right now. +* There's no support for omitting parts of the library from building. As + a workaround, use the ``-ffunction-sections -Wl,--gc-sections`` linker flags + (or equivalent) to remove the unused functions (parsers) from the final image. +* Some systems lack ``timegm``. On these systems, the recommended course of + action is to build with ``-Dtimegm=mktime`` which will work correctly as long + the system runs in the default ``UTC`` timezone. Native Windows builds should + use ``-Dtimegm=_mkgmtime`` instead which will work correctly in all timezones. + +## Bugs + +There are plenty. Report them on GitHub, or - even better - open a pull request. +Please write unit tests for any new functions you add - it's fun! + +## Licensing + +Minmea is open source software; see ``COPYING`` for amusement. Email me if the +license bothers you and I'll happily re-license under anything else under the sun. + +## Author + +Minmea was written by Kosma Moczek <kosma@cloudyourcar.com> at Cloud Your Car. diff --git a/gpsutils/minmea/minmea.c b/gpsutils/minmea/minmea.c new file mode 100644 index 000000000..a715762aa --- /dev/null +++ b/gpsutils/minmea/minmea.c @@ -0,0 +1,854 @@ +/**************************************************************************** + * apps/include/cle.h + * + * Copyright © 2014 Kosma Moczek + * + * This program is free software. It comes without any warranty, to the extent + * permitted by applicable law. You can redistribute it and/or modify it under + * the terms of the Do What The Fuck You Want To Public License, Version 2, as + * published by Sam Hocevar. See the COPYING file for more details. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define boolstr(s) ((s) ? "true" : "false") + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int hex2int(char c) +{ + if (c >= '0' && c <= '9') + { + return c - '0'; + } + + if (c >= 'A' && c <= 'F') + { + return c - 'A' + 10; + } + + if (c >= 'a' && c <= 'f') + { + return c - 'a' + 10; + } + + return -1; +} + +static inline bool minmea_isfield(char c) +{ + return isprint((unsigned char) c) && c != ',' && c != '*'; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +uint8_t minmea_checksum(FAR const char *sentence) +{ + uint8_t checksum = 0x00; + + /* Support senteces with or without the starting dollar sign. */ + + if (*sentence == '$') + { + sentence++; + } + + /* The optional checksum is an XOR of all bytes between "$" and "*". */ + + while (*sentence && *sentence != '*') + { + checksum ^= *sentence++; + } + + return checksum; +} + +bool minmea_check(FAR const char *sentence, bool strict) +{ + uint8_t checksum = 0x00; + + /* Sequence length is limited. */ + + if (strlen(sentence) > MINMEA_MAX_LENGTH + 3) + { + return false; + } + + /* A valid sentence starts with "$". */ + + if (*sentence++ != '$') + { + return false; + } + + /* The optional checksum is an XOR of all bytes between "$" and "*". */ + + while (*sentence && *sentence != '*' && + isprint((unsigned char) *sentence)) + { + checksum ^= *sentence++; + } + + /* If checksum is present... */ + + if (*sentence == '*') + { + int upper; + int lower; + int expected; + + /* Extract checksum. */ + + sentence++; + upper = hex2int(*sentence++); + + if (upper == -1) + { + return false; + } + + lower = hex2int(*sentence++); + if (lower == -1) + { + return false; + } + + expected = upper << 4 | lower; + + /* Check for checksum mismatch. */ + + if (checksum != expected) + { + return false; + } + } + else if (strict) + { + /* Discard non-checksummed frames in strict mode. */ + + return false; + } + + /* The only stuff allowed at this point is a newline. */ + + if (*sentence && strcmp(sentence, "\n") && + strcmp(sentence, "\r\n")) + { + return false; + } + + return true; +} + +bool minmea_scan(FAR const char *sentence, FAR const char *format, ...) +{ + bool result = false; + bool optional = false; + va_list ap; + va_start(ap, format); + + const char *field = sentence; +#define next_field() \ + do \ + { \ + /* Progress to the next field. */ \ + while (minmea_isfield(*sentence)) \ + { \ + sentence++; \ + /* Make sure there is a field there. */ \ + if (*sentence == ',') \ + { \ + sentence++; \ + field = sentence; \ + } \ + else \ + { \ + field = NULL; \ + } \ + } \ + } \ + while (0) + + while (*format) + { + char type = *format++; + + if (type == ';') + { + /* All further fields are optional. */ + + optional = true; + continue; + } + + if (!field && !optional) + { + /* Field requested but we ran out if input. Bail out. */ + + goto parse_error; + } + + switch (type) + { + /* Single character field (char). */ + + case 'c': + { + char value = '\0'; + + if (field && minmea_isfield(*field)) + { + value = *field; + } + + *va_arg(ap, char *) = value; + } + break; + + /* Single character direction field (int). */ + + case 'd': + { + int value = 0; + + if (field && minmea_isfield(*field)) + { + switch (*field) + { + case 'N': + case 'E': + value = 1; + break; + + case 'S': + case 'W': + value = -1; + break; + + default: + goto parse_error; + } + } + + *va_arg(ap, int *) = value; + } + break; + + /* Fractional value with scale (struct minmea_float). */ + + case 'f': + { + int sign = 0; + int_least32_t value = -1; + int_least32_t scale = 0; + + if (field) + { + while (minmea_isfield(*field)) + { + if (*field == '+' && !sign && value == -1) + { + sign = 1; + } + else if (*field == '-' && !sign && value == -1) + { + sign = -1; + } + else if (isdigit((unsigned char) *field)) + { + int digit = *field - '0'; + + if (value == -1) + { + value = 0; + } + + if (value > (INT_LEAST32_MAX-digit) / 10) + { + /* we ran out of bits, what do we do? */ + + if (scale) + { + /* truncate extra precision */ + + break; + } + else + { + /* integer overflow. bail out. */ + + goto parse_error; + } + } + + value = (10 * value) + digit; + if (scale) + { + scale *= 10; + } + } + else if (*field == '.' && scale == 0) + { + scale = 1; + } + else if (*field == ' ') + { + /* Allow spaces at the start of the field. Not NMEA + * conformant, but some modules do this. + */ + + if (sign != 0 || value != -1 || scale != 0) + { + goto parse_error; + } + } + else + { + goto parse_error; + } + + field++; + } + } + + if ((sign || scale) && value == -1) + { + goto parse_error; + } + + if (value == -1) + { + /* No digits were scanned. */ + + value = 0; + scale = 0; + } + else if (scale == 0) + { + /* No decimal point. */ + + scale = 1; + } + + if (sign) + { + value *= sign; + } + + *va_arg(ap, struct minmea_float *) = + (struct minmea_float) {value, scale}; + } + break; + + /* Integer value, default 0 (int). */ + + case 'i': + { + int value = 0; + + if (field) + { + FAR char *endptr; + + value = strtol(field, &endptr, 10); + if (minmea_isfield(*endptr)) + { + goto parse_error; + } + } + + *va_arg(ap, int *) = value; + } + break; + + /* String value (char *). */ + + case 's': + { + char *buf = va_arg(ap, char *); + + if (field) + { + while (minmea_isfield(*field)) + { + *buf++ = *field++; + } + } + + *buf = '\0'; + } + break; + + /* NMEA talker+sentence identifier (char *). */ + + case 't': + { + char *buf; + int f; + + /* This field is always mandatory. */ + + if (!field) + { + goto parse_error; + } + + if (field[0] != '$') + { + goto parse_error; + } + + for (f = 0; f < 5; f++) + { + if (!minmea_isfield(field[1+f])) + { + goto parse_error; + } + } + + buf = va_arg(ap, char *); + memcpy(buf, field+1, 5); + buf[5] = '\0'; + } + break; + + /* Date (int, int, int), -1 if empty. */ + + case 'D': + { + struct minmea_date *date = va_arg(ap, struct minmea_date *); + int d = -1; + int m = -1; + int y = -1; + int f; + + if (field && minmea_isfield(*field)) + { + /* Always six digits. */ + + for (f = 0; f < 6; f++) + { + if (!isdigit((unsigned char) field[f])) + { + goto parse_error; + } + } + + d = strtol((char[]) {field[0], field[1], '\0'}, NULL, 10); + m = strtol((char[]) {field[2], field[3], '\0'}, NULL, 10); + y = strtol((char[]) {field[4], field[5], '\0'}, NULL, 10); + } + + date->day = d; + date->month = m; + date->year = y; + } + break; + + /* Time (int, int, int, int), -1 if empty. */ + + case 'T': + { + struct minmea_time *time_ = va_arg(ap, struct minmea_time *); + int h = -1; + int i = -1; + int s = -1; + int u = -1; + int f; + + if (field && minmea_isfield(*field)) + { + /* Minimum required: integer time. */ + + for (f = 0; f < 6; f++) + { + if (!isdigit((unsigned char) field[f])) + { + goto parse_error; + } + } + + h = strtol((char[]) {field[0], field[1], '\0'}, NULL, 10); + i = strtol((char[]) {field[2], field[3], '\0'}, NULL, 10); + s = strtol((char[]) {field[4], field[5], '\0'}, NULL, 10); + field += 6; + + /* Extra: fractional time. Saved as microseconds. */ + + if (*field++ == '.') + { + int value = 0; + int scale = 1000000; + + while (isdigit((unsigned char) *field) && scale > 1) + { + value = (value * 10) + (*field++ - '0'); + scale /= 10; + } + + u = value * scale; + } + else + { + u = 0; + } + } + + time_->hours = h; + time_->minutes = i; + time_->seconds = s; + time_->microseconds = u; + } + break; + + /* Ignore the field. */ + + case '_': + { + } + break; + + default: + { + goto parse_error; + } + break; + } + + next_field(); + } + + result = true; + +parse_error: + va_end(ap); + return result; +} + +bool minmea_talker_id(char talker[3], FAR const char *sentence) +{ + char type[6]; + + if (!minmea_scan(sentence, "t", type)) + { + return false; + } + + talker[0] = type[0]; + talker[1] = type[1]; + talker[2] = '\0'; + + return true; +} + +enum minmea_sentence_id minmea_sentence_id(FAR const char *sentence, + bool strict) +{ + if (!minmea_check(sentence, strict)) + return MINMEA_INVALID; + + char type[6]; + if (!minmea_scan(sentence, "t", type)) + { + return MINMEA_INVALID; + } + + if (!strcmp(type+2, "RMC")) + { + return MINMEA_SENTENCE_RMC; + } + + if (!strcmp(type+2, "GGA")) + { + return MINMEA_SENTENCE_GGA; + } + + if (!strcmp(type+2, "GSA")) + { + return MINMEA_SENTENCE_GSA; + } + + if (!strcmp(type+2, "GLL")) + { + return MINMEA_SENTENCE_GLL; + } + + if (!strcmp(type+2, "GST")) + { + return MINMEA_SENTENCE_GST; + } + + if (!strcmp(type+2, "GSV")) + { + return MINMEA_SENTENCE_GSV; + } + + return MINMEA_UNKNOWN; +} + +bool minmea_parse_rmc(FAR struct minmea_sentence_rmc *frame, + FAR const char *sentence) +{ + /* $GPRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130998,011.3,E*62 */ + + char type[6]; + char validity; + int latitude_direction; + int longitude_direction; + int variation_direction; + + if (!minmea_scan(sentence, "tTcfdfdffDfd", + type, + &frame->time, + &validity, + &frame->latitude, &latitude_direction, + &frame->longitude, &longitude_direction, + &frame->speed, + &frame->course, + &frame->date, + &frame->variation, &variation_direction)) + { + return false; + } + + if (strcmp(type+2, "RMC")) + { + return false; + } + + frame->valid = (validity == 'A'); + frame->latitude.value *= latitude_direction; + frame->longitude.value *= longitude_direction; + frame->variation.value *= variation_direction; + + return true; +} + +bool minmea_parse_gga(FAR struct minmea_sentence_gga *frame, + FAR const char *sentence) +{ + /* $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47 */ + + char type[6]; + int latitude_direction; + int longitude_direction; + + if (!minmea_scan(sentence, "tTfdfdiiffcfci_", + type, + &frame->time, + &frame->latitude, &latitude_direction, + &frame->longitude, &longitude_direction, + &frame->fix_quality, + &frame->satellites_tracked, + &frame->hdop, + &frame->altitude, &frame->altitude_units, + &frame->height, &frame->height_units, + &frame->dgps_age)) + { + return false; + } + + if (strcmp(type+2, "GGA")) + { + return false; + } + + frame->latitude.value *= latitude_direction; + frame->longitude.value *= longitude_direction; + + return true; +} + +bool minmea_parse_gsa(FAR struct minmea_sentence_gsa *frame, + FAR const char *sentence) +{ + /* $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39 */ + + char type[6]; + + if (!minmea_scan(sentence, "tciiiiiiiiiiiiifff", + type, + &frame->mode, + &frame->fix_type, + &frame->sats[0], + &frame->sats[1], + &frame->sats[2], + &frame->sats[3], + &frame->sats[4], + &frame->sats[5], + &frame->sats[6], + &frame->sats[7], + &frame->sats[8], + &frame->sats[9], + &frame->sats[10], + &frame->sats[11], + &frame->pdop, + &frame->hdop, + &frame->vdop)) + { + return false; + } + + if (strcmp(type+2, "GSA")) + { + return false; + } + + return true; +} + +bool minmea_parse_gll(FAR struct minmea_sentence_gll *frame, + FAR const char *sentence) +{ + /* $GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41$; */ + + char type[6]; + int latitude_direction; + int longitude_direction; + + if (!minmea_scan(sentence, "tfdfdTc;c", + type, + &frame->latitude, &latitude_direction, + &frame->longitude, &longitude_direction, + &frame->time, + &frame->status, + &frame->mode)) + { + return false; + } + + if (strcmp(type+2, "GLL")) + { + return false; + } + + frame->latitude.value *= latitude_direction; + frame->longitude.value *= longitude_direction; + + return true; +} + +bool minmea_parse_gst(FAR struct minmea_sentence_gst *frame, + FAR const char *sentence) +{ + /* $GPGST,024603.00,3.2,6.6,4.7,47.3,5.8,5.6,22.0*58 */ + + char type[6]; + + if (!minmea_scan(sentence, "tTfffffff", + type, + &frame->time, + &frame->rms_deviation, + &frame->semi_major_deviation, + &frame->semi_minor_deviation, + &frame->semi_major_orientation, + &frame->latitude_error_deviation, + &frame->longitude_error_deviation, + &frame->altitude_error_deviation)) + { + return false; + } + + if (strcmp(type+2, "GST")) + { + return false; + } + + return true; +} + +bool minmea_parse_gsv(FAR struct minmea_sentence_gsv *frame, + FAR const char *sentence) +{ + /* $GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74 + * $GPGSV,3,3,11,22,42,067,42,24,14,311,43,27,05,244,00,,,,*4D + * $GPGSV,4,2,11,08,51,203,30,09,45,215,28*75 + * $GPGSV,4,4,13,39,31,170,27*40 + * $GPGSV,4,4,13*7B */ + + char type[6]; + + if (!minmea_scan(sentence, "tiii;iiiiiiiiiiiiiiii", + type, + &frame->total_msgs, + &frame->msg_nr, + &frame->total_sats, + &frame->sats[0].nr, + &frame->sats[0].elevation, + &frame->sats[0].azimuth, + &frame->sats[0].snr, + &frame->sats[1].nr, + &frame->sats[1].elevation, + &frame->sats[1].azimuth, + &frame->sats[1].snr, + &frame->sats[2].nr, + &frame->sats[2].elevation, + &frame->sats[2].azimuth, + &frame->sats[2].snr, + &frame->sats[3].nr, + &frame->sats[3].elevation, + &frame->sats[3].azimuth, + &frame->sats[3].snr)) + { + return false; + } + + if (strcmp(type+2, "GSV")) + { + return false; + } + + return true; +} + +int minmea_gettime(FAR struct timespec *ts, + FAR const struct minmea_date *date, + FAR const struct minmea_time *time_) +{ + struct tm tm; + + if (date->year == -1 || time_->hours == -1) + { + return -1; + } + + memset(&tm, 0, sizeof(tm)); + tm.tm_year = 2000 + date->year - 1900; + tm.tm_mon = date->month - 1; + tm.tm_mday = date->day; + tm.tm_hour = time_->hours; + tm.tm_min = time_->minutes; + tm.tm_sec = time_->seconds; + + /* See README.md if your system lacks timegm(). */ + + time_t timestamp = mktime(&tm); + if (timestamp != -1) + { + ts->tv_sec = timestamp; + ts->tv_nsec = time_->microseconds * 1000; + return 0; + } + else + { + return -1; + } +} diff --git a/include/gpsutils/minmea.h b/include/gpsutils/minmea.h new file mode 100644 index 000000000..a04ffc8ee --- /dev/null +++ b/include/gpsutils/minmea.h @@ -0,0 +1,285 @@ +/**************************************************************************** + * apps/include/cle.h + * + * Copyright © 2014 Kosma Moczek + * + * This program is free software. It comes without any warranty, to the extent + * permitted by applicable law. You can redistribute it and/or modify it under + * the terms of the Do What The Fuck You Want To Public License, Version 2, as + * published by Sam Hocevar. See the COPYING file for more details. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_GPSUTILS_MINMEA_H +#define __APPS_INCLUDE_GPSUTILS_MINMEA_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define MINMEA_MAX_LENGTH 80 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +enum minmea_sentence_id +{ + MINMEA_INVALID = -1, + MINMEA_UNKNOWN = 0, + MINMEA_SENTENCE_RMC, + MINMEA_SENTENCE_GGA, + MINMEA_SENTENCE_GSA, + MINMEA_SENTENCE_GLL, + MINMEA_SENTENCE_GST, + MINMEA_SENTENCE_GSV, +}; + +struct minmea_float +{ + int_least32_t value; + int_least32_t scale; +}; + +struct minmea_date +{ + int day; + int month; + int year; +}; + +struct minmea_time +{ + int hours; + int minutes; + int seconds; + int microseconds; +}; + +struct minmea_sentence_rmc +{ + struct minmea_time time; + bool valid; + struct minmea_float latitude; + struct minmea_float longitude; + struct minmea_float speed; + struct minmea_float course; + struct minmea_date date; + struct minmea_float variation; +}; + +struct minmea_sentence_gga +{ + struct minmea_time time; + struct minmea_float latitude; + struct minmea_float longitude; + int fix_quality; + int satellites_tracked; + struct minmea_float hdop; + struct minmea_float altitude; + char altitude_units; + struct minmea_float height; + char height_units; + int dgps_age; +}; + +enum minmea_gll_status +{ + MINMEA_GLL_STATUS_DATA_VALID = 'A', + MINMEA_GLL_STATUS_DATA_NOT_VALID = 'V', +}; + +enum minmea_gll_mode +{ + MINMEA_GLL_MODE_AUTONOMOUS = 'A', + MINMEA_GLL_MODE_DPGS = 'D', + MINMEA_GLL_MODE_DR = 'E', +}; + +struct minmea_sentence_gll +{ + struct minmea_float latitude; + struct minmea_float longitude; + struct minmea_time time; + char status; + char mode; +}; + +struct minmea_sentence_gst +{ + struct minmea_time time; + struct minmea_float rms_deviation; + struct minmea_float semi_major_deviation; + struct minmea_float semi_minor_deviation; + struct minmea_float semi_major_orientation; + struct minmea_float latitude_error_deviation; + struct minmea_float longitude_error_deviation; + struct minmea_float altitude_error_deviation; +}; + +enum minmea_gsa_mode +{ + MINMEA_GPGSA_MODE_AUTO = 'A', + MINMEA_GPGSA_MODE_FORCED = 'M', +}; + +enum minmea_gsa_fix_type +{ + MINMEA_GPGSA_FIX_NONE = 1, + MINMEA_GPGSA_FIX_2D = 2, + MINMEA_GPGSA_FIX_3D = 3, +}; + +struct minmea_sentence_gsa +{ + char mode; + int fix_type; + int sats[12]; + struct minmea_float pdop; + struct minmea_float hdop; + struct minmea_float vdop; +}; + +struct minmea_sat_info +{ + int nr; + int elevation; + int azimuth; + int snr; +}; + +struct minmea_sentence_gsv +{ + int total_msgs; + int msg_nr; + int total_sats; + struct minmea_sat_info sats[4]; +}; + +#ifdef __cplusplus +extern "C" +{ +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* Calculate raw sentence checksum. Does not check sentence integrity. */ + +uint8_t minmea_checksum(FAR const char *sentence); + +/* Check sentence validity and checksum. Returns true for valid sentences. */ + +bool minmea_check(FAR const char *sentence, bool strict); + +/* Determine talker identifier. */ + +bool minmea_talker_id(char talker[3], FAR const char *sentence); + +/* Determine sentence identifier. */ + +enum minmea_sentence_id minmea_sentence_id(FAR const char *sentence, + bool strict); + +/* Scanf-like processor for NMEA sentences. Supports the following formats: + * c - single character (char *) + * d - direction, returned as 1/-1, default 0 (int *) + * f - fractional, returned as value + scale (int *, int *) + * i - decimal, default zero (int *) + * s - string (char *) + * t - talker identifier and type (char *) + * T - date/time stamp (int *, int *, int *) + * Returns true on success. See library source code for details. + */ + +bool minmea_scan(const char *sentence, const char *format, ...); + +/* Parse a specific type of sentence. Return true on success. */ + +bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence); +bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence); +bool minmea_parse_gsa(struct minmea_sentence_gsa *frame, const char *sentence); +bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence); +bool minmea_parse_gst(struct minmea_sentence_gst *frame, const char *sentence); +bool minmea_parse_gsv(struct minmea_sentence_gsv *frame, const char *sentence); + +/* Convert GPS UTC date/time representation to a UNIX timestamp. */ + +int minmea_gettime(FAR struct timespec *ts, + FAR const struct minmea_date *date, + FAR const struct minmea_time *time_); + +/* Rescale a fixed-point value to a different scale. Rounds towards zero. */ + +static inline int_least32_t minmea_rescale(FAR struct minmea_float *f, + int_least32_t new_scale) +{ + if (f->scale == 0) + { + return 0; + } + + if (f->scale == new_scale) + { + return f->value; + } + + if (f->scale > new_scale) + { + return (f->value + ((f->value > 0) - (f->value < 0)) * f->scale / + new_scale / 2) / (f->scale / new_scale); + } + else + { + return f->value * (new_scale / f->scale); + } +} + +/* Convert a fixed-point value to a floating-point value. + * Returns NaN for "unknown" values. + */ + +static inline float minmea_tofloat(FAR struct minmea_float *f) +{ + if (f->scale == 0) + { + return NAN; + } + + return (float) f->value / (float) f->scale; +} + +/* Convert a raw coordinate to a floating point DD.DDD... value. + * Returns NaN for "unknown" values. + */ + +static inline float minmea_tocoord(FAR struct minmea_float *f) +{ + if (f->scale == 0) + { + return NAN; + } + + int_least32_t degrees = f->value / (f->scale * 100); + int_least32_t minutes = f->value % (f->scale * 100); + + return (float) degrees + (float) minutes / (60 * f->scale); +} + +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_GPSUTILS_MINMEA_H */