Shane Smiskol
2 years ago
commit
f928161f5f
40 changed files with 396 additions and 178 deletions
@ -1 +1 @@ |
|||||||
fba62008efd13fb578de325f0cdb0a87fe5e28f0 |
4f02bcfbf45697c5e6ba0a032797f6b2f37e16d3 |
||||||
|
|||||||
@ -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() |
|
||||||
@ -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() |
||||||
Loading…
Reference in new issue