diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 5a6827a759..fa23a161c4 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -87,16 +87,24 @@ def try_setup_logs(diag, log_types): else: raise Exception(f"setup logs failed, {log_types=}") -def mmcli(cmd: str) -> None: +def at_cmd(cmd: str) -> None: for _ in range(5): try: - subprocess.check_call(f"mmcli -m any --timeout 30 {cmd}", shell=True) + subprocess.check_call(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True) break except subprocess.CalledProcessError: cloudlog.exception("rawgps.mmcli_command_failed") else: raise Exception(f"failed to execute mmcli command {cmd=}") + +def gps_enabled() -> bool: + try: + p = subprocess.check_output("mmcli -m any --command=\"AT+QGPS?\"", shell=True) + return b"QGPS: 1" in p + except subprocess.CalledProcessError as exc: + raise Exception("failed to execute QGPS mmcli command") from exc + def setup_quectel(diag: ModemDiag): # enable OEMDRE in the NV # TODO: it has to reboot for this to take effect @@ -108,11 +116,16 @@ def setup_quectel(diag: ModemDiag): setup_logs(diag, LOG_TYPES) + if gps_enabled(): + at_cmd("AT+QGPSEND") + # disable DPO power savings for more accuracy - mmcli("--command='AT+QGPSCFG=\"dpoenable\",0'") + at_cmd("AT+QGPSCFG=\"dpoenable\",0") # don't automatically turn on GNSS on powerup - mmcli("--command='AT+QGPSCFG=\"autogps\",0'") - mmcli("--location-enable-gps-raw --location-enable-gps-nmea") + at_cmd("AT+QGPSCFG=\"autogps\",0") + + at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"") + at_cmd("AT+QGPS=1") # enable OEMDRE mode DIAG_SUBSYS_CMD_F = 75 @@ -134,7 +147,9 @@ def setup_quectel(diag: ModemDiag): )) def teardown_quectel(diag): - mmcli("--location-disable-gps-raw --location-disable-gps-nmea") + at_cmd("AT+QGPSCFG=\"outport\",\"none\"") + if gps_enabled(): + at_cmd("AT+QGPSEND") try_setup_logs(diag, []) @@ -156,7 +171,7 @@ def main() -> NoReturn: # wait for ModemManager to come up cloudlog.warning("waiting for modem to come up") while True: - ret = subprocess.call("mmcli -m any --timeout 10 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) if ret == 0: break time.sleep(0.1)