Shane Smiskol
2 years ago
commit
0795749154
25 changed files with 263 additions and 59 deletions
@ -1 +1 @@ |
||||
Subproject commit 82bca3a9714b73c05414fdf848b6016a0ffac17d |
||||
Subproject commit 4291784b4d372782c95279e9fe7741e38633ca5e |
||||
@ -1 +1 @@ |
||||
Subproject commit ef1a9338a17f63ad1c666364c695e2b36a47350e |
||||
Subproject commit 3ab4f43de06d7abcc4d594ee2a4efc0466e42c94 |
||||
@ -0,0 +1,18 @@ |
||||
[pytest] |
||||
testpaths = |
||||
common |
||||
selfdrive/athena |
||||
selfdrive/boardd |
||||
selfdrive/car |
||||
selfdrive/controls |
||||
selfdrive/locationd |
||||
selfdrive/monitoring |
||||
selfdrive/thermald |
||||
selfdrive/test/longitudinal_maneuvers |
||||
system/hardware/tici |
||||
system/loggerd |
||||
system/tests |
||||
system/ubloxd |
||||
tools/lib/tests |
||||
markers = |
||||
parallel: mark tests as parallelizable (tests with no global state, so can be run in parallel) |
||||
@ -0,0 +1,169 @@ |
||||
import os |
||||
import sys |
||||
from dataclasses import dataclass, fields |
||||
from subprocess import check_output, CalledProcessError |
||||
from time import sleep |
||||
from typing import NoReturn |
||||
|
||||
DEBUG = int(os.environ.get("DEBUG", "0")) |
||||
|
||||
@dataclass |
||||
class GnssClockNmeaPort: |
||||
# flags bit mask: |
||||
# 0x01 = leap_seconds valid |
||||
# 0x02 = time_uncertainty_ns valid |
||||
# 0x04 = full_bias_ns valid |
||||
# 0x08 = bias_ns valid |
||||
# 0x10 = bias_uncertainty_ns valid |
||||
# 0x20 = drift_nsps valid |
||||
# 0x40 = drift_uncertainty_nsps valid |
||||
flags: int |
||||
leap_seconds: int |
||||
time_ns: int |
||||
time_uncertainty_ns: int # 1-sigma |
||||
full_bias_ns: int |
||||
bias_ns: float |
||||
bias_uncertainty_ns: float # 1-sigma |
||||
drift_nsps: float |
||||
drift_uncertainty_nsps: float # 1-sigma |
||||
|
||||
def __post_init__(self): |
||||
for field in fields(self): |
||||
val = getattr(self, field.name) |
||||
setattr(self, field.name, field.type(val) if val else None) |
||||
|
||||
@dataclass |
||||
class GnssMeasNmeaPort: |
||||
messageCount: int |
||||
messageNum: int |
||||
svCount: int |
||||
# constellation enum: |
||||
# 1 = GPS |
||||
# 2 = SBAS |
||||
# 3 = GLONASS |
||||
# 4 = QZSS |
||||
# 5 = BEIDOU |
||||
# 6 = GALILEO |
||||
constellation: int |
||||
svId: int |
||||
flags: int # always zero |
||||
time_offset_ns: int |
||||
# state bit mask: |
||||
# 0x0001 = CODE LOCK |
||||
# 0x0002 = BIT SYNC |
||||
# 0x0004 = SUBFRAME SYNC |
||||
# 0x0008 = TIME OF WEEK DECODED |
||||
# 0x0010 = MSEC AMBIGUOUS |
||||
# 0x0020 = SYMBOL SYNC |
||||
# 0x0040 = GLONASS STRING SYNC |
||||
# 0x0080 = GLONASS TIME OF DAY DECODED |
||||
# 0x0100 = BEIDOU D2 BIT SYNC |
||||
# 0x0200 = BEIDOU D2 SUBFRAME SYNC |
||||
# 0x0400 = GALILEO E1BC CODE LOCK |
||||
# 0x0800 = GALILEO E1C 2ND CODE LOCK |
||||
# 0x1000 = GALILEO E1B PAGE SYNC |
||||
# 0x2000 = GALILEO E1B PAGE SYNC |
||||
state: int |
||||
time_of_week_ns: int |
||||
time_of_week_uncertainty_ns: int # 1-sigma |
||||
carrier_to_noise_ratio: float |
||||
pseudorange_rate: float |
||||
pseudorange_rate_uncertainty: float # 1-sigma |
||||
|
||||
def __post_init__(self): |
||||
for field in fields(self): |
||||
val = getattr(self, field.name) |
||||
setattr(self, field.name, field.type(val) if val else None) |
||||
|
||||
def nmea_checksum_ok(s): |
||||
checksum = 0 |
||||
for i, c in enumerate(s[1:]): |
||||
if c == "*": |
||||
if i != len(s) - 4: # should be 3rd to last character |
||||
print("ERROR: NMEA string does not have checksum delimiter in correct location:", s) |
||||
return False |
||||
break |
||||
checksum ^= ord(c) |
||||
else: |
||||
print("ERROR: NMEA string does not have checksum delimiter:", s) |
||||
return False |
||||
|
||||
return True |
||||
|
||||
def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn: |
||||
while True: |
||||
try: |
||||
with open(device, "r") as nmeaport: |
||||
for line in nmeaport: |
||||
line = line.strip() |
||||
if DEBUG: |
||||
print(line) |
||||
if not line.startswith("$"): # all NMEA messages start with $ |
||||
continue |
||||
if not nmea_checksum_ok(line): |
||||
continue |
||||
|
||||
fields = line.split(",") |
||||
match fields[0]: |
||||
case "$GNCLK": |
||||
# fields at end are reserved (not used) |
||||
gnss_clock = GnssClockNmeaPort(*fields[1:10]) # type: ignore[arg-type] |
||||
print(gnss_clock) |
||||
case "$GNMEAS": |
||||
# fields at end are reserved (not used) |
||||
gnss_meas = GnssMeasNmeaPort(*fields[1:14]) # type: ignore[arg-type] |
||||
print(gnss_meas) |
||||
except Exception as e: |
||||
print(e) |
||||
sleep(1) |
||||
|
||||
def main() -> NoReturn: |
||||
from openpilot.common.gpio import gpio_init, gpio_set |
||||
from openpilot.system.hardware.tici.pins import GPIO |
||||
from openpilot.system.sensord.rawgps.rawgpsd import at_cmd |
||||
|
||||
try: |
||||
check_output(["pidof", "rawgpsd"]) |
||||
print("rawgpsd is running, please kill openpilot before running this script! (aborted)") |
||||
sys.exit(1) |
||||
except CalledProcessError as e: |
||||
if e.returncode != 1: # 1 == no process found (boardd not running) |
||||
raise e |
||||
|
||||
print("power up antenna ...") |
||||
gpio_init(GPIO.GNSS_PWR_EN, True) |
||||
gpio_set(GPIO.GNSS_PWR_EN, True) |
||||
|
||||
if b"+QGPS: 0" not in (at_cmd("AT+QGPS?") or b""): |
||||
print("stop location tracking ...") |
||||
at_cmd("AT+QGPSEND") |
||||
|
||||
if b'+QGPSCFG: "outport",usbnmea' not in (at_cmd('AT+QGPSCFG="outport"') or b""): |
||||
print("configure outport ...") |
||||
at_cmd('AT+QGPSCFG="outport","usbnmea"') # usbnmea = /dev/ttyUSB1 |
||||
|
||||
if b'+QGPSCFG: "gnssrawdata",3,0' not in (at_cmd('AT+QGPSCFG="gnssrawdata"') or b""): |
||||
print("configure gnssrawdata ...") |
||||
# AT+QGPSCFG="gnssrawdata",<constellation-mask>,<port>' |
||||
# <constellation-mask> values: |
||||
# 0x01 = GPS |
||||
# 0x02 = GLONASS |
||||
# 0x04 = BEIDOU |
||||
# 0x08 = GALILEO |
||||
# 0x10 = QZSS |
||||
# <port> values: |
||||
# 0 = NMEA port |
||||
# 1 = AT port |
||||
at_cmd('AT+QGPSCFG="gnssrawdata",3,0') # enable all constellations, output data to NMEA port |
||||
print("rebooting ...") |
||||
at_cmd('AT+CFUN=1,1') |
||||
print("re-run this script when it is back up") |
||||
sys.exit(2) |
||||
|
||||
print("starting location tracking ...") |
||||
at_cmd("AT+QGPS=1") |
||||
|
||||
process_nmea_port_messages() |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
Loading…
Reference in new issue