Laikad: the basics for ublox msg processing (#24359)

* Add laikad that receives ublox messages and publishes corrected measurements and position fix

* types

* cleanup

* laikad version 1 with uncorrected measurements

* push

* Fix glonass frequency and delete redundant test

* Update after cereal and cleanup

* Add test, fix laikad and remove process replay for now

* update laika

* add hatanaka to packages. Used to decompress orbit data

* Fix pip
old-commit-hash: b64fe6e339

Pipfile

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2f5ed232c10786e92e0931a84a0db2dbac814e7c5aafac6707fd923ac04fb5c6
+oid sha256:baa377b5010815c799bb0b9448e2d126a0c54108ee490f6c98b3e99dd1a5b471
-size 1458
+size 1473

Pipfile.lock

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:b34176ef828177a07f5e899fe8f3f5da46a88b8fce0a5a0eb657c4566fef7cc9
+oid sha256:3d3e2c17d9d4779a9f40cd53a2bd971bee199149de976a577df797fb25e0535a
-size 143553
+size 147776

laika_repo

@@ -1 +1 @@
-Subproject commit 226adc655e1488474468a97ab4a7705aad7e5837
+Subproject commit 1fbc6780d5184efd5ccf4518a01fe947ffbb4ba0

selfdrive/locationd/laikad.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+from typing import List
+
+from cereal import log, messaging
+from laika import AstroDog
+from laika.helpers import UbloxGnssId
+from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox
+
+
+def correct_and_pos_fix(processed_measurements: List[GNSSMeasurement], dog: AstroDog):
+  # pos fix needs more than 5 processed_measurements
+  pos_fix = calc_pos_fix(processed_measurements)
+
+  if len(pos_fix) == 0:
+    return [], []
+  est_pos = pos_fix[0][:3]
+  corrected = correct_measurements(processed_measurements, est_pos, dog)
+  return calc_pos_fix(corrected), corrected
+
+
+def process_ublox_msg(ublox_msg, dog, ublox_mono_time: int):
+  if ublox_msg.which == 'measurementReport':
+    report = ublox_msg.measurementReport
+    if len(report.measurements) == 0:
+      return None
+    new_meas = read_raw_ublox(report)
+    processed_measurements = process_measurements(new_meas, dog)
+
+    corrected = correct_and_pos_fix(processed_measurements, dog)
+    pos_fix, _ = corrected
+    # todo send corrected messages instead of processed_measurements. Need fix for when having less than 6 measurements
+    correct_meas_msgs = [create_measurement_msg(m) for m in processed_measurements]
+    # pos fix can be an empty list if not enough correct measurements are available
+    if len(pos_fix) > 0:
+      corrected_pos = pos_fix[0][:3].tolist()
+    else:
+      corrected_pos = [0., 0., 0.]
+    dat = messaging.new_message('gnssMeasurements')
+    dat.gnssMeasurements = {
+      "position": corrected_pos,
+      "ubloxMonoTime": ublox_mono_time,
+      "correctedMeasurements": correct_meas_msgs
+    }
+    return dat
+
+
+def create_measurement_msg(meas: GNSSMeasurement):
+  c = log.GnssMeasurements.CorrectedMeasurement.new_message()
+  ublox_gnss_id = meas.ublox_gnss_id
+  if ublox_gnss_id is None:
+    # todo never happens will fix in later pr
+    ublox_gnss_id = UbloxGnssId.GPS
+  c.constellationId = ublox_gnss_id.value
+  c.svId = int(meas.prn[1:])
+  c.glonassFrequency = meas.glonass_freq if meas.ublox_gnss_id == UbloxGnssId.GLONASS else 0
+  c.pseudorange = float(meas.observables['C1C'])  # todo should be observables_final when using corrected measurements
+  c.pseudorangeStd = float(meas.observables_std['C1C'])
+  c.pseudorangeRate = float(meas.observables['D1C'])  # todo should be observables_final when using corrected measurements
+  c.pseudorangeRateStd = float(meas.observables_std['D1C'])
+  c.satPos = meas.sat_pos_final.tolist()
+  c.satVel = meas.sat_vel.tolist()
+  return c
+
+
+def main():
+  dog = AstroDog()
+  sm = messaging.SubMaster(['ubloxGnss'])
+  pm = messaging.PubMaster(['gnssMeasurements'])
+
+  while True:
+    sm.update()
+
+    # Todo if no internet available use latest ephemeris
+    if sm.updated['ubloxGnss']:
+      ublox_msg = sm['ubloxGnss']
+      msg = process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss'])
+      if msg is None:
+        msg = messaging.new_message('gnssMeasurements')
+      pm.send('gnssMeasurements', msg)
+
+
+
+if __name__ == "__main__":
+  main()

selfdrive/locationd/test/test_laikad.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python3
+import unittest
+from datetime import datetime
+
+from laika.helpers import UbloxGnssId
+
+from laika.gps_time import GPSTime
+from laika.raw_gnss import GNSSMeasurement
+from selfdrive.locationd.laikad import create_measurement_msg
+
+
+class TestLaikad(unittest.TestCase):
+
+  def test_create_msg_without_errors(self):
+    gpstime = GPSTime.from_datetime(datetime.now())
+    meas = GNSSMeasurement('G01', gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}, ublox_gnss_id=UbloxGnssId.GPS)
+    msg = create_measurement_msg(meas)
+
+    self.assertEqual(msg.constellationId, 'gps')
+
+
+if __name__ == "__main__":
+  unittest.main()