gps.py 11 KB

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