Laika cleaner flow (#27633)

* laika flow

* test fixes

* unexpected type

* unexpected type

* explicit type

* ref commit

* back to prev val

* ref 2
old-commit-hash: e05e7850e1

selfdrive/locationd/laikad.py

@@ -7,7 +7,7 @@ from collections import defaultdict
 from concurrent.futures import Future, ProcessPoolExecutor
 from datetime import datetime
 from enum import IntEnum
-from typing import List, Optional
+from typing import List, Optional, Dict, Any
 
 import numpy as np
 
@@ -241,12 +241,14 @@ class Laikad:
     new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
     processed_measurements = process_measurements(new_meas, self.astro_dog)
     if self.last_fix_pos is not None:
-      corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog)
+      est_pos = self.last_fix_pos
-      instant_fix = self.get_lsq_fix(t, corrected_measurements)
-      #instant_fix = self.get_lsq_fix(t, processed_measurements)
     else:
-      corrected_measurements = []
+      est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
-      instant_fix = self.get_lsq_fix(t, processed_measurements)
+    corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
+    return corrected_measurements
+
+  def calc_fix(self, t, measurements):
+    instant_fix = self.get_lsq_fix(t, measurements)
     if instant_fix is None:
       return None
     else:
@@ -254,64 +256,52 @@ class Laikad:
       self.last_fix_t = t
       self.last_fix_pos = position_estimate
       self.lat_fix_pos_std = position_std
-    if (t*1e9) % 10 == 0:
+      return position_estimate, position_std, velocity_estimate, velocity_std
-      cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
-    return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
 
   def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
     out_msg = messaging.new_message("gnssMeasurements")
-    out_msg.gnssMeasurements = {
+    t = gnss_mono_time * 1e-9
-        "timeToFirstFix": self.ttff,
+    msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time}
-        "ephemerisStatuses": self.create_ephem_statuses(),
-    }
     if self.first_log_time is None:
       self.first_log_time = 1e-9 * gnss_mono_time
     if self.is_ephemeris(gnss_msg):
       self.read_ephemeris(gnss_msg)
-      return out_msg
     elif self.is_good_report(gnss_msg):
       week, tow, new_meas = self.read_report(gnss_msg)
       self.gps_week = week
-      if len(new_meas) == 0:
-        return out_msg
-
-      t = gnss_mono_time * 1e-9
       if week > 0:
         self.got_first_gnss_msg = True
         latest_msg_t = GPSTime(week, tow)
         if self.auto_fetch_navs:
           self.fetch_navs(latest_msg_t, block)
 
-      output = self.process_report(new_meas, t)
+      corrected_measurements = self.process_report(new_meas, t)
-      if output is None:
+      msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
-        return out_msg
-      if self.ttff <= 0:
-        self.ttff = max(1e-3, t - self.first_log_time)
-      position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
 
-      self.update_localizer(position_estimate, t, corrected_measurements)
+      fix = self.calc_fix(t, corrected_measurements)
-      meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
       measurement_msg = log.LiveLocationKalman.Measurement.new_message
-
+      if fix is not None:
+        position_estimate, position_std, velocity_estimate, velocity_std = fix
+        if self.ttff <= 0:
+          self.ttff = max(1e-3, t - self.first_log_time)
+        msg_dict["positionECEF"] = measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t))
+        msg_dict["velocityECEF"] = measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t))
+
+      self.update_localizer(self.last_fix_pos, t, corrected_measurements)
       P_diag = self.gnss_kf.P.diagonal()
       kf_valid = all(self.kf_valid(t))
-      out_msg.gnssMeasurements = {
+      msg_dict["kalmanPositionECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
-        "gpsWeek": week,
-        "gpsTimeOfWeek": tow,
-        "kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
                                         std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(),
-                                        valid=kf_valid),
+                                        valid=kf_valid)
-        "kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
+      msg_dict["kalmanVelocityECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
                                         std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(),
-                                        valid=kf_valid),
+                                        valid=kf_valid)
-        "positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)),
+
-        "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)),
+    msg_dict['gpsWeek'] = self.last_report_time.week
-
+    msg_dict['gpsTimeOfWeek'] = self.last_report_time.tow
-        "measTime": gnss_mono_time,
+    msg_dict['timeToFirstFix'] = self.ttff
-        "correctedMeasurements": meas_msgs,
+    msg_dict['ephemerisStatuses'] = self.create_ephem_statuses()
-        "timeToFirstFix": self.ttff,
+    out_msg.gnssMeasurements = msg_dict
-        "ephemerisStatuses": self.create_ephem_statuses(),
-      }
     return out_msg
 
   def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
@@ -324,7 +314,7 @@ class Laikad:
         cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
       elif not valid[2]:
         cloudlog.error("Gnss kalman filter state is nan")
-      if len(est_pos) > 0:
+      if est_pos is not None and len(est_pos) > 0:
         cloudlog.info(f"Reset kalman filter with {est_pos}")
         self.init_gnss_localizer(est_pos)
       else:

selfdrive/locationd/test/test_laikad.py

@@ -180,7 +180,7 @@ class TestLaikad(unittest.TestCase):
     laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
     correct_msgs = verify_messages(self.logs, laikad)
 
-    correct_msgs_expected = 559
+    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]))
 
@@ -201,9 +201,11 @@ 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 = 42 if use_qcom else 559
+      correct_msgs_expected = 44 if use_qcom else 560
+      valid_fix_expected = 43 if use_qcom else 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]))
+      self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
   @mock.patch('laika.downloader.download_and_cache_file')
   def test_laika_offline(self, downloader_mock):
@@ -216,8 +218,9 @@ class TestLaikad(unittest.TestCase):
     downloader_mock.side_effect = DownloadFailed
     laikad = Laikad(auto_update=False)
     correct_msgs = verify_messages(self.logs, laikad)
-    self.assertEqual(375, len(correct_msgs))
+    expected_msgs = 376
-    self.assertEqual(375, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
+    self.assertEqual(expected_msgs, len(correct_msgs))
+    self.assertEqual(expected_msgs, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
   def test_laika_get_orbits(self):
     laikad = Laikad(auto_update=False)
@@ -325,7 +328,7 @@ class TestLaikad(unittest.TestCase):
       gm = msg.gnssMeasurements
       if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid:
         cnt += 1
-    self.assertEqual(cnt, 559)
+    self.assertEqual(cnt, 560)
 
   def dict_has_values(self, dct):
     self.assertGreater(len(dct), 0)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-a4ee2bfc9a9518a23a4f0fac4df124d1f12deb79
+50f1e873095fe2462d2aadb9c401bda76759c01c