sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add ephemeris parsing to laikad (#24642)

Browse Source 
* Always send valid messages

* Use ephemeris and move astrodog in laikad

* improve test

* Always correct measurements

* Cleanup

* Fix test

* Update laika
pull/24652/head
Gijs Koning 3 years ago committed by GitHub
parent 7d06a366f7
commit a51aaaf197
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 53 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      laika_repo
  2. 51
      selfdrive/locationd/laikad.py
  3. 32
      selfdrive/locationd/test/test_laikad.py

2
laika_repo

@ -1 +1 @@
Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd
Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e

51
selfdrive/locationd/laikad.py
Unescape View File

@ -4,8 +4,11 @@ from typing import List
import numpy as np
from collections import defaultdict
from scipy import linalg
from cereal import log, messaging
from laika import AstroDog
from laika.ephemeris import convert_ublox_ephem
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@ -19,28 +22,25 @@ MAX_TIME_GAP = 10
class Laikad:
def __init__(self):
def __init__(self, use_internet):
self.astro_dog = AstroDog(use_internet=use_internet)
self.gnss_kf = GNSSKalman(GENERATED_DIR)
def process_ublox_msg(self, ublox_msg, dog: AstroDog, ublox_mono_time: int):
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int):
if ublox_msg.which == 'measurementReport':
report = ublox_msg.measurementReport
new_meas = read_raw_ublox(report)
measurements = process_measurements(new_meas, dog)
pos_fix = calc_pos_fix(measurements)
measurements = process_measurements(new_meas, self.astro_dog)
pos_fix = calc_pos_fix(measurements, min_measurements=4)
# To get a position fix a minimum of 5 measurements are needed.
# Each report can contain less and some measurement can't be processed.
if len(pos_fix) > 0:
measurements = correct_measurements(measurements, pos_fix[0][:3], dog)
meas_msgs = [create_measurement_msg(m) for m in measurements]
# Each report can contain less and some measurements can't be processed.
corrected_measurements = []
if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100:
corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)
t = ublox_mono_time * 1e-9
self.update_localizer(pos_fix, t, measurements)
self.update_localizer(pos_fix, t, corrected_measurements)
localizer_valid = self.localizer_valid(t)
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
@ -49,6 +49,8 @@ class Laikad:
bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std)
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.GnssMeasurements.Measurement.new_message
dat.gnssMeasurements = {
@ -59,6 +61,11 @@ class Laikad:
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_ephem(ephem, self.astro_dog.orbits)
# elif ublox_msg.which == 'ionoData':
# todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
@ -67,9 +74,10 @@ class Laikad:
if len(pos_fix) == 0:
return
post_est = pos_fix[0][:3].tolist()
if self.gnss_kf.filter.filter_time is None:
filter_time = self.gnss_kf.filter.filter_time
if filter_time is None:
cloudlog.info("Init gnss kalman filter")
elif (self.gnss_kf.filter.filter_time - t) > MAX_TIME_GAP:
elif (t - filter_time) > MAX_TIME_GAP:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
else:
cloudlog.error("Gnss kalman filter state is nan")
@ -82,7 +90,7 @@ class Laikad:
def localizer_valid(self, t: float):
filter_time = self.gnss_kf.filter.filter_time
return filter_time is not None and (filter_time - t) < MAX_TIME_GAP and \
return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \
all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))
def init_gnss_localizer(self, est_pos):
@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement):
c = log.GnssMeasurements.CorrectedMeasurement.new_message()
c.constellationId = meas.constellation_id.value
c.svId = meas.sv_id
observables = meas.observables_final
c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0
c.pseudorange = float(observables['C1C'])
c.pseudorange = float(meas.observables_final['C1C'])
c.pseudorangeStd = float(meas.observables_std['C1C'])
c.pseudorangeRate = float(observables['D1C'])
c.pseudorangeRate = float(meas.observables_final['D1C'])
c.pseudorangeRateStd = float(meas.observables_std['D1C'])
c.satPos = meas.sat_pos_final.tolist()
c.satVel = meas.sat_vel.tolist()
@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std):
def main():
dog = AstroDog(use_internet=True)
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
laikad = Laikad()
laikad = Laikad(use_internet=True)
while True:
sm.update()
# Todo if no internet available use latest ephemeris
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss'])
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'])
if msg is not None:
pm.send('gnssMeasurements', msg)

32
selfdrive/locationd/test/test_laikad.py
Unescape View File

@ -2,7 +2,6 @@
import unittest
from datetime import datetime
from laika import AstroDog
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement
@ -18,11 +17,11 @@ def get_log(segs=range(0)):
return [m for m in logs if m.which() == 'ubloxGnss']
def verify_messages(lr, dog, laikad):
def process_msgs(lr, laikad: Laikad):
good_msgs = []
for m in lr:
msg = laikad.process_ublox_msg(m.ubloxGnss, dog, m.logMonoTime)
if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime)
if msg is not None:
good_msgs.append(msg)
return good_msgs
@ -44,14 +43,31 @@ class TestLaikad(unittest.TestCase):
def test_laika_online(self):
# Set to offline forces to use ephemeris messages
dog = AstroDog(use_internet=True)
laikad = Laikad()
correct_msgs = verify_messages(self.logs, dog, laikad)
laikad = Laikad(use_internet=True)
msgs = process_msgs(self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
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]))
def test_laika_offline(self):
# Set to offline forces to use ephemeris messages
laikad = Laikad(use_internet=False)
msgs = process_msgs(self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
self.assertEqual(256, len(correct_msgs))
self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_laika_offline_ephem_at_start(self):
# Test offline but process ephemeris msgs of segment first
laikad = Laikad(use_internet=False)
ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris']
msgs = process_msgs(ephemeris_logs+self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
self.assertEqual(554, len(correct_msgs))
self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
if __name__ == "__main__":
unittest.main()

Write Preview
Loading…
Cancel
Save