diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index da57fb7ff4..32fec7724a 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -3,8 +3,8 @@ extern "C" { typedef Localizer* Localizer_t; - Localizer *localizer_init() { - return new Localizer(); + Localizer *localizer_init(bool has_ublox) { + return new Localizer(has_ublox); } void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 442adcd347..2bc67b918c 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -84,6 +84,8 @@ Localizer::Localizer() { this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); } +Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; } + void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd predicted_state = this->kf->get_x(); MatrixXdr predicted_cov = this->kf->get_P(); @@ -227,7 +229,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData } double sensor_time = 1e-9 * log.getTimestamp(); - + // sensor time and log time should be close if (std::abs(current_time - sensor_time) > 0.1) { LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); @@ -437,7 +439,7 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - + if (!this->is_timestamp_valid(current_time)) { this->observation_timings_invalid = true; return; @@ -574,12 +576,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()) { @@ -616,7 +618,7 @@ bool Localizer::critical_services_valid(std::map critical_s return true; } -bool Localizer::is_timestamp_valid(double current_time) { +bool Localizer::is_timestamp_valid(double current_time) { double filter_time = this->kf->get_filter_time(); if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { LOGE("Observation timestamp is older than the max rewind threshold of the filter"); @@ -643,19 +645,12 @@ void Localizer::determine_gps_mode(double current_time) { int Localizer::locationd_thread() { ublox_available = Params().getBool("UbloxAvailable", true); - - 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", + const std::initializer_list service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; - PubMaster pm({"liveLocationKalman"}); // TODO: remove carParams once we're always sending at 100Hz - SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); + SubMaster sm(service_list, {}, nullptr, {"carParams"}); + PubMaster pm({"liveLocationKalman"}); uint64_t cnt = 0; bool filterInitialized = false; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 7a282be9d7..1421c456c9 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -24,6 +24,7 @@ class Localizer { public: Localizer(); + Localizer(bool has_ublox); int locationd_thread(); diff --git a/selfdrive/locationd/test/_test_locationd_lib.py b/selfdrive/locationd/test/_test_locationd_lib.py index c4a053bbc6..819bb1570e 100755 --- a/selfdrive/locationd/test/_test_locationd_lib.py +++ b/selfdrive/locationd/test/_test_locationd_lib.py @@ -19,7 +19,7 @@ LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../ class TestLocationdLib(unittest.TestCase): def setUp(self): header = '''typedef ...* Localizer_t; -Localizer_t localizer_init(); +Localizer_t localizer_init(bool has_ublox); void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' @@ -27,7 +27,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t self.ffi.cdef(header) self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) - self.localizer = self.lib.localizer_init() + self.localizer = self.lib.localizer_init(True) # default to ublox self.buff_size = 2048 self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 7569530211..6e65acaaf9 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -8,13 +8,14 @@ import capnp import cereal.messaging as messaging from cereal.services import service_list from common.params import Params +from common.transformations.coordinates import ecef2geodetic from selfdrive.manager.process_config import managed_processes class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 - LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration', 'accelerometer', 'gyroscope', 'magnetometer'] def setUp(self): @@ -45,16 +46,14 @@ class TestLocationdProc(unittest.TestCase): except capnp.lib.capnp.KjException: msg = messaging.new_message(name, 0) - 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 = self.lat - msg.gpsLocationExternal.longitude = self.lon - msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 - msg.gpsLocationExternal.altitude = 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] @@ -67,9 +66,11 @@ class TestLocationdProc(unittest.TestCase): # first reset params Params().remove('LastGPSPosition') - self.lat = 30 + (random.random() * 10.0) - self.lon = -70 + (random.random() * 10.0) - self.alt = 5 + (random.random() * 10.0) + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + self.fake_duration = 90 # secs # get fake messages at the correct frequency, listed in services.py fake_msgs = [] @@ -83,10 +84,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=3) - self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) - self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4) if __name__ == "__main__":