diff --git a/Pipfile b/Pipfile index 152e3bfb40..e3823fb061 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2f5ed232c10786e92e0931a84a0db2dbac814e7c5aafac6707fd923ac04fb5c6 -size 1458 +oid sha256:baa377b5010815c799bb0b9448e2d126a0c54108ee490f6c98b3e99dd1a5b471 +size 1473 diff --git a/Pipfile.lock b/Pipfile.lock index fd5446638e..594a1ea314 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b34176ef828177a07f5e899fe8f3f5da46a88b8fce0a5a0eb657c4566fef7cc9 -size 143553 +oid sha256:3d3e2c17d9d4779a9f40cd53a2bd971bee199149de976a577df797fb25e0535a +size 147776 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()