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;
022
023/**
024 * Reads a NMEA file. Based on information from
025 * <a href="http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm">http://www.kowoma.de</a>
026 *
027 * @author cbrill
028 */
029public class NmeaReader {
030
031    /** Handler for the different types that NMEA speaks. */
032    public enum NMEA_TYPE {
033
034        /** RMC = recommended minimum sentence C. */
035        GPRMC("$GPRMC"),
036        /** GPS positions. */
037        GPGGA("$GPGGA"),
038        /** SA = satellites active. */
039        GPGSA("$GPGSA"),
040        /** Course over ground and ground speed */
041        GPVTG("$GPVTG");
042
043        private final String type;
044
045        NMEA_TYPE(String type) {
046            this.type = type;
047        }
048
049        public String getType() {
050            return this.type;
051        }
052    }
053
054    // GPVTG
055    public enum GPVTG {
056        COURSE(1), COURSE_REF(2), // true course
057        COURSE_M(3), COURSE_M_REF(4), // magnetic course
058        SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
059        SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
060        REST(9); // version-specific rest
061
062        public final int position;
063
064        GPVTG(int position) {
065            this.position = position;
066        }
067    }
068
069    // The following only applies to GPRMC
070    public enum GPRMC {
071        TIME(1),
072        /** Warning from the receiver (A = data ok, V = warning) */
073        RECEIVER_WARNING(2),
074        WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
075        LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
076        SPEED(7), COURSE(8), DATE(9),           // Speed in knots
077        MAGNETIC_DECLINATION(10), UNKNOWN(11),  // magnetic declination
078        /**
079         * Mode (A = autonom; D = differential; E = estimated; N = not valid; S
080         * = simulated)
081         *
082         * @since NMEA 2.3
083         */
084        MODE(12);
085
086        public final int position;
087
088        GPRMC(int position) {
089            this.position = position;
090        }
091    }
092
093    // The following only applies to GPGGA
094    public enum GPGGA {
095        TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
096        /**
097         * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA
098         * 2.3))
099         */
100        QUALITY(6), SATELLITE_COUNT(7),
101        HDOP(8), // HDOP (horizontal dilution of precision)
102        HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
103        HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
104        GPS_AGE(13), // Age of differential GPS data
105        REF(14); // REF station
106
107        public final int position;
108        GPGGA(int position) {
109            this.position = position;
110        }
111    }
112
113    public enum GPGSA {
114        AUTOMATIC(1),
115        FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
116        // PRN numbers for max 12 satellites
117        PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
118        PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
119        PDOP(15),   // PDOP (precision)
120        HDOP(16),   // HDOP (horizontal precision)
121        VDOP(17);   // VDOP (vertical precision)
122
123        public final int position;
124        GPGSA(int position) {
125            this.position = position;
126        }
127    }
128
129    public GpxData data;
130
131    private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS");
132    private final SimpleDateFormat rmcTimeFmtStd = new SimpleDateFormat("ddMMyyHHmmss");
133
134    private Date readTime(String p) {
135        Date d = rmcTimeFmt.parse(p, new ParsePosition(0));
136        if (d == null) {
137            d = rmcTimeFmtStd.parse(p, new ParsePosition(0));
138        }
139        if (d == null)
140            throw new RuntimeException("Date is malformed"); // malformed
141        return d;
142    }
143
144    // functons for reading the error stats
145    public NMEAParserState ps;
146
147    public int getParserUnknown() {
148        return ps.unknown;
149    }
150
151    public int getParserZeroCoordinates() {
152        return ps.zeroCoord;
153    }
154
155    public int getParserChecksumErrors() {
156        return ps.checksumErrors+ps.noChecksum;
157    }
158
159    public int getParserMalformed() {
160        return ps.malformed;
161    }
162
163    public int getNumberOfCoordinates() {
164        return ps.success;
165    }
166
167    public NmeaReader(InputStream source) throws IOException {
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 loopstartChar = rd.read();
176            ps = new NMEAParserState();
177            if (loopstartChar == -1)
178                //TODO tell user about the problem?
179                return;
180            sb.append((char) loopstartChar);
181            ps.pDate = "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 (IllegalDataException e) {
204            Main.warn(e);
205        }
206    }
207
208    private static class NMEAParserState {
209        protected Collection<WayPoint> waypoints = new ArrayList<>();
210        protected String pTime;
211        protected String pDate;
212        protected WayPoint pWp;
213
214        protected int success; // number of successfully parsed sentences
215        protected int malformed;
216        protected int checksumErrors;
217        protected int noChecksum;
218        protected int unknown;
219        protected int zeroCoord;
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                byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
238                int chk = 0;
239                for (int i = 1; i < chb.length; i++) {
240                    chk ^= chb[i];
241                }
242                if (Integer.parseInt(chkstrings[1].substring(0, 2), 16) != chk) {
243                    ps.checksumErrors++;
244                    ps.pWp = null;
245                    return false;
246                }
247            } else {
248                ps.noChecksum++;
249            }
250            // now for the content
251            String[] e = chkstrings[0].split(",");
252            String accu;
253
254            WayPoint currentwp = ps.pWp;
255            String currentDate = ps.pDate;
256
257            // handle the packet content
258            if ("$GPGGA".equals(e[0]) || "$GNGGA".equals(e[0])) {
259                // Position
260                LatLon latLon = parseLatLon(
261                        e[GPGGA.LATITUDE_NAME.position],
262                        e[GPGGA.LONGITUDE_NAME.position],
263                        e[GPGGA.LATITUDE.position],
264                        e[GPGGA.LONGITUDE.position]
265                );
266                if (latLon == null) {
267                    throw new IllegalDataException("Malformed lat/lon");
268                }
269
270                if (LatLon.ZERO.equals(latLon)) {
271                    ps.zeroCoord++;
272                    return false;
273                }
274
275                // time
276                accu = e[GPGGA.TIME.position];
277                Date d = readTime(currentDate+accu);
278
279                if ((ps.pTime == null) || (currentwp == null) || !ps.pTime.equals(accu)) {
280                    // this node is newer than the previous, create a new waypoint.
281                    // no matter if previous WayPoint was null, we got something better now.
282                    ps.pTime = accu;
283                    currentwp = new WayPoint(latLon);
284                }
285                if (!currentwp.attr.containsKey("time")) {
286                    // As this sentence has no complete time only use it
287                    // if there is no time so far
288                    currentwp.setTime(d);
289                }
290                // elevation
291                accu = e[GPGGA.HEIGHT_UNTIS.position];
292                if ("M".equals(accu)) {
293                    // Ignore heights that are not in meters for now
294                    accu = e[GPGGA.HEIGHT.position];
295                    if (!accu.isEmpty()) {
296                        Double.parseDouble(accu);
297                        // if it throws it's malformed; this should only happen if the
298                        // device sends nonstandard data.
299                        if (!accu.isEmpty()) { // FIX ? same check
300                            currentwp.put(GpxConstants.PT_ELE, accu);
301                        }
302                    }
303                }
304                // number of sattelites
305                accu = e[GPGGA.SATELLITE_COUNT.position];
306                int sat = 0;
307                if (!accu.isEmpty()) {
308                    sat = Integer.parseInt(accu);
309                    currentwp.put(GpxConstants.PT_SAT, accu);
310                }
311                // h-dilution
312                accu = e[GPGGA.HDOP.position];
313                if (!accu.isEmpty()) {
314                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
315                }
316                // fix
317                accu = e[GPGGA.QUALITY.position];
318                if (!accu.isEmpty()) {
319                    int fixtype = Integer.parseInt(accu);
320                    switch(fixtype) {
321                    case 0:
322                        currentwp.put(GpxConstants.PT_FIX, "none");
323                        break;
324                    case 1:
325                        if (sat < 4) {
326                            currentwp.put(GpxConstants.PT_FIX, "2d");
327                        } else {
328                            currentwp.put(GpxConstants.PT_FIX, "3d");
329                        }
330                        break;
331                    case 2:
332                        currentwp.put(GpxConstants.PT_FIX, "dgps");
333                        break;
334                    default:
335                        break;
336                    }
337                }
338            } else if ("$GPVTG".equals(e[0]) || "$GNVTG".equals(e[0])) {
339                // COURSE
340                accu = e[GPVTG.COURSE_REF.position];
341                if ("T".equals(accu)) {
342                    // other values than (T)rue are ignored
343                    accu = e[GPVTG.COURSE.position];
344                    if (!accu.isEmpty()) {
345                        Double.parseDouble(accu);
346                        currentwp.put("course", accu);
347                    }
348                }
349                // SPEED
350                accu = e[GPVTG.SPEED_KMH_UNIT.position];
351                if (accu.startsWith("K")) {
352                    accu = e[GPVTG.SPEED_KMH.position];
353                    if (!accu.isEmpty()) {
354                        double speed = Double.parseDouble(accu);
355                        speed /= 3.6; // speed in m/s
356                        currentwp.put("speed", Double.toString(speed));
357                    }
358                }
359            } else if ("$GPGSA".equals(e[0]) || "$GNGSA".equals(e[0])) {
360                // vdop
361                accu = e[GPGSA.VDOP.position];
362                if (!accu.isEmpty()) {
363                    currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
364                }
365                // hdop
366                accu = e[GPGSA.HDOP.position];
367                if (!accu.isEmpty()) {
368                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
369                }
370                // pdop
371                accu = e[GPGSA.PDOP.position];
372                if (!accu.isEmpty()) {
373                    currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
374                }
375            } else if ("$GPRMC".equals(e[0]) || "$GNRMC".equals(e[0])) {
376                // coordinates
377                LatLon latLon = parseLatLon(
378                        e[GPRMC.WIDTH_NORTH_NAME.position],
379                        e[GPRMC.LENGTH_EAST_NAME.position],
380                        e[GPRMC.WIDTH_NORTH.position],
381                        e[GPRMC.LENGTH_EAST.position]
382                );
383                if (LatLon.ZERO.equals(latLon)) {
384                    ps.zeroCoord++;
385                    return false;
386                }
387                // time
388                currentDate = e[GPRMC.DATE.position];
389                String time = e[GPRMC.TIME.position];
390
391                Date d = readTime(currentDate+time);
392
393                if (ps.pTime == null || currentwp == null || !ps.pTime.equals(time)) {
394                    // this node is newer than the previous, create a new waypoint.
395                    ps.pTime = time;
396                    currentwp = new WayPoint(latLon);
397                }
398                // time: this sentence has complete time so always use it.
399                currentwp.setTime(d);
400                // speed
401                accu = e[GPRMC.SPEED.position];
402                if (!accu.isEmpty() && !currentwp.attr.containsKey("speed")) {
403                    double speed = Double.parseDouble(accu);
404                    speed *= 0.514444444; // to m/s
405                    currentwp.put("speed", Double.toString(speed));
406                }
407                // course
408                accu = e[GPRMC.COURSE.position];
409                if (!accu.isEmpty() && !currentwp.attr.containsKey("course")) {
410                    Double.parseDouble(accu);
411                    currentwp.put("course", accu);
412                }
413
414                // TODO fix?
415                // * Mode (A = autonom; D = differential; E = estimated; N = not valid; S
416                // * = simulated)
417                // *
418                // * @since NMEA 2.3
419                //
420                //MODE(12);
421            } else {
422                ps.unknown++;
423                return false;
424            }
425            ps.pDate = currentDate;
426            if (ps.pWp != currentwp) {
427                if (ps.pWp != null) {
428                    ps.pWp.setTime();
429                }
430                ps.pWp = currentwp;
431                ps.waypoints.add(currentwp);
432                ps.success++;
433                return true;
434            }
435            return true;
436
437        } catch (RuntimeException x) {
438            // out of bounds and such
439            ps.malformed++;
440            ps.pWp = null;
441            return false;
442        }
443    }
444
445    private static LatLon parseLatLon(String ns, String ew, String dlat, String dlon)
446    throws NumberFormatException {
447        String widthNorth = dlat.trim();
448        String lengthEast = dlon.trim();
449
450        // return a zero latlon instead of null so it is logged as zero coordinate
451        // instead of malformed sentence
452        if (widthNorth.isEmpty() && lengthEast.isEmpty()) return LatLon.ZERO;
453
454        // The format is xxDDLL.LLLL
455        // xx optional whitespace
456        // DD (int) degres
457        // LL.LLLL (double) latidude
458        int latdegsep = widthNorth.indexOf('.') - 2;
459        if (latdegsep < 0) return null;
460
461        int latdeg = Integer.parseInt(widthNorth.substring(0, latdegsep));
462        double latmin = Double.parseDouble(widthNorth.substring(latdegsep));
463        if (latdeg < 0) {
464            latmin *= -1.0;
465        }
466        double lat = latdeg + latmin / 60;
467        if ("S".equals(ns)) {
468            lat = -lat;
469        }
470
471        int londegsep = lengthEast.indexOf('.') - 2;
472        if (londegsep < 0) return null;
473
474        int londeg = Integer.parseInt(lengthEast.substring(0, londegsep));
475        double lonmin = Double.parseDouble(lengthEast.substring(londegsep));
476        if (londeg < 0) {
477            lonmin *= -1.0;
478        }
479        double lon = londeg + lonmin / 60;
480        if ("W".equals(ew)) {
481            lon = -lon;
482        }
483        return new LatLon(lat, lon);
484    }
485}