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