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