diff --git a/laika_repo b/laika_repo index 65901a1e6c..989b6a8505 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 65901a1e6c7c7940d8c97027d3edaca7d0132253 +Subproject commit 989b6a8505441a550c94f60b4378be296ea3fe92 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2073813363..6c9c199db1 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -18,7 +18,8 @@ from laika.downloader import DownloadFailed from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId, get_sv_id -from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox +from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman @@ -102,9 +103,10 @@ class Laikad: self.use_qcom = use_qcom self.first_log_time = None self.ttff = -1 + self.measurement_lag = 0.630 if self.use_qcom else 0.095 # qcom specific stuff - self.qcom_reports_received = 1 + self.qcom_reports_received = 4 self.qcom_reports = [] def load_cache(self): @@ -161,47 +163,31 @@ class Laikad: def get_lsq_fix(self, t, measurements): if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4 + position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) if len(position_solution) < 3: return None position_estimate = position_solution[:3] - - position_std_residual = np.median(np.abs(pr_residuals)) - position_std = np.median(np.abs(pos_std))/10 - position_std = max(position_std_residual, position_std) * np.ones(3) + position_std = pos_std[:3] velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) if len(velocity_solution) < 3: return None velocity_estimate = velocity_solution[:3] - - velocity_std_residual = np.median(np.abs(prr_residuals)) - velocity_std = np.median(np.abs(vel_std))/10 - velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3) + velocity_std = vel_std[:3] return position_estimate, position_std, velocity_estimate, velocity_std - def gps_time_from_qcom_report(self, gnss_msg): - report = gnss_msg.drMeasurementReport - if report.source == log.QcomGnss.MeasurementSource.gps: - report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) - elif report.source == log.QcomGnss.MeasurementSource.sbas: - report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0) - elif report.source == log.QcomGnss.MeasurementSource.glonass: - report_time = GPSTime.from_glonass(report.glonassYear, - report.glonassDay, - report.glonassMilliseconds / 1000.0) - else: - raise NotImplementedError(f'Unknownconstellation {report.source}') - return report_time - def is_good_report(self, gnss_msg): - if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: + if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom: # TODO: Understand and use remaining unknown constellations try: - constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) + if gnss_msg.which() == 'drMeasurementReport': + constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) + else: + constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source) good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS] - report_time = self.gps_time_from_qcom_report(gnss_msg) + report_time = gps_time_from_qcom_report(gnss_msg) except NotImplementedError: return False # Garbage timestamps with week > 32767 are sometimes sent by module. @@ -216,21 +202,20 @@ class Laikad: def read_report(self, gnss_msg): if self.use_qcom: # 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) - + # Additionally, the pseudoranges are broken in the measurementReports + # and the doppler filteredSpeed is broken in the drMeasurementReports + report_time = 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] + self.qcom_reports = [gnss_msg] else: - self.qcom_reports.append(report) + self.qcom_reports.append(gnss_msg) self.last_report_time = report_time - new_meas = [] if len(self.qcom_reports) == self.qcom_reports_received: - for report in self.qcom_reports: - new_meas.extend(read_raw_qcom(report)) - + new_meas = get_measurements_from_qcom_reports(self.qcom_reports) + else: + new_meas = [] else: report = gnss_msg.measurementReport self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow) @@ -300,7 +285,7 @@ class Laikad: def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): out_msg = messaging.new_message("gnssMeasurements") t = gnss_mono_time * 1e-9 - msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time} + msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)} if self.first_log_time is None: self.first_log_time = 1e-9 * gnss_mono_time if self.is_ephemeris(gnss_msg): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 58c650e26c..a8d69502cf 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -177,8 +177,8 @@ class TestLaikad(unittest.TestCase): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom) # Disable fetch_orbits to test NAV only correct_msgs = verify_messages(logs, laikad) - correct_msgs_expected = 56 if use_qcom else 560 - valid_fix_expected = 56 if use_qcom else 560 + correct_msgs_expected = 55 if use_qcom else 560 + valid_fix_expected = 55 if use_qcom else 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8aec881aa5..51c84c4bdd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -72e3d7b660ee92f5adcc249112cf04c703f4bf9e \ No newline at end of file +4846f6f416128ba74837829643aa3fe0897c5621 \ No newline at end of file