mawei
/
sunnypilot
mirror of https://github.com/sunnypilot/sunnypilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

unicore gps (#31852) 

* ugpsd

* oops

* cleanup

* disable

* more docs

---------

Co-authored-by: Comma Device <device@comma.ai>
This commit is contained in:
Adeeb Shihadeh 2024-03-12 22:33:45 -07:00 committed by GitHub
parent a819292073
commit 4af932b74b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 166 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
selfdrive/manager/process_config.py
View File

@ -69,6 +69,7 @@ procs = [
PythonProcess("deleter", "system.loggerd.deleter", always_run),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
#PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI),
PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),
PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),

122
system/qcomgpsd/cgpsd.py
View File

@ -1,122 +0,0 @@
#!/usr/bin/env python3
import time
import datetime
from collections import defaultdict
from cereal import log
import cereal.messaging as messaging
from openpilot.common.swaglog import cloudlog
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
# https://campar.in.tum.de/twiki/pub/Chair/NaviGpsDemon/nmea.html#RMC
"""
AT+CGPSGPOS=1
response: '$GNGGA,220212.00,3245.09188,N,11711.76362,W,1,06,24.54,0.0,M,,M,,*77'
AT+CGPSGPOS=2
response: '$GNGSA,A,3,06,17,19,22,,,,,,,,,14.11,8.95,10.91,1*01
$GNGSA,A,3,29,26,,,,,,,,,,,14.11,8.95,10.91,4*03'
AT+CGPSGPOS=3
response: '$GPGSV,3,1,11,06,55,047,22,19,29,053,20,22,19,115,14,05,01,177,,0*68
$GPGSV,3,2,11,11,77,156,23,12,47,322,17,17,08,066,10,20,25,151,,0*6D
$GPGSV,3,3,11,24,44,232,,25,16,312,,29,02,260,,0*5D'
AT+CGPSGPOS=4
response: '$GBGSV,1,1,03,26,75,242,20,29,19,049,16,35,,,24,0*7D'
AT+CGPSGPOS=5
response: '$GNRMC,220216.00,A,3245.09531,N,11711.76043,W,,,070324,,,A,V*20'
"""
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')
def main():
wait_for_modem("AT+CGPS?")
cmds = [
"AT+GPSPORT=1",
"AT+CGPS=1",
]
for c in cmds:
at_cmd(c)
nmea = defaultdict(list)
pm = messaging.PubMaster(['gpsLocation'])
while True:
time.sleep(1)
try:
# TODO: read from streaming AT port instead of polling
out = at_cmd("AT+CGPS?")
if '+CGPS: 1' not in out:
for c in cmds:
at_cmd(c)
sentences = out.split("'")[1].splitlines()
new = {l.split(',')[0]: l.split(',') for l in sentences if l.startswith('$G')}
nmea.update(new)
if '$GNRMC' not in new:
print(f"no GNRMC:\n{out}\n")
continue
# validate checksums
for s in nmea.values():
sent = ','.join(s)
if checksum(sent) != s[-1].split('*')[1]:
cloudlog.error(f"invalid checksum: {repr(sent)}")
continue
gnrmc = nmea['$GNRMC']
#print(gnrmc)
msg = messaging.new_message('gpsLocation', valid=True)
gps = msg.gpsLocation
gps.latitude = (sfloat(gnrmc[3][:2]) + (sfloat(gnrmc[3][2:]) / 60)) * (1 if gnrmc[4] == "N" else -2)
gps.longitude = (sfloat(gnrmc[5][:3]) + (sfloat(gnrmc[5][3:]) / 60)) * (1 if gnrmc[6] == "E" else -1)
date = gnrmc[9][:6]
dt = datetime.datetime.strptime(f"{date} {gnrmc[1]}", '%d%m%y %H%M%S.%f')
gps.unixTimestampMillis = dt.timestamp()*1e3
gps.hasFix = gnrmc[1] == 'A'
gps.source = log.GpsLocationData.SensorSource.unicore
gps.speed = sfloat(gnrmc[7])
gps.bearingDeg = sfloat(gnrmc[8])
if len(nmea['$GNGGA']):
gngga = nmea['$GNGGA']
if gngga[10] == 'M':
gps.altitude = sfloat(gngga[9])
if len(nmea['$GNGSA']):
# TODO: this is only for GPS sats
gngsa = nmea['$GNGSA']
gps.horizontalAccuracy = sfloat(gngsa[4])
gps.verticalAccuracy = sfloat(gngsa[5])
# TODO: set these from the module
gps.bearingAccuracyDeg = 5.
gps.speedAccuracy = 3.
# TODO: can we get this from the NMEA sentences?
#gps.vNED = vNED
pm.send('gpsLocation', msg)
except Exception:
cloudlog.exception("gps.issue")
if __name__ == "__main__":
main()

165
system/ugpsd.py Executable file
View File

@ -0,0 +1,165 @@
#!/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()