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.
 
 
 
 
 
 

290 lines
9.8 KiB

  1. # misc.py - miscellaneous geodesy and time functions
  2. "miscellaneous geodesy and time functions"
  3. #
  4. # This file is Copyright (c) 2010 by the GPSD project
  5. # BSD terms apply: see the file COPYING in the distribution root for details.
  6. # This code runs compatibly under Python 2 and 3.x for x >= 2.
  7. # Preserve this property!
  8. from __future__ import absolute_import, print_function, division
  9. import calendar
  10. import io
  11. import math
  12. import time
  13. def monotonic():
  14. """return monotonic seconds, of unknown epoch.
  15. Python 2 to 3.7 has time.clock(), deprecates in 3.3+, removed in 3.8
  16. Python 3.5+ has time.monotonic()
  17. This always works
  18. """
  19. if hasattr(time, 'monotonic'):
  20. return time.monotonic()
  21. # else
  22. return time.clock()
  23. # Determine a single class for testing "stringness"
  24. try:
  25. STR_CLASS = basestring # Base class for 'str' and 'unicode' in Python 2
  26. except NameError:
  27. STR_CLASS = str # In Python 3, 'str' is the base class
  28. # We need to be able to handle data which may be a mixture of text and binary
  29. # data. The text in this context is known to be limited to US-ASCII, so
  30. # there aren't any issues regarding character sets, but we need to ensure
  31. # that binary data is preserved. In Python 2, this happens naturally with
  32. # "strings" and the 'str' and 'bytes' types are synonyms. But in Python 3,
  33. # these are distinct types (with 'str' being based on Unicode), and conversions
  34. # are encoding-sensitive. The most straightforward encoding to use in this
  35. # context is 'latin-1' (a.k.a.'iso-8859-1'), which directly maps all 256
  36. # 8-bit character values to Unicode page 0. Thus, if we can enforce the use
  37. # of 'latin-1' encoding, we can preserve arbitrary binary data while correctly
  38. # mapping any actual text to the proper characters.
  39. BINARY_ENCODING = 'latin-1'
  40. if bytes is str: # In Python 2 these functions can be null transformations
  41. polystr = str
  42. polybytes = bytes
  43. def make_std_wrapper(stream):
  44. "Dummy stdio wrapper function."
  45. return stream
  46. def get_bytes_stream(stream):
  47. "Dummy stdio bytes buffer function."
  48. return stream
  49. else: # Otherwise we do something real
  50. def polystr(o):
  51. "Convert bytes or str to str with proper encoding."
  52. if isinstance(o, str):
  53. return o
  54. if isinstance(o, bytes):
  55. return str(o, encoding=BINARY_ENCODING)
  56. if isinstance(o, bytearray):
  57. return str(o, encoding=BINARY_ENCODING)
  58. raise ValueError
  59. def polybytes(o):
  60. "Convert bytes or str to bytes with proper encoding."
  61. if isinstance(o, bytes):
  62. return o
  63. if isinstance(o, str):
  64. return bytes(o, encoding=BINARY_ENCODING)
  65. raise ValueError
  66. def make_std_wrapper(stream):
  67. "Standard input/output wrapper factory function"
  68. # This ensures that the encoding of standard output and standard
  69. # error on Python 3 matches the binary encoding we use to turn
  70. # bytes to Unicode in polystr above.
  71. #
  72. # newline="\n" ensures that Python 3 won't mangle line breaks
  73. # line_buffering=True ensures that interactive command sessions
  74. # work as expected
  75. return io.TextIOWrapper(stream.buffer, encoding=BINARY_ENCODING,
  76. newline="\n", line_buffering=True)
  77. def get_bytes_stream(stream):
  78. "Standard input/output bytes buffer function"
  79. return stream.buffer
  80. # some multipliers for interpreting GPS output
  81. # Note: A Texas Foot is ( meters * 3937/1200)
  82. # (Texas Natural Resources Code, Subchapter D, Sec 21.071 - 79)
  83. # not the same as an international fooot.
  84. METERS_TO_FEET = 3.28083989501312 # Meters to U.S./British feet
  85. METERS_TO_MILES = 0.000621371192237334 # Meters to miles
  86. METERS_TO_FATHOMS = 0.546806649168854 # Meters to fathoms
  87. KNOTS_TO_MPH = 1.15077944802354 # Knots to miles per hour
  88. KNOTS_TO_KPH = 1.852 # Knots to kilometers per hour
  89. KNOTS_TO_MPS = 0.514444444444445 # Knots to meters per second
  90. MPS_TO_KPH = 3.6 # Meters per second to klicks/hr
  91. MPS_TO_MPH = 2.2369362920544 # Meters/second to miles per hour
  92. MPS_TO_KNOTS = 1.9438444924406 # Meters per second to knots
  93. def Deg2Rad(x):
  94. "Degrees to radians."
  95. return x * (math.pi / 180)
  96. def Rad2Deg(x):
  97. "Radians to degrees."
  98. return x * (180 / math.pi)
  99. def CalcRad(lat):
  100. "Radius of curvature in meters at specified latitude WGS-84."
  101. # the radius of curvature of an ellipsoidal Earth in the plane of a
  102. # meridian of latitude is given by
  103. #
  104. # R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2)
  105. #
  106. # where
  107. # a is the equatorial radius (surface to center distance),
  108. # b is the polar radius (surface to center distance),
  109. # e is the first eccentricity of the ellipsoid
  110. # e2 is e^2 = (a^2 - b^2) / a^2
  111. # es is the second eccentricity of the ellipsoid (UNUSED)
  112. # es2 is es^2 = (a^2 - b^2) / b^2
  113. #
  114. # for WGS-84:
  115. # a = 6378.137 km (3963 mi)
  116. # b = 6356.752314245 km (3950 mi)
  117. # e2 = 0.00669437999014132
  118. # es2 = 0.00673949674227643
  119. a = 6378.137
  120. e2 = 0.00669437999014132
  121. sc = math.sin(math.radians(lat))
  122. x = a * (1.0 - e2)
  123. z = 1.0 - e2 * pow(sc, 2)
  124. y = pow(z, 1.5)
  125. r = x / y
  126. r = r * 1000.0 # Convert to meters
  127. return r
  128. def EarthDistance(c1, c2):
  129. """
  130. Vincenty's formula (inverse method) to calculate the distance (in
  131. kilometers or miles) between two points on the surface of a spheroid
  132. WGS 84 accurate to 1mm!
  133. """
  134. (lat1, lon1) = c1
  135. (lat2, lon2) = c2
  136. # WGS 84
  137. a = 6378137 # meters
  138. f = 1 / 298.257223563
  139. b = 6356752.314245 # meters; b = (1 - f)a
  140. # MILES_PER_KILOMETER = 1000.0 / (.3048 * 5280.0)
  141. MAX_ITERATIONS = 200
  142. CONVERGENCE_THRESHOLD = 1e-12 # .000,000,000,001
  143. # short-circuit coincident points
  144. if lat1 == lat2 and lon1 == lon2:
  145. return 0.0
  146. U1 = math.atan((1 - f) * math.tan(math.radians(lat1)))
  147. U2 = math.atan((1 - f) * math.tan(math.radians(lat2)))
  148. L = math.radians(lon1 - lon2)
  149. Lambda = L
  150. sinU1 = math.sin(U1)
  151. cosU1 = math.cos(U1)
  152. sinU2 = math.sin(U2)
  153. cosU2 = math.cos(U2)
  154. for _ in range(MAX_ITERATIONS):
  155. sinLambda = math.sin(Lambda)
  156. cosLambda = math.cos(Lambda)
  157. sinSigma = math.sqrt((cosU2 * sinLambda) ** 2 +
  158. (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) ** 2)
  159. if sinSigma == 0:
  160. return 0.0 # coincident points
  161. cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
  162. sigma = math.atan2(sinSigma, cosSigma)
  163. sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma
  164. cosSqAlpha = 1 - sinAlpha ** 2
  165. try:
  166. cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
  167. except ZeroDivisionError:
  168. cos2SigmaM = 0
  169. C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
  170. LambdaPrev = Lambda
  171. Lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma *
  172. (cos2SigmaM + C * cosSigma *
  173. (-1 + 2 * cos2SigmaM ** 2)))
  174. if abs(Lambda - LambdaPrev) < CONVERGENCE_THRESHOLD:
  175. break # successful convergence
  176. else:
  177. # failure to converge
  178. # fall back top EarthDistanceSmall
  179. return EarthDistanceSmall(c1, c2)
  180. uSq = cosSqAlpha * (a ** 2 - b ** 2) / (b ** 2)
  181. A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
  182. B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
  183. deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (
  184. cosSigma * (-1 + 2 * cos2SigmaM ** 2) - B / 6 * cos2SigmaM *
  185. (-3 + 4 * sinSigma ** 2) * (-3 + 4 * cos2SigmaM ** 2)))
  186. s = b * A * (sigma - deltaSigma)
  187. # return meters to 6 decimal places
  188. return round(s, 6)
  189. def EarthDistanceSmall(c1, c2):
  190. "Distance in meters between two close points specified in degrees."
  191. # This calculation is known as an Equirectangular Projection
  192. # fewer numeric issues for small angles that other methods
  193. # the main use here is for when Vincenty's fails to converge.
  194. (lat1, lon1) = c1
  195. (lat2, lon2) = c2
  196. avglat = (lat1 + lat2) / 2
  197. phi = math.radians(avglat) # radians of avg latitude
  198. # meters per degree at this latitude, corrected for WGS84 ellipsoid
  199. # Note the wikipedia numbers are NOT ellipsoid corrected:
  200. # https://en.wikipedia.org/wiki/Decimal_degrees#Precision
  201. m_per_d = (111132.954 - 559.822 * math.cos(2 * phi) +
  202. 1.175 * math.cos(4 * phi))
  203. dlat = (lat1 - lat2) * m_per_d
  204. dlon = (lon1 - lon2) * m_per_d * math.cos(phi)
  205. dist = math.sqrt(math.pow(dlat, 2) + math.pow(dlon, 2))
  206. return dist
  207. def MeterOffset(c1, c2):
  208. "Return offset in meters of second arg from first."
  209. (lat1, lon1) = c1
  210. (lat2, lon2) = c2
  211. dx = EarthDistance((lat1, lon1), (lat1, lon2))
  212. dy = EarthDistance((lat1, lon1), (lat2, lon1))
  213. if lat1 < lat2:
  214. dy = -dy
  215. if lon1 < lon2:
  216. dx = -dx
  217. return (dx, dy)
  218. def isotime(s):
  219. "Convert timestamps in ISO8661 format to and from Unix time."
  220. if isinstance(s, int):
  221. return time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
  222. if isinstance(s, float):
  223. date = int(s)
  224. msec = s - date
  225. date = time.strftime("%Y-%m-%dT%H:%M:%S", time.gmtime(s))
  226. return date + "." + repr(msec)[3:]
  227. if isinstance(s, STR_CLASS):
  228. if s[-1] == "Z":
  229. s = s[:-1]
  230. if "." in s:
  231. (date, msec) = s.split(".")
  232. else:
  233. date = s
  234. msec = "0"
  235. # Note: no leap-second correction!
  236. return calendar.timegm(
  237. time.strptime(date, "%Y-%m-%dT%H:%M:%S")) + float("0." + msec)
  238. # else:
  239. raise TypeError
  240. # End