sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add Kosma Moczek lightweith MEA parser, MINMEA

Browse Source
This commit is contained in:
Alan Carvalho de Assis 2016-02-20 10:24:02 -06:00 committed by Gregory Nutt
parent c6ac4d2581
commit 54d2bb16f3
12 changed files with 1559 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gpsutils/Kconfig Normal file
View File

@ -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

37
gpsutils/Make.defs Normal file
View File

@ -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 <gnutt@nuttx.org>
#
# 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)

36
gpsutils/Makefile Normal file
View File

@ -0,0 +1,36 @@
############################################################################
# apps/gpsutils/Makefile
#
# Copyright (C) 2016 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# 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

12
gpsutils/README.txt Normal file
View File

@ -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.

11
gpsutils/minmea/.gitignore vendored Normal file
View File

@ -0,0 +1,11 @@
/Make.dep
/.depend
/.built
/*.asm
/*.obj
/*.rel
/*.lst
/*.sym
/*.adb
/*.lib
/*.src

14
gpsutils/minmea/COPYING Normal file
View File

@ -0,0 +1,14 @@
DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
Version 2, December 2004
Copyright (C) 2004 Sam Hocevar <sam@hocevar.net>
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.

17
gpsutils/minmea/Kconfig Normal file
View File

@ -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

39
gpsutils/minmea/Make.defs Normal file
View File

@ -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 <gnutt@nuttx.org>
#
# 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

99
gpsutils/minmea/Makefile Normal file
View File

@ -0,0 +1,99 @@
############################################################################
# apps/gpsutils/minmea/Makefile
#
# Copyright (C) 2016 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# 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

145
gpsutils/minmea/README.md Normal file
View File

@ -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 &lt;kosma@cloudyourcar.com&gt; at Cloud Your Car.

854
gpsutils/minmea/minmea.c Normal file
View File

@ -0,0 +1,854 @@
/****************************************************************************
* apps/include/cle.h
*
* Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
*
* 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 <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <stdarg.h>
#include <apps/gpsutils/minmea.h>
/****************************************************************************
* 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;
}
}

285
include/gpsutils/minmea.h Normal file
View File

@ -0,0 +1,285 @@
/****************************************************************************
* apps/include/cle.h
*
* Copyright © 2014 Kosma Moczek <kosma@cloudyourcar.com>
*
* 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 <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <errno.h>
#include <time.h>
#include <math.h>
/****************************************************************************
* 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 */