Advertisement
Guest User

NMEA http://darylrobotproject.wordpress.com/

a guest
Oct 1st, 2012
154
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 3.43 KB | None | 0 0
  1. package com.daryl.utils;
  2.  
  3. import java.util.HashMap;
  4. import java.util.Map;
  5.  
  6. public class Nmea {
  7.    
  8.     interface SentenceParser {
  9.         public boolean parse(String [] tokens, GPSPosition position);
  10.     }
  11.    
  12.     // utils
  13.     static float Latitude2Decimal(String lat, String NS) {
  14.         float med = Float.parseFloat(lat.substring(2))/60.0f;
  15.         med +=  Float.parseFloat(lat.substring(0, 2));
  16.         if(NS.startsWith("S")) {
  17.             med = -med;
  18.         }
  19.         return med;
  20.     }
  21.  
  22.     static float Longitude2Decimal(String lon, String WE) {
  23.         float med = Float.parseFloat(lon.substring(3))/60.0f;
  24.         med +=  Float.parseFloat(lon.substring(0, 3));
  25.         if(WE.startsWith("W")) {
  26.             med = -med;
  27.         }
  28.         return med;
  29.     }
  30.  
  31.     // parsers
  32.     class GPGGA implements SentenceParser {
  33.         public boolean parse(String [] tokens, GPSPosition position) {
  34.             position.time = Float.parseFloat(tokens[1]);
  35.             position.lat = Latitude2Decimal(tokens[2], tokens[3]);
  36.             position.lon = Longitude2Decimal(tokens[4], tokens[5]);
  37.             position.quality = Integer.parseInt(tokens[6]);
  38.             position.altitude = Float.parseFloat(tokens[9]);
  39.             return true;
  40.         }
  41.     }
  42.    
  43.     class GPGGL implements SentenceParser {
  44.         public boolean parse(String [] tokens, GPSPosition position) {
  45.             position.lat = Latitude2Decimal(tokens[1], tokens[2]);
  46.             position.lon = Longitude2Decimal(tokens[3], tokens[4]);
  47.             position.time = Float.parseFloat(tokens[5]);
  48.             return true;
  49.         }
  50.     }
  51.    
  52.     class GPRMC implements SentenceParser {
  53.         public boolean parse(String [] tokens, GPSPosition position) {
  54.             position.time = Float.parseFloat(tokens[1]);
  55.             position.lat = Latitude2Decimal(tokens[3], tokens[4]);
  56.             position.lon = Longitude2Decimal(tokens[5], tokens[6]);
  57.             position.velocity = Float.parseFloat(tokens[7]);
  58.             position.dir = Float.parseFloat(tokens[8]);
  59.             return true;
  60.         }
  61.     }
  62.    
  63.     class GPVTG implements SentenceParser {
  64.         public boolean parse(String [] tokens, GPSPosition position) {
  65.             position.dir = Float.parseFloat(tokens[3]);
  66.             return true;
  67.         }
  68.     }
  69.    
  70.     class GPRMZ implements SentenceParser {
  71.         public boolean parse(String [] tokens, GPSPosition position) {
  72.             position.altitude = Float.parseFloat(tokens[1]);
  73.             return true;
  74.         }
  75.     }
  76.    
  77.     public class GPSPosition {
  78.         public float time = 0.0f;
  79.         public float lat = 0.0f;
  80.         public float lon = 0.0f;
  81.         public boolean fixed = false;
  82.         public int quality = 0;
  83.         public float dir = 0.0f;
  84.         public float altitude = 0.0f;
  85.         public float velocity = 0.0f;
  86.        
  87.         public void updatefix() {
  88.             fixed = quality > 0;
  89.         }
  90.        
  91.         public String toString() {
  92.             return String.format("POSITION: lat: %f, lon: %f, time: %f, Q: %d, dir: %f, alt: %f, vel: %f", lat, lon, time, quality, dir, altitude, velocity);
  93.         }
  94.     }
  95.    
  96.     GPSPosition position = new GPSPosition();
  97.    
  98.     private static final Map<String, SentenceParser> sentenceParsers = new HashMap<String, SentenceParser>();
  99.    
  100.     public Nmea() {
  101.         sentenceParsers.put("GPGGA", new GPGGA());
  102.         sentenceParsers.put("GPGGL", new GPGGL());
  103.         sentenceParsers.put("GPRMC", new GPRMC());
  104.         sentenceParsers.put("GPRMZ", new GPRMZ());
  105.         sentenceParsers.put("GPVTG", new GPVTG());
  106.     }
  107.    
  108.     public GPSPosition parse(String line) {
  109.        
  110.         if(line.startsWith("$")) {
  111.             String nmea = line.substring(1);
  112.             String[] tokens = nmea.split(",");
  113.             String type = tokens[0];
  114.            
  115.             if(sentenceParsers.containsKey(type)) {
  116.                 sentenceParsers.get(type).parse(tokens, position);
  117.             }
  118.             position.updatefix();
  119.         }
  120.        
  121.         return position;
  122.     }
  123. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement