diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 928e19081e..8941b50248 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -354,6 +354,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); } + this->last_gps_msg = sensor_time; this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); } @@ -588,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()) { @@ -657,11 +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_locationd.py b/selfdrive/locationd/test/test_locationd.py index 6e65acaaf9..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] @@ -84,9 +95,9 @@ class TestLocationdProc(unittest.TestCase): time.sleep(1) # wait for async params write lastGPS = json.loads(Params().get('LastGPSPosition')) - self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4) - self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4) - self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4) + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) if __name__ == "__main__": 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 ea45fb4957..a173cf5205 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c714ab010923f2bce732bd22e903ccc4454136fd \ No newline at end of file +18a70665bdb6b6aee4a224e826417049415e8290 diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index c625564f1d..dd30b5feff 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -125,7 +125,8 @@ void MapWindow::updateState(const UIState &s) { } } - if (sm.updated("gnssMeasurements")) { + // TODO should check a valid/status flag + if (sm.updated("gnssMeasurements") && sm["gnssMeasurements"].getGnssMeasurements().getGpsWeek() > 0){ auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); auto laikad_pos = laikad_location.getPositionECEF(); auto laikad_pos_ecef = laikad_pos.getValue();