From 221cd0ab09fb3e33db010efde52459148e0aba2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Tue, 11 Jul 2023 10:25:49 +0200 Subject: [PATCH] Laikad: fix qcom report collection (#28879) * Make report aggregation variable * Add laikad test now it passes --------- Co-authored-by: Comma Device --- selfdrive/locationd/laikad.py | 9 +++++++-- system/sensord/rawgps/test_rawgps.py | 18 +++++++++++------- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index a9a1e78470..3b5609bf6e 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -103,6 +103,10 @@ class Laikad: self.first_log_time = None self.ttff = -1 + # qcom specific stuff + self.qcom_reports_received = 1 + self.qcom_reports = [] + def load_cache(self): if not self.save_ephemeris: return @@ -211,18 +215,19 @@ class Laikad: def read_report(self, gnss_msg): if self.use_qcom: - # QCOM reports are per constellation, should always send 3 reports + # QCOM reports are per constellation, so we need to aggregate them report = gnss_msg.drMeasurementReport report_time = self.gps_time_from_qcom_report(gnss_msg) if report_time - self.last_report_time > 0: + self.qcom_reports_received = max(1, len(self.qcom_reports)) self.qcom_reports = [report] else: self.qcom_reports.append(report) self.last_report_time = report_time new_meas = [] - if len(self.qcom_reports) == 3: + if len(self.qcom_reports) == self.qcom_reports_received: for report in self.qcom_reports: new_meas.extend(read_raw_qcom(report)) diff --git a/system/sensord/rawgps/test_rawgps.py b/system/sensord/rawgps/test_rawgps.py index 96bd422960..5bf4fa9643 100755 --- a/system/sensord/rawgps/test_rawgps.py +++ b/system/sensord/rawgps/test_rawgps.py @@ -5,12 +5,13 @@ import time import datetime import unittest import subprocess +import numpy as np import cereal.messaging as messaging from system.hardware import TICI from system.sensord.rawgps.rawgpsd import at_cmd from selfdrive.manager.process_config import managed_processes - +from common.transformations.coordinates import ecef_from_geodetic GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0'))) UPDATE_MS = 100 @@ -106,14 +107,17 @@ class TestRawgpsd(unittest.TestCase): at_cmd("AT+QGPSDEL=0") managed_processes['rawgpsd'].start() - #managed_processes['laikad'].start() + managed_processes['laikad'].start() assert self._wait_for_location(120) assert self.sm_gps_location['gpsLocation'].flags == 1 - #module_fix = [self.sm['gpsLocation'].latitude, self.sm['gpsLocation'].longitude, self.sm['gpsLocation'].altitude] - #assert self._wait_for_laikad_location(90) - #assert self.sm['gnssMeasurements'].flags == 1 - #print(self.sm_gnss_measurements['gnssMeasurements']) - #managed_processes['laikad'].stop() + module_fix = ecef_from_geodetic([self.sm_gps_location['gpsLocation'].latitude, + self.sm_gps_location['gpsLocation'].longitude, + self.sm_gps_location['gpsLocation'].altitude]) + assert self._wait_for_laikad_location(90) + total_diff = np.array(self.sm_gnss_measurements['gnssMeasurements'].positionECEF.value) - module_fix + print(total_diff) + self.assertLess(np.linalg.norm(total_diff), 100) + managed_processes['laikad'].stop() managed_processes['rawgpsd'].stop() if __name__ == "__main__":