car-os-sensor/lib/car_os/gps.py

import busio
import adafruit_gps
import time

SAT_CODE_MAP = {
    "GA": "Galileo",
    "GB": "BeiDou",
    "GI": "NavIC",
    "GL": "GLONASS",
    "GP": "GPS",
    "GQ": "QZSS",
    "GN": "GNSS",
}
    
class GPS:
    def __init__(self, scl_pin, sda_pin, address):
        self._gps_i2c = busio.I2C(scl_pin, sda_pin)
        self._gps = adafruit_gps.GPS_GtopI2C(self._gps_i2c, address=address, debug=False)
        time.sleep(1)
        self._gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0", add_checksum=True)
        self._gps.send_command(b"PMTK220,500", add_checksum=True)
        self._gps_time = (0, 0, 0)
        self._previous_gps_time = (0, 0, 0)
        self.time_updated = False
        self._time_ready = False
        self.gps_updated = False
        self.sats_updated = False
        self._gps_update_time = [0, 0, 0, 0]
        self._sats_update_time = [0, 0, 0, 0]
        self._previous_latitude = 999999
        self._previous_sats = []
        
    
    def update(self, utc):
        if not self._gps.update() or not self._gps.has_fix:
            return False
        self._gps_time = (self._gps.timestamp_utc.tm_hour, self._gps.timestamp_utc.tm_min, self._gps.timestamp_utc.tm_sec)
        if self._gps_time != self._previous_gps_time:
            self.time_updated = True
            self._time_ready = True
            self._previous_gps_time = self._gps_time[:]
        if self._gps.latitude != self._previous_latitude:
            self.gps_updated = True
            self._gps_update_time = utc
            self._previous_latitude = self._gps.latitude
        if self._gps._sats is not None and (self._gps.sats != self._previous_sats):
            self._previous_sats = self._gps._sats[:]
            self.sats_updated =True
            self._sats_update_time = utc
        return True
    
    def get_position_message(self):
        if not self.ready:
            return None
        if not self.gps_updated:
            return None
        self.gps_updated = False
        message = bytearray(b'\xd4\x53\x2a\x7c')
        message += self._gps_update_time[0].to_bytes(1, 'little')
        message += self._gps_update_time[1].to_bytes(1, 'little')
        message += self._gps_update_time[2].to_bytes(1, 'little')
        message += self._gps_update_time[3].to_bytes(4, 'little')
        message += round((self._gps.latitude + 90) * 1_000_000_000).to_bytes(8, 'little')
        message += round((self._gps.longitude + 180) * 1_000_000_000).to_bytes(8, 'little')
        message += int(self._gps.altitude_m < 0).to_bytes(1, 'little')
        message += round(abs(self._gps.altitude_m) * 1_000).to_bytes(4, 'little')
        message += int(self._gps.height_geoid < 0).to_bytes(1, 'little')
        message += round(abs(self._gps.height_geoid) * 100).to_bytes(2, 'little')
        message += int(self._gps.speed_kmh < 0).to_bytes(1, 'little')
        message += round(abs(self._gps.speed_kmh) * 1_000_000).to_bytes(4, 'little')
        message += round(self._gps.pdop * 1_000).to_bytes(4, 'little')
        message += round(self._gps.hdop * 1_000).to_bytes(4, 'little')
        message += round(self._gps.vdop * 1_000).to_bytes(4, 'little')
        message += b'\x00\x00'
        return message
    
    def get_sat_message(self, utc):
        if not self.ready:
            return None
        if not self.sats_updated:
            return None
        self.sats_updated = False
        message = bytearray(b'\xd4\x53\x2a\x7b')
        message += self._sats_update_time[0].to_bytes(1, 'little')
        message += self._sats_update_time[1].to_bytes(1, 'little')
        message += self._sats_update_time[2].to_bytes(1, 'little')
        message += self._sats_update_time[3].to_bytes(4, 'little')
        talkers = {
            "GA": 0,
            "GB": 0,
            "GI": 0,
            "GL": 0,
            "GP": 0,
            "GQ": 0,
            "GN": 0,
        }
        if self._gps.sats:
            for sat in self._gps.sats:
                talkers[sat[:2]] += 1
        for talker in ["GA", "GB", "GI", "GL", "GP", "GQ", "GN"]:
            message += talkers[talker].to_bytes(1, 'little')
        message += len(self._gps.sats).to_bytes(1, 'little')
        for gps_id in self._gps.sats:
            string_encode = bytearray(self._gps.sats[gps_id][0].encode('ascii'))
            while len(string_encode) < 4:
                string_encode += b'\x00'
            message += string_encode
            message += max(0, self._gps.sats[gps_id][1]).to_bytes(1, 'little')
            message += self._gps.sats[gps_id][2].to_bytes(2, 'little')
        message += b'\x00\x00'
        return message
    
    @property
    def time(self):
        return self._gps_time
    
    @property
    def has_fix(self):
        return self._gps.has_fix
    
    @property
    def ready(self):
        if self._gps.latitude_degrees is None:
            return False
        if self._gps.longitude_degrees is None:
            return False
        if self._gps.altitude_m is None:
            return False
        if self._gps.speed_kmh is None:
            return False
        if self._gps.pdop is None:
            return False
        if self._gps.hdop is None:
            return False
        if self._gps.vdop is None:
            return False
        if not self._time_ready:
            return False
        return True