virtualgps.java

一个基于PlaceLab的室内和室外的智能导航系统

package org.placelab.demo.virtualgps;

import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import gnu.io.UnsupportedCommOperationException;

import java.io.IOException;
import java.io.PrintStream;
import java.util.Calendar;
import java.util.TimeZone;

import javax.microedition.io.Connector;
import javax.microedition.io.StreamConnection;

import org.placelab.client.PlacelabException;
import org.placelab.client.PlacelabFusion;
import org.placelab.client.tracker.BeaconAndGPSParticleFilterTracker;
import org.placelab.client.tracker.BeaconTracker;
import org.placelab.client.tracker.Estimate;
import org.placelab.client.tracker.Tracker;
import org.placelab.core.BeaconMeasurement;
import org.placelab.core.Measurement;
import org.placelab.core.PlacelabProperties;
import org.placelab.core.TwoDCoordinate;
import org.placelab.mapper.CompoundMapper;
import org.placelab.mapper.Mapper;
import org.placelab.spotter.Spotter;
import org.placelab.spotter.SpotterException;

/**
 * Takes the Place Lab location and outputs it as NMEA formatted sentences
 * over a serial port allowing you to use any external application that
 * understands NMEA (e.g. Microsoft MapPoint or Delorme Street Atlas).
 * 
 * The serial port should be specified with the system
 * properties placelab.virutal_gps_device, and placelab.virtual_gps_baud
 * 
 */

public class VirtualGPS extends PlacelabFusion {
	protected PrintStream out;
	
	public VirtualGPS(Mapper _mapper, BeaconTracker _tracker) {
	    super(_mapper, _tracker);
	    
	    // setup input and output streams for the virtual GPS port to which we will
	    // write the NMEA sentences
		String osName = System.getProperty("os.name").toLowerCase();
		String gpsDevice = PlacelabProperties.get("placelab.virtual_gps_device");
		String baud = PlacelabProperties.get("placelab.virtual_gps_baud");
		if (baud.length()==0) {
			baud = "4800";
		}
		System.out.println("VirtualGPS using "+gpsDevice+" at "+baud+" baud.");
		if ((gpsDevice.length() == 0) || (gpsDevice.toLowerCase().trim().equals("stdout"))) {
			out = System.out;
		} else {
			if (osName.equals("windows ce")) {
			    // use the J9 serial port
				try {
					StreamConnection serialPort = (StreamConnection)Connector.open("comm:"+gpsDevice+";baudrate="+baud);
					out = new PrintStream(serialPort.openOutputStream());
				} catch (IOException e) {
					System.err.println("ERR: unable to open port: " + e);
					return;
				}
			} else {
			    // default to the RX/TX serial port
			    SerialPort sp;
			    CommPortIdentifier cpi;

			    try {
			      cpi = CommPortIdentifier.getPortIdentifier(gpsDevice);
			      sp = (SerialPort) cpi.open("placelabvirtualgps", 1000);
			      sp.setSerialPortParams(Integer.parseInt(baud),
			                             SerialPort.DATABITS_8,
			                             SerialPort.STOPBITS_1,
			                             SerialPort.PARITY_NONE);
			      out = new PrintStream(sp.getOutputStream());
			    } catch (UnsupportedCommOperationException e) {
			    	System.err.println("ERR: UnsupportedCommOperationException: " + e);
					return;
				} catch (NoSuchPortException e) {
					System.err.println("ERR: NoSuchPortException: " + e);
					return;
				} catch (PortInUseException e) {
					System.err.println("ERR: PortInUseException: " + e);
					return;
				} catch (IOException e) {
					System.err.println("ERR: IOException: " + e);
					return;
				}
			}
		}
	}
	
	public void estimateUpdated (Tracker t, Estimate e, Measurement m) {
	    super.estimateUpdated(t, e, m);
	    
		Calendar cal = Calendar.getInstance(TimeZone.getTimeZone("UTC"));
		String hhmmss = NMEAFactory.twoDigitFormat(cal.get(Calendar.HOUR_OF_DAY)) +
						NMEAFactory.twoDigitFormat(cal.get(Calendar.MINUTE)) +
						NMEAFactory.twoDigitFormat(cal.get(Calendar.SECOND)) +
						"." + cal.get(Calendar.MILLISECOND);
		String ddmmyy = NMEAFactory.twoDigitFormat(cal.get(Calendar.DATE)) +
						NMEAFactory.twoDigitFormat(cal.get(Calendar.MONTH)+1) +
						NMEAFactory.twoDigitFormat(cal.get(Calendar.YEAR)-2000);
		
		double lat = ((TwoDCoordinate) e.getCoord()).getLatitude();
		double lon = ((TwoDCoordinate) e.getCoord()).getLongitude();
		String latDir = (lat < 0) ? "S" : "N";
		String lonDir = (lon < 0) ? "W" : "E";
		String latNMEA = NMEAFactory.toLatNMEA(lat);
		String lonNMEA = NMEAFactory.toLonNMEA(lon);
		if (Double.isNaN(lat) || Double.isNaN(lon)) {
			out.println(NMEAFactory.GGA(hhmmss, "", "", "", "", 0, 0, "", "", "M", "", "M"));
			out.println(NMEAFactory.GLL("", "", "", "", hhmmss, "V"));
			out.println(NMEAFactory.RMC(hhmmss, "V", "", "", "", "", "", "", ddmmyy, "", ""));
			return;
		}

		// say the number of satellites is 4 (minimum for a valid fix unless we are using
		// beacons in which case we set the number of satellites to be the number
		// of beacons in the most recent measurement
		int numberOfSatellites = 4;
		if (m instanceof BeaconMeasurement) {
		    BeaconMeasurement bm = (BeaconMeasurement)m;
		    numberOfSatellites = bm.numberOfReadings();
		}
		out.println(NMEAFactory.GGA(
			hhmmss,
			latNMEA,
			latDir,
			lonNMEA,
			lonDir,
			1, // valid GPS FIX
			numberOfSatellites,
			"20.0", // lie about HDOP right now until a better plan comes along
			"0", // 0m altitude
			"M",
			"0", // 0m geoid seperation
			"M"
			));
		out.println(NMEAFactory.GLL(
				latNMEA,
				latDir,
				lonNMEA,
				lonDir,
				hhmmss,
				"A"
			));
		out.println(NMEAFactory.RMC(
				hhmmss,
				"A", //valid
				latNMEA,
				latDir,
				lonNMEA,
				lonDir,
				"000.0", // 0 knots (not moving) TODO: we should put the real velocity here
				"000.0", // course 0 degrees TODO: we should put the real course here
				ddmmyy,
				"", // variation
				"" // direction
			));
	}
	
	public void spotterExceptionThrown(Spotter s,SpotterException ex) {
		System.err.println("Spotter "+s+" threw exception "+ex);
		ex.printStackTrace();
	}

	public static void main (String[] args) {
    	Mapper mapper = CompoundMapper.createDefaultMapper(true,true);
    	VirtualGPS virtualGPS = new VirtualGPS(mapper, new BeaconAndGPSParticleFilterTracker(mapper));
	    try {
	        virtualGPS.start();
	    } catch (PlacelabException pe) {
	        System.err.println(pe);
	    }
	}
}