source: josm/trunk/src/org/openstreetmap/josm/io/nmea/NmeaReader.java @ 14010

Last change on this file since 14010 was 14010, checked in by Don-vip, 6 weeks ago

fix #16471 - Support NMEA files when correlating images to a GPX track

  • Property svn:eol-style set to native
File size: 19.9 KB
Line 
1// License: GPL. For details, see LICENSE file.
2package org.openstreetmap.josm.io.nmea;
3
4import java.io.BufferedReader;
5import java.io.IOException;
6import java.io.InputStream;
7import java.io.InputStreamReader;
8import java.nio.charset.StandardCharsets;
9import java.text.ParsePosition;
10import java.text.SimpleDateFormat;
11import java.util.ArrayList;
12import java.util.Collection;
13import java.util.Collections;
14import java.util.Date;
15import java.util.Locale;
16import java.util.Objects;
17import java.util.Optional;
18
19import org.openstreetmap.josm.data.coor.LatLon;
20import org.openstreetmap.josm.data.gpx.GpxConstants;
21import org.openstreetmap.josm.data.gpx.GpxData;
22import org.openstreetmap.josm.data.gpx.ImmutableGpxTrack;
23import org.openstreetmap.josm.data.gpx.WayPoint;
24import org.openstreetmap.josm.io.IGpxReader;
25import org.openstreetmap.josm.io.IllegalDataException;
26import org.openstreetmap.josm.tools.Logging;
27import org.openstreetmap.josm.tools.date.DateUtils;
28import org.xml.sax.SAXException;
29
30/**
31 * Reads a NMEA 0183 file. Based on information from
32 * <a href="http://www.catb.org/gpsd/NMEA.html">http://www.catb.org/gpsd</a>.
33 *
34 * NMEA files are in printable ASCII form and may include information such as position,
35 * speed, depth, frequency allocation, etc.
36 * Typical messages might be 11 to a maximum of 79 characters in length.
37 *
38 * NMEA standard aims to support one-way serial data transmission from a single "talker"
39 * to one or more "listeners". The type of talker is identified by a 2-character mnemonic.
40 *
41 * NMEA information is encoded through a list of "sentences".
42 *
43 * @author cbrill
44 */
45public class NmeaReader implements IGpxReader {
46
47    enum VTG {
48        COURSE(1), COURSE_REF(2), // true course
49        COURSE_M(3), COURSE_M_REF(4), // magnetic course
50        SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
51        SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
52        REST(9); // version-specific rest
53
54        final int position;
55
56        VTG(int position) {
57            this.position = position;
58        }
59    }
60
61    enum RMC {
62        TIME(1),
63        /** Warning from the receiver (A = data ok, V = warning) */
64        RECEIVER_WARNING(2),
65        WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
66        LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
67        SPEED(7), COURSE(8), DATE(9),           // Speed in knots
68        MAGNETIC_DECLINATION(10), UNKNOWN(11),  // magnetic declination
69        /**
70         * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
71         *
72         * @since NMEA 2.3
73         */
74        MODE(12);
75
76        final int position;
77
78        RMC(int position) {
79            this.position = position;
80        }
81    }
82
83    enum GGA {
84        TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
85        /**
86         * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA 2.3))
87         */
88        QUALITY(6), SATELLITE_COUNT(7),
89        HDOP(8), // HDOP (horizontal dilution of precision)
90        HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
91        HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
92        GPS_AGE(13), // Age of differential GPS data
93        REF(14); // REF station
94
95        final int position;
96        GGA(int position) {
97            this.position = position;
98        }
99    }
100
101    enum GSA {
102        AUTOMATIC(1),
103        FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
104        // PRN numbers for max 12 satellites
105        PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
106        PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
107        PDOP(15),   // PDOP (precision)
108        HDOP(16),   // HDOP (horizontal precision)
109        VDOP(17);   // VDOP (vertical precision)
110
111        final int position;
112        GSA(int position) {
113            this.position = position;
114        }
115    }
116
117    enum GLL {
118        LATITUDE(1), LATITUDE_NS(2), // Latitude, NS
119        LONGITUDE(3), LONGITUDE_EW(4), // Latitude, EW
120        UTC(5), // Universal Time Coordinated
121        STATUS(6), // Status: A = Data valid, V = Data not valid
122        /**
123         * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
124         * @since NMEA 2.3
125         */
126        MODE(7);
127
128        final int position;
129        GLL(int position) {
130            this.position = position;
131        }
132    }
133
134    private final InputStream source;
135    GpxData data;
136
137    private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS", Locale.ENGLISH);
138    private final SimpleDateFormat rmcTimeFmtStd = new SimpleDateFormat("ddMMyyHHmmss", Locale.ENGLISH);
139
140    private Date readTime(String p) throws IllegalDataException {
141        Date d = Optional.ofNullable(rmcTimeFmt.parse(p, new ParsePosition(0)))
142                .orElseGet(() -> rmcTimeFmtStd.parse(p, new ParsePosition(0)));
143        if (d == null)
144            throw new IllegalDataException("Date is malformed: '" + p + "'");
145        return d;
146    }
147
148    // functons for reading the error stats
149    public NMEAParserState ps;
150
151    public int getParserUnknown() {
152        return ps.unknown;
153    }
154
155    public int getParserZeroCoordinates() {
156        return ps.zeroCoord;
157    }
158
159    public int getParserChecksumErrors() {
160        return ps.checksumErrors+ps.noChecksum;
161    }
162
163    public int getParserMalformed() {
164        return ps.malformed;
165    }
166
167    public int getNumberOfCoordinates() {
168        return ps.success;
169    }
170
171    /**
172     * Constructs a new {@code NmeaReader}
173     * @param source NMEA file input stream
174     * @throws IOException if an I/O error occurs
175     */
176    public NmeaReader(InputStream source) throws IOException {
177        this.source = Objects.requireNonNull(source);
178        rmcTimeFmt.setTimeZone(DateUtils.UTC);
179        rmcTimeFmtStd.setTimeZone(DateUtils.UTC);
180    }
181
182    @Override
183    public boolean parse(boolean tryToFinish) throws SAXException, IOException {
184        // create the data tree
185        data = new GpxData();
186        Collection<Collection<WayPoint>> currentTrack = new ArrayList<>();
187
188        try (BufferedReader rd = new BufferedReader(new InputStreamReader(source, StandardCharsets.UTF_8))) {
189            StringBuilder sb = new StringBuilder(1024);
190            int loopstartChar = rd.read();
191            ps = new NMEAParserState();
192            if (loopstartChar == -1)
193                //TODO tell user about the problem?
194                return false;
195            sb.append((char) loopstartChar);
196            ps.pDate = "010100"; // TODO date problem
197            while (true) {
198                // don't load unparsable files completely to memory
199                if (sb.length() >= 1020) {
200                    sb.delete(0, sb.length()-1);
201                }
202                int c = rd.read();
203                if (c == '$') {
204                    parseNMEASentence(sb.toString(), ps);
205                    sb.delete(0, sb.length());
206                    sb.append('$');
207                } else if (c == -1) {
208                    // EOF: add last WayPoint if it works out
209                    parseNMEASentence(sb.toString(), ps);
210                    break;
211                } else {
212                    sb.append((char) c);
213                }
214            }
215            currentTrack.add(ps.waypoints);
216            data.tracks.add(new ImmutableGpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
217
218        } catch (IllegalDataException e) {
219            Logging.warn(e);
220            return false;
221        }
222        return true;
223    }
224
225    private static class NMEAParserState {
226        protected Collection<WayPoint> waypoints = new ArrayList<>();
227        protected String pTime;
228        protected String pDate;
229        protected WayPoint pWp;
230
231        protected int success; // number of successfully parsed sentences
232        protected int malformed;
233        protected int checksumErrors;
234        protected int noChecksum;
235        protected int unknown;
236        protected int zeroCoord;
237    }
238
239    /**
240     * Determines if the given address denotes the given NMEA sentence formatter of a known talker.
241     * @param address first tag of an NMEA sentence
242     * @param formatter sentence formatter mnemonic code
243     * @return {@code true} if the {@code address} denotes the given NMEA sentence formatter of a known talker
244     */
245    static boolean isSentence(String address, Sentence formatter) {
246        for (TalkerId talker : TalkerId.values()) {
247            if (address.equals('$' + talker.name() + formatter.name())) {
248                return true;
249            }
250        }
251        return false;
252    }
253
254    // Parses split up sentences into WayPoints which are stored
255    // in the collection in the NMEAParserState object.
256    // Returns true if the input made sense, false otherwise.
257    private boolean parseNMEASentence(String s, NMEAParserState ps) throws IllegalDataException {
258        try {
259            if (s.isEmpty()) {
260                throw new IllegalArgumentException("s is empty");
261            }
262
263            // checksum check:
264            // the bytes between the $ and the * are xored
265            // if there is no * or other meanities it will throw
266            // and result in a malformed packet.
267            String[] chkstrings = s.split("\\*");
268            if (chkstrings.length > 1) {
269                byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
270                int chk = 0;
271                for (int i = 1; i < chb.length; i++) {
272                    chk ^= chb[i];
273                }
274                if (Integer.parseInt(chkstrings[1].substring(0, 2), 16) != chk) {
275                    ps.checksumErrors++;
276                    ps.pWp = null;
277                    return false;
278                }
279            } else {
280                ps.noChecksum++;
281            }
282            // now for the content
283            String[] e = chkstrings[0].split(",");
284            String accu;
285
286            WayPoint currentwp = ps.pWp;
287            String currentDate = ps.pDate;
288
289            // handle the packet content
290            if (isSentence(e[0], Sentence.GGA)) {
291                // Position
292                LatLon latLon = parseLatLon(
293                        e[GGA.LATITUDE_NAME.position],
294                        e[GGA.LONGITUDE_NAME.position],
295                        e[GGA.LATITUDE.position],
296                        e[GGA.LONGITUDE.position]
297                );
298                if (latLon == null) {
299                    throw new IllegalDataException("Malformed lat/lon");
300                }
301
302                if (LatLon.ZERO.equals(latLon)) {
303                    ps.zeroCoord++;
304                    return false;
305                }
306
307                // time
308                accu = e[GGA.TIME.position];
309                Date d = readTime(currentDate+accu);
310
311                if ((ps.pTime == null) || (currentwp == null) || !ps.pTime.equals(accu)) {
312                    // this node is newer than the previous, create a new waypoint.
313                    // no matter if previous WayPoint was null, we got something better now.
314                    ps.pTime = accu;
315                    currentwp = new WayPoint(latLon);
316                }
317                if (!currentwp.attr.containsKey("time")) {
318                    // As this sentence has no complete time only use it
319                    // if there is no time so far
320                    currentwp.setTime(d);
321                }
322                // elevation
323                accu = e[GGA.HEIGHT_UNTIS.position];
324                if ("M".equals(accu)) {
325                    // Ignore heights that are not in meters for now
326                    accu = e[GGA.HEIGHT.position];
327                    if (!accu.isEmpty()) {
328                        Double.parseDouble(accu);
329                        // if it throws it's malformed; this should only happen if the
330                        // device sends nonstandard data.
331                        if (!accu.isEmpty()) { // FIX ? same check
332                            currentwp.put(GpxConstants.PT_ELE, accu);
333                        }
334                    }
335                }
336                // number of satellites
337                accu = e[GGA.SATELLITE_COUNT.position];
338                int sat = 0;
339                if (!accu.isEmpty()) {
340                    sat = Integer.parseInt(accu);
341                    currentwp.put(GpxConstants.PT_SAT, accu);
342                }
343                // h-dilution
344                accu = e[GGA.HDOP.position];
345                if (!accu.isEmpty()) {
346                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
347                }
348                // fix
349                accu = e[GGA.QUALITY.position];
350                if (!accu.isEmpty()) {
351                    int fixtype = Integer.parseInt(accu);
352                    switch(fixtype) {
353                    case 0:
354                        currentwp.put(GpxConstants.PT_FIX, "none");
355                        break;
356                    case 1:
357                        if (sat < 4) {
358                            currentwp.put(GpxConstants.PT_FIX, "2d");
359                        } else {
360                            currentwp.put(GpxConstants.PT_FIX, "3d");
361                        }
362                        break;
363                    case 2:
364                        currentwp.put(GpxConstants.PT_FIX, "dgps");
365                        break;
366                    default:
367                        break;
368                    }
369                }
370            } else if (isSentence(e[0], Sentence.VTG)) {
371                // COURSE
372                accu = e[VTG.COURSE_REF.position];
373                if ("T".equals(accu)) {
374                    // other values than (T)rue are ignored
375                    accu = e[VTG.COURSE.position];
376                    if (!accu.isEmpty() && currentwp != null) {
377                        Double.parseDouble(accu);
378                        currentwp.put("course", accu);
379                    }
380                }
381                // SPEED
382                accu = e[VTG.SPEED_KMH_UNIT.position];
383                if (accu.startsWith("K")) {
384                    accu = e[VTG.SPEED_KMH.position];
385                    if (!accu.isEmpty() && currentwp != null) {
386                        double speed = Double.parseDouble(accu);
387                        speed /= 3.6; // speed in m/s
388                        currentwp.put("speed", Double.toString(speed));
389                    }
390                }
391            } else if (isSentence(e[0], Sentence.GSA)) {
392                // vdop
393                accu = e[GSA.VDOP.position];
394                if (!accu.isEmpty() && currentwp != null) {
395                    currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
396                }
397                // hdop
398                accu = e[GSA.HDOP.position];
399                if (!accu.isEmpty() && currentwp != null) {
400                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
401                }
402                // pdop
403                accu = e[GSA.PDOP.position];
404                if (!accu.isEmpty() && currentwp != null) {
405                    currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
406                }
407            } else if (isSentence(e[0], Sentence.RMC)) {
408                // coordinates
409                LatLon latLon = parseLatLon(
410                        e[RMC.WIDTH_NORTH_NAME.position],
411                        e[RMC.LENGTH_EAST_NAME.position],
412                        e[RMC.WIDTH_NORTH.position],
413                        e[RMC.LENGTH_EAST.position]
414                );
415                if (LatLon.ZERO.equals(latLon)) {
416                    ps.zeroCoord++;
417                    return false;
418                }
419                // time
420                currentDate = e[RMC.DATE.position];
421                String time = e[RMC.TIME.position];
422
423                Date d = readTime(currentDate+time);
424
425                if (ps.pTime == null || currentwp == null || !ps.pTime.equals(time)) {
426                    // this node is newer than the previous, create a new waypoint.
427                    ps.pTime = time;
428                    currentwp = new WayPoint(latLon);
429                }
430                // time: this sentence has complete time so always use it.
431                currentwp.setTime(d);
432                // speed
433                accu = e[RMC.SPEED.position];
434                if (!accu.isEmpty() && !currentwp.attr.containsKey("speed")) {
435                    double speed = Double.parseDouble(accu);
436                    speed *= 0.514444444; // to m/s
437                    currentwp.put("speed", Double.toString(speed));
438                }
439                // course
440                accu = e[RMC.COURSE.position];
441                if (!accu.isEmpty() && !currentwp.attr.containsKey("course")) {
442                    Double.parseDouble(accu);
443                    currentwp.put("course", accu);
444                }
445
446                // TODO fix?
447                // * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
448                // *
449                // * @since NMEA 2.3
450                //
451                //MODE(12);
452            } else if (isSentence(e[0], Sentence.GLL)) {
453                // coordinates
454                LatLon latLon = parseLatLon(
455                        e[GLL.LATITUDE_NS.position],
456                        e[GLL.LONGITUDE_EW.position],
457                        e[GLL.LATITUDE.position],
458                        e[GLL.LONGITUDE.position]
459                );
460                if (LatLon.ZERO.equals(latLon)) {
461                    ps.zeroCoord++;
462                    return false;
463                }
464                // only consider valid data
465                if (!"A".equals(e[GLL.STATUS.position])) {
466                    return false;
467                }
468
469                // RMC sentences contain a full date while GLL sentences contain only time,
470                // so create new waypoints only of the NMEA file does not contain RMC sentences
471                if (ps.pTime == null || currentwp == null) {
472                    currentwp = new WayPoint(latLon);
473                }
474            } else {
475                ps.unknown++;
476                return false;
477            }
478            ps.pDate = currentDate;
479            if (ps.pWp != currentwp) {
480                if (ps.pWp != null) {
481                    ps.pWp.setTime();
482                }
483                ps.pWp = currentwp;
484                ps.waypoints.add(currentwp);
485                ps.success++;
486                return true;
487            }
488            return true;
489
490        } catch (IllegalArgumentException | IndexOutOfBoundsException | IllegalDataException ex) {
491            if (ps.malformed < 5) {
492                Logging.warn(ex);
493            } else {
494                Logging.debug(ex);
495            }
496            ps.malformed++;
497            ps.pWp = null;
498            return false;
499        }
500    }
501
502    private static LatLon parseLatLon(String ns, String ew, String dlat, String dlon) {
503        String widthNorth = dlat.trim();
504        String lengthEast = dlon.trim();
505
506        // return a zero latlon instead of null so it is logged as zero coordinate
507        // instead of malformed sentence
508        if (widthNorth.isEmpty() && lengthEast.isEmpty()) return LatLon.ZERO;
509
510        // The format is xxDDLL.LLLL
511        // xx optional whitespace
512        // DD (int) degres
513        // LL.LLLL (double) latidude
514        int latdegsep = widthNorth.indexOf('.') - 2;
515        if (latdegsep < 0) return null;
516
517        int latdeg = Integer.parseInt(widthNorth.substring(0, latdegsep));
518        double latmin = Double.parseDouble(widthNorth.substring(latdegsep));
519        if (latdeg < 0) {
520            latmin *= -1.0;
521        }
522        double lat = latdeg + latmin / 60;
523        if ("S".equals(ns)) {
524            lat = -lat;
525        }
526
527        int londegsep = lengthEast.indexOf('.') - 2;
528        if (londegsep < 0) return null;
529
530        int londeg = Integer.parseInt(lengthEast.substring(0, londegsep));
531        double lonmin = Double.parseDouble(lengthEast.substring(londegsep));
532        if (londeg < 0) {
533            lonmin *= -1.0;
534        }
535        double lon = londeg + lonmin / 60;
536        if ("W".equals(ew)) {
537            lon = -lon;
538        }
539        return new LatLon(lat, lon);
540    }
541
542    @Override
543    public GpxData getGpxData() {
544        return data;
545    }
546}
Note: See TracBrowser for help on using the repository browser.