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.date.DateUtils;
023
024/**
025 * Reads a NMEA file. Based on information from
026 * <a href="http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm">http://www.kowoma.de</a>
027 *
028 * @author cbrill
029 */
030public class NmeaReader {
031
032    /** Handler for the different types that NMEA speaks. */
033    public enum NMEA_TYPE {
034
035        /** RMC = recommended minimum sentence C. */
036        GPRMC("$GPRMC"),
037        /** GPS positions. */
038        GPGGA("$GPGGA"),
039        /** SA = satellites active. */
040        GPGSA("$GPGSA"),
041        /** Course over ground and ground speed */
042        GPVTG("$GPVTG");
043
044        private final String type;
045
046        NMEA_TYPE(String type) {
047            this.type = type;
048        }
049
050        public String getType() {
051            return this.type;
052        }
053    }
054
055    // GPVTG
056    public enum GPVTG {
057        COURSE(1), COURSE_REF(2), // true course
058        COURSE_M(3), COURSE_M_REF(4), // magnetic course
059        SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
060        SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
061        REST(9); // version-specific rest
062
063        public final int position;
064
065        GPVTG(int position) {
066            this.position = position;
067        }
068    }
069
070    // The following only applies to GPRMC
071    public enum GPRMC {
072        TIME(1),
073        /** Warning from the receiver (A = data ok, V = warning) */
074        RECEIVER_WARNING(2),
075        WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
076        LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
077        SPEED(7), COURSE(8), DATE(9),           // Speed in knots
078        MAGNETIC_DECLINATION(10), UNKNOWN(11),  // magnetic declination
079        /**
080         * Mode (A = autonom; D = differential; E = estimated; N = not valid; S
081         * = simulated)
082         *
083         * @since NMEA 2.3
084         */
085        MODE(12);
086
087        public final int position;
088
089        GPRMC(int position) {
090            this.position = position;
091        }
092    }
093
094    // The following only applies to GPGGA
095    public enum GPGGA {
096        TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
097        /**
098         * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA
099         * 2.3))
100         */
101        QUALITY(6), SATELLITE_COUNT(7),
102        HDOP(8), // HDOP (horizontal dilution of precision)
103        HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
104        HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
105        GPS_AGE(13), // Age of differential GPS data
106        REF(14); // REF station
107
108        public final int position;
109        GPGGA(int position) {
110            this.position = position;
111        }
112    }
113
114    public enum GPGSA {
115        AUTOMATIC(1),
116        FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
117        // PRN numbers for max 12 satellites
118        PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
119        PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
120        PDOP(15),   // PDOP (precision)
121        HDOP(16),   // HDOP (horizontal precision)
122        VDOP(17);   // VDOP (vertical precision)
123
124        public final int position;
125        GPGSA(int position) {
126            this.position = position;
127        }
128    }
129
130    public GpxData data;
131
132    private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS");
133    private final SimpleDateFormat rmcTimeFmtStd = new SimpleDateFormat("ddMMyyHHmmss");
134
135    private Date readTime(String p) {
136        Date d = rmcTimeFmt.parse(p, new ParsePosition(0));
137        if (d == null) {
138            d = rmcTimeFmtStd.parse(p, new ParsePosition(0));
139        }
140        if (d == null)
141            throw new RuntimeException("Date is malformed"); // malformed
142        return d;
143    }
144
145    // functons for reading the error stats
146    public NMEAParserState ps;
147
148    public int getParserUnknown() {
149        return ps.unknown;
150    }
151
152    public int getParserZeroCoordinates() {
153        return ps.zeroCoord;
154    }
155
156    public int getParserChecksumErrors() {
157        return ps.checksumErrors+ps.noChecksum;
158    }
159
160    public int getParserMalformed() {
161        return ps.malformed;
162    }
163
164    public int getNumberOfCoordinates() {
165        return ps.success;
166    }
167
168    public NmeaReader(InputStream source) throws IOException {
169        rmcTimeFmt.setTimeZone(DateUtils.UTC);
170        rmcTimeFmtStd.setTimeZone(DateUtils.UTC);
171
172        // create the data tree
173        data = new GpxData();
174        Collection<Collection<WayPoint>> currentTrack = new ArrayList<>();
175
176        try (BufferedReader rd = new BufferedReader(new InputStreamReader(source, StandardCharsets.UTF_8))) {
177            StringBuilder sb = new StringBuilder(1024);
178            int loopstartChar = rd.read();
179            ps = new NMEAParserState();
180            if (loopstartChar == -1)
181                //TODO tell user about the problem?
182                return;
183            sb.append((char) loopstartChar);
184            ps.pDate = "010100"; // TODO date problem
185            while (true) {
186                // don't load unparsable files completely to memory
187                if (sb.length() >= 1020) {
188                    sb.delete(0, sb.length()-1);
189                }
190                int c = rd.read();
191                if (c == '$') {
192                    parseNMEASentence(sb.toString(), ps);
193                    sb.delete(0, sb.length());
194                    sb.append('$');
195                } else if (c == -1) {
196                    // EOF: add last WayPoint if it works out
197                    parseNMEASentence(sb.toString(), ps);
198                    break;
199                } else {
200                    sb.append((char) c);
201                }
202            }
203            currentTrack.add(ps.waypoints);
204            data.tracks.add(new ImmutableGpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
205
206        } catch (IllegalDataException e) {
207            Main.warn(e);
208        }
209    }
210
211    private static class NMEAParserState {
212        protected Collection<WayPoint> waypoints = new ArrayList<>();
213        protected String pTime;
214        protected String pDate;
215        protected WayPoint pWp;
216
217        protected int success; // number of successfully parsed sentences
218        protected int malformed;
219        protected int checksumErrors;
220        protected int noChecksum;
221        protected int unknown;
222        protected int zeroCoord;
223    }
224
225    // Parses split up sentences into WayPoints which are stored
226    // in the collection in the NMEAParserState object.
227    // Returns true if the input made sence, false otherwise.
228    private boolean parseNMEASentence(String s, NMEAParserState ps) throws IllegalDataException {
229        try {
230            if (s.isEmpty()) {
231                throw new IllegalArgumentException("s is empty");
232            }
233
234            // checksum check:
235            // the bytes between the $ and the * are xored
236            // if there is no * or other meanities it will throw
237            // and result in a malformed packet.
238            String[] chkstrings = s.split("\\*");
239            if (chkstrings.length > 1) {
240                byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
241                int chk = 0;
242                for (int i = 1; i < chb.length; i++) {
243                    chk ^= chb[i];
244                }
245                if (Integer.parseInt(chkstrings[1].substring(0, 2), 16) != chk) {
246                    ps.checksumErrors++;
247                    ps.pWp = null;
248                    return false;
249                }
250            } else {
251                ps.noChecksum++;
252            }
253            // now for the content
254            String[] e = chkstrings[0].split(",");
255            String accu;
256
257            WayPoint currentwp = ps.pWp;
258            String currentDate = ps.pDate;
259
260            // handle the packet content
261            if ("$GPGGA".equals(e[0]) || "$GNGGA".equals(e[0])) {
262                // Position
263                LatLon latLon = parseLatLon(
264                        e[GPGGA.LATITUDE_NAME.position],
265                        e[GPGGA.LONGITUDE_NAME.position],
266                        e[GPGGA.LATITUDE.position],
267                        e[GPGGA.LONGITUDE.position]
268                );
269                if (latLon == null) {
270                    throw new IllegalDataException("Malformed lat/lon");
271                }
272
273                if (LatLon.ZERO.equals(latLon)) {
274                    ps.zeroCoord++;
275                    return false;
276                }
277
278                // time
279                accu = e[GPGGA.TIME.position];
280                Date d = readTime(currentDate+accu);
281
282                if ((ps.pTime == null) || (currentwp == null) || !ps.pTime.equals(accu)) {
283                    // this node is newer than the previous, create a new waypoint.
284                    // no matter if previous WayPoint was null, we got something better now.
285                    ps.pTime = accu;
286                    currentwp = new WayPoint(latLon);
287                }
288                if (!currentwp.attr.containsKey("time")) {
289                    // As this sentence has no complete time only use it
290                    // if there is no time so far
291                    currentwp.setTime(d);
292                }
293                // elevation
294                accu = e[GPGGA.HEIGHT_UNTIS.position];
295                if ("M".equals(accu)) {
296                    // Ignore heights that are not in meters for now
297                    accu = e[GPGGA.HEIGHT.position];
298                    if (!accu.isEmpty()) {
299                        Double.parseDouble(accu);
300                        // if it throws it's malformed; this should only happen if the
301                        // device sends nonstandard data.
302                        if (!accu.isEmpty()) { // FIX ? same check
303                            currentwp.put(GpxConstants.PT_ELE, accu);
304                        }
305                    }
306                }
307                // number of sattelites
308                accu = e[GPGGA.SATELLITE_COUNT.position];
309                int sat = 0;
310                if (!accu.isEmpty()) {
311                    sat = Integer.parseInt(accu);
312                    currentwp.put(GpxConstants.PT_SAT, accu);
313                }
314                // h-dilution
315                accu = e[GPGGA.HDOP.position];
316                if (!accu.isEmpty()) {
317                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
318                }
319                // fix
320                accu = e[GPGGA.QUALITY.position];
321                if (!accu.isEmpty()) {
322                    int fixtype = Integer.parseInt(accu);
323                    switch(fixtype) {
324                    case 0:
325                        currentwp.put(GpxConstants.PT_FIX, "none");
326                        break;
327                    case 1:
328                        if (sat < 4) {
329                            currentwp.put(GpxConstants.PT_FIX, "2d");
330                        } else {
331                            currentwp.put(GpxConstants.PT_FIX, "3d");
332                        }
333                        break;
334                    case 2:
335                        currentwp.put(GpxConstants.PT_FIX, "dgps");
336                        break;
337                    default:
338                        break;
339                    }
340                }
341            } else if ("$GPVTG".equals(e[0]) || "$GNVTG".equals(e[0])) {
342                // COURSE
343                accu = e[GPVTG.COURSE_REF.position];
344                if ("T".equals(accu)) {
345                    // other values than (T)rue are ignored
346                    accu = e[GPVTG.COURSE.position];
347                    if (!accu.isEmpty()) {
348                        Double.parseDouble(accu);
349                        currentwp.put("course", accu);
350                    }
351                }
352                // SPEED
353                accu = e[GPVTG.SPEED_KMH_UNIT.position];
354                if (accu.startsWith("K")) {
355                    accu = e[GPVTG.SPEED_KMH.position];
356                    if (!accu.isEmpty()) {
357                        double speed = Double.parseDouble(accu);
358                        speed /= 3.6; // speed in m/s
359                        currentwp.put("speed", Double.toString(speed));
360                    }
361                }
362            } else if ("$GPGSA".equals(e[0]) || "$GNGSA".equals(e[0])) {
363                // vdop
364                accu = e[GPGSA.VDOP.position];
365                if (!accu.isEmpty()) {
366                    currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
367                }
368                // hdop
369                accu = e[GPGSA.HDOP.position];
370                if (!accu.isEmpty()) {
371                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
372                }
373                // pdop
374                accu = e[GPGSA.PDOP.position];
375                if (!accu.isEmpty()) {
376                    currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
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.ZERO.equals(latLon)) {
387                    ps.zeroCoord++;
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.pTime == null || currentwp == null || !ps.pTime.equals(time)) {
397                    // this node is newer than the previous, create a new waypoint.
398                    ps.pTime = time;
399                    currentwp = new WayPoint(latLon);
400                }
401                // time: this sentence has complete time so always use it.
402                currentwp.setTime(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.pDate = currentDate;
429            if (ps.pWp != currentwp) {
430                if (ps.pWp != null) {
431                    ps.pWp.setTime();
432                }
433                ps.pWp = 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.pWp = null;
444            return false;
445        }
446    }
447
448    private static 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 LatLon.ZERO;
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}