diff --git a/system/ugpsd.py b/system/ugpsd.py deleted file mode 100755 index 34b20b01c8..0000000000 --- a/system/ugpsd.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import traceback -import serial -import datetime -import numpy as np -from collections import defaultdict - -from cereal import log -import cereal.messaging as messaging -from openpilot.common.retry import retry -from openpilot.common.swaglog import cloudlog -from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem - - -def sfloat(n: str): - return float(n) if len(n) > 0 else 0 - -def checksum(s: str): - ret = 0 - for c in s[1:-3]: - ret ^= ord(c) - return format(ret, '02X') - -class Unicore: - def __init__(self): - self.s = serial.Serial('/dev/ttyHS0', 115200) - self.s.timeout = 1 - self.s.writeTimeout = 1 - self.s.newline = b'\r\n' - - self.s.flush() - self.s.reset_input_buffer() - self.s.reset_output_buffer() - self.s.read(2048) - - def send(self, cmd): - self.s.write(cmd.encode('utf8') + b'\r') - resp = self.s.read(2048) - print(len(resp), cmd, "\n", resp) - assert b"OK" in resp - - def recv(self): - return self.s.readline() - -def build_msg(state): - """ - NMEA sentences: - https://campar.in.tum.de/twiki/pub/Chair/NaviGpsDemon/nmea.html#RMC - NAV messages: - https://www.unicorecomm.com/assets/upload/file/UFirebird_Standard_Positioning_Products_Protocol_Specification_CH.pdf - """ - - msg = messaging.new_message('gpsLocation', valid=True) - gps = msg.gpsLocation - - gnrmc = state['$GNRMC'] - gps.hasFix = gnrmc[1] == 'A' - gps.source = log.GpsLocationData.SensorSource.unicore - gps.latitude = (sfloat(gnrmc[3][:2]) + (sfloat(gnrmc[3][2:]) / 60)) * (1 if gnrmc[4] == "N" else -1) - gps.longitude = (sfloat(gnrmc[5][:3]) + (sfloat(gnrmc[5][3:]) / 60)) * (1 if gnrmc[6] == "E" else -1) - - try: - date = gnrmc[9][:6] - dt = datetime.datetime.strptime(f"{date} {gnrmc[1]}", '%d%m%y %H%M%S.%f') - gps.unixTimestampMillis = dt.timestamp()*1e3 - except Exception: - pass - - gps.bearingDeg = sfloat(gnrmc[8]) - - if len(state['$GNGGA']): - gngga = state['$GNGGA'] - if gngga[10] == 'M': - gps.altitude = sfloat(gngga[9]) - - if len(state['$GNGSA']): - gngsa = state['$GNGSA'] - gps.horizontalAccuracy = sfloat(gngsa[4]) - gps.verticalAccuracy = sfloat(gngsa[5]) - - #if len(state['$NAVACC']): - # # $NAVVEL,264415000,5,3,0.375,0.141,-0.735,-65.450*2A - # navacc = state['$NAVACC'] - # gps.horizontalAccuracy = sfloat(navacc[3]) - # gps.speedAccuracy = sfloat(navacc[4]) - # gps.bearingAccuracyDeg = sfloat(navacc[5]) - - if len(state['$NAVVEL']): - # $NAVVEL,264415000,5,3,0.375,0.141,-0.735,-65.450*2A - navvel = state['$NAVVEL'] - vECEF = [ - sfloat(navvel[4]), - sfloat(navvel[5]), - sfloat(navvel[6]), - ] - - lat = np.radians(gps.latitude) - lon = np.radians(gps.longitude) - R = np.array([ - [-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)], - [-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)], - [np.cos(lat), 0, -np.sin(lat)] - ]) - - vNED = [float(x) for x in R.dot(vECEF)] - gps.vNED = vNED - gps.speed = np.linalg.norm(vNED) - - # TODO: set these from the module - gps.bearingAccuracyDeg = 5. - gps.speedAccuracy = 3. - - return msg - - -@retry(attempts=10, delay=0.1) -def setup(u): - at_cmd('AT+CGPS=0') - at_cmd('AT+CGPS=1') - time.sleep(1.0) - - # setup NAVXXX outputs - for i in range(4): - u.send(f"$CFGMSG,1,{i},1") - for i in (1, 3): - u.send(f"$CFGMSG,3,{i},1") - - # 10Hz NAV outputs - u.send("$CFGNAV,100,100,1000") - - -def main(): - wait_for_modem("AT+CGPS?") - - u = Unicore() - setup(u) - - state = defaultdict(list) - pm = messaging.PubMaster(['gpsLocation']) - while True: - try: - msg = u.recv().decode('utf8').strip() - if "DEBUG" in os.environ: - print(repr(msg)) - - if len(msg) > 0: - if checksum(msg) != msg.split('*')[1]: - cloudlog.error(f"invalid checksum: {repr(msg)}") - continue - - k = msg.split(',')[0] - state[k] = msg.split(',') - if '$GNRMC' not in msg: - continue - - pm.send('gpsLocation', build_msg(state)) - except Exception: - traceback.print_exc() - cloudlog.exception("gps.issue") - - -if __name__ == "__main__": - main()