gps.py 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442
  1. try:
  2. import machine
  3. import time
  4. import struct
  5. import math
  6. except:
  7. pass
  8. '''
  9. Parsing
  10. '''
  11. class NMEA(object):
  12. '''
  13. Just a class to describe the type of the received entry.
  14. We should consider removing this to save space.
  15. '''
  16. def __init__(self, id):
  17. self.__id = str(id)
  18. class Value(object):
  19. '''
  20. Defines a value received from the GPS
  21. v, float: floating value
  22. u, string: unit (e.g m, knot etc)
  23. t, tuple: time when measured.
  24. '''
  25. def __init__(self, v, u, t=(0,0,0.0)):
  26. self.__val = float(v)
  27. self.__unit = u
  28. self.__time = t
  29. def value(self):
  30. '''
  31. Returns the value
  32. '''
  33. return self.__val
  34. def unit(self):
  35. '''
  36. Returns the unit
  37. '''
  38. return self.__unit
  39. def time(self):
  40. '''
  41. Returns the measurment time.
  42. '''
  43. return self.__time
  44. def rad(self):
  45. '''
  46. Returns the value in radians.
  47. '''
  48. return self.__val * math.pi / 180
  49. def __repr__(self):
  50. return "%f %s" % (self.value(), self.unit())
  51. def __float__(self):
  52. return float(self.__val)
  53. class Distance(Value):
  54. '''
  55. Value: Distance
  56. '''
  57. def __init__(self, v, u, t):
  58. super().__init__(v, u, t)
  59. class Position(Value):
  60. '''
  61. Value: Position
  62. '''
  63. def __init__(self, v, u, t):
  64. super().__init__(v if u in ("N", "E") else -v, u, t)
  65. def __sub__(self, other):
  66. '''
  67. Returns a new position with the subtracted value
  68. '''
  69. u = self.unit()
  70. if self.unit() != other.unit():
  71. u += "/" + other.unit()
  72. return Position(self.value() - other.value(), u, self.time())
  73. class Speed(Value):
  74. '''
  75. Value: Speed
  76. '''
  77. def __init__(self, v, u, t):
  78. super().__init__(v, u, t)
  79. def to_kmh(self):
  80. '''
  81. Returns the speed in Km/h
  82. '''
  83. if self.unit() in ("knot", "N"):
  84. return Speed(self.value() * 1.85200, "kmh", self.time())
  85. return self
  86. def to_knot(self):
  87. '''
  88. Returns the speed in knot
  89. '''
  90. if self.unit() in ("kmh", "K"):
  91. return Speed(self.value() / 1.85200, "knot", self.time())
  92. return self
  93. class HDOP(Value):
  94. '''
  95. HDOP: Horizontal dilution of precision.
  96. '''
  97. def __init__(self, v, t):
  98. super().__init__(v, "", t)
  99. def __str__(self):
  100. if self.value() < 1:
  101. return "Ide."
  102. elif self.value() <= 2:
  103. return "Exe."
  104. elif self.value() <= 5:
  105. return "Good"
  106. elif self.value() <= 10:
  107. return "Mod."
  108. elif self.value() <= 20:
  109. return "Fair"
  110. else:
  111. return "Poor"
  112. def __repr__(self):
  113. return self.__str__()
  114. class Course(Value):
  115. '''
  116. Value: Course/direction
  117. '''
  118. def __init__(self, v, t):
  119. super().__init__(v, "D", t)
  120. class Location(object):
  121. '''
  122. Location:
  123. Tries to read data received from the GPS.
  124. Sometimes the GPS will return data for som
  125. of the segments, e.g it may happens that
  126. it wont receive the GPGGA-segment every time.
  127. '''
  128. def __init__(self):
  129. self.__valid = False
  130. self.__lat = None
  131. self.__long = None
  132. self.__alt = None
  133. self.__height = None
  134. self.__speed = None
  135. self.__course = None
  136. self.__satellites = -1
  137. self.__hdop = None
  138. def __repr__(self):
  139. return "Satelittes: %s, Quality: %s, %s\n\tLat/long: %s/%s\n\tAlt/h: %s/%s\n\tSpeed/course: %s/%s" % (
  140. self.__satellites, self.__hdop, repr(self.__valid),
  141. self.__lat, self.__lat,
  142. self.__alt, self.__height,
  143. self.__speed, self.__course,
  144. )
  145. def set(self, msgid, segment):
  146. '''
  147. Sets data based on the segment received
  148. msgid, []byte: the segment type, e.g b'$GPGGA'
  149. segment, []byte: the segment itself
  150. '''
  151. data = segment.split(",")
  152. if msgid == b'$GPGGA':
  153. if len(data) >= 6 and data[5] in ("1", "2", "6"):
  154. t = self.__time_from_seg(data[0])
  155. self.__set_lat(data[1], data[2], t)
  156. self.__set_long(data[3], data[4], t)
  157. if self.__satellites < 0 and self.__seg_set(data, 6):
  158. self.__satellites = int(data[6])
  159. if self.__seg_set(data, 7):
  160. self.__set_hdop(data[7], t)
  161. if self.__seg_set(data, 8):
  162. self.__alt = Distance(data[8], "M" if not self.__seg_set(data, 9) else data[9], t)
  163. if self.__seg_set(data, 10):
  164. self.__height = Distance(data[10], "M" if not self.__seg_set(data, 11) else data[10], t)
  165. elif msgid == b'$GPGLL':
  166. if len(data) >= 6 and data[5] == "A":
  167. t = self.__time_from_seg(data[4])
  168. self.__set_lat(data[0], data[1], t)
  169. self.__set_long(data[2], data[3], t)
  170. elif msgid == b'$GPRMC':
  171. if len(data) >= 6 and data[1] == "A":
  172. t = self.__time_from_seg(data[0])
  173. self.__set_lat(data[2], data[3], t)
  174. self.__set_long(data[4], data[5], t)
  175. self.__set_speed(data[6], "N", t)
  176. self.__set_course(data[7], t)
  177. elif msgid == b'$GPVTG':
  178. if len(data) >= 7 and data[2] == "T":
  179. self.__set_speed(data[4], data[6], (0, 0, 0.0))
  180. self.__set_course(data[5], data[0], (0, 0, 0.0))
  181. elif msgid == b'$GPGSV':
  182. if self.__seg_set(data, 2) and data[0] == data[1]:
  183. self.__satellites = int(data[2])
  184. elif msgid == b'$GPMSS':
  185. pass
  186. # This location is only valid when it was possible to set lat/long
  187. self.__valid = False if self.__lat is None or self.__long is None else True
  188. return self.valid()
  189. def valid(self):
  190. '''
  191. Returns wether or not the location is valid
  192. '''
  193. return self.__valid
  194. def __seg_set(self, d, i):
  195. '''
  196. Set position i from segment-part.
  197. d, []strings: segments in parts
  198. i, integer: position to read
  199. '''
  200. return len(d) >= (i + 1) and len(d[i]) != 0 and "*" not in d[i]
  201. def __set_hdop(self, v, t):
  202. '''
  203. Set Horizontal dilution of precision if the HDOP
  204. is missing or newer than the previous measured.
  205. Note: We may have several segments returning this values,
  206. which are (may have been) measured at different times.
  207. '''
  208. if len(v) == 0:
  209. return None
  210. if self.__hdop is None or self.__hdop.time() < t:
  211. self.__hdop = HDOP(v, t)
  212. def __set_speed(self, v, u, t):
  213. '''
  214. Set speed, but only if the speed is missing or the
  215. previous measured speed is older.
  216. Note: We may have several segments returning this values,
  217. which are (may have been) measured at different times.
  218. '''
  219. if len(v) == 0:
  220. return None
  221. if self.__speed is None or self.__speed.time() < t:
  222. self.__speed = Speed(v, u, t)
  223. def __set_course(self, v, t):
  224. '''
  225. Set course, but only if the course is missing or the
  226. previous measured speed is older.
  227. Note: We may have several segments returning this values,
  228. which are (may have been) measured at different times.
  229. '''
  230. if len(v) == 0:
  231. return None
  232. if self.__course is None or self.__course.time() < t:
  233. self.__course = Course(v, t)
  234. def __set_lat(self, v, d, t):
  235. '''
  236. Set latitude, but only if the latitude is missing or the
  237. previous measured speed is older.
  238. Note: We may have several segments returning this values,
  239. which are (may have been) measured at different times.
  240. '''
  241. if len(v) == 0:
  242. return None
  243. if self.__lat is None or self.__lat.time() < t:
  244. self.__lat = Position(float(v[0:2]) + (float(v[2:]) / 60), d, t)
  245. def __set_long(self, v, d, t):
  246. '''
  247. Set longitude, but only if the longitude is missing or the
  248. previous measured speed is older.
  249. Note: We may have several segments returning this values,
  250. which are (may have been) measured at different times.
  251. '''
  252. if len(v) == 0:
  253. return None
  254. if self.__long is None or self.__long.time() < t:
  255. self.__long = Position(float(v[0:3]) + (float(v[3:]) / 60), d, t)
  256. def __time_from_seg(self, ts):
  257. '''
  258. Get the time tuple from the time-segment
  259. '''
  260. return (int(ts[0:2]), int(ts[2:4]), float(ts[4:]))
  261. def longitude(self):
  262. '''
  263. Returns the longitude.
  264. '''
  265. return self.__lat if self.__lat is not None else False
  266. def latitude(self):
  267. '''
  268. Returns the latitude
  269. '''
  270. return self.__long if self.__long is not None else False
  271. def altitude(self):
  272. '''
  273. Returns the altitude
  274. '''
  275. return self.__alt if self.__alt is not None else False
  276. def height(self):
  277. '''
  278. Returns the height
  279. '''
  280. return self.__height if self.__height is not None else False
  281. def speed(self):
  282. '''
  283. Returns the speed
  284. '''
  285. return self.__speed if self.__speed is not None else False
  286. def course(self):
  287. '''
  288. Returns the course
  289. '''
  290. return self.__course if self.__course is not None else False
  291. def satellites(self):
  292. '''
  293. Returns the amount of satellites the GPS is connected to.
  294. '''
  295. return self.__satellites if self.__satellites >= 0 else False
  296. def hdop(self):
  297. '''
  298. Returns the precision
  299. '''
  300. return self.__hdop if self.__hdop is not None else False
  301. def delta(self, other):
  302. '''
  303. Checks the delta between this location and other location.
  304. Returns float with distance in meters
  305. '''
  306. earth_r = 6371000.0
  307. # radians
  308. lat1r = self.latitude().rad()
  309. lat2r = other.latitude().rad()
  310. # delta-radians
  311. latdr = lat2r - lat1r
  312. longdr = (other.longitude() - self.longitude()).rad()
  313. a1 = (math.sin(latdr / 2) * math.sin(latdr / 2))
  314. a2 = (math.cos(lat1r) * math.cos(lat2r))
  315. a3 = (math.sin(longdr / 2) * math.sin(longdr / 2))
  316. a = a1 + a2 * a3
  317. return earth_r * (2 * math.atan2(math.sqrt(a), math.sqrt(1-a)))
  318. class Data(object):
  319. '''
  320. Reads and stores data from the GPS
  321. '''
  322. def __init__(self, pins=("P3", "P4"), baud=9600):
  323. if machine: # Fix for pydoc
  324. self.__com = machine.UART(1, pins=pins, baudrate=baud)
  325. self.__location = None
  326. self.__last_update = time.time()
  327. def new_location(self, ttw=5):
  328. '''
  329. Waits for the GPS to return data with an 'time to wait'-interval.
  330. Returns True if there is a new VALID location.
  331. '''
  332. if time.time() - (self.__last_update + ttw) < 0:
  333. return False
  334. self.__data = Location()
  335. data = []
  336. while self.__com.any():
  337. tmp_data = self.__com.readline()
  338. if tmp_data[0:1] == b'$':
  339. self.__update(data)
  340. data = [tmp_data]
  341. elif len(data) != 0:
  342. data.append(tmp_data)
  343. else:
  344. self.__update(data)
  345. if self.__data.valid():
  346. print(self.__data)
  347. return self.__data.valid()
  348. def get_location(self):
  349. '''
  350. Returns the location-data. Should be used when new_location returns True.
  351. '''
  352. return self.__data
  353. def __update(self, data):
  354. '''
  355. Prepares the segments for reading.
  356. '''
  357. if len(data) == 0:
  358. return False
  359. data = b''.join(data)
  360. if data[len(data)-1:len(data)] not in (b'\n', b'\r'):
  361. print("False data: %s" % (str(data),))
  362. return False
  363. if data[len(data)-1:len(data)] != b'\n':
  364. data += '\n'
  365. if self.__data.set(data[0:6], ("%s" % (data[7:len(data)-2],))[2:-1]):
  366. self.__last_update = time.time()