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