diff --git a/system/qcomgpsd/modemdiag.py b/system/qcomgpsd/modemdiag.py index 5d72aeba9e..dc869bfc87 100644 --- a/system/qcomgpsd/modemdiag.py +++ b/system/qcomgpsd/modemdiag.py @@ -3,6 +3,8 @@ from serial import Serial from crcmod import mkCrcFun from struct import pack, unpack_from, calcsize +from openpilot.common.swaglog import cloudlog + class ModemDiag: def __init__(self): self.serial = self.open_serial() @@ -77,18 +79,37 @@ def setup_logs(diag, types_to_log): log_masks = unpack_from('<16I', payload, calcsize(header_spec)) + # Track which requested log ids the modem actually accepted. A partial failure here is the + # difference between measurements streaming with no position reports and a working GPS fix. + registered = set() + for log_type, log_mask_bitsize in enumerate(log_masks): if log_mask_bitsize: log_mask = [0] * ((log_mask_bitsize+7)//8) + requested = set() for i in range(log_mask_bitsize): - if ((log_type<<12)|i) in types_to_log: + log_id = (log_type<<12)|i + if log_id in types_to_log: log_mask[i//8] |= 1 << (i%8) + requested.add(log_id) 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 + if opcode != DIAG_LOG_CONFIG_F or operation != LOG_CONFIG_SET_MASK_OP or status != LOG_CONFIG_SUCCESS_S: + # Previously bare asserts: the mask write silently failed and the affected log ids + # never arrived, with no indication of which ones were lost. + lost = sorted(hex(i) for i in requested) + raise RuntimeError(f"log mask set failed for group {log_type}: opcode={opcode} operation={operation} status={status} lost={lost}") + registered |= requested + + missing = set(types_to_log) - registered + if missing: + # The modem reported no mask range covering these ids, so they will never be delivered. + raise RuntimeError(f"log ids not registered (no mask range): {sorted(hex(i) for i in missing)}") + + if registered: + cloudlog.warning(f"diag log registration complete: {sorted(hex(i) for i in registered)}") + return registered diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index d2c97d56c0..b762cd74b3 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -36,6 +36,9 @@ ASSIST_DATA_FILE = '/tmp/xtra3grc.bin' ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download' ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin' +# How often to report that the modem is delivering GNSS logs but no position reports. +POSITION_REPORT_WARN_INTERVAL = 30.0 + LOG_TYPES = [ LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, @@ -294,11 +297,25 @@ def main() -> NoReturn: pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) + # Diagnostics for the two silent paths that yield streaming measurements but no gpsLocation: + # the modem never sending position reports at all, and position reports being dropped for + # their source field. Without these the failure leaves no trace in the logs. + position_reports_seen = 0 + gnss_logs_seen = 0 + last_logged_pos_source = -1 + last_position_warn = time.monotonic() + while 1: if os.path.exists(ASSIST_DATA_FILE) and want_assistance: setup_quectel(diag) want_assistance = False + now = time.monotonic() + if now - last_position_warn >= POSITION_REPORT_WARN_INTERVAL: + last_position_warn = now + if position_reports_seen == 0 and gnss_logs_seen > 0: + cloudlog.error(f"no position reports after {gnss_logs_seen} GNSS logs: {hex(LOG_GNSS_POSITION_REPORT)} is not being delivered by the modem") + opcode, payload = diag.recv() if opcode != DIAG_LOG_F: cloudlog.error(f"Unhandled opcode: {opcode}") @@ -315,6 +332,8 @@ def main() -> NoReturn: if log_type not in LOG_TYPES: continue + gnss_logs_seen += 1 + if DEBUG: print(f"{time.time():.4f}: got log: {log_type} len {len(log_payload)}") # noqa: TID251 @@ -362,9 +381,18 @@ def main() -> NoReturn: setattr(sv, k, v) pm.send('qcomGnss', msg) elif log_type == LOG_GNSS_POSITION_REPORT: + position_reports_seen += 1 report = unpack_position(log_payload) if report["u_PosSource"] != 2: + # 0: none, 1: weighted least-squares, 3: externally injected, 4: internal database. + # Only the Kalman filter source (2) is published, so a modem stuck on another source + # yields measurement reports with no gpsLocation and no other trace. + if report["u_PosSource"] != last_logged_pos_source: + last_logged_pos_source = report["u_PosSource"] + failure_code, fix_events = report.get("u_FailureCode"), report.get("w_FixEvents") + cloudlog.warning(f"dropping position report: u_PosSource={last_logged_pos_source} (need 2), failureCode={failure_code}, fixEvents={fix_events}") continue + last_logged_pos_source = 2 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]"]]