1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
|
/**
* @file nmea.c
* @note Copyright (C) 2020 Richard Cochran <richardcochran@gmail.com>
* @note SPDX-License-Identifier: GPL-2.0+
*/
#include <malloc.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "nmea.h"
#include "print.h"
#define NMEA_CHAR_MIN ' '
#define NMEA_CHAR_MAX '~'
#define NMEA_MAX_LENGTH 256
enum nmea_state {
NMEA_IDLE,
NMEA_HAVE_DOLLAR,
NMEA_HAVE_STARTG,
NMEA_HAVE_STARTX,
NMEA_HAVE_BODY,
NMEA_HAVE_CSUMA,
NMEA_HAVE_CSUM_MSB,
NMEA_HAVE_CSUM_LSB,
NMEA_HAVE_PENULTIMATE,
};
struct nmea_parser {
char sentence[NMEA_MAX_LENGTH + 1];
char payload_checksum[3];
enum nmea_state state;
uint8_t checksum;
int offset;
};
static void nmea_reset(struct nmea_parser *np);
static void nmea_accumulate(struct nmea_parser *np, char c)
{
if (c < NMEA_CHAR_MIN || c > NMEA_CHAR_MAX) {
nmea_reset(np);
return;
}
if (np->offset == NMEA_MAX_LENGTH) {
nmea_reset(np);
}
np->sentence[np->offset++] = c;
np->checksum ^= c;
}
static int nmea_parse_symbol(struct nmea_parser *np, char c)
{
switch (np->state) {
case NMEA_IDLE:
if (c == '$') {
np->state = NMEA_HAVE_DOLLAR;
}
break;
case NMEA_HAVE_DOLLAR:
if (c == 'G') {
np->state = NMEA_HAVE_STARTG;
nmea_accumulate(np, c);
} else {
nmea_reset(np);
}
break;
case NMEA_HAVE_STARTG:
np->state = NMEA_HAVE_STARTX;
nmea_accumulate(np, c);
break;
case NMEA_HAVE_STARTX:
np->state = NMEA_HAVE_BODY;
nmea_accumulate(np, c);
break;
case NMEA_HAVE_BODY:
if (c == '*') {
np->state = NMEA_HAVE_CSUMA;
} else {
nmea_accumulate(np, c);
}
break;
case NMEA_HAVE_CSUMA:
np->state = NMEA_HAVE_CSUM_MSB;
np->payload_checksum[0] = c;
break;
case NMEA_HAVE_CSUM_MSB:
np->state = NMEA_HAVE_CSUM_LSB;
np->payload_checksum[1] = c;
break;
case NMEA_HAVE_CSUM_LSB:
if (c == '\n') {
/*skip the CR*/
return 0;
}
if (c == '\r') {
np->state = NMEA_HAVE_PENULTIMATE;
} else {
nmea_reset(np);
}
break;
case NMEA_HAVE_PENULTIMATE:
if (c == '\n') {
return 0;
}
nmea_reset(np);
break;
}
return -1;
}
static void nmea_reset(struct nmea_parser *np)
{
memset(np, 0, sizeof(*np));
}
static int nmea_scan_rmc(struct nmea_parser *np, struct nmea_rmc *result)
{
int cnt, i, msec = 0;
char *ptr, status;
uint8_t checksum;
struct tm tm;
pr_debug("nmea sentence: %s", np->sentence);
cnt = sscanf(np->payload_checksum, "%02hhx", &checksum);
if (cnt != 1) {
return -1;
}
if (checksum != np->checksum) {
pr_err("checksum mismatch 0x%02hhx != 0x%02hhx on %s",
checksum, np->checksum, np->sentence);
return -1;
}
cnt = sscanf(np->sentence,
"G%*cRMC,%2d%2d%2d.%d,%c",
&tm.tm_hour, &tm.tm_min, &tm.tm_sec, &msec, &status);
if (cnt != 5) {
cnt = sscanf(np->sentence,
"G%*cRMC,%2d%2d%2d,%c",
&tm.tm_hour, &tm.tm_min, &tm.tm_sec, &status);
if (cnt != 4) {
return -1;
}
}
ptr = np->sentence;
for (i = 0; i < 9; i++) {
ptr = strchr(ptr, ',');
if (!ptr) {
return -1;
}
ptr++;
}
cnt = sscanf(ptr, "%2d%2d%2d", &tm.tm_mday, &tm.tm_mon, &tm.tm_year);
if (cnt != 3) {
return -1;
}
tm.tm_year += 100;
tm.tm_mon--;
tm.tm_isdst = 0;
result->ts.tv_sec = mktime(&tm);
result->ts.tv_nsec = msec * 1000000UL;
result->fix_valid = status == 'A' ? true : false;
return 0;
}
int nmea_parse(struct nmea_parser *np, const char *ptr, int buflen,
struct nmea_rmc *result, int *parsed)
{
int count = 0;
while (buflen) {
if (!nmea_parse_symbol(np, *ptr)) {
if (!nmea_scan_rmc(np, result)) {
*parsed = count + 1;
return 0;
}
nmea_reset(np);
}
buflen--;
count++;
ptr++;
}
*parsed = count;
return -1;
}
struct nmea_parser *nmea_parser_create(void)
{
struct nmea_parser *np;
np = malloc(sizeof(*np));
if (!np) {
return NULL;
}
nmea_reset(np);
/* Ensure that mktime(3) returns a value in the UTC time scale. */
setenv("TZ", "UTC", 1);
return np;
}
void nmea_parser_destroy(struct nmea_parser *np)
{
free(np);
}
|
