gps.py

gpsd, a popular GPS daemon.

	      if data == '?':
		  self.cycle = -1
		  self.device = None
	      elif len(data.split()) == 2:
		  (self.cycle, self.mincycle) = map(float, data)
              else:
		  self.mincycle = self.cycle = float(data)
	    elif cmd in ('D', 'd'):
	      self.utc = data
	      self.gps_time = isotime(self.utc)
	      self.valid |= TIME_SET
	    elif cmd in ('E', 'e'):
	      parts = data.split()
	      (self.epe, self.fix.eph, self.fix.epv) = map(float, parts)
	      self.valid |= HERR_SET | VERR_SET | PERR_SET
	    elif cmd in ('F', 'f'):
		if data == '?':
		    self.device = None
		else:
		    self.device = data
	    elif cmd in ('I', 'i'):
	      if data == '?':
		  self.cycle = -1
		  self.gps_id = None
	      else:
		  self.gps_id = data
	    elif cmd in ('K', 'K'):
		if data == '?':
		    self.devices = None
		else:
		    self.devices = data[1:].split()
	    elif cmd in ('M', 'm'):
	      self.fix.mode = int(data)
	      self.valid |= MODE_SET
	    elif cmd in ('N', 'n'):
	      if data == '?':
		  self.driver_mode = -1
		  self.device = None
	      else:
		  self.driver_mode = int(data)
	    elif cmd in ('O', 'o'):
		fields = data.split()
		if fields[0] == '?':
		    self.fix.mode = MODE_NO_FIX
		else:
		    self.timings.sentence_tag = fields[0]
		    def default(i, cnv=float):
			if fields[i] == '?':
			    return NaN
			else:
			    return cnv(fields[i])
		    self.fix.time = default(1)
		    self.fix.ept = default(2)
		    self.fix.latitude = default(3)
		    self.fix.longitude = default(4)
		    self.fix.altitude = default(5)
		    self.fix.eph = default(6)
		    self.fix.epv = default(7)
		    self.fix.track = default(8)
		    self.fix.speed = default(9)
		    self.fix.climb = default(10)
		    self.fix.epd = default(11)
		    self.fix.eps = default(12)
		    self.fix.epc = default(13)
                    if len(fields) > 14:
                        self.fix.mode = default(14, int)
                    else:
                        if not isnan(self.fix.altitude):
                            self.fix.mode = MODE_2D
                        else:
                            self.fix.mode = MODE_3D
		    self.valid |= TIME_SET|TIMERR_SET|LATLON_SET|MODE_SET
		    if self.fix.mode == MODE_3D:
			self.valid |= ALTITUDE_SET | CLIMB_SET
		    if self.fix.eph:
			self.valid |= HERR_SET
		    if self.fix.epv:
			self.valid |= VERR_SET
		    if not isnan(self.fix.track):
			self.valid |= TRACK_SET | SPEED_SET
		    if self.fix.eps:
			self.valid |= SPEEDERR_SET
		    if self.fix.epc:
			self.valid |= CLIMBERR_SET
	    elif cmd in ('P', 'p'):
		(self.fix.latitude, self.fix.longitude) = map(float, data.split())
		self.valid |= LATLON_SET
	    elif cmd in ('Q', 'q'):
		parts = data.split()
		self.satellites_used = int(parts[0])
		(self.pdop, self.hdop, self.vdop, self.tdop, self.gdop) = map(float, parts[1:])
		self.valid |= HDOP_SET | VDOP_SET | PDOP_SET | TDOP_SET | GDOP_SET
	    elif cmd in ('S', 's'):
		self.status = int(data)
		self.valid |= STATUS_SET
	    elif cmd in ('T', 't'):
		self.fix.track = float(data)
		self.valid |= TRACK_SET
	    elif cmd in ('U', 'u'):
		self.fix.climb = float(data)
		self.valid |= CLIMB_SET
	    elif cmd in ('V', 'v'):
		self.fix.speed = float(data)
		self.valid |= SPEED_SET
	    elif cmd in ('X', 'x'):
		if data == '?':
		    self.online = -1
		    self.device = None
		else:
		    self.online = float(data)
		    self.valid |= ONLINE_SET
	    elif cmd in ('Y', 'y'):
                satellites = data.split(":")
                prefix = satellites.pop(0).split()
                self.timings.sentence_tag = prefix.pop(0)
                self.timings.sentence_time = prefix.pop(0)
                if self.timings.sentence_time != "?":
                    self.timings.sentence_time = float(self.timings.sentence_time)
                d1 = int(prefix.pop(0))
                newsats = []
	        for i in range(d1):
                    newsats.append(gps.satellite(*map(int, satellites[i].split())))
                self.satellites = newsats
                self.valid |= SATELLITE_SET
	    elif cmd in ('Z', 'z'):
		self.profiling = (data[0] == '1')
	    elif cmd == '$':
		self.timings.collect(*data.split())
	if self.raw_hook:
	    self.raw_hook(buf);

    def waiting(self):
        "Return True if data is ready for the client."
        if self.sockfile._rbuf:	# Ugh...relies on socket library internals.
            return True
        else:
            (winput,woutput,wexceptions) = select.select((self.sock,), (),(), 0)
            return winput != []

    def poll(self):
	"Wait for and read data being streamed from gpsd."
	self.response = self.sockfile.readline()
	if not self.response:
	    return -1
	if self.verbose:
	    sys.stderr.write("GPS DATA %s\n" % repr(self.response))
	self.timings.c_recv_time = time.time()
	self.__unpack(self.response)
	if self.profiling:
	    if self.timings.sentence_time != '?':
		basetime = self.timings.sentence_time
	    else:
		basetime = self.timings.d_xmit_time
	    self.timings.c_decode_time = time.time() - basetime
	    self.timings.c_recv_time -= basetime
	return 0

    def send(self, commands):
        "Ship commands to the daemon."
        if not commands.endswith("\n"):
            commands += "\n"
 	self.sock.send(commands)

    def query(self, commands):
	"Send a command, get back a response."
        self.send(commands)
	return self.poll()

# some multipliers for interpreting GPS output
METERS_TO_FEET	= 3.2808399
METERS_TO_MILES	= 0.00062137119
KNOTS_TO_MPH	= 1.1507794

# EarthDistance code swiped from Kismet and corrected
# (As yet, this stuff is not in the libgps C library.)

def Deg2Rad(x):
    "Degrees to radians."
    return x * (math.pi/180)

def CalcRad(lat):
    "Radius of curvature in meters at specified latitude."
    a = 6378.137
    e2 = 0.081082 * 0.081082
    # the radius of curvature of an ellipsoidal Earth in the plane of a
    # meridian of latitude is given by
    #
    # R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2)
    #
    # where a is the equatorial radius,
    # b is the polar radius, and
    # e is the eccentricity of the ellipsoid = sqrt(1 - b^2/a^2)
    #
    # a = 6378 km (3963 mi) Equatorial radius (surface to center distance)
    # b = 6356.752 km (3950 mi) Polar radius (surface to center distance)
    # e = 0.081082 Eccentricity
    sc = math.sin(Deg2Rad(lat))
    x = a * (1.0 - e2)
    z = 1.0 - e2 * sc * sc
    y = pow(z, 1.5)
    r = x / y

    r = r * 1000.0	# Convert to meters
    return r

def EarthDistance((lat1, lon1), (lat2, lon2)):
    "Distance in meters between two points specified in degrees."
    x1 = CalcRad(lat1) * math.cos(Deg2Rad(lon1)) * math.sin(Deg2Rad(90-lat1))
    x2 = CalcRad(lat2) * math.cos(Deg2Rad(lon2)) * math.sin(Deg2Rad(90-lat2))
    y1 = CalcRad(lat1) * math.sin(Deg2Rad(lon1)) * math.sin(Deg2Rad(90-lat1))
    y2 = CalcRad(lat2) * math.sin(Deg2Rad(lon2)) * math.sin(Deg2Rad(90-lat2))
    z1 = CalcRad(lat1) * math.cos(Deg2Rad(90-lat1))
    z2 = CalcRad(lat2) * math.cos(Deg2Rad(90-lat2))
    a = (x1*x2 + y1*y2 + z1*z2)/pow(CalcRad((lat1+lat2)/2), 2)
    # a should be in [1, -1] but can sometimes fall outside it by
    # a very small amount due to rounding errors in the preceding
    # calculations (this is prone to happen when the argument points
    # are very close together).  Thus we constrain it here.
    if abs(a) > 1: a = 1
    elif a < -1: a = -1
    return CalcRad((lat1+lat2) / 2) * math.acos(a)

def MeterOffset((lat1, lon1), (lat2, lon2)):
    "Return offset in meters of second arg from first."
    dx = EarthDistance((lat1, lon1), (lat1, lon2))
    dy = EarthDistance((lat1, lon1), (lat2, lon1))
    if lat1 < lat2: dy *= -1
    if lon1 < lon2: dx *= -1
    return (dx, dy)

def isotime(s):
    "Convert timestamps in ISO8661 format to and from Unix time."
    if type(s) == type(1):
	return time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
    elif type(s) == type(1.0):
	date = int(s)
	msec = s - date
	date = time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
	return date + "." + `msec`[2:]
    elif type(s) == type(""):
	if s[-1] == "Z":
	    s = s[:-1]
	if "." in s:
	    (date, msec) = s.split(".")
	else:
	    date = s
	    msec = "0"
	# Note: no leap-second correction! 
	return calendar.timegm(time.strptime(date, "%Y-%m-%dT%H:%M:%S")) + float("0." + msec)
    else:
	raise TypeError

if __name__ == '__main__':
    import readline
    print "This is the exerciser for the Python gps interface."
    session = gps()
    session.set_raw_hook(lambda s: sys.stdout.write(s + "\n"))
    try:
	while True:
	    commands = raw_input("> ")
	    session.query(commands+"\n")
	    print session
    except EOFError:
        print "Goodbye!"
    del session

# gps.py ends here