diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ac8fac01f0..b8d86c63fd 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -391,17 +391,13 @@ def process_msg(laikad, gnss_msg, mono_time, block=False): def clear_tmp_cache(): - # TODO: remove this function once laika downloader is changed to not write to disk - if "CI" in os.environ or "REPLAY" in os.environ: - return - if os.path.exists(DOWNLOADS_CACHE_FOLDER): shutil.rmtree(DOWNLOADS_CACHE_FOLDER) os.mkdir(DOWNLOADS_CACHE_FOLDER) def main(sm=None, pm=None, qc=None): - clear_tmp_cache() + #clear_tmp_cache() use_qcom = not Params().get_bool("UbloxAvailable", block=True) if use_qcom or (qc is not None and qc): diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index e78b3d3903..8941b50248 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -589,12 +589,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { this->handle_sensor(t, log.getAccelerometer()); } else if (log.isGyroscope()) { this->handle_sensor(t, log.getGyroscope()); - //} else if (log.isGpsLocation()) { - // this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - //} else if (log.isGpsLocationExternal()) { - // this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - } else if (log.isGnssMeasurements()) { - this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); } else if (log.isCarState()) { this->handle_car_state(t, log.getCarState()); } else if (log.isCameraOdometry()) { @@ -658,12 +658,17 @@ void Localizer::determine_gps_mode(double current_time) { int Localizer::locationd_thread() { ublox_available = Params().getBool("UbloxAvailable", true); - - const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", + const char* gps_location_socket; + if (ublox_available) { + gps_location_socket = "gpsLocationExternal"; + } else { + gps_location_socket = "gpsLocation"; + } + const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"}); + SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 8cd84d91c2..6059eab68f 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -22,26 +22,7 @@ def get_log(segs=range(0)): logs = [] for i in segs: logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i))) - all_logs = [m for m in logs if m.which() == 'ubloxGnss'] - - low_gnss = [] - for m in all_logs: - if m.ubloxGnss.which() != 'measurementReport': - continue - if m.ubloxGnss.measurementReport.numMeas == 0: - continue - - MAX_MEAS = 7 - if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS: - mb = m.as_builder() - mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS - mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS] - mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000 - low_gnss.append(mb.as_reader()) - else: - low_gnss.append(m) - - return all_logs, low_gnss + return [m for m in logs if m.which() == 'ubloxGnss'] def verify_messages(lr, laikad, return_one_success=False): @@ -78,9 +59,8 @@ class TestLaikad(unittest.TestCase): @classmethod def setUpClass(cls): - logs, low_gnss = get_log(range(1)) + logs = get_log(range(1)) cls.logs = logs - cls.low_gnss = low_gnss first_gps_time = get_first_gps_time(logs) cls.first_gps_time = first_gps_time @@ -274,18 +254,6 @@ class TestLaikad(unittest.TestCase): # Verify orbit data is not downloaded mock_method.assert_not_called() - def test_low_gnss_meas(self): - cnt = 0 - laikad = Laikad() - for m in self.low_gnss: - msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) - if msg is None: - continue - gm = msg.gnssMeasurements - if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid: - cnt += 1 - self.assertEqual(cnt, 554) - def dict_has_values(self, dct): self.assertGreater(len(dct), 0) self.assertGreater(min([len(v) for v in dct.values()]), 0) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 943969a49e..9f643e2b8f 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -15,7 +15,7 @@ from selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 - LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -46,14 +46,25 @@ class TestLocationdProc(unittest.TestCase): except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - if name == "gnssMeasurements": - msg.gnssMeasurements.measTime = t - msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] - msg.gnssMeasurements.positionECEF.std = [0,0,0] - msg.gnssMeasurements.positionECEF.valid = True - msg.gnssMeasurements.velocityECEF.value = [] - msg.gnssMeasurements.velocityECEF.std = [0,0,0] - msg.gnssMeasurements.velocityECEF.valid = True + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 24ee87e471..22ec099285 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -331,7 +331,7 @@ CONFIGS = [ pub_sub={ "cameraOdometry": ["liveLocationKalman"], "accelerometer": [], "gyroscope": [], - "gnssMeasurements": [], "liveCalibration": [], "carState": [], + "gpsLocationExternal": [], "liveCalibration": [], "carState": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 629a9a2f51..1aa3aa6f1e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -660149849230afe36363100ab0f5935038d8191e \ No newline at end of file +acc5655633af6ade096ad41f680fd8a28c2e3790 \ No newline at end of file