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}