laikad: use non-conflating socket for gnss msgs (#24888)

* Use subsock

* wait for one

* wrong name

* try cpp proc replay

* fix recv pattern

---------

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: fc5a575ba3
beeps
Gijs Koning 2 years ago committed by GitHub
parent cf52038858
commit 5814ecfbfe
  1. 18
      selfdrive/locationd/laikad.py

@ -425,12 +425,13 @@ def main(sm=None, pm=None):
use_qcom = not Params().get_bool("UbloxAvailable", block=True)
if use_qcom:
raw_gnss_socket = "qcomGnss"
raw_name = "qcomGnss"
else:
raw_gnss_socket = "ubloxGnss"
raw_name = "ubloxGnss"
raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False, timeout=1000)
if sm is None:
sm = messaging.SubMaster([raw_gnss_socket, 'clocks'])
sm = messaging.SubMaster(['clocks',])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])
@ -444,18 +445,17 @@ def main(sm=None, pm=None):
laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
while True:
sm.update()
if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket]
msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
pm.send('gnssMeasurements', msg)
for in_msg in messaging.drain_sock(raw_gnss_sock):
out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay)
pm.send('gnssMeasurements', out_msg)
sm.update(0)
if not laikad.got_first_gnss_msg and sm.updated['clocks']:
clocks_msg = sm['clocks']
t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
if laikad.auto_fetch_navs:
laikad.fetch_navs(t, block=replay)
if __name__ == "__main__":
main()

Loading…
Cancel
Save