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

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

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