openpilot v0.9.6 release
date: 2024-02-21T23:02:42 master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5
This commit is contained in:
FrogAi
94
system/qcomgpsd/modemdiag.py
Normal file
94
system/qcomgpsd/modemdiag.py
Normal file
@@ -0,0 +1,94 @@
|
||||
import select
|
||||
from serial import Serial
|
||||
from crcmod import mkCrcFun
|
||||
from struct import pack, unpack_from, calcsize
|
||||
|
||||
class ModemDiag:
|
||||
def __init__(self):
|
||||
self.serial = self.open_serial()
|
||||
self.pend = b''
|
||||
|
||||
def open_serial(self):
|
||||
serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
|
||||
serial.flush()
|
||||
serial.reset_input_buffer()
|
||||
serial.reset_output_buffer()
|
||||
return serial
|
||||
|
||||
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
|
||||
ESCAPE_CHAR = b'\x7d'
|
||||
TRAILER_CHAR = b'\x7e'
|
||||
|
||||
def hdlc_encapsulate(self, payload):
|
||||
payload += pack('<H', ModemDiag.ccitt_crc16(payload))
|
||||
payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
|
||||
payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
|
||||
payload += self.TRAILER_CHAR
|
||||
return payload
|
||||
|
||||
def hdlc_decapsulate(self, payload):
|
||||
assert len(payload) >= 3
|
||||
assert payload[-1:] == self.TRAILER_CHAR
|
||||
payload = payload[:-1]
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
|
||||
assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
|
||||
return payload[:-2]
|
||||
|
||||
def recv(self):
|
||||
# self.serial.read_until makes tons of syscalls!
|
||||
raw_payload = [self.pend]
|
||||
while self.TRAILER_CHAR not in raw_payload[-1]:
|
||||
select.select([self.serial.fd], [], [])
|
||||
raw = self.serial.read(0x10000)
|
||||
raw_payload.append(raw)
|
||||
raw_payload = b''.join(raw_payload)
|
||||
raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
|
||||
raw_payload += self.TRAILER_CHAR
|
||||
unframed_message = self.hdlc_decapsulate(raw_payload)
|
||||
return unframed_message[0], unframed_message[1:]
|
||||
|
||||
def send(self, packet_type, packet_payload):
|
||||
self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
|
||||
|
||||
# *** end class ***
|
||||
|
||||
DIAG_LOG_F = 16
|
||||
DIAG_LOG_CONFIG_F = 115
|
||||
LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
|
||||
LOG_CONFIG_SET_MASK_OP = 3
|
||||
LOG_CONFIG_SUCCESS_S = 0
|
||||
|
||||
def send_recv(diag, packet_type, packet_payload):
|
||||
diag.send(packet_type, packet_payload)
|
||||
while 1:
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
break
|
||||
return opcode, payload
|
||||
|
||||
def setup_logs(diag, types_to_log):
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
|
||||
|
||||
header_spec = '<3xII'
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
|
||||
log_masks = unpack_from('<16I', payload, calcsize(header_spec))
|
||||
|
||||
for log_type, log_mask_bitsize in enumerate(log_masks):
|
||||
if log_mask_bitsize:
|
||||
log_mask = [0] * ((log_mask_bitsize+7)//8)
|
||||
for i in range(log_mask_bitsize):
|
||||
if ((log_type<<12)|i) in types_to_log:
|
||||
log_mask[i//8] |= 1 << (i%8)
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
|
||||
LOG_CONFIG_SET_MASK_OP,
|
||||
log_type,
|
||||
log_mask_bitsize
|
||||
) + bytes(log_mask))
|
||||
assert opcode == DIAG_LOG_CONFIG_F
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_SET_MASK_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
169
system/qcomgpsd/nmeaport.py
Normal file
169
system/qcomgpsd/nmeaport.py
Normal file
@@ -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.qcomgpsd.qcomgpsd import at_cmd
|
||||
|
||||
try:
|
||||
check_output(["pidof", "qcomgpsd"])
|
||||
print("qcomgpsd 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()
|
||||
458
system/qcomgpsd/qcomgpsd.py
Executable file
458
system/qcomgpsd/qcomgpsd.py
Executable file
@@ -0,0 +1,458 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import signal
|
||||
import itertools
|
||||
import math
|
||||
import time
|
||||
import requests
|
||||
import shutil
|
||||
import subprocess
|
||||
import datetime
|
||||
from multiprocessing import Process, Event
|
||||
from typing import NoReturn, Optional
|
||||
from struct import unpack_from, calcsize, pack
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.gpio import gpio_init, gpio_set
|
||||
from openpilot.common.retry import retry
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
|
||||
from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist,
|
||||
gps_measurement_report, gps_measurement_report_sv,
|
||||
glonass_measurement_report, glonass_measurement_report_sv,
|
||||
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
|
||||
|
||||
DEBUG = int(os.getenv("DEBUG", "0"))==1
|
||||
ASSIST_DATA_FILE = '/tmp/xtra3grc.bin'
|
||||
ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download'
|
||||
ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin'
|
||||
|
||||
LOG_TYPES = [
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
|
||||
]
|
||||
|
||||
|
||||
miscStatusFields = {
|
||||
"multipathEstimateIsValid": 0,
|
||||
"directionIsValid": 1,
|
||||
}
|
||||
|
||||
measurementStatusFields = {
|
||||
"subMillisecondIsValid": 0,
|
||||
"subBitTimeIsKnown": 1,
|
||||
"satelliteTimeIsKnown": 2,
|
||||
"bitEdgeConfirmedFromSignal": 3,
|
||||
"measuredVelocity": 4,
|
||||
"fineOrCoarseVelocity": 5,
|
||||
"lockPointValid": 6,
|
||||
"lockPointPositive": 7,
|
||||
|
||||
"lastUpdateFromDifference": 9,
|
||||
"lastUpdateFromVelocityDifference": 10,
|
||||
"strongIndicationOfCrossCorelation": 11,
|
||||
"tentativeMeasurement": 12,
|
||||
"measurementNotUsable": 13,
|
||||
"sirCheckIsNeeded": 14,
|
||||
"probationMode": 15,
|
||||
|
||||
"multipathIndicator": 24,
|
||||
"imdJammingIndicator": 25,
|
||||
"lteB13TxJammingIndicator": 26,
|
||||
"freshMeasurementIndicator": 27,
|
||||
}
|
||||
|
||||
measurementStatusGPSFields = {
|
||||
"gpsRoundRobinRxDiversity": 18,
|
||||
"gpsRxDiversity": 19,
|
||||
"gpsLowBandwidthRxDiversityCombined": 20,
|
||||
"gpsHighBandwidthNu4": 21,
|
||||
"gpsHighBandwidthNu8": 22,
|
||||
"gpsHighBandwidthUniform": 23,
|
||||
}
|
||||
|
||||
measurementStatusGlonassFields = {
|
||||
"glonassMeanderBitEdgeValid": 16,
|
||||
"glonassTimeMarkValid": 17
|
||||
}
|
||||
|
||||
@retry(attempts=10, delay=1.0)
|
||||
def try_setup_logs(diag, logs):
|
||||
return setup_logs(diag, logs)
|
||||
|
||||
@retry(attempts=3, delay=1.0)
|
||||
def at_cmd(cmd: str) -> Optional[str]:
|
||||
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
|
||||
|
||||
def gps_enabled() -> bool:
|
||||
return "QGPS: 1" in at_cmd("AT+QGPS?")
|
||||
|
||||
def download_assistance():
|
||||
try:
|
||||
response = requests.get(ASSISTANCE_URL, timeout=5, stream=True)
|
||||
|
||||
with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp:
|
||||
for chunk in response.iter_content(chunk_size=8192):
|
||||
fp.write(chunk)
|
||||
if fp.tell() > 1e5:
|
||||
cloudlog.error("Qcom assistance data larger than expected")
|
||||
return
|
||||
|
||||
os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
|
||||
|
||||
except requests.exceptions.RequestException:
|
||||
cloudlog.exception("Failed to download assistance file")
|
||||
return
|
||||
|
||||
def downloader_loop(event):
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
|
||||
alt_path = os.getenv("QCOM_ALT_ASSISTANCE_PATH", None)
|
||||
if alt_path is not None and os.path.exists(alt_path):
|
||||
shutil.copyfile(alt_path, ASSIST_DATA_FILE)
|
||||
|
||||
try:
|
||||
while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
|
||||
download_assistance()
|
||||
event.wait(timeout=10)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
@retry(attempts=5, delay=0.2, ignore_failure=True)
|
||||
def inject_assistance():
|
||||
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
|
||||
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
|
||||
cloudlog.info("successfully loaded assistance data")
|
||||
|
||||
@retry(attempts=5, delay=1.0)
|
||||
def setup_quectel(diag: ModemDiag) -> bool:
|
||||
ret = False
|
||||
|
||||
# enable OEMDRE in the NV
|
||||
# TODO: it has to reboot for this to take effect
|
||||
DIAG_NV_READ_F = 38
|
||||
DIAG_NV_WRITE_F = 39
|
||||
NV_GNSS_OEM_FEATURE_MASK = 7165
|
||||
send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
|
||||
send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
|
||||
|
||||
try_setup_logs(diag, LOG_TYPES)
|
||||
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
|
||||
if "GPS_COLD_START" in os.environ:
|
||||
# deletes all assistance
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
else:
|
||||
# allow module to perform hot start
|
||||
at_cmd("AT+QGPSDEL=1")
|
||||
|
||||
# disable DPO power savings for more accuracy
|
||||
at_cmd("AT+QGPSCFG=\"dpoenable\",0")
|
||||
# don't automatically turn on GNSS on powerup
|
||||
at_cmd("AT+QGPSCFG=\"autogps\",0")
|
||||
|
||||
# Do internet assistance
|
||||
at_cmd("AT+QGPSXTRA=1")
|
||||
at_cmd("AT+QGPSSUPLURL=\"NULL\"")
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
ret = True
|
||||
inject_assistance()
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
#at_cmd("AT+QGPSXTRADATA?")
|
||||
time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
|
||||
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
|
||||
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
|
||||
at_cmd("AT+QGPS=1")
|
||||
|
||||
# enable OEMDRE mode
|
||||
DIAG_SUBSYS_CMD_F = 75
|
||||
DIAG_SUBSYS_GPS = 13
|
||||
CGPS_DIAG_PDAPI_CMD = 0x64
|
||||
CGPS_OEM_CONTROL = 202
|
||||
GPSDIAG_OEMFEATURE_DRE = 1
|
||||
GPSDIAG_OEM_DRE_ON = 1
|
||||
|
||||
# gpsdiag_OemControlReqType
|
||||
send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
|
||||
DIAG_SUBSYS_GPS, # Subsystem Id
|
||||
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
|
||||
CGPS_OEM_CONTROL, # CGPS Command Code
|
||||
0, # Version
|
||||
GPSDIAG_OEMFEATURE_DRE,
|
||||
GPSDIAG_OEM_DRE_ON,
|
||||
0,0
|
||||
))
|
||||
|
||||
return ret
|
||||
|
||||
def teardown_quectel(diag):
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
try_setup_logs(diag, [])
|
||||
|
||||
|
||||
def wait_for_modem():
|
||||
cloudlog.warning("waiting for modem to come up")
|
||||
while True:
|
||||
ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
|
||||
if ret == 0:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
|
||||
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
|
||||
|
||||
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
|
||||
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
|
||||
|
||||
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
|
||||
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
|
||||
|
||||
unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
wait_for_modem()
|
||||
|
||||
stop_download_event = Event()
|
||||
assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,))
|
||||
assist_fetch_proc.start()
|
||||
def cleanup(sig, frame):
|
||||
cloudlog.warning("caught sig disabling quectel gps")
|
||||
|
||||
gpio_set(GPIO.GNSS_PWR_EN, False)
|
||||
teardown_quectel(diag)
|
||||
cloudlog.warning("quectel cleanup done")
|
||||
|
||||
stop_download_event.set()
|
||||
assist_fetch_proc.kill()
|
||||
assist_fetch_proc.join()
|
||||
|
||||
sys.exit(0)
|
||||
signal.signal(signal.SIGINT, cleanup)
|
||||
signal.signal(signal.SIGTERM, cleanup)
|
||||
|
||||
# connect to modem
|
||||
diag = ModemDiag()
|
||||
r = setup_quectel(diag)
|
||||
want_assistance = not r
|
||||
cloudlog.warning("quectel setup done")
|
||||
gpio_init(GPIO.GNSS_PWR_EN, True)
|
||||
gpio_set(GPIO.GNSS_PWR_EN, True)
|
||||
|
||||
pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
|
||||
|
||||
while 1:
|
||||
if os.path.exists(ASSIST_DATA_FILE) and want_assistance:
|
||||
setup_quectel(diag)
|
||||
want_assistance = False
|
||||
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
cloudlog.error(f"Unhandled opcode: {opcode}")
|
||||
continue
|
||||
|
||||
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
|
||||
if pending_msgs > 0:
|
||||
cloudlog.debug("have %d pending messages" % pending_msgs)
|
||||
assert log_outer_length == len(inner_log_packet)
|
||||
|
||||
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
|
||||
assert log_inner_length == len(inner_log_packet)
|
||||
|
||||
if log_type not in LOG_TYPES:
|
||||
continue
|
||||
|
||||
if DEBUG:
|
||||
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
|
||||
|
||||
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drMeasurementReport')
|
||||
report = gnss.drMeasurementReport
|
||||
|
||||
dat = unpack_oemdre_meas(log_payload)
|
||||
for k,v in dat.items():
|
||||
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
|
||||
k += "Ms"
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "svCount" or k.startswith("cdmaClockInfo["):
|
||||
# TODO: should we save cdmaClockInfo?
|
||||
pass
|
||||
elif k == "systemRtcValid":
|
||||
setattr(report, k, bool(v))
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
|
||||
report.init('sv', dat['svCount'])
|
||||
sats = log_payload[size_oemdre_meas:]
|
||||
for i in range(dat['svCount']):
|
||||
sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
for k,v in sat.items():
|
||||
if k in ["unkn", "measurementStatus2"]:
|
||||
pass
|
||||
elif k == "multipathEstimateValid":
|
||||
sv.measurementStatus.multipathEstimateIsValid = bool(v)
|
||||
elif k == "directionValid":
|
||||
sv.measurementStatus.directionIsValid = bool(v)
|
||||
elif k == "goodParity":
|
||||
setattr(sv, k, bool(v))
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in measurementStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
pm.send('qcomGnss', msg)
|
||||
elif log_type == LOG_GNSS_POSITION_REPORT:
|
||||
report = unpack_position(log_payload)
|
||||
if report["u_PosSource"] != 2:
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
msg = messaging.new_message('gpsLocation', valid=True)
|
||||
gps = msg.gpsLocation
|
||||
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
|
||||
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
|
||||
gps.altitude = report["q_FltFinalPosAlt"]
|
||||
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
|
||||
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
|
||||
|
||||
# TODO needs update if there is another leap second, after june 2024?
|
||||
dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.timezone.utc) +
|
||||
datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
|
||||
datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
|
||||
gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
|
||||
gps.source = log.GpsLocationData.SensorSource.qcomdiag
|
||||
gps.vNED = vNED
|
||||
gps.verticalAccuracy = report["q_FltVdop"]
|
||||
gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180
|
||||
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
|
||||
# quectel gps verticalAccuracy is clipped to 500, set invalid if so
|
||||
gps.flags = 1 if gps.verticalAccuracy != 500 else 0
|
||||
if gps.flags:
|
||||
want_assistance = False
|
||||
stop_download_event.set()
|
||||
pm.send('gpsLocation', msg)
|
||||
|
||||
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
dat = unpack_svpoly(log_payload)
|
||||
dat = relist(dat)
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drSvPoly')
|
||||
poly = gnss.drSvPoly
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "flags":
|
||||
pass
|
||||
else:
|
||||
setattr(poly, k, v)
|
||||
|
||||
'''
|
||||
# Timestamp glonass polys with GPSTime
|
||||
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
|
||||
from laika.helpers import get_prn_from_nmea_id
|
||||
prn = get_prn_from_nmea_id(poly.svId)
|
||||
if prn[0] == 'R':
|
||||
epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
|
||||
else:
|
||||
epoch = GPSTime(current_gps_time.week, poly.t0)
|
||||
|
||||
# handle week rollover
|
||||
if epoch.tow < SECS_IN_DAY and current_gps_time.tow > 6*SECS_IN_DAY:
|
||||
epoch.week += 1
|
||||
elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY:
|
||||
epoch.week -= 1
|
||||
|
||||
poly.gpsWeek = epoch.week
|
||||
poly.gpsTow = epoch.tow
|
||||
'''
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('measurementReport')
|
||||
report = gnss.measurementReport
|
||||
|
||||
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
|
||||
dat = unpack_gps_meas(log_payload)
|
||||
sats = log_payload[size_gps_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
|
||||
report.source = 0 # gps
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
|
||||
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
|
||||
dat = unpack_glonass_meas(log_payload)
|
||||
sats = log_payload[size_glonass_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
|
||||
report.source = 1 # glonass
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
|
||||
else:
|
||||
raise RuntimeError(f"invalid log_type: {log_type}")
|
||||
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 0
|
||||
elif k == "week":
|
||||
report.gpsWeek = v
|
||||
elif k == "svCount":
|
||||
pass
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
report.init('sv', dat['svCount'])
|
||||
if dat['svCount'] > 0:
|
||||
assert len(sats)//dat['svCount'] == size_meas_sv
|
||||
for i in range(dat['svCount']):
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
|
||||
for k,v in sat.items():
|
||||
if k == "parityErrorCount":
|
||||
sv.gpsParityErrorCount = v
|
||||
elif k == "frequencyIndex":
|
||||
sv.glonassFrequencyIndex = v
|
||||
elif k == "hemmingErrorCount":
|
||||
sv.glonassHemmingErrorCount = v
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in itertools.chain(*measurement_status_fields):
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "miscStatus":
|
||||
for kk,vv in miscStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "pad":
|
||||
pass
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
353
system/qcomgpsd/structs.py
Normal file
353
system/qcomgpsd/structs.py
Normal file
@@ -0,0 +1,353 @@
|
||||
from struct import unpack_from, calcsize
|
||||
|
||||
LOG_GNSS_POSITION_REPORT = 0x1476
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
|
||||
LOG_GNSS_CLOCK_REPORT = 0x1478
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
|
||||
LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
|
||||
LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
|
||||
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
|
||||
|
||||
LOG_GNSS_ME_DPO_STATUS = 0x1838
|
||||
LOG_GNSS_CD_DB_REPORT = 0x147B
|
||||
LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
|
||||
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
|
||||
LOG_GNSS_CONFIGURATION_STATE = 0x1516
|
||||
|
||||
oemdre_measurement_report = """
|
||||
uint8_t version;
|
||||
uint8_t reason;
|
||||
uint8_t sv_count;
|
||||
uint8_t seq_num;
|
||||
uint8_t seq_max;
|
||||
uint16_t rf_loss;
|
||||
|
||||
uint8_t system_rtc_valid;
|
||||
uint32_t f_count;
|
||||
uint32_t clock_resets;
|
||||
uint64_t system_rtc_time;
|
||||
|
||||
uint8_t gps_leap_seconds;
|
||||
uint8_t gps_leap_seconds_uncertainty;
|
||||
float gps_to_glonass_time_bias_milliseconds;
|
||||
float gps_to_glonass_time_bias_milliseconds_uncertainty;
|
||||
|
||||
uint16_t gps_week;
|
||||
uint32_t gps_milliseconds;
|
||||
uint32_t gps_time_bias;
|
||||
uint32_t gps_clock_time_uncertainty;
|
||||
uint8_t gps_clock_source;
|
||||
|
||||
uint8_t glonass_clock_source;
|
||||
uint8_t glonass_year;
|
||||
uint16_t glonass_day;
|
||||
uint32_t glonass_milliseconds;
|
||||
float glonass_time_bias;
|
||||
float glonass_clock_time_uncertainty;
|
||||
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t frequency_source;
|
||||
|
||||
uint32_t cdma_clock_info[5];
|
||||
|
||||
uint8_t source;
|
||||
"""
|
||||
|
||||
oemdre_svpoly_report = """
|
||||
uint8_t version;
|
||||
uint16_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t flags;
|
||||
uint16_t iode;
|
||||
double t0;
|
||||
double xyz0[3];
|
||||
double xyzN[9];
|
||||
float other[4];
|
||||
float position_uncertainty;
|
||||
float iono_delay;
|
||||
float iono_dot;
|
||||
float sbas_iono_delay;
|
||||
float sbas_iono_dot;
|
||||
float tropo_delay;
|
||||
float elevation;
|
||||
float elevation_dot;
|
||||
float elevation_uncertainty;
|
||||
double velocity_coeff[12];
|
||||
"""
|
||||
|
||||
|
||||
oemdre_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
uint8_t unkn;
|
||||
int8_t glonass_frequency_index;
|
||||
uint32_t observation_state;
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t filter_stages;
|
||||
uint8_t predetect_interval;
|
||||
uint8_t cycle_slip_count;
|
||||
uint16_t postdetections;
|
||||
|
||||
uint32_t measurement_status;
|
||||
uint32_t measurement_status2;
|
||||
|
||||
uint16_t carrier_noise;
|
||||
uint16_t rf_loss;
|
||||
int16_t latency;
|
||||
|
||||
float filtered_measurement_fraction;
|
||||
uint32_t filtered_measurement_integral;
|
||||
float filtered_time_uncertainty;
|
||||
float filtered_speed;
|
||||
float filtered_speed_uncertainty;
|
||||
|
||||
float unfiltered_measurement_fraction;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
|
||||
uint8_t multipath_estimate_valid;
|
||||
uint32_t multipath_estimate;
|
||||
uint8_t direction_valid;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
float doppler_acceleration;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
|
||||
uint64_t carrier_phase;
|
||||
uint32_t f_count;
|
||||
|
||||
uint16_t parity_error_count;
|
||||
uint8_t good_parity;
|
||||
"""
|
||||
|
||||
glonass_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint8_t glonass_cycle_number;
|
||||
uint16_t glonass_number_of_days;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
glonass_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t observation_state; // SVObservationStates
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t hemming_error_count;
|
||||
uint8_t filter_stages;
|
||||
uint16_t carrier_noise;
|
||||
int16_t latency;
|
||||
uint8_t predetect_interval;
|
||||
uint16_t postdetections;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_measurement_fraction;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
uint8_t cycle_slip_count;
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
gps_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint16_t week;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
gps_measurement_report_sv = """
|
||||
uint8_t sv_id; // SV PRN
|
||||
uint8_t observation_state; // SV Observation state
|
||||
uint8_t observations; // Count of all observation (both success and failure)
|
||||
uint8_t good_observations; // Count of Good observations
|
||||
uint16_t parity_error_count; // Carrier to Code filtering N count
|
||||
uint8_t filter_stages; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t carrier_noise; // CNo. Units of 0.1 dB
|
||||
int16_t latency; // Age of the measurement in msecs (+ve meas Meas precedes ref time)
|
||||
uint8_t predetect_interval; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t postdetections; // Num Post-Detections (uints of PreInts
|
||||
uint32_t unfiltered_measurement_integral; // Range of 0 thru (WEEK_MSECS-1) [msecs]
|
||||
float unfiltered_measurement_fraction; // Range of 0 thru 0.99999 [msecs]
|
||||
float unfiltered_time_uncertainty; // Time uncertainty (msec)
|
||||
float unfiltered_speed; // Speed estimate (meters/sec)
|
||||
float unfiltered_speed_uncertainty; // Speed uncertainty estimate (meters/sec)
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth; // Azimuth (radians)
|
||||
float elevation; // Elevation (radians)
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed; // Carrier phase derived speed
|
||||
float fine_speed_uncertainty; // Carrier phase derived speed UNC
|
||||
uint8_t cycle_slip_count; // Increments when a CSlip is detected
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
position_report = """
|
||||
uint8 u_Version; /* Version number of DM log */
|
||||
uint32 q_Fcount; /* Local millisecond counter */
|
||||
uint8 u_PosSource; /* Source of position information */ /* 0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database */
|
||||
uint32 q_Reserved1; /* Reserved memory field */
|
||||
uint16 w_PosVelFlag; /* Position velocity bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 q_PosVelFlag2; /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
|
||||
uint8 u_FailureCode; /* Failure code: (see DM log 0x1476 documentation) */
|
||||
uint16 w_FixEvents; /* Fix events bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 _fake_align_week_number;
|
||||
uint16 w_GpsWeekNumber; /* GPS week number of position */
|
||||
uint32 q_GpsFixTimeMs; /* GPS fix time of week of in milliseconds */
|
||||
uint8 u_GloNumFourYear; /* Number of Glonass four year cycles */
|
||||
uint16 w_GloNumDaysInFourYear; /* Glonass calendar day in four year cycle */
|
||||
uint32 q_GloFixTimeMs; /* Glonass fix time of day in milliseconds */
|
||||
uint32 q_PosCount; /* Integer count of the number of unique positions reported */
|
||||
uint64 t_DblFinalPosLatLon[2]; /* Final latitude and longitude of position in radians */
|
||||
uint32 q_FltFinalPosAlt; /* Final height-above-ellipsoid altitude of position */
|
||||
uint32 q_FltHeadingRad; /* User heading in radians */
|
||||
uint32 q_FltHeadingUncRad; /* User heading uncertainty in radians */
|
||||
uint32 q_FltVelEnuMps[3]; /* User velocity in east, north, up coordinate frame. In meters per second. */
|
||||
uint32 q_FltVelSigmaMps[3]; /* Gaussian 1-sigma value for east, north, up components of user velocity */
|
||||
uint32 q_FltClockBiasMeters; /* Receiver clock bias in meters */
|
||||
uint32 q_FltClockBiasSigmaMeters; /* Gaussian 1-sigma value for receiver clock bias in meters */
|
||||
uint32 q_FltGGTBMeters; /* GPS to Glonass time bias in meters */
|
||||
uint32 q_FltGGTBSigmaMeters; /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltGBTBMeters; /* GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltGBTBSigmaMeters; /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltBGTBMeters; /* BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltBGTBSigmaMeters; /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGGTBMeters; /* Filtered GPS to Glonass time bias in meters */
|
||||
uint32 q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGBTBMeters; /* Filtered GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltFiltBGTBMeters; /* Filtered BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltSftOffsetSec; /* SFT offset as computed by WLS in seconds */
|
||||
uint32 q_FltSftOffsetSigmaSec; /* Gaussian 1-sigma value for SFT offset in seconds */
|
||||
uint32 q_FltClockDriftMps; /* Clock drift (clock frequency bias) in meters per second */
|
||||
uint32 q_FltClockDriftSigmaMps; /* Gaussian 1-sigma value for clock drift in meters per second */
|
||||
uint32 q_FltFilteredAlt; /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltFilteredAltSigma; /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
|
||||
uint32 q_FltRawAlt; /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltRawAltSigma; /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
|
||||
uint32 align_Flt[14];
|
||||
uint32 q_FltPdop; /* 3D position dilution of precision as computed from the unweighted
|
||||
uint32 q_FltHdop; /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint32 q_FltVdop; /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint8 u_EllipseConfidence; /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
|
||||
uint32 q_FltEllipseAngle; /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
|
||||
uint32 q_FltEllipseSemimajorAxis; /* Semimajor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltEllipseSemiminorAxis; /* Semiminor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltPosSigmaVertical; /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
|
||||
uint8 u_HorizontalReliability; /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High */
|
||||
uint8 u_VerticalReliability; /* Vertical position reliability */
|
||||
uint16 w_Reserved2; /* Reserved memory field */
|
||||
uint32 q_FltGnssHeadingRad; /* User heading in radians derived from GNSS only solution */
|
||||
uint32 q_FltGnssHeadingUncRad; /* User heading uncertainty in radians derived from GNSS only solution */
|
||||
uint32 q_SensorDataUsageMask; /* Denotes which additional sensor data were used to compute this position fix. BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
|
||||
uint32 q_SensorAidMask; /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
|
||||
uint8 u_NumGpsSvsUsed; /* The number of GPS SVs used in the fix */
|
||||
uint8 u_TotalGpsSvs; /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumGloSvsUsed; /* The number of Glonass SVs used in the fix */
|
||||
uint8 u_TotalGloSvs; /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumBdsSvsUsed; /* The number of BeiDou SVs used in the fix */
|
||||
uint8 u_TotalBdsSvs; /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
|
||||
""" # noqa: E501
|
||||
|
||||
def name_to_camelcase(nam):
|
||||
ret = []
|
||||
i = 0
|
||||
while i < len(nam):
|
||||
if nam[i] == "_":
|
||||
ret.append(nam[i+1].upper())
|
||||
i += 2
|
||||
else:
|
||||
ret.append(nam[i])
|
||||
i += 1
|
||||
return ''.join(ret)
|
||||
|
||||
def parse_struct(ss):
|
||||
st = "<"
|
||||
nams = []
|
||||
for l in ss.strip().split("\n"):
|
||||
if len(l.strip()) == 0:
|
||||
continue
|
||||
typ, nam = l.split(";")[0].split()
|
||||
#print(typ, nam)
|
||||
if typ == "float" or '_Flt' in nam:
|
||||
st += "f"
|
||||
elif typ == "double" or '_Dbl' in nam:
|
||||
st += "d"
|
||||
elif typ in ["uint8", "uint8_t"]:
|
||||
st += "B"
|
||||
elif typ in ["int8", "int8_t"]:
|
||||
st += "b"
|
||||
elif typ in ["uint32", "uint32_t"]:
|
||||
st += "I"
|
||||
elif typ in ["int32", "int32_t"]:
|
||||
st += "i"
|
||||
elif typ in ["uint16", "uint16_t"]:
|
||||
st += "H"
|
||||
elif typ in ["int16", "int16_t"]:
|
||||
st += "h"
|
||||
elif typ in ["uint64", "uint64_t"]:
|
||||
st += "Q"
|
||||
else:
|
||||
raise RuntimeError(f"unknown type {typ}")
|
||||
if '[' in nam:
|
||||
cnt = int(nam.split("[")[1].split("]")[0])
|
||||
st += st[-1]*(cnt-1)
|
||||
for i in range(cnt):
|
||||
nams.append("%s[%d]" % (nam.split("[")[0], i))
|
||||
else:
|
||||
nams.append(nam)
|
||||
return st, nams
|
||||
|
||||
def dict_unpacker(ss, camelcase = False):
|
||||
st, nams = parse_struct(ss)
|
||||
if camelcase:
|
||||
nams = [name_to_camelcase(x) for x in nams]
|
||||
sz = calcsize(st)
|
||||
return lambda x: dict(zip(nams, unpack_from(st, x), strict=True)), sz
|
||||
|
||||
def relist(dat):
|
||||
list_keys = set()
|
||||
for key in dat.keys():
|
||||
if '[' in key:
|
||||
list_keys.add(key.split('[')[0])
|
||||
list_dict = {}
|
||||
for list_key in list_keys:
|
||||
list_dict[list_key] = []
|
||||
i = 0
|
||||
while True:
|
||||
key = list_key + f'[{i}]'
|
||||
if key not in dat:
|
||||
break
|
||||
list_dict[list_key].append(dat[key])
|
||||
del dat[key]
|
||||
i += 1
|
||||
return {**dat, **list_dict}
|
||||
124
system/qcomgpsd/tests/test_qcomgpsd.py
Executable file
124
system/qcomgpsd/tests/test_qcomgpsd.py
Executable file
@@ -0,0 +1,124 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import pytest
|
||||
import json
|
||||
import time
|
||||
import datetime
|
||||
import unittest
|
||||
import subprocess
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
|
||||
from openpilot.selfdrive.manager.process_config import managed_processes
|
||||
|
||||
GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
|
||||
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestRawgpsd(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
os.system("sudo systemctl start systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
wait_for_modem()
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
|
||||
def setUp(self):
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements'])
|
||||
|
||||
def tearDown(self):
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
|
||||
def _wait_for_output(self, t):
|
||||
dt = 0.1
|
||||
for _ in range(t*int(1/dt)):
|
||||
self.sm.update(0)
|
||||
if self.sm.updated['qcomGnss']:
|
||||
break
|
||||
time.sleep(dt)
|
||||
return self.sm.updated['qcomGnss']
|
||||
|
||||
def test_no_crash_double_command(self):
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
|
||||
def test_wait_for_modem(self):
|
||||
os.system("sudo systemctl stop ModemManager")
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert not self._wait_for_output(5)
|
||||
|
||||
os.system("sudo systemctl restart ModemManager")
|
||||
assert self._wait_for_output(30)
|
||||
|
||||
def test_startup_time(self):
|
||||
for internet in (True, False):
|
||||
if not internet:
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
with self.subTest(internet=internet):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(7)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
|
||||
def test_turns_off_gnss(self):
|
||||
for s in (0.1, 1, 5):
|
||||
with self.subTest(runtime=s):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
time.sleep(s)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
|
||||
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
|
||||
loc_status = json.loads(ls)
|
||||
assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'}
|
||||
|
||||
|
||||
def check_assistance(self, should_be_loaded):
|
||||
# after QGPSDEL: '+QGPSXTRADATA: 0,"1980/01/05,19:00:00"'
|
||||
# after loading: '+QGPSXTRADATA: 10080,"2023/06/24,19:00:00"'
|
||||
out = at_cmd("AT+QGPSXTRADATA?")
|
||||
out = out.split("+QGPSXTRADATA:")[1].split("'")[0].strip()
|
||||
valid_duration, injected_time_str = out.split(",", 1)
|
||||
if should_be_loaded:
|
||||
assert valid_duration == "10080" # should be max time
|
||||
injected_time = datetime.datetime.strptime(injected_time_str.replace("\"", ""), "%Y/%m/%d,%H:%M:%S")
|
||||
self.assertLess(abs((datetime.datetime.utcnow() - injected_time).total_seconds()), 60*60*12)
|
||||
else:
|
||||
valid_duration, injected_time_str = out.split(",", 1)
|
||||
injected_time_str = injected_time_str.replace('\"', '').replace('\'', '')
|
||||
assert injected_time_str[:] == '1980/01/05,19:00:00'[:]
|
||||
assert valid_duration == '0'
|
||||
|
||||
def test_assistance_loading(self):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(10)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
self.check_assistance(True)
|
||||
|
||||
def test_no_assistance_loading(self):
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(10)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
self.check_assistance(False)
|
||||
|
||||
def test_late_assistance_loading(self):
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
|
||||
managed_processes['qcomgpsd'].start()
|
||||
self._wait_for_output(17)
|
||||
assert self.sm.updated['qcomGnss']
|
||||
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
time.sleep(15)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
self.check_assistance(True)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main(failfast=True)
|
||||
Reference in New Issue
Block a user