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

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

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