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

Last change on this file since 12225 was 12059, checked in by stoecker, 7 years ago

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