init

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,

selfdrive/locationd/locationd.cc

@@ -84,6 +84,8 @@ Localizer::Localizer() {
   this->converter = std::make_unique<LocalCoord>((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();
@@ -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()) {
@@ -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<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", 
+  const std::initializer_list<const char *> 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;

selfdrive/locationd/locationd.h

@@ -24,6 +24,7 @@
 class Localizer {
 public:
   Localizer();
+  Localizer(bool has_ublox);
 
   int locationd_thread();
 

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}]')

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__":