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

  1. # This file is Copyright (c) 2010 by the GPSD project
  2. # SPDX-License-Identifier: BSD-2-clause
  3. """
  4. A GPS simulator.
  5. This is proof-of-concept code, not production ready; some functions are stubs.
  6. """
  7. import math
  8. import random
  9. import sys
  10. import time
  11. # pylint wants local modules last
  12. try:
  13. import gps
  14. import gpslib
  15. except ImportError as e:
  16. sys.stderr.write(
  17. "gpssim.py: can't load Python gps libraries -- check PYTHONPATH.\n")
  18. sys.stderr.write("%s\n" % e)
  19. sys.exit(1)
  20. # First, the mathematics. We simulate a moving viewpoint on the Earth
  21. # and a satellite with specified orbital elements in the sky.
  22. class ksv(object):
  23. "Kinematic state vector."
  24. def __init__(self, time=0, lat=0, lon=0, alt=0, course=0,
  25. speed=0, climb=0, h_acc=0, v_acc=0):
  26. self.time = time # Seconds from epoch
  27. self.lat = lat # Decimal degrees
  28. self.lon = lon # Decimal degrees
  29. self.alt = alt # Meters
  30. self.course = course # Degrees from true North
  31. self.speed = speed # Meters per second
  32. self.climb = climb # Meters per second
  33. self.h_acc = h_acc # Meters per second per second
  34. self.v_acc = v_acc # Meters per second per second
  35. def next(self, quantum=1):
  36. "State after quantum."
  37. self.time += quantum
  38. avspeed = (2 * self.speed + self.h_acc * quantum) / 2
  39. avclimb = (2 * self.climb + self.v_acc * quantum) / 2
  40. self.alt += avclimb * quantum
  41. self.speed += self.h_acc * quantum
  42. self.climb += self.v_acc * quantum
  43. distance = avspeed * quantum
  44. # Formula from <http://williams.best.vwh.net/avform.htm#Rhumb>
  45. # Initial point cannot be a pole, but GPS doesn't work at high.
  46. # latitudes anyway so it would be OK to fail there.
  47. # Seems to assume a spherical Earth, which means it's going
  48. # to have a slight inaccuracy rising towards the poles.
  49. # The if/then avoids 0/0 indeterminacies on E-W courses.
  50. tc = gps.Deg2Rad(self.course)
  51. lat = gps.Deg2Rad(self.lat)
  52. lon = gps.Deg2Rad(self.lon)
  53. lat += distance * math.cos(tc)
  54. dphi = math.log(math.tan(lat / 2 + math.pi / 4) /
  55. math.tan(self.lat / 2 + math.pi / 4))
  56. if abs(lat - self.lat) < math.sqrt(1e-15):
  57. q = math.cos(self.lat)
  58. else:
  59. q = (lat - self.lat) / dphi
  60. dlon = -distance * math.sin(tc) / q
  61. self.lon = gps.Rad2Deg(math.mod(lon + dlon + math.pi, 2 * math.pi) -
  62. math.pi)
  63. self.lat = gps.Rad2Deg(lat)
  64. # Satellite orbital elements are available at:
  65. # <http://www.ngs.noaa.gov/orbits/>
  66. # Orbital theory at:
  67. # <http://www.wolffdata.se/gps/gpshtml/anomalies.html>
  68. class satellite(object):
  69. "Orbital elements of one satellite. PRESENTLY A STUB"
  70. def __init__(self, prn):
  71. self.prn = prn
  72. def position(self, time):
  73. "Return right ascension and declination of satellite,"
  74. return
  75. # Next, the command interpreter. This is an object that takes an
  76. # input source in the track description language, interprets it into
  77. # sammples that might be reported by a GPS, and calls a reporting
  78. # class to generate output.
  79. class gpssimException(BaseException):
  80. def __init__(self, message, filename, lineno):
  81. BaseException.__init__(self)
  82. self.message = message
  83. self.filename = filename
  84. self.lineno = lineno
  85. def __str__(self):
  86. return '"%s", %d:' % (self.filename, self.lineno)
  87. class gpssim(object):
  88. "Simulate a moving sensor, with skyview."
  89. active_PRNs = list(range(1, 24 + 1)) + [134, ]
  90. def __init__(self, outfmt):
  91. self.ksv = ksv()
  92. self.ephemeris = {}
  93. # This sets up satellites at random. Not really what we want.
  94. for prn in gpssim.active_PRNs:
  95. for (prn, _satellite) in list(self.ephemeris.items()):
  96. self.skyview[prn] = (random.randint(-60, +61),
  97. random.randint(0, 359))
  98. self.have_ephemeris = False
  99. self.channels = {}
  100. self.outfmt = outfmt
  101. self.status = gps.STATUS_NO_FIX
  102. self.mode = gps.MODE_NO_FIX
  103. self.validity = "V"
  104. self.satellites_used = 0
  105. self.filename = None
  106. self.lineno = 0
  107. def parse_tdl(self, line):
  108. "Interpret one TDL directive."
  109. line = line.strip()
  110. if "#" in line:
  111. line = line[:line.find("#")]
  112. if line == '':
  113. return
  114. fields = line.split()
  115. command = fields[0]
  116. if command == "time":
  117. self.ksv.time = gps.isotime(fields[1])
  118. elif command == "location":
  119. (self.lat, self.lon, self.alt) = list(map(float, fields[1:]))
  120. elif command == "course":
  121. self.ksv.time = float(fields[1])
  122. elif command == "speed":
  123. self.ksv.speed = float(fields[1])
  124. elif command == "climb":
  125. self.ksv.climb = float(fields[1])
  126. elif command == "acceleration":
  127. (self.ksv.h_acc, self.ksv.h_acc) = list(map(float, fields[1:]))
  128. elif command == "snr":
  129. self.channels[int(fields[1])] = float(fields[2])
  130. elif command == "go":
  131. self.go(int(fields[1]))
  132. elif command == "status":
  133. try:
  134. code = fields[1]
  135. self.status = {"no_fix": 0, "fix": 1, "dgps_fix": 2}[
  136. code.lower()]
  137. except KeyError:
  138. raise gpssimException("invalid status code '%s'" % code,
  139. self.filename, self.lineno)
  140. elif command == "mode":
  141. try:
  142. code = fields[1]
  143. self.status = {"no_fix": 1, "2d": 2, "3d": 3}[code.lower()]
  144. except KeyError:
  145. raise gpssimException("invalid mode code '%s'" % code,
  146. self.filename, self.lineno)
  147. elif command == "satellites":
  148. self.satellites_used = int(fields[1])
  149. elif command == "validity":
  150. self.validity = fields[1]
  151. else:
  152. raise gpssimException("unknown command '%s'" % fields[1],
  153. self.filename, self.lineno)
  154. # FIX-ME: add syntax for ephemeris elements
  155. self.lineno += 1
  156. def filter(self, inp, outp):
  157. "Make this a filter for file-like objects."
  158. self.filename = input.name
  159. self.lineno = 1
  160. self.output = outp
  161. for line in inp:
  162. self.execute(line)
  163. def go(self, seconds):
  164. "Run the simulation for a specified number of seconds."
  165. for i in range(seconds):
  166. next(self.ksv)
  167. if self.have_ephemeris:
  168. self.skyview = {}
  169. for (prn, satellite) in list(self.ephemeris.items()):
  170. self.skyview[prn] = satellite.position(i)
  171. self.output.write(self.gpstype.report(self))
  172. # Reporting classes need to have a report() method returning a string
  173. # that is a sentence (or possibly several sentences) reporting the
  174. # state of the simulation. Presently we have only one, for NMEA
  175. # devices, but the point of the architecture is so that we could simulate
  176. # others - SirF, Evermore, whatever.
  177. MPS_TO_KNOTS = 1.9438445 # Meters per second to knots
  178. class NMEA(object):
  179. "NMEA output generator."
  180. def __init__(self):
  181. self.sentences = ("RMC", "GGA",)
  182. self.counter = 0
  183. def add_checksum(self, mstr):
  184. "Concatenate NMEA checksum and trailer to a string"
  185. csum = 0
  186. for (i, c) in enumerate(mstr):
  187. if i == 0 and c == "$":
  188. continue
  189. csum ^= ord(c)
  190. mstr += "*%02X\r\n" % csum
  191. return mstr
  192. def degtodm(self, angle):
  193. "Decimal degrees to GPS-style, degrees first followed by minutes."
  194. (fraction, _integer) = math.modf(angle)
  195. return math.floor(angle) * 100 + fraction * 60
  196. def GGA(self, sim):
  197. "Emit GGA sentence describing the simulation state."
  198. tm = time.gmtime(sim.ksv.time)
  199. gga = "$GPGGA,%02d%02d%02d,%09.4f,%c,%010.4f,%c,%d,%02d," % (
  200. tm.tm_hour,
  201. tm.tm_min,
  202. tm.tm_sec,
  203. self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0],
  204. self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0],
  205. sim.status,
  206. sim.satellites_used)
  207. # HDOP calculation goes here
  208. gga += ","
  209. if sim.mode == gps.MODE_3D:
  210. gga += "%.1f,M" % self.ksv.lat
  211. gga += ","
  212. gga += "%.3f,M," % gpslib.wg84_separation(sim.ksv.lat, sim.ksv.lon)
  213. # Magnetic variation goes here
  214. # gga += "%3.2f,M," % mag_var
  215. gga += ",,"
  216. # Time in seconds since last DGPS update goes here
  217. gga += ","
  218. # DGPS station ID goes here
  219. return self.add_checksum(gga)
  220. def GLL(self, sim):
  221. "Emit GLL sentence describing the simulation state."
  222. tm = time.gmtime(sim.ksv.time)
  223. gll = "$GPLL,%09.4f,%c,%010.4f,%c,%02d%02d%02d,%s," % (
  224. self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0],
  225. self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0],
  226. tm.tm_hour,
  227. tm.tm_min,
  228. tm.tm_sec,
  229. sim.validity, )
  230. # FAA mode indicator could go after these fields.
  231. return self.add_checksum(gll)
  232. def RMC(self, sim):
  233. "Emit RMC sentence describing the simulation state."
  234. tm = time.gmtime(sim.ksv.time)
  235. rmc = \
  236. "GPRMC,%02d%02d%02d,%s,%09.4f,%c,%010.4f,%c,%.1f,%02d%02d%02d," % (
  237. tm.tm_hour,
  238. tm.tm_min,
  239. tm.tm_sec,
  240. sim.validity,
  241. self.degtodm(abs(sim.ksv.lat)), "SN"[sim.ksv.lat > 0],
  242. self.degtodm(abs(sim.ksv.lon)), "WE"[sim.ksv.lon > 0],
  243. sim.course * MPS_TO_KNOTS,
  244. tm.tm_mday,
  245. tm.tm_mon,
  246. tm.tm_year % 100)
  247. # Magnetic variation goes here
  248. # rmc += "%3.2f,M," % mag_var
  249. rmc += ",,"
  250. # FAA mode goes here
  251. return self.add_checksum(rmc)
  252. def ZDA(self, sim):
  253. "Emit ZDA sentence describing the simulation state."
  254. tm = time.gmtime(sim.ksv.time)
  255. zda = "$GPZDA,%02d%2d%02d,%02d,%02d,%04d" % (
  256. tm.tm_hour,
  257. tm.tm_min,
  258. tm.tm_sec,
  259. tm.tm_mday,
  260. tm.tm_mon,
  261. tm.tm_year, )
  262. # Local zone description, 00 to +- 13 hours, goes here
  263. zda += ","
  264. # Local zone minutes description goes here
  265. zda += ","
  266. return self.add_checksum(zda)
  267. def report(self, sim):
  268. "Report the simulation state."
  269. out = ""
  270. for sentence in self.sentences:
  271. if isinstance(sentence, tuple):
  272. (interval, sentence) = sentence
  273. if self.counter % interval:
  274. continue
  275. out += getattr(self, sentence)(*[sim])
  276. self.counter += 1
  277. return out
  278. # The very simple main line.
  279. if __name__ == "__main__":
  280. try:
  281. gpssim(NMEA).filter(sys.stdin, sys.stdout)
  282. except gpssimException as e:
  283. sys.stderr.write(repr(e) + "\n")
  284. # gpssim.py ends here.