From b64fe6e339c8b8b3f5016a9c58b553c91abc5bdf Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Fri, 6 May 2022 04:41:14 -0700 Subject: [PATCH] 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 --- Pipfile | 1 + Pipfile.lock | 63 ++++++++++++++++++- laika_repo | 2 +- selfdrive/locationd/laikad.py | 84 +++++++++++++++++++++++++ selfdrive/locationd/test/test_laikad.py | 23 +++++++ 5 files changed, 170 insertions(+), 3 deletions(-) create mode 100755 selfdrive/locationd/laikad.py create mode 100755 selfdrive/locationd/test/test_laikad.py diff --git a/Pipfile b/Pipfile index 473b873077..af3e02f54a 100644 --- a/Pipfile +++ b/Pipfile @@ -82,6 +82,7 @@ tqdm = "*" urllib3 = "*" utm = "*" websocket_client = "*" +hatanaka = "*" [requires] python_version = "3.8" diff --git a/Pipfile.lock b/Pipfile.lock index 4398c37d29..5ee3f2b031 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "918c8cba5c6a0242dc0f6ea74246176a1c54f0a9395feddf35af2189cc813378" + "sha256": "19a7b58f24cd7542ccb9fd386c7716d77fff3c1f87de496f3f42753cf34a5dde" }, "pipfile-spec": 6, "requires": { @@ -281,6 +281,16 @@ "index": "pypi", "version": "==20.1.0" }, + "hatanaka": { + "hashes": [ + "sha256:0e095d35ed4f607eb77ae47ecb310e4c25f5a6267037b703ea258ed03e5c47da", + "sha256:84faa953b4f641a6d3cf8187f1775ba7e7f8d815f7bcd48cfb18553b766cbc41", + "sha256:ccf8be554deee2fc70be52bd2f1d3d4dd370001caa74333bf041933d69a19023", + "sha256:ce1628029c6b50c142a8fc5f15453c4cf2a3fd88a7128075415aeb5c9a2727d0" + ], + "index": "pypi", + "version": "==2.8.0" + }, "hexdump": { "hashes": [ "sha256:d781a43b0c16ace3f9366aade73e8ad3a7bd5137d58f0b45ab2d3f54876f20db" @@ -304,12 +314,20 @@ "markers": "python_version < '3.10'", "version": "==4.11.3" }, + "importlib-resources": { + "hashes": [ + "sha256:b6062987dfc51f0fcb809187cffbd60f35df7acb4589091f154214af6d0d49d3", + "sha256:e447dc01619b1e951286f3929be820029d48c75eb25d265c28b92a16548212b8" + ], + "markers": "python_version >= '3.7'", + "version": "==5.7.1" + }, "isort": { "hashes": [ "sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7", "sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951" ], - "markers": "python_version < '4.0' and python_full_version >= '3.6.1'", + "markers": "python_version < '4' and python_full_version >= '3.6.1'", "version": "==5.10.1" }, "itsdangerous": { @@ -449,6 +467,47 @@ ], "version": "==1.2.1" }, + "ncompress": { + "hashes": [ + "sha256:0349d7de11edd70a7efea9ce9dc67f0e47b5774832dd063f7ae68a9e3e36ea31", + "sha256:070044eab19586a45d1855c3e50e000ce86d6075b122a5ec8cffd480713dffac", + "sha256:13fa26ec8000d786a8079bb265508b5df4b445a4f460481a13549b4bd3c83824", + "sha256:15f10fbfa11345ff0af090e3e6ae13a1fe2b52a2bb39d4f2373c2d6aeac75e5d", + "sha256:2a104803fbe3ab0a96edb14927fa22c8142be838aabe7e938b4a52a4e82db56e", + "sha256:34754041d9bac2d6908ae0d07ba541e4d6d606cca222ddd53f3a57e15f386b0a", + "sha256:34c6496168fd4dbc13f1fc0c0fcbadded1957639956f8cbc6894c39999817e36", + "sha256:3590e66313041721ae81e72ece06b7048c9293321bb30900358638673608e264", + "sha256:393cc3c126b9451fb32fe2bc07773264c90e73afbd37da0df472ac23bfd1a2d5", + "sha256:5336a8831a7e587829ce54e9e27d1fb2e04ddbc7d2d983693e35a3a03ac3ce79", + "sha256:5a2ae8a9170fa1f45df7efa292eb8c437b18c225b63d4adca4f50f9da0e8e0c7", + "sha256:6540556d47670a8fb93878a44d0206bbdc87f32e4c5b57d6fe63691efafbb982", + "sha256:66d991155a1655ccd98e8433c4a7e811d63eb649adb55f47d8f9528a30cc4b7a", + "sha256:736dbae078107742cf6ac7ccc11ae9c5eab77ef2c02aab3ef64802877bb01cab", + "sha256:7608fbda43d04d9f476be2dbf4ef3c96e72d83b9557a48b07fbc9ff3ad29cdd2", + "sha256:78674f246878938387b6f82b10d1aa2192e02544d214541943d12ef1a45e66c6", + "sha256:8322482e72ac2802d1dca1007ec06aa281a4d5cf1cf9f8c75bb51e917382b756", + "sha256:8b9acc46cf36bb998ed215d6e76a94e2bd1e827b9a4cb5362982b7004b5a7620", + "sha256:8eb4a55cbeaeb238a3b412952077be6b3f37b3416cd0211cc22776391ff2fef7", + "sha256:916671d62167191af58d6b4a17b1c09c647e349dcff1fc0b7d764aa64c3773ee", + "sha256:94b3f4e851f5b37e1d4cf2d8da911fa10783a59cba3d7f1f2ae5bd2842558077", + "sha256:9cd040ad73a3b0e917e01cdfba507e10e0bb56849daaac3ac3d86382d4d7ad82", + "sha256:9d89acf209858e7940223cf35324e1b2effec119bb009a41f039e2ea4db22177", + "sha256:9da7c81313aed4b6c6e8020442ed8d03d04bff72947f9380ea1ce2c63ffb8ad1", + "sha256:aaa18a509d9fc173b4b47d53c834e43ced1eda63d2aa7d4613dc59b2f802a31a", + "sha256:ab9fc62baaa55faf8ed8ac67f2c64a7295fec91d7c1f306ac46aa894ca4edf91", + "sha256:af0011bae90e44121f4e4edbff3dccdce7e4c5fc5e354db7eb48410d71f496df", + "sha256:b031e06b42037b181e3514261e1e85a9eae4af990c12b9348a9f22b8042201ff", + "sha256:d11df815d280985dfa660974df11dbe051a1a18dca2f91f9d30fbd6548237b8f", + "sha256:d45ec59a8a3ce00613df0c81e5567854574dbbbf01ecd1a5a0929cd8fb04844d", + "sha256:da216a53db7cd4c0247376f87367dd71df457443567e55310f6d3d23a9aff2f2", + "sha256:e0ebd71990ef7909b6627b5341a2fe1977dcce61dd3760a29e19e3f9e4c6a275", + "sha256:e6f5bf381412e9d3847b76e8b6bd1f84dfadcd3d9c25903c8592facb437909a0", + "sha256:e7bbf10cca1376f4f17ae2c447e33a9d4067525abb0c71d488c9a5ced50552f1", + "sha256:f9ba6ab2aadd6fd90365fdad5219e4dc7bc2459b94f1e900a733dddaf4e9b2e6", + "sha256:fe0a671a2f7dc1ee0438d278ef30ab425a969536100c4352b5cb6bc0b6210818" + ], + "version": "==1.0.0" + }, "nose": { "hashes": [ "sha256:9ff7c6cc443f8c51994b34a667bbcf45afd6d945be7477b52e97516fd17c53ac", diff --git a/laika_repo b/laika_repo index 226adc655e..1fbc6780d5 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 226adc655e1488474468a97ab4a7705aad7e5837 +Subproject commit 1fbc6780d5184efd5ccf4518a01fe947ffbb4ba0 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py new file mode 100755 index 0000000000..28150bfea9 --- /dev/null +++ b/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() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py new file mode 100755 index 0000000000..877623f0bd --- /dev/null +++ b/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()