123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442 |
- try:
- import machine
- import time
- import struct
- import math
- except:
- pass
- '''
- Parsing
- '''
- class NMEA(object):
- '''
- Just a class to describe the type of the received entry.
- We should consider removing this to save space.
- '''
- def __init__(self, id):
- self.__id = str(id)
- class Value(object):
- '''
- Defines a value received from the GPS
- v, float: floating value
- u, string: unit (e.g m, knot etc)
- t, tuple: time when measured.
- '''
- def __init__(self, v, u, t=(0,0,0.0)):
- self.__val = float(v)
- self.__unit = u
- self.__time = t
- def value(self):
- '''
- Returns the value
- '''
- return self.__val
- def unit(self):
- '''
- Returns the unit
- '''
- return self.__unit
- def time(self):
- '''
- Returns the measurment time.
- '''
- return self.__time
- def rad(self):
- '''
- Returns the value in radians.
- '''
- return self.__val * math.pi / 180
- def __repr__(self):
- return "%f %s" % (self.value(), self.unit())
- def __float__(self):
- return float(self.__val)
- class Distance(Value):
- '''
- Value: Distance
- '''
- def __init__(self, v, u, t):
- super().__init__(v, u, t)
- class Position(Value):
- '''
- Value: Position
- '''
- def __init__(self, v, u, t):
- super().__init__(v if u in ("N", "E") else -v, u, t)
- def __sub__(self, other):
- '''
- Returns a new position with the subtracted value
- '''
- u = self.unit()
- if self.unit() != other.unit():
- u += "/" + other.unit()
- return Position(self.value() - other.value(), u, self.time())
- class Speed(Value):
- '''
- Value: Speed
- '''
- def __init__(self, v, u, t):
- super().__init__(v, u, t)
- def to_kmh(self):
- '''
- Returns the speed in Km/h
- '''
- if self.unit() in ("knot", "N"):
- return Speed(self.value() * 1.85200, "kmh", self.time())
- return self
- def to_knot(self):
- '''
- Returns the speed in knot
- '''
- if self.unit() in ("kmh", "K"):
- return Speed(self.value() / 1.85200, "knot", self.time())
- return self
- class HDOP(Value):
- '''
- HDOP: Horizontal dilution of precision.
- '''
- def __init__(self, v, t):
- super().__init__(v, "", t)
- def __str__(self):
- if self.value() < 1:
- return "Ide."
- elif self.value() <= 2:
- return "Exe."
- elif self.value() <= 5:
- return "Good"
- elif self.value() <= 10:
- return "Mod."
- elif self.value() <= 20:
- return "Fair"
- else:
- return "Poor"
- def __repr__(self):
- return self.__str__()
- class Course(Value):
- '''
- Value: Course/direction
- '''
- def __init__(self, v, t):
- super().__init__(v, "D", t)
- class Location(object):
- '''
- Location:
- Tries to read data received from the GPS.
- Sometimes the GPS will return data for som
- of the segments, e.g it may happens that
- it wont receive the GPGGA-segment every time.
- '''
- def __init__(self):
- self.__valid = False
- self.__lat = None
- self.__long = None
- self.__alt = None
- self.__height = None
- self.__speed = None
- self.__course = None
- self.__satellites = -1
- self.__hdop = None
- def __repr__(self):
- return "Satelittes: %s, Quality: %s, %s\n\tLat/long: %s/%s\n\tAlt/h: %s/%s\n\tSpeed/course: %s/%s" % (
- self.__satellites, self.__hdop, repr(self.__valid),
- self.__lat, self.__lat,
- self.__alt, self.__height,
- self.__speed, self.__course,
- )
- def set(self, msgid, segment):
- '''
- Sets data based on the segment received
- msgid, []byte: the segment type, e.g b'$GPGGA'
- segment, []byte: the segment itself
- '''
- data = segment.split(",")
- if msgid == b'$GPGGA':
- if len(data) >= 6 and data[5] in ("1", "2", "6"):
- t = self.__time_from_seg(data[0])
- self.__set_lat(data[1], data[2], t)
- self.__set_long(data[3], data[4], t)
- if self.__satellites < 0 and self.__seg_set(data, 6):
- self.__satellites = int(data[6])
- if self.__seg_set(data, 7):
- self.__set_hdop(data[7], t)
- if self.__seg_set(data, 8):
- self.__alt = Distance(data[8], "M" if not self.__seg_set(data, 9) else data[9], t)
- if self.__seg_set(data, 10):
- self.__height = Distance(data[10], "M" if not self.__seg_set(data, 11) else data[10], t)
- elif msgid == b'$GPGLL':
- if len(data) >= 6 and data[5] == "A":
- t = self.__time_from_seg(data[4])
- self.__set_lat(data[0], data[1], t)
- self.__set_long(data[2], data[3], t)
- elif msgid == b'$GPRMC':
- if len(data) >= 6 and data[1] == "A":
- t = self.__time_from_seg(data[0])
- self.__set_lat(data[2], data[3], t)
- self.__set_long(data[4], data[5], t)
- self.__set_speed(data[6], "N", t)
- self.__set_course(data[7], t)
- elif msgid == b'$GPVTG':
- if len(data) >= 7 and data[2] == "T":
- self.__set_speed(data[4], data[6], (0, 0, 0.0))
- self.__set_course(data[5], data[0], (0, 0, 0.0))
- elif msgid == b'$GPGSV':
- if self.__seg_set(data, 2) and data[0] == data[1]:
- self.__satellites = int(data[2])
- elif msgid == b'$GPMSS':
- pass
- # This location is only valid when it was possible to set lat/long
- self.__valid = False if self.__lat is None or self.__long is None else True
- return self.valid()
- def valid(self):
- '''
- Returns wether or not the location is valid
- '''
- return self.__valid
- def __seg_set(self, d, i):
- '''
- Set position i from segment-part.
- d, []strings: segments in parts
- i, integer: position to read
- '''
- return len(d) >= (i + 1) and len(d[i]) != 0 and "*" not in d[i]
- def __set_hdop(self, v, t):
- '''
- Set Horizontal dilution of precision if the HDOP
- is missing or newer than the previous measured.
- Note: We may have several segments returning this values,
- which are (may have been) measured at different times.
- '''
- if len(v) == 0:
- return None
- if self.__hdop is None or self.__hdop.time() < t:
- self.__hdop = HDOP(v, t)
- def __set_speed(self, v, u, t):
- '''
- Set speed, but only if the speed is missing or the
- previous measured speed is older.
- Note: We may have several segments returning this values,
- which are (may have been) measured at different times.
- '''
- if len(v) == 0:
- return None
- if self.__speed is None or self.__speed.time() < t:
- self.__speed = Speed(v, u, t)
- def __set_course(self, v, t):
- '''
- Set course, but only if the course is missing or the
- previous measured speed is older.
- Note: We may have several segments returning this values,
- which are (may have been) measured at different times.
- '''
- if len(v) == 0:
- return None
- if self.__course is None or self.__course.time() < t:
- self.__course = Course(v, t)
- def __set_lat(self, v, d, t):
- '''
- Set latitude, but only if the latitude is missing or the
- previous measured speed is older.
- Note: We may have several segments returning this values,
- which are (may have been) measured at different times.
- '''
- if len(v) == 0:
- return None
- if self.__lat is None or self.__lat.time() < t:
- self.__lat = Position(float(v[0:2]) + (float(v[2:]) / 60), d, t)
- def __set_long(self, v, d, t):
- '''
- Set longitude, but only if the longitude is missing or the
- previous measured speed is older.
- Note: We may have several segments returning this values,
- which are (may have been) measured at different times.
- '''
- if len(v) == 0:
- return None
- if self.__long is None or self.__long.time() < t:
- self.__long = Position(float(v[0:3]) + (float(v[3:]) / 60), d, t)
- def __time_from_seg(self, ts):
- '''
- Get the time tuple from the time-segment
- '''
- return (int(ts[0:2]), int(ts[2:4]), float(ts[4:]))
- def longitude(self):
- '''
- Returns the longitude.
- '''
- return self.__lat if self.__lat is not None else False
- def latitude(self):
- '''
- Returns the latitude
- '''
- return self.__long if self.__long is not None else False
- def altitude(self):
- '''
- Returns the altitude
- '''
- return self.__alt if self.__alt is not None else False
- def height(self):
- '''
- Returns the height
- '''
- return self.__height if self.__height is not None else False
- def speed(self):
- '''
- Returns the speed
- '''
- return self.__speed if self.__speed is not None else False
- def course(self):
- '''
- Returns the course
- '''
- return self.__course if self.__course is not None else False
- def satellites(self):
- '''
- Returns the amount of satellites the GPS is connected to.
- '''
- return self.__satellites if self.__satellites >= 0 else False
- def hdop(self):
- '''
- Returns the precision
- '''
- return self.__hdop if self.__hdop is not None else False
- def delta(self, other):
- '''
- Checks the delta between this location and other location.
- Returns float with distance in meters
- '''
- earth_r = 6371000.0
- # radians
- lat1r = self.latitude().rad()
- lat2r = other.latitude().rad()
- # delta-radians
- latdr = lat2r - lat1r
- longdr = (other.longitude() - self.longitude()).rad()
- a1 = (math.sin(latdr / 2) * math.sin(latdr / 2))
- a2 = (math.cos(lat1r) * math.cos(lat2r))
- a3 = (math.sin(longdr / 2) * math.sin(longdr / 2))
- a = a1 + a2 * a3
- return earth_r * (2 * math.atan2(math.sqrt(a), math.sqrt(1-a)))
- class Data(object):
- '''
- Reads and stores data from the GPS
- '''
- def __init__(self, pins=("P3", "P4"), baud=9600):
- if machine: # Fix for pydoc
- self.__com = machine.UART(1, pins=pins, baudrate=baud)
- self.__location = None
- self.__last_update = time.time()
- def new_location(self, ttw=5):
- '''
- Waits for the GPS to return data with an 'time to wait'-interval.
- Returns True if there is a new VALID location.
- '''
- if time.time() - (self.__last_update + ttw) < 0:
- return False
- self.__data = Location()
- data = []
- while self.__com.any():
- tmp_data = self.__com.readline()
- if tmp_data[0:1] == b'$':
- self.__update(data)
- data = [tmp_data]
- elif len(data) != 0:
- data.append(tmp_data)
- else:
- self.__update(data)
- if self.__data.valid():
- print(self.__data)
-
- return self.__data.valid()
- def get_location(self):
- '''
- Returns the location-data. Should be used when new_location returns True.
- '''
- return self.__data
- def __update(self, data):
- '''
- Prepares the segments for reading.
- '''
- if len(data) == 0:
- return False
- data = b''.join(data)
- if data[len(data)-1:len(data)] not in (b'\n', b'\r'):
- print("False data: %s" % (str(data),))
- return False
- if data[len(data)-1:len(data)] != b'\n':
- data += '\n'
- if self.__data.set(data[0:6], ("%s" % (data[7:len(data)-2],))[2:-1]):
- self.__last_update = time.time()
|