Devuan fork of gpsd
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

#### 324 lines 11 KiB Raw Permalink Blame History

 ``````# This file is Copyright (c) 2010 by the GPSD project # SPDX-License-Identifier: BSD-2-clause """ A GPS simulator. This is proof-of-concept code, not production ready; some functions are stubs. """ import math import random import sys import time # pylint wants local modules last try: import gps import gpslib except ImportError as e: sys.stderr.write( "gpssim.py: can't load Python gps libraries -- check PYTHONPATH.\n") sys.stderr.write("%s\n" % e) sys.exit(1) # First, the mathematics. We simulate a moving viewpoint on the Earth # and a satellite with specified orbital elements in the sky. class ksv(object): "Kinematic state vector." def __init__(self, time=0, lat=0, lon=0, alt=0, course=0, speed=0, climb=0, h_acc=0, v_acc=0): self.time = time # Seconds from epoch self.lat = lat # Decimal degrees self.lon = lon # Decimal degrees self.alt = alt # Meters self.course = course # Degrees from true North self.speed = speed # Meters per second self.climb = climb # Meters per second self.h_acc = h_acc # Meters per second per second self.v_acc = v_acc # Meters per second per second def next(self, quantum=1): "State after quantum." self.time += quantum avspeed = (2 * self.speed + self.h_acc * quantum) / 2 avclimb = (2 * self.climb + self.v_acc * quantum) / 2 self.alt += avclimb * quantum self.speed += self.h_acc * quantum self.climb += self.v_acc * quantum distance = avspeed * quantum # Formula from # Initial point cannot be a pole, but GPS doesn't work at high. # latitudes anyway so it would be OK to fail there. # Seems to assume a spherical Earth, which means it's going # to have a slight inaccuracy rising towards the poles. # The if/then avoids 0/0 indeterminacies on E-W courses. tc = gps.Deg2Rad(self.course) lat = gps.Deg2Rad(self.lat) lon = gps.Deg2Rad(self.lon) lat += distance * math.cos(tc) dphi = math.log(math.tan(lat / 2 + math.pi / 4) / math.tan(self.lat / 2 + math.pi / 4)) if abs(lat - self.lat) < math.sqrt(1e-15): q = math.cos(self.lat) else: q = (lat - self.lat) / dphi dlon = -distance * math.sin(tc) / q self.lon = gps.Rad2Deg(math.mod(lon + dlon + math.pi, 2 * math.pi) - math.pi) self.lat = gps.Rad2Deg(lat) # Satellite orbital elements are available at: # # Orbital theory at: # class satellite(object): "Orbital elements of one satellite. PRESENTLY A STUB" def __init__(self, prn): self.prn = prn def position(self, time): "Return right ascension and declination of satellite," return # Next, the command interpreter. This is an object that takes an # input source in the track description language, interprets it into # sammples that might be reported by a GPS, and calls a reporting # class to generate output. class gpssimException(BaseException): def __init__(self, message, filename, lineno): BaseException.__init__(self) self.message = message self.filename = filename self.lineno = lineno def __str__(self): return '"%s", %d:' % (self.filename, self.lineno) class gpssim(object): "Simulate a moving sensor, with skyview." active_PRNs = list(range(1, 24 + 1)) + [134, ] def __init__(self, outfmt): self.ksv = ksv() self.ephemeris = {} # This sets up satellites at random. Not really what we want. for prn in gpssim.active_PRNs: for (prn, _satellite) in list(self.ephemeris.items()): self.skyview[prn] = (random.randint(-60, +61), random.randint(0, 359)) self.have_ephemeris = False self.channels = {} self.outfmt = outfmt self.status = gps.STATUS_NO_FIX self.mode = gps.MODE_NO_FIX self.validity = "V" self.satellites_used = 0 self.filename = None self.lineno = 0 def parse_tdl(self, line): "Interpret one TDL directive." line = line.strip() if "#" in line: line = line[:line.find("#")] if line == '': return fields = line.split() command = fields[0] if command == "time": self.ksv.time = gps.isotime(fields[1]) elif command == "location": (self.lat, self.lon, self.alt) = list(map(float, fields[1:])) elif command == "course": self.ksv.time = float(fields[1]) elif command == "speed": self.ksv.speed = float(fields[1]) elif command == "climb": self.ksv.climb = float(fields[1]) elif command == "acceleration": (self.ksv.h_acc, self.ksv.h_acc) = list(map(float, fields[1:])) elif command == "snr": self.channels[int(fields[1])] = float(fields[2]) elif command == "go": self.go(int(fields[1])) elif command == "status": try: code = fields[1] self.status = {"no_fix": 0, "fix": 1, "dgps_fix": 2}[ code.lower()] except KeyError: raise gpssimException("invalid status code '%s'" % code, self.filename, self.lineno) elif command == "mode": try: code = fields[1] self.status = {"no_fix": 1, "2d": 2, "3d": 3}[code.lower()] except KeyError: raise gpssimException("invalid mode code '%s'" % code, self.filename, self.lineno) elif command == "satellites": self.satellites_used = int(fields[1]) elif command == "validity": self.validity = fields[1] else: raise gpssimException("unknown command '%s'" % fields[1], self.filename, self.lineno) # FIX-ME: add syntax for ephemeris elements self.lineno += 1 def filter(self, inp, outp): "Make this a filter for file-like objects." self.filename = input.name self.lineno = 1 self.output = outp for line in inp: self.execute(line) def go(self, seconds): "Run the simulation for a specified number of seconds." for i in range(seconds): next(self.ksv) if self.have_ephemeris: self.skyview = {} for (prn, satellite) in list(self.ephemeris.items()): self.skyview[prn] = satellite.position(i) self.output.write(self.gpstype.report(self)) # Reporting classes need to have a report() method returning a string # that is a sentence (or possibly several sentences) reporting the # state of the simulation. Presently we have only one, for NMEA # devices, but the point of the architecture is so that we could simulate # others - SirF, Evermore, whatever. MPS_TO_KNOTS = 1.9438445 # Meters per second to knots class NMEA(object): "NMEA output generator." def __init__(self): self.sentences = ("RMC", "GGA",) self.counter = 0 def add_checksum(self, mstr): "Concatenate NMEA checksum and trailer to a string" csum = 0 for (i, c) in enumerate(mstr): if i == 0 and c == "\$": continue csum ^= ord(c) mstr += "*%02X\r\n" % csum return mstr def degtodm(self, angle): "Decimal degrees to GPS-style, degrees first followed by minutes." (fraction, _integer) = math.modf(angle) return math.floor(angle) * 100 + fraction * 60 def GGA(self, sim): "Emit GGA sentence describing the simulation state." tm = time.gmtime(sim.ksv.time) gga = "\$GPGGA,%02d%02d%02d,%09.4f,%c,%010.4f,%c,%d,%02d," % ( tm.tm_hour, tm.tm_min, tm.tm_sec, self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0], self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0], sim.status, sim.satellites_used) # HDOP calculation goes here gga += "," if sim.mode == gps.MODE_3D: gga += "%.1f,M" % self.ksv.lat gga += "," gga += "%.3f,M," % gpslib.wg84_separation(sim.ksv.lat, sim.ksv.lon) # Magnetic variation goes here # gga += "%3.2f,M," % mag_var gga += ",," # Time in seconds since last DGPS update goes here gga += "," # DGPS station ID goes here return self.add_checksum(gga) def GLL(self, sim): "Emit GLL sentence describing the simulation state." tm = time.gmtime(sim.ksv.time) gll = "\$GPLL,%09.4f,%c,%010.4f,%c,%02d%02d%02d,%s," % ( self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0], self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0], tm.tm_hour, tm.tm_min, tm.tm_sec, sim.validity, ) # FAA mode indicator could go after these fields. return self.add_checksum(gll) def RMC(self, sim): "Emit RMC sentence describing the simulation state." tm = time.gmtime(sim.ksv.time) rmc = \ "GPRMC,%02d%02d%02d,%s,%09.4f,%c,%010.4f,%c,%.1f,%02d%02d%02d," % ( tm.tm_hour, tm.tm_min, tm.tm_sec, sim.validity, self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0], self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0], sim.course * MPS_TO_KNOTS, tm.tm_mday, tm.tm_mon, tm.tm_year % 100) # Magnetic variation goes here # rmc += "%3.2f,M," % mag_var rmc += ",," # FAA mode goes here return self.add_checksum(rmc) def ZDA(self, sim): "Emit ZDA sentence describing the simulation state." tm = time.gmtime(sim.ksv.time) zda = "\$GPZDA,%02d%2d%02d,%02d,%02d,%04d" % ( tm.tm_hour, tm.tm_min, tm.tm_sec, tm.tm_mday, tm.tm_mon, tm.tm_year, ) # Local zone description, 00 to +- 13 hours, goes here zda += "," # Local zone minutes description goes here zda += "," return self.add_checksum(zda) def report(self, sim): "Report the simulation state." out = "" for sentence in self.sentences: if isinstance(sentence, tuple): (interval, sentence) = sentence if self.counter % interval: continue out += getattr(self, sentence)(*[sim]) self.counter += 1 return out # The very simple main line. if __name__ == "__main__": try: gpssim(NMEA).filter(sys.stdin, sys.stdout) except gpssimException as e: sys.stderr.write(repr(e) + "\n") # gpssim.py ends here.``````