001// License: GPL. For details, see LICENSE file. 002package org.openstreetmap.josm.io; 003 004import java.io.BufferedReader; 005import java.io.IOException; 006import java.io.InputStream; 007import java.io.InputStreamReader; 008import java.nio.charset.StandardCharsets; 009import java.text.ParsePosition; 010import java.text.SimpleDateFormat; 011import java.util.ArrayList; 012import java.util.Collection; 013import java.util.Collections; 014import java.util.Date; 015 016import org.openstreetmap.josm.Main; 017import org.openstreetmap.josm.data.coor.LatLon; 018import org.openstreetmap.josm.data.gpx.GpxConstants; 019import org.openstreetmap.josm.data.gpx.GpxData; 020import org.openstreetmap.josm.data.gpx.ImmutableGpxTrack; 021import org.openstreetmap.josm.data.gpx.WayPoint; 022import org.openstreetmap.josm.tools.JosmRuntimeException; 023import org.openstreetmap.josm.tools.date.DateUtils; 024 025/** 026 * Reads a NMEA file. Based on information from 027 * <a href="http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm">http://www.kowoma.de</a> 028 * 029 * @author cbrill 030 */ 031public class NmeaReader { 032 033 // GPVTG 034 public enum GPVTG { 035 COURSE(1), COURSE_REF(2), // true course 036 COURSE_M(3), COURSE_M_REF(4), // magnetic course 037 SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots 038 SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h 039 REST(9); // version-specific rest 040 041 public final int position; 042 043 GPVTG(int position) { 044 this.position = position; 045 } 046 } 047 048 // The following only applies to GPRMC 049 public enum GPRMC { 050 TIME(1), 051 /** Warning from the receiver (A = data ok, V = warning) */ 052 RECEIVER_WARNING(2), 053 WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS 054 LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW 055 SPEED(7), COURSE(8), DATE(9), // Speed in knots 056 MAGNETIC_DECLINATION(10), UNKNOWN(11), // magnetic declination 057 /** 058 * Mode (A = autonom; D = differential; E = estimated; N = not valid; S 059 * = simulated) 060 * 061 * @since NMEA 2.3 062 */ 063 MODE(12); 064 065 public final int position; 066 067 GPRMC(int position) { 068 this.position = position; 069 } 070 } 071 072 // The following only applies to GPGGA 073 public enum GPGGA { 074 TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5), 075 /** 076 * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA 077 * 2.3)) 078 */ 079 QUALITY(6), SATELLITE_COUNT(7), 080 HDOP(8), // HDOP (horizontal dilution of precision) 081 HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid) 082 HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84) 083 GPS_AGE(13), // Age of differential GPS data 084 REF(14); // REF station 085 086 public final int position; 087 GPGGA(int position) { 088 this.position = position; 089 } 090 } 091 092 public enum GPGSA { 093 AUTOMATIC(1), 094 FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed) 095 // PRN numbers for max 12 satellites 096 PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8), 097 PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14), 098 PDOP(15), // PDOP (precision) 099 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}