daily pastebin goal
59%
SHARE
TWEET

NMEA http://darylrobotproject.wordpress.com/

a guest Oct 1st, 2012 32 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top