source: josm/trunk/src/org/openstreetmap/josm/io/nmea/NmeaReader.java@ 14067

Last change on this file since 14067 was 14067, checked in by Don-vip, 6 years ago

fix #16496 - fix invalid parsing of NMEA time (decimal-fraction of seconds)
  • Property svn:eol-style set to native
File size: 22.0 KB
Line 
1// License: GPL. For details, see LICENSE file.
2package org.openstreetmap.josm.io.nmea;
3
4import java.io.BufferedReader;
5import java.io.IOException;
6import java.io.InputStream;
7import java.io.InputStreamReader;
8import java.nio.charset.StandardCharsets;
9import java.text.ParsePosition;
10import java.text.SimpleDateFormat;
11import java.util.ArrayList;
12import java.util.Collection;
13import java.util.Collections;
14import java.util.Date;
15import java.util.Locale;
16import java.util.Objects;
17import java.util.regex.Matcher;
18import java.util.regex.Pattern;
19
20import org.openstreetmap.josm.data.coor.LatLon;
21import org.openstreetmap.josm.data.gpx.GpxConstants;
22import org.openstreetmap.josm.data.gpx.GpxData;
23import org.openstreetmap.josm.data.gpx.ImmutableGpxTrack;
24import org.openstreetmap.josm.data.gpx.WayPoint;
25import org.openstreetmap.josm.io.IGpxReader;
26import org.openstreetmap.josm.io.IllegalDataException;
27import org.openstreetmap.josm.tools.Logging;
28import org.openstreetmap.josm.tools.date.DateUtils;
29import org.xml.sax.SAXException;
30
31/**
32 * Reads a NMEA 0183 file. Based on information from
33 * <a href="http://www.catb.org/gpsd/NMEA.html">http://www.catb.org/gpsd</a>.
34 *
35 * NMEA files are in printable ASCII form and may include information such as position,
36 * speed, depth, frequency allocation, etc.
37 * Typical messages might be 11 to a maximum of 79 characters in length.
38 *
39 * NMEA standard aims to support one-way serial data transmission from a single "talker"
40 * to one or more "listeners". The type of talker is identified by a 2-character mnemonic.
41 *
42 * NMEA information is encoded through a list of "sentences".
43 *
44 * @author cbrill
45 */
46public class NmeaReader implements IGpxReader {
47
48 /**
49 * Course Over Ground and Ground Speed.
50 * <p>
51 * The actual course and speed relative to the ground
52 */
53 enum VTG {
54 COURSE(1), COURSE_REF(2), // true course
55 COURSE_M(3), COURSE_M_REF(4), // magnetic course
56 SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
57 SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
58 REST(9); // version-specific rest
59
60 final int position;
61
62 VTG(int position) {
63 this.position = position;
64 }
65 }
66
67 /**
68 * Recommended Minimum Specific GNSS Data.
69 * <p>
70 * Time, date, position, course and speed data provided by a GNSS navigation receiver.
71 * This sentence is transmitted at intervals not exceeding 2-seconds.
72 * RMC is the recommended minimum data to be provided by a GNSS receiver.
73 * All data fields must be provided, null fields used only when data is temporarily unavailable.
74 */
75 enum RMC {
76 TIME(1),
77 /** Warning from the receiver (A = data ok, V = warning) */
78 RECEIVER_WARNING(2),
79 WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
80 LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
81 SPEED(7), COURSE(8), DATE(9), // Speed in knots
82 MAGNETIC_DECLINATION(10), UNKNOWN(11), // magnetic declination
83 /**
84 * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
85 *
86 * @since NMEA 2.3
87 */
88 MODE(12);
89
90 final int position;
91
92 RMC(int position) {
93 this.position = position;
94 }
95 }
96
97 /**
98 * Global Positioning System Fix Data.
99 * <p>
100 * Time, position and fix related data for a GPS receiver.
101 */
102 enum GGA {
103 TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
104 /**
105 * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA 2.3))
106 */
107 QUALITY(6), SATELLITE_COUNT(7),
108 HDOP(8), // HDOP (horizontal dilution of precision)
109 HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
110 HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
111 GPS_AGE(13), // Age of differential GPS data
112 REF(14); // REF station
113
114 final int position;
115 GGA(int position) {
116 this.position = position;
117 }
118 }
119
120 /**
121 * GNSS DOP and Active Satellites.
122 * <p>
123 * GNSS receiver operating mode, satellites used in the navigation solution reported by the GGA or GNS sentence,
124 * and DOP values.
125 * If only GPS, GLONASS, etc. is used for the reported position solution the talker ID is GP, GL, etc.
126 * and the DOP values pertain to the individual system. If GPS, GLONASS, etc. are combined to obtain the
127 * reported position solution multiple GSA sentences are produced, one with the GPS satellites, another with
128 * the GLONASS satellites, etc. Each of these GSA sentences shall have talker ID GN, to indicate that the
129 * satellites are used in a combined solution and each shall have the PDOP, HDOP and VDOP for the
130 * combined satellites used in the position.
131 */
132 enum GSA {
133 AUTOMATIC(1),
134 FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
135 // PRN numbers for max 12 satellites
136 PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
137 PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
138 PDOP(15), // PDOP (precision)
139 HDOP(16), // HDOP (horizontal precision)
140 VDOP(17); // VDOP (vertical precision)
141
142 final int position;
143 GSA(int position) {
144 this.position = position;
145 }
146 }
147
148 /**
149 * Geographic Position - Latitude/Longitude.
150 * <p>
151 * Latitude and Longitude of vessel position, time of position fix and status.
152 */
153 enum GLL {
154 LATITUDE(1), LATITUDE_NS(2), // Latitude, NS
155 LONGITUDE(3), LONGITUDE_EW(4), // Latitude, EW
156 UTC(5), // Universal Time Coordinated
157 STATUS(6), // Status: A = Data valid, V = Data not valid
158 /**
159 * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
160 * @since NMEA 2.3
161 */
162 MODE(7);
163
164 final int position;
165 GLL(int position) {
166 this.position = position;
167 }
168 }
169
170 private final InputStream source;
171 GpxData data;
172
173 private static final Pattern DATE_TIME_PATTERN = Pattern.compile("(\\d{12})(\\.\\d+)?");
174
175 private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS", Locale.ENGLISH);
176
177 private Date readTime(String p) throws IllegalDataException {
178 // NMEA defines time with "a variable number of digits for decimal-fraction of seconds"
179 // This variable decimal fraction cannot be parsed by SimpleDateFormat
180 Matcher m = DATE_TIME_PATTERN.matcher(p);
181 if (m.matches()) {
182 String date = m.group(1);
183 double milliseconds = 0d;
184 if (m.groupCount() > 1 && m.group(2) != null) {
185 milliseconds = 1000d * Double.parseDouble("0" + m.group(2));
186 }
187 // Add milliseconds on three digits to match SimpleDateFormat pattern
188 date += String.format(".%03d", (int) milliseconds);
189 Date d = rmcTimeFmt.parse(date, new ParsePosition(0));
190 if (d != null)
191 return d;
192 }
193 throw new IllegalDataException("Date is malformed: '" + p + "'");
194 }
195
196 // functons for reading the error stats
197 public NMEAParserState ps;
198
199 public int getParserUnknown() {
200 return ps.unknown;
201 }
202
203 public int getParserZeroCoordinates() {
204 return ps.zeroCoord;
205 }
206
207 public int getParserChecksumErrors() {
208 return ps.checksumErrors+ps.noChecksum;
209 }
210
211 public int getParserMalformed() {
212 return ps.malformed;
213 }
214
215 public int getNumberOfCoordinates() {
216 return ps.success;
217 }
218
219 /**
220 * Constructs a new {@code NmeaReader}
221 * @param source NMEA file input stream
222 * @throws IOException if an I/O error occurs
223 */
224 public NmeaReader(InputStream source) throws IOException {
225 this.source = Objects.requireNonNull(source);
226 rmcTimeFmt.setTimeZone(DateUtils.UTC);
227 }
228
229 @Override
230 public boolean parse(boolean tryToFinish) throws SAXException, IOException {
231 // create the data tree
232 data = new GpxData();
233 Collection<Collection<WayPoint>> currentTrack = new ArrayList<>();
234
235 try (BufferedReader rd = new BufferedReader(new InputStreamReader(source, StandardCharsets.UTF_8))) {
236 StringBuilder sb = new StringBuilder(1024);
237 int loopstartChar = rd.read();
238 ps = new NMEAParserState();
239 if (loopstartChar == -1)
240 //TODO tell user about the problem?
241 return false;
242 sb.append((char) loopstartChar);
243 ps.pDate = "010100"; // TODO date problem
244 while (true) {
245 // don't load unparsable files completely to memory
246 if (sb.length() >= 1020) {
247 sb.delete(0, sb.length()-1);
248 }
249 int c = rd.read();
250 if (c == '$') {
251 parseNMEASentence(sb.toString(), ps);
252 sb.delete(0, sb.length());
253 sb.append('$');
254 } else if (c == -1) {
255 // EOF: add last WayPoint if it works out
256 parseNMEASentence(sb.toString(), ps);
257 break;
258 } else {
259 sb.append((char) c);
260 }
261 }
262 currentTrack.add(ps.waypoints);
263 data.tracks.add(new ImmutableGpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
264
265 } catch (IllegalDataException e) {
266 Logging.warn(e);
267 return false;
268 }
269 return true;
270 }
271
272 private static class NMEAParserState {
273 protected Collection<WayPoint> waypoints = new ArrayList<>();
274 protected String pTime;
275 protected String pDate;
276 protected WayPoint pWp;
277
278 protected int success; // number of successfully parsed sentences
279 protected int malformed;
280 protected int checksumErrors;
281 protected int noChecksum;
282 protected int unknown;
283 protected int zeroCoord;
284 }
285
286 /**
287 * Determines if the given address denotes the given NMEA sentence formatter of a known talker.
288 * @param address first tag of an NMEA sentence
289 * @param formatter sentence formatter mnemonic code
290 * @return {@code true} if the {@code address} denotes the given NMEA sentence formatter of a known talker
291 */
292 static boolean isSentence(String address, Sentence formatter) {
293 for (TalkerId talker : TalkerId.values()) {
294 if (address.equals('$' + talker.name() + formatter.name())) {
295 return true;
296 }
297 }
298 return false;
299 }
300
301 // Parses split up sentences into WayPoints which are stored
302 // in the collection in the NMEAParserState object.
303 // Returns true if the input made sense, false otherwise.
304 private boolean parseNMEASentence(String s, NMEAParserState ps) throws IllegalDataException {
305 try {
306 if (s.isEmpty()) {
307 throw new IllegalArgumentException("s is empty");
308 }
309
310 // checksum check:
311 // the bytes between the $ and the * are xored
312 // if there is no * or other meanities it will throw
313 // and result in a malformed packet.
314 String[] chkstrings = s.split("\\*");
315 if (chkstrings.length > 1) {
316 byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
317 int chk = 0;
318 for (int i = 1; i < chb.length; i++) {
319 chk ^= chb[i];
320 }
321 if (Integer.parseInt(chkstrings[1].substring(0, 2), 16) != chk) {
322 ps.checksumErrors++;
323 ps.pWp = null;
324 return false;
325 }
326 } else {
327 ps.noChecksum++;
328 }
329 // now for the content
330 String[] e = chkstrings[0].split(",");
331 String accu;
332
333 WayPoint currentwp = ps.pWp;
334 String currentDate = ps.pDate;
335
336 // handle the packet content
337 if (isSentence(e[0], Sentence.GGA)) {
338 // Position
339 LatLon latLon = parseLatLon(
340 e[GGA.LATITUDE_NAME.position],
341 e[GGA.LONGITUDE_NAME.position],
342 e[GGA.LATITUDE.position],
343 e[GGA.LONGITUDE.position]
344 );
345 if (latLon == null) {
346 throw new IllegalDataException("Malformed lat/lon");
347 }
348
349 if (LatLon.ZERO.equals(latLon)) {
350 ps.zeroCoord++;
351 return false;
352 }
353
354 // time
355 accu = e[GGA.TIME.position];
356 Date d = readTime(currentDate+accu);
357
358 if ((ps.pTime == null) || (currentwp == null) || !ps.pTime.equals(accu)) {
359 // this node is newer than the previous, create a new waypoint.
360 // no matter if previous WayPoint was null, we got something better now.
361 ps.pTime = accu;
362 currentwp = new WayPoint(latLon);
363 }
364 if (!currentwp.attr.containsKey("time")) {
365 // As this sentence has no complete time only use it
366 // if there is no time so far
367 currentwp.setTime(d);
368 }
369 // elevation
370 accu = e[GGA.HEIGHT_UNTIS.position];
371 if ("M".equals(accu)) {
372 // Ignore heights that are not in meters for now
373 accu = e[GGA.HEIGHT.position];
374 if (!accu.isEmpty()) {
375 Double.parseDouble(accu);
376 // if it throws it's malformed; this should only happen if the
377 // device sends nonstandard data.
378 if (!accu.isEmpty()) { // FIX ? same check
379 currentwp.put(GpxConstants.PT_ELE, accu);
380 }
381 }
382 }
383 // number of satellites
384 accu = e[GGA.SATELLITE_COUNT.position];
385 int sat = 0;
386 if (!accu.isEmpty()) {
387 sat = Integer.parseInt(accu);
388 currentwp.put(GpxConstants.PT_SAT, accu);
389 }
390 // h-dilution
391 accu = e[GGA.HDOP.position];
392 if (!accu.isEmpty()) {
393 currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
394 }
395 // fix
396 accu = e[GGA.QUALITY.position];
397 if (!accu.isEmpty()) {
398 int fixtype = Integer.parseInt(accu);
399 switch(fixtype) {
400 case 0:
401 currentwp.put(GpxConstants.PT_FIX, "none");
402 break;
403 case 1:
404 if (sat < 4) {
405 currentwp.put(GpxConstants.PT_FIX, "2d");
406 } else {
407 currentwp.put(GpxConstants.PT_FIX, "3d");
408 }
409 break;
410 case 2:
411 currentwp.put(GpxConstants.PT_FIX, "dgps");
412 break;
413 default:
414 break;
415 }
416 }
417 } else if (isSentence(e[0], Sentence.VTG)) {
418 // COURSE
419 accu = e[VTG.COURSE_REF.position];
420 if ("T".equals(accu)) {
421 // other values than (T)rue are ignored
422 accu = e[VTG.COURSE.position];
423 if (!accu.isEmpty() && currentwp != null) {
424 Double.parseDouble(accu);
425 currentwp.put("course", accu);
426 }
427 }
428 // SPEED
429 accu = e[VTG.SPEED_KMH_UNIT.position];
430 if (accu.startsWith("K")) {
431 accu = e[VTG.SPEED_KMH.position];
432 if (!accu.isEmpty() && currentwp != null) {
433 double speed = Double.parseDouble(accu);
434 speed /= 3.6; // speed in m/s
435 currentwp.put("speed", Double.toString(speed));
436 }
437 }
438 } else if (isSentence(e[0], Sentence.GSA)) {
439 // vdop
440 accu = e[GSA.VDOP.position];
441 if (!accu.isEmpty() && currentwp != null) {
442 currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
443 }
444 // hdop
445 accu = e[GSA.HDOP.position];
446 if (!accu.isEmpty() && currentwp != null) {
447 currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
448 }
449 // pdop
450 accu = e[GSA.PDOP.position];
451 if (!accu.isEmpty() && currentwp != null) {
452 currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
453 }
454 } else if (isSentence(e[0], Sentence.RMC)) {
455 // coordinates
456 LatLon latLon = parseLatLon(
457 e[RMC.WIDTH_NORTH_NAME.position],
458 e[RMC.LENGTH_EAST_NAME.position],
459 e[RMC.WIDTH_NORTH.position],
460 e[RMC.LENGTH_EAST.position]
461 );
462 if (LatLon.ZERO.equals(latLon)) {
463 ps.zeroCoord++;
464 return false;
465 }
466 // time
467 currentDate = e[RMC.DATE.position];
468 String time = e[RMC.TIME.position];
469
470 Date d = readTime(currentDate+time);
471
472 if (ps.pTime == null || currentwp == null || !ps.pTime.equals(time)) {
473 // this node is newer than the previous, create a new waypoint.
474 ps.pTime = time;
475 currentwp = new WayPoint(latLon);
476 }
477 // time: this sentence has complete time so always use it.
478 currentwp.setTime(d);
479 // speed
480 accu = e[RMC.SPEED.position];
481 if (!accu.isEmpty() && !currentwp.attr.containsKey("speed")) {
482 double speed = Double.parseDouble(accu);
483 speed *= 0.514444444; // to m/s
484 currentwp.put("speed", Double.toString(speed));
485 }
486 // course
487 accu = e[RMC.COURSE.position];
488 if (!accu.isEmpty() && !currentwp.attr.containsKey("course")) {
489 Double.parseDouble(accu);
490 currentwp.put("course", accu);
491 }
492
493 // TODO fix?
494 // * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
495 // *
496 // * @since NMEA 2.3
497 //
498 //MODE(12);
499 } else if (isSentence(e[0], Sentence.GLL)) {
500 // coordinates
501 LatLon latLon = parseLatLon(
502 e[GLL.LATITUDE_NS.position],
503 e[GLL.LONGITUDE_EW.position],
504 e[GLL.LATITUDE.position],
505 e[GLL.LONGITUDE.position]
506 );
507 if (LatLon.ZERO.equals(latLon)) {
508 ps.zeroCoord++;
509 return false;
510 }
511 // only consider valid data
512 if (!"A".equals(e[GLL.STATUS.position])) {
513 return false;
514 }
515
516 // RMC sentences contain a full date while GLL sentences contain only time,
517 // so create new waypoints only of the NMEA file does not contain RMC sentences
518 if (ps.pTime == null || currentwp == null) {
519 currentwp = new WayPoint(latLon);
520 }
521 } else {
522 ps.unknown++;
523 return false;
524 }
525 ps.pDate = currentDate;
526 if (ps.pWp != currentwp) {
527 if (ps.pWp != null) {
528 ps.pWp.setTime();
529 }
530 ps.pWp = currentwp;
531 ps.waypoints.add(currentwp);
532 ps.success++;
533 return true;
534 }
535 return true;
536
537 } catch (IllegalArgumentException | IndexOutOfBoundsException | IllegalDataException ex) {
538 if (ps.malformed < 5) {
539 Logging.warn(ex);
540 } else {
541 Logging.debug(ex);
542 }
543 ps.malformed++;
544 ps.pWp = null;
545 return false;
546 }
547 }
548
549 private static LatLon parseLatLon(String ns, String ew, String dlat, String dlon) {
550 String widthNorth = dlat.trim();
551 String lengthEast = dlon.trim();
552
553 // return a zero latlon instead of null so it is logged as zero coordinate
554 // instead of malformed sentence
555 if (widthNorth.isEmpty() && lengthEast.isEmpty()) return LatLon.ZERO;
556
557 // The format is xxDDLL.LLLL
558 // xx optional whitespace
559 // DD (int) degres
560 // LL.LLLL (double) latidude
561 int latdegsep = widthNorth.indexOf('.') - 2;
562 if (latdegsep < 0) return null;
563
564 int latdeg = Integer.parseInt(widthNorth.substring(0, latdegsep));
565 double latmin = Double.parseDouble(widthNorth.substring(latdegsep));
566 if (latdeg < 0) {
567 latmin *= -1.0;
568 }
569 double lat = latdeg + latmin / 60;
570 if ("S".equals(ns)) {
571 lat = -lat;
572 }
573
574 int londegsep = lengthEast.indexOf('.') - 2;
575 if (londegsep < 0) return null;
576
577 int londeg = Integer.parseInt(lengthEast.substring(0, londegsep));
578 double lonmin = Double.parseDouble(lengthEast.substring(londegsep));
579 if (londeg < 0) {
580 lonmin *= -1.0;
581 }
582 double lon = londeg + lonmin / 60;
583 if ("W".equals(ew)) {
584 lon = -lon;
585 }
586 return new LatLon(lat, lon);
587 }
588
589 @Override
590 public GpxData getGpxData() {
591 return data;
592 }
593}
Note: See TracBrowser for help on using the repository browser.