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 pippull/24404/head^2
Gijs Koning
3 years ago
committed by
GitHub
parent
df8f024e19
commit
b64fe6e339
5 changed files with 170 additions and 3 deletions
@ -1 +1 @@ |
|||||||
Subproject commit 226adc655e1488474468a97ab4a7705aad7e5837 |
Subproject commit 1fbc6780d5184efd5ccf4518a01fe947ffbb4ba0 |
||||||
@ -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() |
||||||
@ -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() |
||||||
Loading…
Reference in new issue