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

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

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