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

Last change on this file since 12421 was 12421, checked in by Don-vip, 4 months ago

see #14924 - improve NMEA documentation thanks to gpsd (http://www.catb.org/gpsd/NMEA.html) + add support for NMEA sentences coming from GLONASS, Galileo or Beidu receivers

  • Property svn:eol-style set to native
File size: 17.8 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.Optional;
16
17import org.openstreetmap.josm.Main;
18import org.openstreetmap.josm.data.coor.LatLon;
19import org.openstreetmap.josm.data.gpx.GpxConstants;
20import org.openstreetmap.josm.data.gpx.GpxData;
21import org.openstreetmap.josm.data.gpx.ImmutableGpxTrack;
22import org.openstreetmap.josm.data.gpx.WayPoint;
23import org.openstreetmap.josm.io.IllegalDataException;
24import org.openstreetmap.josm.tools.JosmRuntimeException;
25import org.openstreetmap.josm.tools.date.DateUtils;
26
27/**
28 * Reads a NMEA 0183 file. Based on information from
29 * <a href="http://www.catb.org/gpsd/NMEA.html">http://www.catb.org/gpsd</a>.
30 *
31 * NMEA files are in printable ASCII form and may include information such as position,
32 * speed, depth, frequency allocation, etc.
33 * Typical messages might be 11 to a maximum of 79 characters in length.
34 *
35 * NMEA standard aims to support one-way serial data transmission from a single "talker"
36 * to one or more "listeners". The type of talker is identified by a 2-character mnemonic.
37 *
38 * NMEA information is encoded through a list of "sentences".
39 *
40 * @author cbrill
41 */
42public class NmeaReader {
43
44    public enum VTG {
45        COURSE(1), COURSE_REF(2), // true course
46        COURSE_M(3), COURSE_M_REF(4), // magnetic course
47        SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
48        SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
49        REST(9); // version-specific rest
50
51        public final int position;
52
53        VTG(int position) {
54            this.position = position;
55        }
56    }
57
58    public enum RMC {
59        TIME(1),
60        /** Warning from the receiver (A = data ok, V = warning) */
61        RECEIVER_WARNING(2),
62        WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
63        LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
64        SPEED(7), COURSE(8), DATE(9),           // Speed in knots
65        MAGNETIC_DECLINATION(10), UNKNOWN(11),  // magnetic declination
66        /**
67         * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
68         *
69         * @since NMEA 2.3
70         */
71        MODE(12);
72
73        public final int position;
74
75        RMC(int position) {
76            this.position = position;
77        }
78    }
79
80    public enum GGA {
81        TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
82        /**
83         * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA 2.3))
84         */
85        QUALITY(6), SATELLITE_COUNT(7),
86        HDOP(8), // HDOP (horizontal dilution of precision)
87        HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
88        HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
89        GPS_AGE(13), // Age of differential GPS data
90        REF(14); // REF station
91
92        public final int position;
93        GGA(int position) {
94            this.position = position;
95        }
96    }
97
98    public enum GSA {
99        AUTOMATIC(1),
100        FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
101        // PRN numbers for max 12 satellites
102        PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
103        PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
104        PDOP(15),   // PDOP (precision)
105        HDOP(16),   // HDOP (horizontal precision)
106        VDOP(17);   // VDOP (vertical precision)
107
108        public final int position;
109        GSA(int position) {
110            this.position = position;
111        }
112    }
113
114    public GpxData data;
115
116    private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS");
117    private final SimpleDateFormat rmcTimeFmtStd = new SimpleDateFormat("ddMMyyHHmmss");
118
119    private Date readTime(String p) {
120        Date d = Optional.ofNullable(rmcTimeFmt.parse(p, new ParsePosition(0)))
121                .orElseGet(() -> rmcTimeFmtStd.parse(p, new ParsePosition(0)));
122        if (d == null)
123            throw new JosmRuntimeException("Date is malformed");
124        return d;
125    }
126
127    // functons for reading the error stats
128    public NMEAParserState ps;
129
130    public int getParserUnknown() {
131        return ps.unknown;
132    }
133
134    public int getParserZeroCoordinates() {
135        return ps.zeroCoord;
136    }
137
138    public int getParserChecksumErrors() {
139        return ps.checksumErrors+ps.noChecksum;
140    }
141
142    public int getParserMalformed() {
143        return ps.malformed;
144    }
145
146    public int getNumberOfCoordinates() {
147        return ps.success;
148    }
149
150    public NmeaReader(InputStream source) throws IOException {
151        rmcTimeFmt.setTimeZone(DateUtils.UTC);
152        rmcTimeFmtStd.setTimeZone(DateUtils.UTC);
153
154        // create the data tree
155        data = new GpxData();
156        Collection<Collection<WayPoint>> currentTrack = new ArrayList<>();
157
158        try (BufferedReader rd = new BufferedReader(new InputStreamReader(source, StandardCharsets.UTF_8))) {
159            StringBuilder sb = new StringBuilder(1024);
160            int loopstartChar = rd.read();
161            ps = new NMEAParserState();
162            if (loopstartChar == -1)
163                //TODO tell user about the problem?
164                return;
165            sb.append((char) loopstartChar);
166            ps.pDate = "010100"; // TODO date problem
167            while (true) {
168                // don't load unparsable files completely to memory
169                if (sb.length() >= 1020) {
170                    sb.delete(0, sb.length()-1);
171                }
172                int c = rd.read();
173                if (c == '$') {
174                    parseNMEASentence(sb.toString(), ps);
175                    sb.delete(0, sb.length());
176                    sb.append('$');
177                } else if (c == -1) {
178                    // EOF: add last WayPoint if it works out
179                    parseNMEASentence(sb.toString(), ps);
180                    break;
181                } else {
182                    sb.append((char) c);
183                }
184            }
185            currentTrack.add(ps.waypoints);
186            data.tracks.add(new ImmutableGpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
187
188        } catch (IllegalDataException e) {
189            Main.warn(e);
190        }
191    }
192
193    private static class NMEAParserState {
194        protected Collection<WayPoint> waypoints = new ArrayList<>();
195        protected String pTime;
196        protected String pDate;
197        protected WayPoint pWp;
198
199        protected int success; // number of successfully parsed sentences
200        protected int malformed;
201        protected int checksumErrors;
202        protected int noChecksum;
203        protected int unknown;
204        protected int zeroCoord;
205    }
206
207    /**
208     * Determines if the given address denotes the given NMEA sentence formatter of a known talker.
209     * @param address first tag of an NMEA sentence
210     * @param formatter sentence formatter mnemonic code
211     * @return {@code true} if the {@code address} denotes the given NMEA sentence formatter of a known talker
212     */
213    static boolean isSentence(String address, Sentence formatter) {
214        for (TalkerId talker : TalkerId.values()) {
215            if (address.equals('$' + talker.name() + formatter.name())) {
216                return true;
217            }
218        }
219        return false;
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 sense, 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 (isSentence(e[0], Sentence.GGA)) {
259                // Position
260                LatLon latLon = parseLatLon(
261                        e[GGA.LATITUDE_NAME.position],
262                        e[GGA.LONGITUDE_NAME.position],
263                        e[GGA.LATITUDE.position],
264                        e[GGA.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[GGA.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[GGA.HEIGHT_UNTIS.position];
292                if ("M".equals(accu)) {
293                    // Ignore heights that are not in meters for now
294                    accu = e[GGA.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 satellites
305                accu = e[GGA.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[GGA.HDOP.position];
313                if (!accu.isEmpty()) {
314                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
315                }
316                // fix
317                accu = e[GGA.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 (isSentence(e[0], Sentence.VTG)) {
339                // COURSE
340                accu = e[VTG.COURSE_REF.position];
341                if ("T".equals(accu)) {
342                    // other values than (T)rue are ignored
343                    accu = e[VTG.COURSE.position];
344                    if (!accu.isEmpty()) {
345                        Double.parseDouble(accu);
346                        currentwp.put("course", accu);
347                    }
348                }
349                // SPEED
350                accu = e[VTG.SPEED_KMH_UNIT.position];
351                if (accu.startsWith("K")) {
352                    accu = e[VTG.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 (isSentence(e[0], Sentence.GSA)) {
360                // vdop
361                accu = e[GSA.VDOP.position];
362                if (!accu.isEmpty() && currentwp != null) {
363                    currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
364                }
365                // hdop
366                accu = e[GSA.HDOP.position];
367                if (!accu.isEmpty() && currentwp != null) {
368                    currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
369                }
370                // pdop
371                accu = e[GSA.PDOP.position];
372                if (!accu.isEmpty() && currentwp != null) {
373                    currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
374                }
375            } else if (isSentence(e[0], Sentence.RMC)) {
376                // coordinates
377                LatLon latLon = parseLatLon(
378                        e[RMC.WIDTH_NORTH_NAME.position],
379                        e[RMC.LENGTH_EAST_NAME.position],
380                        e[RMC.WIDTH_NORTH.position],
381                        e[RMC.LENGTH_EAST.position]
382                );
383                if (LatLon.ZERO.equals(latLon)) {
384                    ps.zeroCoord++;
385                    return false;
386                }
387                // time
388                currentDate = e[RMC.DATE.position];
389                String time = e[RMC.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[RMC.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[RMC.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 (IllegalArgumentException | IndexOutOfBoundsException | IllegalDataException ex) {
438            // out of bounds and such
439            Main.debug(ex);
440            ps.malformed++;
441            ps.pWp = null;
442            return false;
443        }
444    }
445
446    private static LatLon parseLatLon(String ns, String ew, String dlat, String dlon) {
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}
Note: See TracBrowser for help on using the repository browser.