Revert locationd to gpsLocation (#26963)

* revert locationd laika change

* switch msg

* change proc replay

* back to gpslocation

* update ref

* no np floats in msg

* Ignore empty laikad messages in mapos

* more tolerance

* c++ doesnt work like that
old-commit-hash: 4c33d94d3f088c6f1fc88a0f40f3a828968c1c52

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<const char *> 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<const char *> 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;

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

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,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c714ab010923f2bce732bd22e903ccc4454136fd
+18a70665bdb6b6aee4a224e826417049415e8290

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();