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

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

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