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

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

fix #10489 - Add additional attributes to GPX Export for nodes + GPX code improvements
  • Property svn:eol-style set to native
File size: 17.4 KB
Line 
1//License: GPL. See README for details.
2package org.openstreetmap.josm.io;
3
4import java.io.BufferedReader;
5import java.io.InputStream;
6import java.io.InputStreamReader;
7import java.nio.charset.StandardCharsets;
8import java.text.ParsePosition;
9import java.text.SimpleDateFormat;
10import java.util.ArrayList;
11import java.util.Collection;
12import java.util.Collections;
13import java.util.Date;
14
15import org.openstreetmap.josm.Main;
16import org.openstreetmap.josm.data.coor.LatLon;
17import org.openstreetmap.josm.data.gpx.GpxConstants;
18import org.openstreetmap.josm.data.gpx.GpxData;
19import org.openstreetmap.josm.data.gpx.ImmutableGpxTrack;
20import org.openstreetmap.josm.data.gpx.WayPoint;
21import org.openstreetmap.josm.tools.date.DateUtils;
22
23/**
24 * Reads a NMEA file. Based on information from
25 * <a href="http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm">http://www.kowoma.de</a>
26 *
27 * @author cbrill
28 */
29public class NmeaReader {
30
31 /** Handler for the different types that NMEA speaks. */
32 public static enum NMEA_TYPE {
33
34 /** RMC = recommended minimum sentence C. */
35 GPRMC("$GPRMC"),
36 /** GPS positions. */
37 GPGGA("$GPGGA"),
38 /** SA = satellites active. */
39 GPGSA("$GPGSA"),
40 /** Course over ground and ground speed */
41 GPVTG("$GPVTG");
42
43 private final String type;
44
45 NMEA_TYPE(String type) {
46 this.type = type;
47 }
48
49 public String getType() {
50 return this.type;
51 }
52
53 public boolean equals(String type) {
54 return this.type.equals(type);
55 }
56 }
57
58 // GPVTG
59 public static enum GPVTG {
60 COURSE(1),COURSE_REF(2), // true course
61 COURSE_M(3), COURSE_M_REF(4), // magnetic course
62 SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
63 SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
64 REST(9); // version-specific rest
65
66 public final int position;
67
68 GPVTG(int position) {
69 this.position = position;
70 }
71 }
72
73 // The following only applies to GPRMC
74 public static enum GPRMC {
75 TIME(1),
76 /** Warning from the receiver (A = data ok, V = warning) */
77 RECEIVER_WARNING(2),
78 WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
79 LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
80 SPEED(7), COURSE(8), DATE(9), // Speed in knots
81 MAGNETIC_DECLINATION(10), UNKNOWN(11), // magnetic declination
82 /**
83 * Mode (A = autonom; D = differential; E = estimated; N = not valid; S
84 * = simulated)
85 *
86 * @since NMEA 2.3
87 */
88 MODE(12);
89
90 public final int position;
91
92 GPRMC(int position) {
93 this.position = position;
94 }
95 }
96
97 // The following only applies to GPGGA
98 public static enum GPGGA {
99 TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
100 /**
101 * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA
102 * 2.3))
103 */
104 QUALITY(6), SATELLITE_COUNT(7),
105 HDOP(8), // HDOP (horizontal dilution of precision)
106 HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
107 HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
108 GPS_AGE(13),// Age of differential GPS data
109 REF(14); // REF station
110
111 public final int position;
112 GPGGA(int position) {
113 this.position = position;
114 }
115 }
116
117 public static enum GPGSA {
118 AUTOMATIC(1),
119 FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
120 // PRN numbers for max 12 satellites
121 PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
122 PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
123 PDOP(15), // PDOP (precision)
124 HDOP(16), // HDOP (horizontal precision)
125 VDOP(17), ; // VDOP (vertical precision)
126
127 public final int position;
128 GPGSA(int position) {
129 this.position = position;
130 }
131 }
132
133 public GpxData data;
134
135 private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS");
136 private final SimpleDateFormat rmcTimeFmtStd = new SimpleDateFormat("ddMMyyHHmmss");
137
138 private Date readTime(String p) {
139 Date d = rmcTimeFmt.parse(p, new ParsePosition(0));
140 if (d == null) {
141 d = rmcTimeFmtStd.parse(p, new ParsePosition(0));
142 }
143 if (d == null)
144 throw new RuntimeException("Date is malformed"); // malformed
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 public int getParserZeroCoordinates() {
155 return ps.zero_coord;
156 }
157 public int getParserChecksumErrors() {
158 return ps.checksum_errors+ps.no_checksum;
159 }
160 public int getParserMalformed() {
161 return ps.malformed;
162 }
163 public int getNumberOfCoordinates() {
164 return ps.success;
165 }
166
167 public NmeaReader(InputStream source) {
168
169 // create the data tree
170 data = new GpxData();
171 Collection<Collection<WayPoint>> currentTrack = new ArrayList<>();
172
173 try (BufferedReader rd = new BufferedReader(new InputStreamReader(source, StandardCharsets.UTF_8))) {
174 StringBuilder sb = new StringBuilder(1024);
175 int loopstart_char = rd.read();
176 ps = new NMEAParserState();
177 if(loopstart_char == -1)
178 //TODO tell user about the problem?
179 return;
180 sb.append((char)loopstart_char);
181 ps.p_Date="010100"; // TODO date problem
182 while(true) {
183 // don't load unparsable files completely to memory
184 if(sb.length()>=1020) {
185 sb.delete(0, sb.length()-1);
186 }
187 int c = rd.read();
188 if(c=='$') {
189 parseNMEASentence(sb.toString(), ps);
190 sb.delete(0, sb.length());
191 sb.append('$');
192 } else if(c == -1) {
193 // EOF: add last WayPoint if it works out
194 parseNMEASentence(sb.toString(),ps);
195 break;
196 } else {
197 sb.append((char)c);
198 }
199 }
200 currentTrack.add(ps.waypoints);
201 data.tracks.add(new ImmutableGpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
202
203 } catch (Exception e) {
204 Main.warn(e);
205 }
206 }
207
208 private static class NMEAParserState {
209 protected Collection<WayPoint> waypoints = new ArrayList<>();
210 protected String p_Time;
211 protected String p_Date;
212 protected WayPoint p_Wp;
213
214 protected int success = 0; // number of successfully parsend sentences
215 protected int malformed = 0;
216 protected int checksum_errors = 0;
217 protected int no_checksum = 0;
218 protected int unknown = 0;
219 protected int zero_coord = 0;
220 }
221
222 // Parses split up sentences into WayPoints which are stored
223 // in the collection in the NMEAParserState object.
224 // Returns true if the input made sence, false otherwise.
225 private boolean parseNMEASentence(String s, NMEAParserState ps) throws IllegalDataException {
226 try {
227 if (s.isEmpty()) {
228 throw new IllegalArgumentException("s is empty");
229 }
230
231 // checksum check:
232 // the bytes between the $ and the * are xored
233 // if there is no * or other meanities it will throw
234 // and result in a malformed packet.
235 String[] chkstrings = s.split("\\*");
236 if(chkstrings.length > 1)
237 {
238 byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
239 int chk=0;
240 for (int i = 1; i < chb.length; i++) {
241 chk ^= chb[i];
242 }
243 if (Integer.parseInt(chkstrings[1].substring(0,2),16) != chk) {
244 ps.checksum_errors++;
245 ps.p_Wp=null;
246 return false;
247 }
248 } else {
249 ps.no_checksum++;
250 }
251 // now for the content
252 String[] e = chkstrings[0].split(",");
253 String accu;
254
255 WayPoint currentwp = ps.p_Wp;
256 String currentDate = ps.p_Date;
257
258 // handle the packet content
259 if("$GPGGA".equals(e[0]) || "$GNGGA".equals(e[0])) {
260 // Position
261 LatLon latLon = parseLatLon(
262 e[GPGGA.LATITUDE_NAME.position],
263 e[GPGGA.LONGITUDE_NAME.position],
264 e[GPGGA.LATITUDE.position],
265 e[GPGGA.LONGITUDE.position]
266 );
267 if (latLon==null) {
268 throw new IllegalDataException("Malformed lat/lon");
269 }
270
271 if ((latLon.lat()==0.0) && (latLon.lon()==0.0)) {
272 ps.zero_coord++;
273 return false;
274 }
275
276 // time
277 accu = e[GPGGA.TIME.position];
278 Date d = readTime(currentDate+accu);
279
280 if((ps.p_Time==null) || (currentwp==null) || !ps.p_Time.equals(accu)) {
281 // this node is newer than the previous, create a new waypoint.
282 // no matter if previous WayPoint was null, we got something
283 // better now.
284 ps.p_Time=accu;
285 currentwp = new WayPoint(latLon);
286 }
287 if(!currentwp.attr.containsKey("time")) {
288 // As this sentence has no complete time only use it
289 // if there is no time so far
290 currentwp.put(GpxConstants.PT_TIME, DateUtils.fromDate(d));
291 }
292 // elevation
293 accu=e[GPGGA.HEIGHT_UNTIS.position];
294 if("M".equals(accu)) {
295 // Ignore heights that are not in meters for now
296 accu=e[GPGGA.HEIGHT.position];
297 if(!accu.isEmpty()) {
298 Double.parseDouble(accu);
299 // if it throws it's malformed; this should only happen if the
300 // device sends nonstandard data.
301 if(!accu.isEmpty()) { // FIX ? same check
302 currentwp.put(GpxConstants.PT_ELE, accu);
303 }
304 }
305 }
306 // number of sattelites
307 accu=e[GPGGA.SATELLITE_COUNT.position];
308 int sat = 0;
309 if(!accu.isEmpty()) {
310 sat = Integer.parseInt(accu);
311 currentwp.put(GpxConstants.PT_SAT, accu);
312 }
313 // h-dilution
314 accu=e[GPGGA.HDOP.position];
315 if(!accu.isEmpty()) {
316 currentwp.put(GpxConstants.PT_HDOP, Float.parseFloat(accu));
317 }
318 // fix
319 accu=e[GPGGA.QUALITY.position];
320 if(!accu.isEmpty()) {
321 int fixtype = Integer.parseInt(accu);
322 switch(fixtype) {
323 case 0:
324 currentwp.put(GpxConstants.PT_FIX, "none");
325 break;
326 case 1:
327 if(sat < 4) {
328 currentwp.put(GpxConstants.PT_FIX, "2d");
329 } else {
330 currentwp.put(GpxConstants.PT_FIX, "3d");
331 }
332 break;
333 case 2:
334 currentwp.put(GpxConstants.PT_FIX, "dgps");
335 break;
336 default:
337 break;
338 }
339 }
340 } else if("$GPVTG".equals(e[0]) || "$GNVTG".equals(e[0])) {
341 // COURSE
342 accu = e[GPVTG.COURSE_REF.position];
343 if("T".equals(accu)) {
344 // other values than (T)rue are ignored
345 accu = e[GPVTG.COURSE.position];
346 if(!accu.isEmpty()) {
347 Double.parseDouble(accu);
348 currentwp.put("course", accu);
349 }
350 }
351 // SPEED
352 accu = e[GPVTG.SPEED_KMH_UNIT.position];
353 if(accu.startsWith("K")) {
354 accu = e[GPVTG.SPEED_KMH.position];
355 if(!accu.isEmpty()) {
356 double speed = Double.parseDouble(accu);
357 speed /= 3.6; // speed in m/s
358 currentwp.put("speed", Double.toString(speed));
359 }
360 }
361 } else if("$GPGSA".equals(e[0]) || "$GNGSA".equals(e[0])) {
362 // vdop
363 accu=e[GPGSA.VDOP.position];
364 if(!accu.isEmpty()) {
365 currentwp.put(GpxConstants.PT_VDOP, Float.parseFloat(accu));
366 }
367 // hdop
368 accu=e[GPGSA.HDOP.position];
369 if(!accu.isEmpty()) {
370 currentwp.put(GpxConstants.PT_HDOP, Float.parseFloat(accu));
371 }
372 // pdop
373 accu=e[GPGSA.PDOP.position];
374 if(!accu.isEmpty()) {
375 currentwp.put(GpxConstants.PT_PDOP, Float.parseFloat(accu));
376 }
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.lat()==0.0) && (latLon.lon()==0.0)) {
387 ps.zero_coord++;
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.p_Time==null) || (currentwp==null) || !ps.p_Time.equals(time)) {
397 // this node is newer than the previous, create a new waypoint.
398 ps.p_Time=time;
399 currentwp = new WayPoint(latLon);
400 }
401 // time: this sentence has complete time so always use it.
402 currentwp.put(GpxConstants.PT_TIME, DateUtils.fromDate(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.p_Date = currentDate;
429 if(ps.p_Wp != currentwp) {
430 if(ps.p_Wp!=null) {
431 ps.p_Wp.setTime();
432 }
433 ps.p_Wp = 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.p_Wp=null;
444 return false;
445 }
446 }
447
448 private 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 new LatLon(0.0,0.0);
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.