diff --git a/laika_repo b/laika_repo index f5f76d28b4..231eafbf65 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd +Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index d07fe4a78a..146783e53d 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -4,8 +4,11 @@ from typing import List import numpy as np from collections import defaultdict +from scipy import linalg + from cereal import log, messaging from laika import AstroDog +from laika.ephemeris import convert_ublox_ephem from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind @@ -19,28 +22,25 @@ MAX_TIME_GAP = 10 class Laikad: - def __init__(self): + def __init__(self, use_internet): + self.astro_dog = AstroDog(use_internet=use_internet) self.gnss_kf = GNSSKalman(GENERATED_DIR) - def process_ublox_msg(self, ublox_msg, dog: AstroDog, ublox_mono_time: int): + def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport new_meas = read_raw_ublox(report) - measurements = process_measurements(new_meas, dog) - - - pos_fix = calc_pos_fix(measurements) + measurements = process_measurements(new_meas, self.astro_dog) + pos_fix = calc_pos_fix(measurements, min_measurements=4) # To get a position fix a minimum of 5 measurements are needed. - # Each report can contain less and some measurement can't be processed. - if len(pos_fix) > 0: - measurements = correct_measurements(measurements, pos_fix[0][:3], dog) - meas_msgs = [create_measurement_msg(m) for m in measurements] + # Each report can contain less and some measurements can't be processed. + corrected_measurements = [] + if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100: + corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) t = ublox_mono_time * 1e-9 - - self.update_localizer(pos_fix, t, measurements) + self.update_localizer(pos_fix, t, corrected_measurements) localizer_valid = self.localizer_valid(t) - ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() @@ -49,6 +49,8 @@ class Laikad: bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) + meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] + dat = messaging.new_message("gnssMeasurements") measurement_msg = log.GnssMeasurements.Measurement.new_message dat.gnssMeasurements = { @@ -59,6 +61,11 @@ class Laikad: "correctedMeasurements": meas_msgs } return dat + elif ublox_msg.which == 'ephemeris': + ephem = convert_ublox_ephem(ublox_msg.ephemeris) + self.astro_dog.add_ephem(ephem, self.astro_dog.orbits) + # elif ublox_msg.which == 'ionoData': + # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid @@ -67,9 +74,10 @@ class Laikad: if len(pos_fix) == 0: return post_est = pos_fix[0][:3].tolist() - if self.gnss_kf.filter.filter_time is None: + filter_time = self.gnss_kf.filter.filter_time + if filter_time is None: cloudlog.info("Init gnss kalman filter") - elif (self.gnss_kf.filter.filter_time - t) > MAX_TIME_GAP: + elif (t - filter_time) > MAX_TIME_GAP: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") else: cloudlog.error("Gnss kalman filter state is nan") @@ -82,7 +90,7 @@ class Laikad: def localizer_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (filter_time - t) < MAX_TIME_GAP and \ + return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \ all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) def init_gnss_localizer(self, est_pos): @@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement): c = log.GnssMeasurements.CorrectedMeasurement.new_message() c.constellationId = meas.constellation_id.value c.svId = meas.sv_id - observables = meas.observables_final c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0 - c.pseudorange = float(observables['C1C']) + c.pseudorange = float(meas.observables_final['C1C']) c.pseudorangeStd = float(meas.observables_std['C1C']) - c.pseudorangeRate = float(observables['D1C']) + c.pseudorangeRate = float(meas.observables_final['D1C']) c.pseudorangeRateStd = float(meas.observables_std['D1C']) c.satPos = meas.sat_pos_final.tolist() c.satVel = meas.sat_vel.tolist() @@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): def main(): - dog = AstroDog(use_internet=True) sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) - laikad = Laikad() + laikad = Laikad(use_internet=True) while True: sm.update() - # Todo if no internet available use latest ephemeris if sm.updated['ubloxGnss']: ublox_msg = sm['ubloxGnss'] - msg = laikad.process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss']) + msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 3d94399e41..bf984becb8 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -2,7 +2,6 @@ import unittest from datetime import datetime -from laika import AstroDog from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement @@ -18,11 +17,11 @@ def get_log(segs=range(0)): return [m for m in logs if m.which() == 'ubloxGnss'] -def verify_messages(lr, dog, laikad): +def process_msgs(lr, laikad: Laikad): good_msgs = [] for m in lr: - msg = laikad.process_ublox_msg(m.ubloxGnss, dog, m.logMonoTime) - if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: + msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime) + if msg is not None: good_msgs.append(msg) return good_msgs @@ -44,14 +43,31 @@ class TestLaikad(unittest.TestCase): def test_laika_online(self): # Set to offline forces to use ephemeris messages - dog = AstroDog(use_internet=True) - laikad = Laikad() - correct_msgs = verify_messages(self.logs, dog, laikad) - + laikad = Laikad(use_internet=True) + msgs = process_msgs(self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + def test_laika_offline(self): + # Set to offline forces to use ephemeris messages + laikad = Laikad(use_internet=False) + msgs = process_msgs(self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + + self.assertEqual(256, len(correct_msgs)) + self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + + def test_laika_offline_ephem_at_start(self): + # Test offline but process ephemeris msgs of segment first + laikad = Laikad(use_internet=False) + ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris'] + msgs = process_msgs(ephemeris_logs+self.logs, laikad) + correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + self.assertEqual(554, len(correct_msgs)) + self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + if __name__ == "__main__": unittest.main()