Changeset 18787 in josm for trunk/src/org


Ignore:
Timestamp:
2023-07-27T22:22:25+02:00 (11 months ago)
Author:
stoecker
Message:

separate NmeaParser from NmeaReader

Location:
trunk/src/org/openstreetmap/josm/io/nmea
Files:
1 added
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/src/org/openstreetmap/josm/io/nmea/NmeaReader.java

    r18742 r18787  
    77import java.io.InputStreamReader;
    88import java.nio.charset.StandardCharsets;
    9 import java.text.ParsePosition;
    10 import java.text.SimpleDateFormat;
    11 import java.time.Instant;
    129import java.util.ArrayList;
    13 import java.util.Arrays;
    1410import java.util.Collection;
    1511import java.util.Collections;
    16 import java.util.Date;
    17 import java.util.Locale;
    1812import java.util.Objects;
    19 import java.util.regex.Matcher;
    20 import java.util.regex.Pattern;
    2113
    22 import org.openstreetmap.josm.data.coor.LatLon;
    23 import org.openstreetmap.josm.data.gpx.GpxConstants;
    2414import org.openstreetmap.josm.data.gpx.GpxData;
    2515import org.openstreetmap.josm.data.gpx.GpxTrack;
    … …  
    2818import org.openstreetmap.josm.io.IllegalDataException;
    2919import org.openstreetmap.josm.tools.Logging;
    30 import org.openstreetmap.josm.tools.date.DateUtils;
    31 import org.xml.sax.SAXException;
    3220
    3321/**
    … …  
    4836public class NmeaReader implements IGpxReader {
    4937
     38    private final InputStream source;
     39    GpxData data;
     40
     41    private NmeaParser ps;
     42
     43    /* functions for reading the error stats */
     44
    5045    /**
    51      * Course Over Ground and Ground Speed.
    52      * <p>
    53      * The actual course and speed relative to the ground
     46     * Number of unknown sentences
     47     * @return return the number of unknown sentences encountered
    5448     */
    55     enum VTG {
    56         COURSE(1), COURSE_REF(2), // true course
    57         COURSE_M(3), COURSE_M_REF(4), // magnetic course
    58         SPEED_KN(5), SPEED_KN_UNIT(6), // speed in knots
    59         SPEED_KMH(7), SPEED_KMH_UNIT(8), // speed in km/h
    60         REST(9); // version-specific rest
    61 
    62         final int position;
    63 
    64         VTG(int position) {
    65             this.position = position;
    66         }
     49    public int getParserUnknown() {
     50        return ps.getParserUnknown();
    6751    }
    6852
    6953    /**
    70      * Recommended Minimum Specific GNSS Data.
    71      * <p>
    72      * Time, date, position, course and speed data provided by a GNSS navigation receiver.
    73      * This sentence is transmitted at intervals not exceeding 2-seconds.
    74      * RMC is the recommended minimum data to be provided by a GNSS receiver.
    75      * All data fields must be provided, null fields used only when data is temporarily unavailable.
     54     * Number of empty coordinates
     55     * @return return the number of coordinates which have been zero
    7656     */
    77     enum RMC {
    78         TIME(1),
    79         /** Warning from the receiver (A = data ok, V = warning) */
    80         RECEIVER_WARNING(2),
    81         WIDTH_NORTH(3), WIDTH_NORTH_NAME(4), // Latitude, NS
    82         LENGTH_EAST(5), LENGTH_EAST_NAME(6), // Longitude, EW
    83         SPEED(7), COURSE(8), DATE(9),           // Speed in knots
    84         MAGNETIC_DECLINATION(10), UNKNOWN(11),  // magnetic declination
    85         /**
    86          * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
    87          *
    88          * @since NMEA 2.3
    89          */
    90         MODE(12);
    91 
    92         final int position;
    93 
    94         RMC(int position) {
    95             this.position = position;
    96         }
    97     }
    98 
    99     /**
    100      * Global Positioning System Fix Data.
    101      * <p>
    102      * Time, position and fix related data for a GPS receiver.
    103      */
    104     enum GGA {
    105         TIME(1), LATITUDE(2), LATITUDE_NAME(3), LONGITUDE(4), LONGITUDE_NAME(5),
    106         /**
    107          * Quality (0 = invalid, 1 = GPS, 2 = DGPS, 6 = estimanted (@since NMEA 2.3))
    108          */
    109         QUALITY(6), SATELLITE_COUNT(7),
    110         HDOP(8), // HDOP (horizontal dilution of precision)
    111         HEIGHT(9), HEIGHT_UNTIS(10), // height above NN (above geoid)
    112         HEIGHT_2(11), HEIGHT_2_UNTIS(12), // height geoid - height ellipsoid (WGS84)
    113         GPS_AGE(13), // Age of differential GPS data
    114         REF(14); // REF station
    115 
    116         final int position;
    117         GGA(int position) {
    118             this.position = position;
    119         }
    120     }
    121 
    122     /**
    123      * GNSS DOP and Active Satellites.
    124      * <p>
    125      * GNSS receiver operating mode, satellites used in the navigation solution reported by the GGA or GNS sentence,
    126      * and DOP values.
    127      * If only GPS, GLONASS, etc. is used for the reported position solution the talker ID is GP, GL, etc.
    128      * and the DOP values pertain to the individual system. If GPS, GLONASS, etc. are combined to obtain the
    129      * reported position solution multiple GSA sentences are produced, one with the GPS satellites, another with
    130      * the GLONASS satellites, etc. Each of these GSA sentences shall have talker ID GN, to indicate that the
    131      * satellites are used in a combined solution and each shall have the PDOP, HDOP and VDOP for the
    132      * combined satellites used in the position.
    133      */
    134     enum GSA {
    135         AUTOMATIC(1),
    136         FIX_TYPE(2), // 1 = not fixed, 2 = 2D fixed, 3 = 3D fixed)
    137         // PRN numbers for max 12 satellites
    138         PRN_1(3), PRN_2(4), PRN_3(5), PRN_4(6), PRN_5(7), PRN_6(8),
    139         PRN_7(9), PRN_8(10), PRN_9(11), PRN_10(12), PRN_11(13), PRN_12(14),
    140         PDOP(15),   // PDOP (precision)
    141         HDOP(16),   // HDOP (horizontal precision)
    142         VDOP(17);   // VDOP (vertical precision)
    143 
    144         final int position;
    145         GSA(int position) {
    146             this.position = position;
    147         }
    148     }
    149 
    150     /**
    151      * Geographic Position - Latitude/Longitude.
    152      * <p>
    153      * Latitude and Longitude of vessel position, time of position fix and status.
    154      */
    155     enum GLL {
    156         LATITUDE(1), LATITUDE_NS(2), // Latitude, NS
    157         LONGITUDE(3), LONGITUDE_EW(4), // Latitude, EW
    158         UTC(5), // Universal Time Coordinated
    159         STATUS(6), // Status: A = Data valid, V = Data not valid
    160         /**
    161          * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
    162          * @since NMEA 2.3
    163          */
    164         MODE(7);
    165 
    166         final int position;
    167         GLL(int position) {
    168             this.position = position;
    169         }
    170     }
    171 
    172     private final InputStream source;
    173     GpxData data;
    174 
    175     private static final Pattern DATE_TIME_PATTERN = Pattern.compile("(\\d{12})(\\.\\d+)?");
    176 
    177     private final SimpleDateFormat rmcTimeFmt = new SimpleDateFormat("ddMMyyHHmmss.SSS", Locale.ENGLISH);
    178 
    179     private Instant readTime(String p) throws IllegalDataException {
    180         // NMEA defines time with "a variable number of digits for decimal-fraction of seconds"
    181         // This variable decimal fraction cannot be parsed by SimpleDateFormat
    182         Matcher m = DATE_TIME_PATTERN.matcher(p);
    183         if (m.matches()) {
    184             String date = m.group(1);
    185             double milliseconds = 0d;
    186             if (m.groupCount() > 1 && m.group(2) != null) {
    187                 milliseconds = 1000d * Double.parseDouble("0" + m.group(2));
    188             }
    189             // Add milliseconds on three digits to match SimpleDateFormat pattern
    190             date += String.format(".%03d", (int) milliseconds);
    191             Date d = rmcTimeFmt.parse(date, new ParsePosition(0));
    192             if (d != null)
    193                 return d.toInstant();
    194         }
    195         throw new IllegalDataException("Date is malformed: '" + p + "'");
    196     }
    197 
    198     // functons for reading the error stats
    199     public NMEAParserState ps;
    200 
    201     public int getParserUnknown() {
    202         return ps.unknown;
    203     }
    204 
    20557    public int getParserZeroCoordinates() {
    20658        return ps.zeroCoord;
    20759    }
    20860
     61    /**
     62     * Number of checksum errors
     63     * @return return the number of sentences with checksum errors
     64     */
    20965    public int getParserChecksumErrors() {
    21066        return ps.checksumErrors+ps.noChecksum;
    21167    }
    21268
     69    /**
     70     * Number of malformed errors
     71     * @return return the number of malformed sentences
     72     */
    21373    public int getParserMalformed() {
    21474        return ps.malformed;
    … …  
    21777    @Override
    21878    public int getNumberOfCoordinates() {
    219         return ps.success;
     79        return ps.getSuccess();
    22080    }
    22181
    … …  
    22787    public NmeaReader(InputStream source) throws IOException {
    22888        this.source = Objects.requireNonNull(source);
    229         rmcTimeFmt.setTimeZone(DateUtils.UTC);
    23089    }
    23190
    23291    @Override
    233     public boolean parse(boolean tryToFinish) throws SAXException, IOException {
     92    public boolean parse(boolean tryToFinish) throws IOException {
    23493        // create the data tree
    23594        data = new GpxData();
    … …  
    23998            StringBuilder sb = new StringBuilder(1024);
    24099            int loopstartChar = rd.read();
    241             ps = new NMEAParserState();
     100            ps = new NmeaParser();
    242101            if (loopstartChar == -1)
    243102                //TODO tell user about the problem?
    244103                return false;
    245104            sb.append((char) loopstartChar);
    246             ps.pDate = "010100"; // TODO date problem
    247105            while (true) {
    248106                // don't load unparsable files completely to memory
    … …  
    252110                int c = rd.read();
    253111                if (c == '$') {
    254                     parseNMEASentence(sb.toString(), ps);
     112                    ps.parseNMEASentence(sb.toString());
    255113                    sb.delete(0, sb.length());
    256114                    sb.append('$');
    257115                } else if (c == -1) {
    258116                    // EOF: add last WayPoint if it works out
    259                     parseNMEASentence(sb.toString(), ps);
     117                    ps.parseNMEASentence(sb.toString());
    260118                    break;
    261119                } else {
    … …  
    263121                }
    264122            }
    265             currentTrack.add(ps.waypoints);
     123            currentTrack.add(ps.getWaypoints());
    266124            data.tracks.add(new GpxTrack(currentTrack, Collections.<String, Object>emptyMap()));
    267125
    … …  
    273131    }
    274132
    275     private static class NMEAParserState {
    276         protected Collection<WayPoint> waypoints = new ArrayList<>();
    277         protected String pTime;
    278         protected String pDate;
    279         protected WayPoint pWp;
    280 
    281         protected int success; // number of successfully parsed sentences
    282         protected int malformed;
    283         protected int checksumErrors;
    284         protected int noChecksum;
    285         protected int unknown;
    286         protected int zeroCoord;
    287     }
    288 
    289     /**
    290      * Determines if the given address denotes the given NMEA sentence formatter of a known talker.
    291      * @param address first tag of an NMEA sentence
    292      * @param formatter sentence formatter mnemonic code
    293      * @return {@code true} if the {@code address} denotes the given NMEA sentence formatter of a known talker
    294      */
    295     static boolean isSentence(String address, Sentence formatter) {
    296         return Arrays.stream(TalkerId.values())
    297                 .anyMatch(talker -> address.equals('$' + talker.name() + formatter.name()));
    298     }
    299 
    300     // Parses split up sentences into WayPoints which are stored
    301     // in the collection in the NMEAParserState object.
    302     // Returns true if the input made sense, false otherwise.
    303     private boolean parseNMEASentence(String s, NMEAParserState ps) throws IllegalDataException {
    304         try {
    305             if (s.isEmpty()) {
    306                 throw new IllegalArgumentException("s is empty");
    307             }
    308 
    309             // checksum check:
    310             // the bytes between the $ and the * are xored
    311             // if there is no * or other meanities it will throw
    312             // and result in a malformed packet.
    313             String[] chkstrings = s.split("\\*", -1);
    314             if (chkstrings.length > 1) {
    315                 byte[] chb = chkstrings[0].getBytes(StandardCharsets.UTF_8);
    316                 int chk = 0;
    317                 for (int i = 1; i < chb.length; i++) {
    318                     chk ^= chb[i];
    319                 }
    320                 if (Integer.parseInt(chkstrings[1].substring(0, 2), 16) != chk) {
    321                     ps.checksumErrors++;
    322                     ps.pWp = null;
    323                     return false;
    324                 }
    325             } else {
    326                 ps.noChecksum++;
    327             }
    328             // now for the content
    329             String[] e = chkstrings[0].split(",", -1);
    330             String accu;
    331 
    332             WayPoint currentwp = ps.pWp;
    333             String currentDate = ps.pDate;
    334 
    335             // handle the packet content
    336             if (isSentence(e[0], Sentence.GGA)) {
    337                 // Position
    338                 LatLon latLon = parseLatLon(
    339                         e[GGA.LATITUDE_NAME.position],
    340                         e[GGA.LONGITUDE_NAME.position],
    341                         e[GGA.LATITUDE.position],
    342                         e[GGA.LONGITUDE.position]
    343                 );
    344                 if (latLon == null) {
    345                     throw new IllegalDataException("Malformed lat/lon");
    346                 }
    347 
    348                 if (LatLon.ZERO.equals(latLon)) {
    349                     ps.zeroCoord++;
    350                     return false;
    351                 }
    352 
    353                 // time
    354                 accu = e[GGA.TIME.position];
    355                 Instant instant = readTime(currentDate+accu);
    356 
    357                 if ((ps.pTime == null) || (currentwp == null) || !ps.pTime.equals(accu)) {
    358                     // this node is newer than the previous, create a new waypoint.
    359                     // no matter if previous WayPoint was null, we got something better now.
    360                     ps.pTime = accu;
    361                     currentwp = new WayPoint(latLon);
    362                 }
    363                 if (!currentwp.attr.containsKey("time")) {
    364                     // As this sentence has no complete time only use it
    365                     // if there is no time so far
    366                     currentwp.setInstant(instant);
    367                 }
    368                 // elevation
    369                 accu = e[GGA.HEIGHT_UNTIS.position];
    370                 if ("M".equals(accu)) {
    371                     // Ignore heights that are not in meters for now
    372                     accu = e[GGA.HEIGHT.position];
    373                     if (!accu.isEmpty()) {
    374                         Double.parseDouble(accu);
    375                         // if it throws it's malformed; this should only happen if the
    376                         // device sends nonstandard data.
    377                         if (!accu.isEmpty()) { // FIX ? same check
    378                             currentwp.put(GpxConstants.PT_ELE, accu);
    379                         }
    380                     }
    381                 }
    382                 // number of satellites
    383                 accu = e[GGA.SATELLITE_COUNT.position];
    384                 int sat = 0;
    385                 if (!accu.isEmpty()) {
    386                     sat = Integer.parseInt(accu);
    387                     currentwp.put(GpxConstants.PT_SAT, accu);
    388                 }
    389                 // h-dilution
    390                 accu = e[GGA.HDOP.position];
    391                 if (!accu.isEmpty()) {
    392                     currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
    393                 }
    394                 // fix
    395                 accu = e[GGA.QUALITY.position];
    396                 if (!accu.isEmpty()) {
    397                     int fixtype = Integer.parseInt(accu);
    398                     switch(fixtype) {
    399                     case 0:
    400                         currentwp.put(GpxConstants.PT_FIX, "none");
    401                         break;
    402                     case 1:
    403                         if (sat < 4) {
    404                             currentwp.put(GpxConstants.PT_FIX, "2d");
    405                         } else {
    406                             currentwp.put(GpxConstants.PT_FIX, "3d");
    407                         }
    408                         break;
    409                     case 2:
    410                         currentwp.put(GpxConstants.PT_FIX, "dgps");
    411                         break;
    412                     case 3:
    413                         currentwp.put(GpxConstants.PT_FIX, "pps");
    414                         break;
    415                     case 4:
    416                         currentwp.put(GpxConstants.PT_FIX, "rtk");
    417                         break;
    418                     case 5:
    419                         currentwp.put(GpxConstants.PT_FIX, "float rtk");
    420                         break;
    421                     case 6:
    422                         currentwp.put(GpxConstants.PT_FIX, "estimated");
    423                         break;
    424                     case 7:
    425                         currentwp.put(GpxConstants.PT_FIX, "manual");
    426                         break;
    427                     case 8:
    428                         currentwp.put(GpxConstants.PT_FIX, "simulated");
    429                         break;
    430                     default:
    431                         break;
    432                     }
    433                 }
    434                 // reference ID
    435                 if (GGA.REF.position < e.length) {
    436                     accu = e[GGA.REF.position];
    437                     if (!accu.isEmpty()) {
    438                         currentwp.put(GpxConstants.PT_DGPSID, accu);
    439                     }
    440                 }
    441             } else if (isSentence(e[0], Sentence.VTG)) {
    442                 // COURSE
    443                 accu = e[VTG.COURSE_REF.position];
    444                 if ("T".equals(accu)) {
    445                     // other values than (T)rue are ignored
    446                     accu = e[VTG.COURSE.position];
    447                     if (!accu.isEmpty() && currentwp != null) {
    448                         Double.parseDouble(accu);
    449                         currentwp.put("course", accu);
    450                     }
    451                 }
    452                 // SPEED
    453                 accu = e[VTG.SPEED_KMH_UNIT.position];
    454                 if (accu.startsWith("K")) {
    455                     accu = e[VTG.SPEED_KMH.position];
    456                     if (!accu.isEmpty() && currentwp != null) {
    457                         double speed = Double.parseDouble(accu);
    458                         currentwp.put("speed", Double.toString(speed)); // speed in km/h
    459                     }
    460                 }
    461             } else if (isSentence(e[0], Sentence.GSA)) {
    462                 // vdop
    463                 accu = e[GSA.VDOP.position];
    464                 if (!accu.isEmpty() && currentwp != null) {
    465                     currentwp.put(GpxConstants.PT_VDOP, Float.valueOf(accu));
    466                 }
    467                 // hdop
    468                 accu = e[GSA.HDOP.position];
    469                 if (!accu.isEmpty() && currentwp != null) {
    470                     currentwp.put(GpxConstants.PT_HDOP, Float.valueOf(accu));
    471                 }
    472                 // pdop
    473                 accu = e[GSA.PDOP.position];
    474                 if (!accu.isEmpty() && currentwp != null) {
    475                     currentwp.put(GpxConstants.PT_PDOP, Float.valueOf(accu));
    476                 }
    477             } else if (isSentence(e[0], Sentence.RMC)) {
    478                 // coordinates
    479                 LatLon latLon = parseLatLon(
    480                         e[RMC.WIDTH_NORTH_NAME.position],
    481                         e[RMC.LENGTH_EAST_NAME.position],
    482                         e[RMC.WIDTH_NORTH.position],
    483                         e[RMC.LENGTH_EAST.position]
    484                 );
    485                 if (LatLon.ZERO.equals(latLon)) {
    486                     ps.zeroCoord++;
    487                     return false;
    488                 }
    489                 // time
    490                 currentDate = e[RMC.DATE.position];
    491                 String time = e[RMC.TIME.position];
    492 
    493                 Instant instant = readTime(currentDate+time);
    494 
    495                 if (ps.pTime == null || currentwp == null || !ps.pTime.equals(time)) {
    496                     // this node is newer than the previous, create a new waypoint.
    497                     ps.pTime = time;
    498                     currentwp = new WayPoint(latLon);
    499                 }
    500                 // time: this sentence has complete time so always use it.
    501                 currentwp.setInstant(instant);
    502                 // speed
    503                 accu = e[RMC.SPEED.position];
    504                 if (!accu.isEmpty() && !currentwp.attr.containsKey("speed")) {
    505                     double speed = Double.parseDouble(accu);
    506                     speed *= 0.514444444 * 3.6; // to km/h
    507                     currentwp.put("speed", Double.toString(speed));
    508                 }
    509                 // course
    510                 accu = e[RMC.COURSE.position];
    511                 if (!accu.isEmpty() && !currentwp.attr.containsKey("course")) {
    512                     Double.parseDouble(accu);
    513                     currentwp.put("course", accu);
    514                 }
    515 
    516                 // TODO fix?
    517                 // * Mode (A = autonom; D = differential; E = estimated; N = not valid; S = simulated)
    518                 // *
    519                 // * @since NMEA 2.3
    520                 //
    521                 //MODE(12);
    522             } else if (isSentence(e[0], Sentence.GLL)) {
    523                 // coordinates
    524                 LatLon latLon = parseLatLon(
    525                         e[GLL.LATITUDE_NS.position],
    526                         e[GLL.LONGITUDE_EW.position],
    527                         e[GLL.LATITUDE.position],
    528                         e[GLL.LONGITUDE.position]
    529                 );
    530                 if (LatLon.ZERO.equals(latLon)) {
    531                     ps.zeroCoord++;
    532                     return false;
    533                 }
    534                 // only consider valid data
    535                 if (!"A".equals(e[GLL.STATUS.position])) {
    536                     return false;
    537                 }
    538 
    539                 // RMC sentences contain a full date while GLL sentences contain only time,
    540                 // so create new waypoints only of the NMEA file does not contain RMC sentences
    541                 if (ps.pTime == null || currentwp == null) {
    542                     currentwp = new WayPoint(latLon);
    543                 }
    544             } else {
    545                 ps.unknown++;
    546                 return false;
    547             }
    548             ps.pDate = currentDate;
    549             if (ps.pWp != currentwp) {
    550                 if (ps.pWp != null) {
    551                     ps.pWp.getInstant();
    552                 }
    553                 ps.pWp = currentwp;
    554                 ps.waypoints.add(currentwp);
    555                 ps.success++;
    556                 return true;
    557             }
    558             return true;
    559 
    560         } catch (IllegalArgumentException | IndexOutOfBoundsException | IllegalDataException ex) {
    561             if (ps.malformed < 5) {
    562                 Logging.warn(ex);
    563             } else {
    564                 Logging.debug(ex);
    565             }
    566             ps.malformed++;
    567             ps.pWp = null;
    568             return false;
    569         }
    570     }
    571 
    572     private static LatLon parseLatLon(String ns, String ew, String dlat, String dlon) {
    573         String widthNorth = dlat.trim();
    574         String lengthEast = dlon.trim();
    575 
    576         // return a zero latlon instead of null so it is logged as zero coordinate
    577         // instead of malformed sentence
    578         if (widthNorth.isEmpty() && lengthEast.isEmpty()) return LatLon.ZERO;
    579 
    580         // The format is xxDDLL.LLLL
    581         // xx optional whitespace
    582         // DD (int) degres
    583         // LL.LLLL (double) latidude
    584         int latdegsep = widthNorth.indexOf('.') - 2;
    585         if (latdegsep < 0) return null;
    586 
    587         int latdeg = Integer.parseInt(widthNorth.substring(0, latdegsep));
    588         double latmin = Double.parseDouble(widthNorth.substring(latdegsep));
    589         if (latdeg < 0) {
    590             latmin *= -1.0;
    591         }
    592         double lat = latdeg + latmin / 60;
    593         if ("S".equals(ns)) {
    594             lat = -lat;
    595         }
    596 
    597         int londegsep = lengthEast.indexOf('.') - 2;
    598         if (londegsep < 0) return null;
    599 
    600         int londeg = Integer.parseInt(lengthEast.substring(0, londegsep));
    601         double lonmin = Double.parseDouble(lengthEast.substring(londegsep));
    602         if (londeg < 0) {
    603             lonmin *= -1.0;
    604         }
    605         double lon = londeg + lonmin / 60;
    606         if ("W".equals(ew)) {
    607             lon = -lon;
    608         }
    609         return new LatLon(lat, lon);
    610     }
    611 
    612133    @Override
    613134    public GpxData getGpxData() {
Note: See TracChangeset for help on using the changeset viewer.