kopia lustrzana https://github.com/OpenRTX/OpenRTX
Add minmea integration for GPS data decoding
Added a platform test to decode NMEA sentences received from GPS modules in the MD3x0 GPS radios.replace/21aed3ceeeb9db487aa88bbc2c96271321e49f76
Niccolò Izzo
rodzic
d64cb8a1ba
commit
7192659899
|
|
@ -32,9 +32,9 @@ To reach out, come to our [M17 Reflector](https://m17.openrtx.org) or into our [
|
|||
|
||||
## License
|
||||
|
||||
This software is released under the GNU GPL v3, the modified wrapping scripts
|
||||
from Travis Goodspeed are licensed in exchange of two liters of India Pale Ale,
|
||||
we still owe you those two liters, Travis!
|
||||
This software is released under the GNU GPL v3.
|
||||
|
||||
minmea is released under the DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE v2.
|
||||
|
||||
## Disclaimer
|
||||
|
||||
|
|
|
|||
|
|
@ -72,9 +72,14 @@ rtos_inc = ['rtos/uC-OS3/Source',
|
|||
'rtos/uC-LIB',
|
||||
'rtos/uC-LIB/Cfg']
|
||||
|
||||
## minmea, a lightweight GPS NMEA 0183 parser library
|
||||
|
||||
src = openrtx_src + rtos_src
|
||||
inc = openrtx_inc + rtos_inc
|
||||
minmea_src = ['minmea/minmea.c']
|
||||
|
||||
minmea_inc = ['minmea/include']
|
||||
|
||||
src = openrtx_src + rtos_src + minmea_src
|
||||
inc = openrtx_inc + rtos_inc + minmea_inc
|
||||
|
||||
##
|
||||
## Definitions
|
||||
|
|
|
|||
|
|
@ -0,0 +1,13 @@
|
|||
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.
|
||||
|
|
@ -0,0 +1,261 @@
|
|||
/*
|
||||
* 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 MINMEA_H
|
||||
#define MINMEA_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
#ifdef MINMEA_INCLUDE_COMPAT
|
||||
#include <minmea_compat.h>
|
||||
#endif
|
||||
|
||||
#define MINMEA_MAX_LENGTH 80
|
||||
|
||||
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,
|
||||
MINMEA_SENTENCE_VTG,
|
||||
MINMEA_SENTENCE_ZDA,
|
||||
};
|
||||
|
||||
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;
|
||||
struct minmea_float dgps_age;
|
||||
};
|
||||
|
||||
enum minmea_gll_status {
|
||||
MINMEA_GLL_STATUS_DATA_VALID = 'A',
|
||||
MINMEA_GLL_STATUS_DATA_NOT_VALID = 'V',
|
||||
};
|
||||
|
||||
// FAA mode added to some fields in NMEA 2.3.
|
||||
enum minmea_faa_mode {
|
||||
MINMEA_FAA_MODE_AUTONOMOUS = 'A',
|
||||
MINMEA_FAA_MODE_DIFFERENTIAL = 'D',
|
||||
MINMEA_FAA_MODE_ESTIMATED = 'E',
|
||||
MINMEA_FAA_MODE_MANUAL = 'M',
|
||||
MINMEA_FAA_MODE_SIMULATED = 'S',
|
||||
MINMEA_FAA_MODE_NOT_VALID = 'N',
|
||||
MINMEA_FAA_MODE_PRECISE = 'P',
|
||||
};
|
||||
|
||||
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];
|
||||
};
|
||||
|
||||
struct minmea_sentence_vtg {
|
||||
struct minmea_float true_track_degrees;
|
||||
struct minmea_float magnetic_track_degrees;
|
||||
struct minmea_float speed_knots;
|
||||
struct minmea_float speed_kph;
|
||||
enum minmea_faa_mode faa_mode;
|
||||
};
|
||||
|
||||
struct minmea_sentence_zda {
|
||||
struct minmea_time time;
|
||||
struct minmea_date date;
|
||||
int hour_offset;
|
||||
int minute_offset;
|
||||
};
|
||||
|
||||
/**
|
||||
* Calculate raw sentence checksum. Does not check sentence integrity.
|
||||
*/
|
||||
uint8_t minmea_checksum(const char *sentence);
|
||||
|
||||
/**
|
||||
* Check sentence validity and checksum. Returns true for valid sentences.
|
||||
*/
|
||||
bool minmea_check(const char *sentence, bool strict);
|
||||
|
||||
/**
|
||||
* Determine talker identifier.
|
||||
*/
|
||||
bool minmea_talker_id(char talker[3], const char *sentence);
|
||||
|
||||
/**
|
||||
* Determine sentence identifier.
|
||||
*/
|
||||
enum minmea_sentence_id minmea_sentence_id(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);
|
||||
bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence);
|
||||
bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence);
|
||||
|
||||
/**
|
||||
* Convert GPS UTC date/time representation to a UNIX timestamp.
|
||||
*/
|
||||
int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_);
|
||||
|
||||
/**
|
||||
* Rescale a fixed-point value to a different scale. Rounds towards zero.
|
||||
*/
|
||||
static inline int_least32_t minmea_rescale(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(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(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 /* MINMEA_H */
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,645 @@
|
|||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "minmea.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <stdarg.h>
|
||||
#include <time.h>
|
||||
|
||||
#define boolstr(s) ((s) ? "true" : "false")
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
uint8_t minmea_checksum(const char *sentence)
|
||||
{
|
||||
// Support senteces with or without the starting dollar sign.
|
||||
if (*sentence == '$')
|
||||
sentence++;
|
||||
|
||||
uint8_t checksum = 0x00;
|
||||
|
||||
// The optional checksum is an XOR of all bytes between "$" and "*".
|
||||
while (*sentence && *sentence != '*')
|
||||
checksum ^= *sentence++;
|
||||
|
||||
return checksum;
|
||||
}
|
||||
|
||||
bool minmea_check(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 == '*') {
|
||||
// Extract checksum.
|
||||
sentence++;
|
||||
int upper = hex2int(*sentence++);
|
||||
if (upper == -1)
|
||||
return false;
|
||||
int lower = hex2int(*sentence++);
|
||||
if (lower == -1)
|
||||
return false;
|
||||
int 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;
|
||||
}
|
||||
|
||||
static inline bool minmea_isfield(char c) {
|
||||
return isprint((unsigned char) c) && c != ',' && c != '*';
|
||||
}
|
||||
|
||||
bool minmea_scan(const char *sentence, 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) {
|
||||
case 'c': { // Single character field (char).
|
||||
char value = '\0';
|
||||
|
||||
if (field && minmea_isfield(*field))
|
||||
value = *field;
|
||||
|
||||
*va_arg(ap, char *) = value;
|
||||
} break;
|
||||
|
||||
case 'd': { // Single character direction field (int).
|
||||
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;
|
||||
|
||||
case 'f': { // Fractional value with scale (struct minmea_float).
|
||||
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;
|
||||
|
||||
case 'i': { // Integer value, default 0 (int).
|
||||
int value = 0;
|
||||
|
||||
if (field) {
|
||||
char *endptr;
|
||||
value = strtol(field, &endptr, 10);
|
||||
if (minmea_isfield(*endptr))
|
||||
goto parse_error;
|
||||
}
|
||||
|
||||
*va_arg(ap, int *) = value;
|
||||
} break;
|
||||
|
||||
case 's': { // String value (char *).
|
||||
char *buf = va_arg(ap, char *);
|
||||
|
||||
if (field) {
|
||||
while (minmea_isfield(*field))
|
||||
*buf++ = *field++;
|
||||
}
|
||||
|
||||
*buf = '\0';
|
||||
} break;
|
||||
|
||||
case 't': { // NMEA talker+sentence identifier (char *).
|
||||
// This field is always mandatory.
|
||||
if (!field)
|
||||
goto parse_error;
|
||||
|
||||
if (field[0] != '$')
|
||||
goto parse_error;
|
||||
for (int f=0; f<5; f++)
|
||||
if (!minmea_isfield(field[1+f]))
|
||||
goto parse_error;
|
||||
|
||||
char *buf = va_arg(ap, char *);
|
||||
memcpy(buf, field+1, 5);
|
||||
buf[5] = '\0';
|
||||
} break;
|
||||
|
||||
case 'D': { // Date (int, int, int), -1 if empty.
|
||||
struct minmea_date *date = va_arg(ap, struct minmea_date *);
|
||||
|
||||
int d = -1, m = -1, y = -1;
|
||||
|
||||
if (field && minmea_isfield(*field)) {
|
||||
// Always six digits.
|
||||
for (int f=0; f<6; f++)
|
||||
if (!isdigit((unsigned char) field[f]))
|
||||
goto parse_error;
|
||||
|
||||
char dArr[] = {field[0], field[1], '\0'};
|
||||
char mArr[] = {field[2], field[3], '\0'};
|
||||
char yArr[] = {field[4], field[5], '\0'};
|
||||
d = strtol(dArr, NULL, 10);
|
||||
m = strtol(mArr, NULL, 10);
|
||||
y = strtol(yArr, NULL, 10);
|
||||
}
|
||||
|
||||
date->day = d;
|
||||
date->month = m;
|
||||
date->year = y;
|
||||
} break;
|
||||
|
||||
case 'T': { // Time (int, int, int, int), -1 if empty.
|
||||
struct minmea_time *time_ = va_arg(ap, struct minmea_time *);
|
||||
|
||||
int h = -1, i = -1, s = -1, u = -1;
|
||||
|
||||
if (field && minmea_isfield(*field)) {
|
||||
// Minimum required: integer time.
|
||||
for (int f=0; f<6; f++)
|
||||
if (!isdigit((unsigned char) field[f]))
|
||||
goto parse_error;
|
||||
|
||||
char hArr[] = {field[0], field[1], '\0'};
|
||||
char iArr[] = {field[2], field[3], '\0'};
|
||||
char sArr[] = {field[4], field[5], '\0'};
|
||||
h = strtol(hArr, NULL, 10);
|
||||
i = strtol(iArr, NULL, 10);
|
||||
s = strtol(sArr, NULL, 10);
|
||||
field += 6;
|
||||
|
||||
// Extra: fractional time. Saved as microseconds.
|
||||
if (*field++ == '.') {
|
||||
uint32_t value = 0;
|
||||
uint32_t scale = 1000000LU;
|
||||
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;
|
||||
|
||||
case '_': { // Ignore the field.
|
||||
} break;
|
||||
|
||||
default: { // Unknown.
|
||||
goto parse_error;
|
||||
}
|
||||
}
|
||||
|
||||
next_field();
|
||||
}
|
||||
|
||||
result = true;
|
||||
|
||||
parse_error:
|
||||
va_end(ap);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool minmea_talker_id(char talker[3], 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(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;
|
||||
if (!strcmp(type+2, "VTG"))
|
||||
return MINMEA_SENTENCE_VTG;
|
||||
if (!strcmp(type+2, "ZDA"))
|
||||
return MINMEA_SENTENCE_ZDA;
|
||||
|
||||
return MINMEA_UNKNOWN;
|
||||
}
|
||||
|
||||
bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, 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(struct minmea_sentence_gga *frame, 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, "tTfdfdiiffcfcf_",
|
||||
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(struct minmea_sentence_gsa *frame, 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(struct minmea_sentence_gll *frame, 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(struct minmea_sentence_gst *frame, 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(struct minmea_sentence_gsv *frame, 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;
|
||||
}
|
||||
|
||||
bool minmea_parse_vtg(struct minmea_sentence_vtg *frame, const char *sentence)
|
||||
{
|
||||
// $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
|
||||
// $GPVTG,156.1,T,140.9,M,0.0,N,0.0,K*41
|
||||
// $GPVTG,096.5,T,083.5,M,0.0,N,0.0,K,D*22
|
||||
// $GPVTG,188.36,T,,M,0.820,N,1.519,K,A*3F
|
||||
char type[6];
|
||||
char c_true, c_magnetic, c_knots, c_kph, c_faa_mode;
|
||||
|
||||
if (!minmea_scan(sentence, "tfcfcfcfc;c",
|
||||
type,
|
||||
&frame->true_track_degrees,
|
||||
&c_true,
|
||||
&frame->magnetic_track_degrees,
|
||||
&c_magnetic,
|
||||
&frame->speed_knots,
|
||||
&c_knots,
|
||||
&frame->speed_kph,
|
||||
&c_kph,
|
||||
&c_faa_mode))
|
||||
return false;
|
||||
if (strcmp(type+2, "VTG"))
|
||||
return false;
|
||||
// check chars
|
||||
if (c_true != 'T' ||
|
||||
c_magnetic != 'M' ||
|
||||
c_knots != 'N' ||
|
||||
c_kph != 'K')
|
||||
return false;
|
||||
frame->faa_mode = (enum minmea_faa_mode)c_faa_mode;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool minmea_parse_zda(struct minmea_sentence_zda *frame, const char *sentence)
|
||||
{
|
||||
// $GPZDA,201530.00,04,07,2002,00,00*60
|
||||
char type[6];
|
||||
|
||||
if(!minmea_scan(sentence, "tTiiiii",
|
||||
type,
|
||||
&frame->time,
|
||||
&frame->date.day,
|
||||
&frame->date.month,
|
||||
&frame->date.year,
|
||||
&frame->hour_offset,
|
||||
&frame->minute_offset))
|
||||
return false;
|
||||
if (strcmp(type+2, "ZDA"))
|
||||
return false;
|
||||
|
||||
// check offsets
|
||||
if (abs(frame->hour_offset) > 13 ||
|
||||
frame->minute_offset > 59 ||
|
||||
frame->minute_offset < 0)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int minmea_gettime(struct timespec *ts, const struct minmea_date *date, const struct minmea_time *time_)
|
||||
{
|
||||
if (date->year == -1 || time_->hours == -1)
|
||||
return -1;
|
||||
|
||||
struct tm tm;
|
||||
memset(&tm, 0, sizeof(tm));
|
||||
if (date->year < 80) {
|
||||
tm.tm_year = 2000 + date->year - 1900; // 2000-2079
|
||||
} else if (date->year >= 1900) {
|
||||
tm.tm_year = date->year - 1900; // 4 digit year, use directly
|
||||
} else {
|
||||
tm.tm_year = date->year; // 1980-1999
|
||||
}
|
||||
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;
|
||||
|
||||
time_t timestamp = timegm(&tm); /* See README.md if your system lacks timegm(). */
|
||||
if (timestamp != (time_t)-1) {
|
||||
ts->tv_sec = timestamp;
|
||||
ts->tv_nsec = time_->microseconds * 1000;
|
||||
return 0;
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/* vim: set ts=4 sw=4 et: */
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
/***************************************************************************
|
||||
* Copyright (C) 2021 by Federico Amedeo Izzo IU2NUO, *
|
||||
* Niccolò Izzo IU2KIN, *
|
||||
* Frederik Saraci IU2NRO, *
|
||||
* Silvano Seva IU2KWO *
|
||||
* *
|
||||
* 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 3 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, see <http://www.gnu.org/licenses/> *
|
||||
***************************************************************************/
|
||||
|
||||
#include <interfaces/platform.h>
|
||||
#include <interfaces/delays.h>
|
||||
#include <interfaces/gpio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <hwconfig.h>
|
||||
#include <minmea.h>
|
||||
|
||||
#define GPS_EN GPIOD,8
|
||||
#define GPS_DATA GPIOD,9
|
||||
|
||||
char line[MINMEA_MAX_LENGTH*10];
|
||||
|
||||
int main()
|
||||
{
|
||||
platform_init();
|
||||
|
||||
gpio_setMode(GPS_EN, OUTPUT);
|
||||
gpio_setMode(GPS_DATA, INPUT_PULL_DOWN);
|
||||
|
||||
gpio_setPin(GPS_EN);
|
||||
|
||||
uint8_t elapsedTime = 0;
|
||||
|
||||
printf("Waiting for GPS_DATA to rise");
|
||||
while((gpio_readPin(GPS_DATA) == 0) && (elapsedTime < 20))
|
||||
{
|
||||
printf(".");
|
||||
elapsedTime += 1;
|
||||
delayMs(500);
|
||||
}
|
||||
|
||||
printf(" %s.\r\n", elapsedTime < 20 ? "OK" : "TIMEOUT");
|
||||
|
||||
gpio_setMode(GPS_DATA, ALTERNATE);
|
||||
gpio_setAlternateFunction(GPS_DATA, 7);
|
||||
|
||||
RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
|
||||
__DSB();
|
||||
|
||||
const unsigned int quot = 2*42000000/9600;
|
||||
USART3->BRR = quot/2 + (quot & 1);
|
||||
USART3->CR3 |= USART_CR3_ONEBIT;
|
||||
USART3->CR1 = USART_CR1_UE
|
||||
| USART_CR1_RE;
|
||||
|
||||
int i = 0;
|
||||
while(1)
|
||||
{
|
||||
while((USART3->SR & USART_SR_RXNE) == 0) ;
|
||||
line[i++] = USART3->DR;
|
||||
// If a NMEA sentence is complete
|
||||
if (line[i - 1] == '\n') {
|
||||
line[i] = '\0';
|
||||
printf("%s\n\r", line);
|
||||
switch (minmea_sentence_id(line, false)) {
|
||||
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\r",
|
||||
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\r",
|
||||
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\r",
|
||||
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\r", 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\r", frame.msg_nr, frame.total_msgs);
|
||||
printf("$GSV: satellites in view: %d\n\r", frame.total_sats);
|
||||
for (int i = 0; i < 4; i++)
|
||||
printf("$GSV: sat nr %d, elevation: %d, azimuth: %d, snr: %d dbm\n\r",
|
||||
frame.sats[i].nr,
|
||||
frame.sats[i].elevation,
|
||||
frame.sats[i].azimuth,
|
||||
frame.sats[i].snr);
|
||||
}
|
||||
} break;
|
||||
|
||||
case MINMEA_SENTENCE_VTG:
|
||||
{
|
||||
|
||||
} break;
|
||||
|
||||
// Ignore this message as we take data from RMC
|
||||
case MINMEA_SENTENCE_GLL:
|
||||
;
|
||||
break;
|
||||
|
||||
// These messages are never sent by the Jumpstar JS-M710 Module
|
||||
case MINMEA_SENTENCE_GSA: break;
|
||||
case MINMEA_SENTENCE_GST: break;
|
||||
case MINMEA_SENTENCE_ZDA: break;
|
||||
|
||||
// Error handling
|
||||
case MINMEA_INVALID:
|
||||
{
|
||||
printf("Error: Invalid NMEA sentence!\n\r");
|
||||
} break;
|
||||
|
||||
case MINMEA_UNKNOWN:
|
||||
{
|
||||
printf("Error: Unsupported NMEA sentence!\n\r");
|
||||
} break;
|
||||
}
|
||||
i = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Ładowanie…
Reference in New Issue