mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-07-26 20:02:11 +08:00
qcomgpsd: rm XTRA assistance (#37893)
* qcomgpsd: rm XTRA assistance * lil more * lil more
This commit is contained in:
Adeeb Shihadeh
@@ -5,11 +5,8 @@ import signal
|
||||
import itertools
|
||||
import math
|
||||
import time
|
||||
import requests
|
||||
import shutil
|
||||
from serial import Serial
|
||||
import datetime
|
||||
from multiprocessing import Process, Event
|
||||
from typing import NoReturn
|
||||
from struct import unpack_from, calcsize, pack
|
||||
|
||||
@@ -30,9 +27,6 @@ from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, r
|
||||
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,
|
||||
@@ -112,49 +106,8 @@ def at_cmd(cmd: str) -> str:
|
||||
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():
|
||||
import subprocess
|
||||
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
|
||||
|
||||
def setup_quectel(diag: ModemDiag):
|
||||
# enable OEMDRE in the NV
|
||||
# TODO: it has to reboot for this to take effect
|
||||
DIAG_NV_READ_F = 38
|
||||
@@ -168,26 +121,11 @@ def setup_quectel(diag: ModemDiag) -> bool:
|
||||
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?")
|
||||
if system_time_valid():
|
||||
time_str = datetime.datetime.now(datetime.UTC).replace(tzinfo=None).strftime("%Y/%m/%d,%H:%M:%S")
|
||||
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
|
||||
@@ -214,7 +152,6 @@ def setup_quectel(diag: ModemDiag) -> bool:
|
||||
0,0
|
||||
))
|
||||
|
||||
return ret
|
||||
|
||||
def teardown_quectel(diag):
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
|
||||
@@ -255,9 +192,6 @@ def main() -> NoReturn:
|
||||
|
||||
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")
|
||||
|
||||
@@ -268,18 +202,13 @@ def main() -> NoReturn:
|
||||
except NameError:
|
||||
cloudlog.warning('quectel not yet setup')
|
||||
|
||||
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
|
||||
setup_quectel(diag)
|
||||
cloudlog.warning("quectel setup done")
|
||||
gpio_init(GPIO.GNSS_PWR_EN, True)
|
||||
gpio_set(GPIO.GNSS_PWR_EN, True)
|
||||
@@ -287,10 +216,6 @@ def main() -> NoReturn:
|
||||
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}")
|
||||
@@ -383,9 +308,6 @@ def main() -> NoReturn:
|
||||
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
|
||||
# quectel gps verticalAccuracy is clipped to 500, set invalid if so
|
||||
gps.hasFix = gps.verticalAccuracy != 500
|
||||
if gps.hasFix:
|
||||
want_assistance = False
|
||||
stop_download_event.set()
|
||||
pm.send('gpsLocation', msg)
|
||||
|
||||
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
|
||||
|
||||
@@ -1,28 +1,22 @@
|
||||
import os
|
||||
import pytest
|
||||
import time
|
||||
import datetime
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
|
||||
GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
|
||||
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestRawgpsd:
|
||||
class TestQcomgpsd:
|
||||
@classmethod
|
||||
def setup_class(cls):
|
||||
os.environ['GPS_COLD_START'] = '1'
|
||||
os.system("sudo systemctl start systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
wait_for_modem()
|
||||
|
||||
@classmethod
|
||||
def teardown_class(cls):
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
|
||||
def setup_method(self):
|
||||
@@ -30,7 +24,6 @@ class TestRawgpsd:
|
||||
|
||||
def teardown_method(self):
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
|
||||
def _wait_for_output(self, t):
|
||||
dt = 0.1
|
||||
@@ -41,24 +34,10 @@ class TestRawgpsd:
|
||||
time.sleep(dt)
|
||||
return self.sm.updated['qcomGnss']
|
||||
|
||||
def test_no_crash_double_command(self):
|
||||
wait_for_modem()
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
|
||||
def test_wait_for_modem(self):
|
||||
os.system("sudo systemctl stop ModemManager")
|
||||
def test_startup_time(self):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(30)
|
||||
|
||||
def test_startup_time(self, subtests):
|
||||
for internet in (True, False):
|
||||
if not internet:
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
with subtests.test(internet=internet):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(30)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
|
||||
def test_turns_off_gnss(self, subtests):
|
||||
for s in (0.1, 1, 5):
|
||||
@@ -70,50 +49,3 @@ class TestRawgpsd:
|
||||
wait_for_modem()
|
||||
resp = at_cmd("AT+QGPS?")
|
||||
assert "+QGPS: 0" in resp
|
||||
|
||||
|
||||
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"'
|
||||
wait_for_modem()
|
||||
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")
|
||||
assert abs((datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - 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'
|
||||
|
||||
@pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
|
||||
def test_assistance_loading(self):
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(30)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
self.check_assistance(True)
|
||||
|
||||
@pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
|
||||
def test_no_assistance_loading(self):
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
|
||||
managed_processes['qcomgpsd'].start()
|
||||
assert self._wait_for_output(30)
|
||||
managed_processes['qcomgpsd'].stop()
|
||||
self.check_assistance(False)
|
||||
|
||||
@pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user