diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index b67c483497..830eb33207 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -19,7 +19,7 @@ from laika.downloader import DownloadFailed from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_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 +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind from selfdrive.locationd.models.gnss_kf import GNSSKalman @@ -36,7 +36,7 @@ POS_FIX_RESIDUAL_THRESHOLD = 100.0 class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), - save_ephemeris=False): + save_ephemeris=False, use_qcom=False): """ valid_const: GNSS constellation which can be used auto_fetch_orbits: If true fetch orbits from internet when needed @@ -45,14 +45,14 @@ class Laikad: save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. """ self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER) - self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True) + self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom) self.auto_fetch_orbits = auto_fetch_orbits self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None - self.got_first_ublox_msg = False + self.got_first_gnss_msg = False self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() @@ -61,6 +61,7 @@ class Laikad: self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None + self.use_qcom = use_qcom def load_cache(self): if not self.save_ephemeris: @@ -105,17 +106,40 @@ class Laikad: cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}") return self.last_pos_fix - def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): - if ublox_msg.which == 'measurementReport': - t = ublox_mono_time * 1e-9 - report = ublox_msg.measurementReport - if report.gpsWeek > 0: - self.got_first_ublox_msg = True - latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) + def is_good_report(self, gnss_msg): + 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: + return True + else: + return False + + def read_report(self, gnss_msg): + if self.use_qcom: + report = gnss_msg.drMeasurementReport + week = report.gpsWeek + tow = report.gpsMilliseconds / 1000.0 + new_meas = read_raw_qcom(report) + else: + report = gnss_msg.measurementReport + week = report.gpsWeek + tow = report.rcvTow + new_meas = read_raw_ublox(report) + return week, tow, new_meas + + 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) + + t = gnss_mono_time * 1e-9 + if week > 0: + self.got_first_gnss_msg = True + latest_msg_t = GPSTime(week, tow) if self.auto_fetch_orbits: self.fetch_orbits(latest_msg_t, block) - new_meas = read_raw_ublox(report) # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7] @@ -123,7 +147,7 @@ class Laikad: est_pos = self.get_est_pos(t, processed_measurements) corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else [] - if ublox_mono_time % 10 == 0: + if gnss_mono_time % 10 == 0: cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}") self.update_localizer(est_pos, t, corrected_measurements) @@ -139,20 +163,21 @@ class Laikad: dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { - "gpsWeek": report.gpsWeek, - "gpsTimeOfWeek": report.rcvTow, + "gpsWeek": week, + "gpsTimeOfWeek": tow, "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), - "ubloxMonoTime": ublox_mono_time, + "ubloxMonoTime": gnss_mono_time, "correctedMeasurements": meas_msgs } return dat - elif ublox_msg.which == 'ephemeris': - ephem = convert_ublox_ephem(ublox_msg.ephemeris) + # 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 ublox_msg.which == 'ionoData': + #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]): @@ -303,24 +328,30 @@ class EphemerisSourceType(IntEnum): def main(sm=None, pm=None): + use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps") + if use_qcom: + raw_gnss_socket = "qcomGnss" + else: + raw_gnss_socket = "ubloxGnss" + if sm is None: - sm = messaging.SubMaster(['ubloxGnss', 'clocks']) + sm = messaging.SubMaster([raw_gnss_socket, 'clocks']) if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) replay = "REPLAY" in os.environ use_internet = "LAIKAD_NO_INTERNET" not in os.environ - laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet) + laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom) while True: sm.update() - if sm.updated['ubloxGnss']: - ublox_msg = sm['ubloxGnss'] - msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'], block=replay) + if sm.updated[raw_gnss_socket]: + gnss_msg = sm[raw_gnss_socket] + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) if msg is not None: pm.send('gnssMeasurements', msg) - if not laikad.got_first_ublox_msg and sm.updated['clocks']: + 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_orbits: diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index c33ff6a497..ffdd649a84 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -443,6 +443,8 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->time_check(t); if (log.isSensorEvents()) { this->handle_sensors(t, log.getSensorEvents()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation()); } else if (log.isGpsLocationExternal()) { this->handle_gps(t, log.getGpsLocationExternal()); } else if (log.isCarState()) { @@ -490,11 +492,17 @@ void Localizer::determine_gps_mode(double current_time) { } int Localizer::locationd_thread() { - const std::initializer_list service_list = {"gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"}; + const char* gps_location_socket; + if (util::file_exists("/persist/comma/use-quectel-rawgps")) { + gps_location_socket = "gpsLocation"; + } else { + gps_location_socket = "gpsLocationExternal"; + } + const std::initializer_list service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"}; PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {"gpsLocationExternal", "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); uint64_t cnt = 0; bool filterInitialized = false; diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py index 5c9cb4b3d1..0d661dc321 100755 --- a/selfdrive/locationd/models/gnss_kf.py +++ b/selfdrive/locationd/models/gnss_kf.py @@ -39,12 +39,6 @@ class GNSSKalman(): 1e14, (100)**2, (0.2)**2, (10)**2, (1)**2]) - # process noise - Q = np.diag([0.03**2, 0.03**2, 0.03**2, - 3**2, 3**2, 3**2, - (.1)**2, (0)**2, (0.005)**2, - .1**2, (.01)**2]) - maha_test_kinds: List[int] = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] @staticmethod @@ -120,7 +114,14 @@ class GNSSKalman(): gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds) - def __init__(self, generated_dir, cython=False): + def __init__(self, generated_dir, cython=False, erratic_clock=False): + # process noise + clock_error_drift = 100.0 if erratic_clock else 0.1 + self.Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 3**2, 3**2, 3**2, + (clock_error_drift)**2, (0)**2, (0.005)**2, + .1**2, (.01)**2]) + self.dim_state = self.x_initial.shape[0] # init filter diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index b1ed8599f1..25bf36d2cb 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -91,22 +91,6 @@ class LocKalman(): 0.05**2, 0.05**2, 0.05**2, 0.01**2, 0.01**2, 0.01**2]) - # process noise - Q = np.diag([0.03**2, 0.03**2, 0.03**2, - 0.0**2, 0.0**2, 0.0**2, - 0.0**2, 0.0**2, 0.0**2, - 0.1**2, 0.1**2, 0.1**2, - (.1)**2, (0.0)**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - (0.02 / 100)**2, - 3**2, 3**2, 3**2, - 0.001**2, - (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2, - (.1)**2, (.01)**2, - 0.005**2, - (0.02 / 100)**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) # measurements that need to pass mahalanobis distance outlier rejector maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] @@ -345,9 +329,29 @@ class LocKalman(): msckf_params = None gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds) - def __init__(self, generated_dir, N=4): + def __init__(self, generated_dir, N=4, erratic_clock=False): name = f"{self.name}_{N}" + + # process noise + clock_error_drift = 100.0 if erratic_clock else 0.1 + self.Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 0.0**2, 0.0**2, 0.0**2, + 0.0**2, 0.0**2, 0.0**2, + 0.1**2, 0.1**2, 0.1**2, + (clock_error_drift)**2, (0)**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.02 / 100)**2, + 3**2, 3**2, 3**2, + 0.001**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2, + (.1)**2, (.01)**2, + 0.005**2, + (0.02 / 100)**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + + self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index bc7a0d7fa4..418625f9bc 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -28,7 +28,7 @@ def get_log(segs=range(0)): def verify_messages(lr, laikad, return_one_success=False): good_msgs = [] for m in lr: - msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True) + msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: good_msgs.append(msg) if return_one_success: @@ -152,7 +152,7 @@ class TestLaikad(unittest.TestCase): self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9))) kf_valid = False for m in self.logs: - laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True) + laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9)) if kf_valid: break @@ -201,7 +201,7 @@ class TestLaikad(unittest.TestCase): laikad = Laikad(auto_update=False) has_orbits = False for m in self.logs: - laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=False) + laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False) if laikad.orbit_fetch_future is not None: laikad.orbit_fetch_future.result() vals = laikad.astro_dog.orbits.values()