diff --git a/cereal b/cereal index 5ba96b6ded..b29717c4c3 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5ba96b6ded57bcd91e60140ce0036f61370f8512 +Subproject commit b29717c4c328d5cf34d46f682f25267150f82637 diff --git a/laika_repo b/laika_repo index c8bc1fa01b..e1049cde0a 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 +Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index c4fdfb9871..2769f394c5 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -61,6 +61,7 @@ class Laikad: self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None + self.gps_week = None self.use_qcom = use_qcom def load_cache(self): @@ -107,11 +108,11 @@ class Laikad: return self.last_pos_fix def is_good_report(self, gnss_msg): - if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: + if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) # TODO support GLONASS return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] - elif gnss_msg.which == 'measurementReport' and not self.use_qcom: + elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: return True else: return False @@ -129,9 +130,28 @@ class Laikad: new_meas = read_raw_ublox(report) return week, tow, new_meas + def is_ephemeris(self, gnss_msg): + if self.use_qcom: + return gnss_msg.which() == 'drSvPoly' + else: + return gnss_msg.which() == 'ephemeris' + + def read_ephemeris(self, gnss_msg): + # TODO this only works on GLONASS + if self.use_qcom: + # TODO this is not robust to gps week rollover + if self.gps_week is None: + return + ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + else: + ephem = convert_ublox_ephem(gnss_msg.ephemeris) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) + self.gps_week = week t = gnss_mono_time * 1e-9 if week > 0: @@ -172,12 +192,10 @@ class Laikad: "correctedMeasurements": meas_msgs } return dat - # TODO this only works on GLONASS, qcom needs live ephemeris parsing too - elif gnss_msg.which == 'ephemeris': - ephem = convert_ublox_ephem(gnss_msg.ephemeris) - self.astro_dog.add_navs({ephem.prn: [ephem]}) - self.cache_ephemeris(t=ephem.epoch) - #elif gnss_msg.which == 'ionoData': + elif self.is_ephemeris(gnss_msg): + self.read_ephemeris(gnss_msg) + + #elif gnss_msg.which() == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): @@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement): c.satVel = meas.sat_vel.tolist() ephem = meas.sat_ephemeris assert ephem is not None + week, time_of_week = -1, -1 if ephem.eph_type == EphemerisType.NAV: source_type = EphemerisSourceType.nav - week, time_of_week = -1, -1 + elif ephem.eph_type == EphemerisType.QCOM_POLY: + source_type = EphemerisSourceType.qcom else: assert ephem.file_epoch is not None week = ephem.file_epoch.week @@ -325,6 +345,7 @@ class EphemerisSourceType(IntEnum): nav = 0 nasaUltraRapid = 1 glonassIacUltraRapid = 2 + qcom = 3 def main(sm=None, pm=None): @@ -348,6 +369,17 @@ def main(sm=None, pm=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] + + # TODO: Understand and use remaining unknown constellations + if gnss_msg.which() == "drMeasurementReport": + if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: + continue + + if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: + # gpsWeek 65535 is received rarely from quectel, this cannot be + # passed to GnssMeasurements's gpsWeek (Int16) + continue + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index c430acc5e5..c68e7aff99 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -14,12 +14,13 @@ import cereal.messaging as messaging from laika.gps_time import GPSTime from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv -from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, +from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, glonass_measurement_report, glonass_measurement_report_sv, - oemdre_measurement_report, oemdre_measurement_report_sv, + oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report, LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT) + LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT) DEBUG = int(os.getenv("DEBUG", "0"))==1 @@ -28,6 +29,7 @@ LOG_TYPES = [ LOG_GNSS_GLONASS_MEASUREMENT_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, LOG_GNSS_POSITION_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT, ] @@ -146,6 +148,9 @@ def main() -> NoReturn: unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True) + unpack_position, _ = dict_unpacker(position_report) + unpack_position, _ = dict_unpacker(position_report) # wait for ModemManager to come up @@ -258,7 +263,24 @@ def main() -> NoReturn: pm.send('gpsLocation', msg) - if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: + elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: + msg = messaging.new_message('qcomGnss') + dat = unpack_svpoly(log_payload) + dat = relist(dat) + gnss = msg.qcomGnss + gnss.logTs = log_time + gnss.init('drSvPoly') + poly = gnss.drSvPoly + for k,v in dat.items(): + if k == "version": + assert v == 2 + elif k == "flags": + pass + else: + setattr(poly, k, v) + pm.send('qcomGnss', msg) + + elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py index 4bc9eca875..97e3d3d605 100644 --- a/selfdrive/sensord/rawgps/structs.py +++ b/selfdrive/sensord/rawgps/structs.py @@ -56,6 +56,29 @@ oemdre_measurement_report = """ uint8_t source; """ +oemdre_svpoly_report = """ + uint8_t version; + uint16_t sv_id; + int8_t frequency_index; + uint8_t flags; + uint16_t iode; + double t0; + double xyz0[3]; + double xyzN[9]; + float other[4]; + float position_uncertainty; + float iono_delay; + float iono_dot; + float sbas_iono_delay; + float sbas_iono_dot; + float tropo_delay; + float elevation; + float elevation_dot; + float elevation_uncertainty; + double velocity_coeff[12]; +""" + + oemdre_measurement_report_sv = """ uint8_t sv_id; uint8_t unkn; @@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False): nams = [name_to_camelcase(x) for x in nams] sz = calcsize(st) return lambda x: dict(zip(nams, unpack_from(st, x))), sz + +def relist(dat): + list_keys = set() + for key in dat.keys(): + if '[' in key: + list_keys.add(key.split('[')[0]) + list_dict = {} + for list_key in list_keys: + list_dict[list_key] = [] + i = 0 + while True: + key = list_key + f'[{i}]' + if key not in dat: + break + list_dict[list_key].append(dat[key]) + del dat[key] + i += 1 + return {**dat, **list_dict}