Add ephemeris parsing to laikad (#24642)

* Always send valid messages

* Use ephemeris and move astrodog in laikad

* improve test

* Always correct measurements

* Cleanup

* Fix test

* Update laika

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)
 

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()