Laikad: set active (#26850)

* laikad update, renaming

* update locationd

* fix naming

* address PR comments

* upsi

* .

* draft to fix replay

* fix process relay to allow no response for messages

* final fix for process replay

* .

* bump cereal

* update process replay ref commit

* reduce wait time

* .

* last ref change

* move laikad helpers to laika

* .

* fix ublox test

* update refs

* add proper qcom replay support

* fix gnss support if both is available

* update refs

* remove left over

* revert laikad msg

* move laika back to master

* init

* fix gps valid flag

* change time

* add gnss to ignore

* remove gps_valid flag

* .

* adopt orientation reset threshold

* .

* update laikad

* .

* fix stanstill KF resets

* test orienation reset count

* update laika

* bump cereal

* fix process replay

* update laika repo

* remove handle gps

* add extra logging for cache

* .

* add more log

* .

* .

* update laika

* dont remove gps code

* inc min satellite count

* update magic vals and add acc drop

* update laika

* upsi

* rem

* bump laika

* use nav and correct

* more fixes

* use sftp

* No more glonass

* Revert "No more glonass"

This reverts commit a76124da50a1e25f423ad1137c7a046e1d57811d.

* nump laika

* back support old ephemeris cache

* add health to ephemeris message

* bump laika

* remove print

* fix laikad tests

* clean

* remove extra log

* bump laika

* inc timeout for plotjuggler build

* rem cache clear

* .

* enable gps after checks

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>
Co-authored-by: Shane Smiskol <shane@smiskol.com>
old-commit-hash: 88423e25df8bfbd8e6a3275eb086f468cf9a59f1

.github/workflows/tools_tests.yaml

@@ -38,7 +38,7 @@ jobs:
     - name: Build Docker image
       run: eval "$BUILD"
     - name: Unit test
-      timeout-minutes: 2
+      timeout-minutes: 6
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --directory=/tmp/openpilot/cereal && \
                         apt-get update && \

selfdrive/locationd/laikad.py

@@ -3,6 +3,7 @@ import json
 import math
 import os
 import time
+import shutil
 from collections import defaultdict
 from concurrent.futures import Future, ProcessPoolExecutor
 from datetime import datetime
@@ -29,17 +30,17 @@ from system.swaglog import cloudlog
 MAX_TIME_GAP = 10
 EPHEMERIS_CACHE = 'LaikadEphemeris'
 DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/"
-CACHE_VERSION = 0.1
+CACHE_VERSION = 0.2
 POS_FIX_RESIDUAL_THRESHOLD = 100.0
 
 
 class Laikad:
-  def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False,
-               valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV),
+  def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False,
+               valid_ephem_types=(EphemerisType.NAV,),
                save_ephemeris=False, use_qcom=False):
     """
     valid_const: GNSS constellation which can be used
-    auto_fetch_orbits: If true fetch orbits from internet when needed
+    auto_fetch_navs: If true fetch navs from internet when needed
     auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere.
     valid_ephem_types: Valid ephemeris types to be used by AstroDog
     save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
@@ -47,11 +48,11 @@ class Laikad:
     self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER)
     self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
 
-    self.auto_fetch_orbits = auto_fetch_orbits
+    self.auto_fetch_navs = auto_fetch_navs
     self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
     self.orbit_fetch_future: Optional[Future] = None
 
-    self.last_fetch_orbits_t = None
+    self.last_fetch_navs_t = None
     self.got_first_gnss_msg = False
     self.last_cached_t = None
     self.save_ephemeris = save_ephemeris
@@ -74,29 +75,30 @@ class Laikad:
 
     try:
       cache = json.loads(cache, object_hook=deserialize_hook)
-      self.astro_dog.add_orbits(cache['orbits'])
-      self.astro_dog.add_navs(cache['nav'])
-      self.last_fetch_orbits_t = cache['last_fetch_orbits_t']
+      if cache['version'] == CACHE_VERSION:
+        self.astro_dog.add_navs(cache['navs'])
+        self.last_fetch_navs_t = cache['last_fetch_navs_t']
+      else:
+        cache['navs'] = {}
     except json.decoder.JSONDecodeError:
       cloudlog.exception("Error parsing cache")
-    timestamp = self.last_fetch_orbits_t.as_datetime() if self.last_fetch_orbits_t is not None else 'Nan'
+    timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan'
     cloudlog.debug(
-      f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and orbits ({sum([len(v) for v in cache['orbits']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['orbits'].keys())} {list(cache['nav'].keys())} " +
-      f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.orbit_fetched_times._ranges]}")
+      f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " +
+      f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}")
 
   def cache_ephemeris(self, t: GPSTime):
     if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
       put_nonblocking(EPHEMERIS_CACHE, json.dumps(
-        {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav},
+        {'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs},
         cls=CacheSerializer))
       cloudlog.debug("Cache saved")
       self.last_cached_t = t
 
   def get_lsq_fix(self, t, measurements):
     if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
-      min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 5
+      min_measurements = 7 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 6
       position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
-
       if len(position_solution) < 3:
         return None
       position_estimate = position_solution[:3]
@@ -155,8 +157,13 @@ class Laikad:
     # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
     new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
     processed_measurements = process_measurements(new_meas, self.astro_dog)
-
-    instant_fix = self.get_lsq_fix(t, processed_measurements)
+    if self.last_fix_pos is not None:
+      corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog)
+      instant_fix = self.get_lsq_fix(t, corrected_measurements)
+      #instant_fix = self.get_lsq_fix(t, processed_measurements)
+    else:
+      corrected_measurements = []
+      instant_fix = self.get_lsq_fix(t, processed_measurements)
     if instant_fix is None:
       return None
     else:
@@ -164,8 +171,6 @@ class Laikad:
       self.last_fix_t = t
       self.last_fix_pos = position_estimate
       self.lat_fix_pos_std = position_std
-
-    corrected_measurements = correct_measurements(processed_measurements, position_estimate, self.astro_dog)
     if (t*1e9) % 10 == 0:
       cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
     return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
@@ -185,8 +190,8 @@ class Laikad:
       if week > 0:
         self.got_first_gnss_msg = True
         latest_msg_t = GPSTime(week, tow)
-        if self.auto_fetch_orbits:
-          self.fetch_orbits(latest_msg_t, block)
+        if self.auto_fetch_navs:
+          self.fetch_navs(latest_msg_t, block)
 
       output = self.process_report(new_meas, t)
       if output is None:
@@ -254,9 +259,9 @@ class Laikad:
     p_initial_diag[GStates.ECEF_POS] = 1000 ** 2
     self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)
 
-  def fetch_orbits(self, t: GPSTime, block):
-    # Download new orbits if 1 hour of orbits data left
-    if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or abs(t - self.last_fetch_orbits_t) > SECS_IN_MIN):
+  def fetch_navs(self, t: GPSTime, block):
+    # Download new navs if 1 hour of navs data left
+    if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
       astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
       ret = None
 
@@ -271,22 +276,22 @@ class Laikad:
 
       if ret is not None:
         if ret[0] is None:
-          self.last_fetch_orbits_t = ret[2]
+          self.last_fetch_navs_t = ret[2]
         else:
-          self.astro_dog.orbits, self.astro_dog.orbit_fetched_times, self.last_fetch_orbits_t = ret
+          self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret
           self.cache_ephemeris(t=t)
 
 
 def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
   astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir)
-  cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
+  cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}")
   start_time = time.monotonic()
   try:
-    astro_dog.get_orbit_data(t, only_predictions=True)
-    cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
-    cloudlog.debug(f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}" +
+    astro_dog.get_navs(t)
+    cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s")
+    cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" +
                    f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}")
-    return astro_dog.orbits, astro_dog.orbit_fetched_times, t
+    return astro_dog.navs, astro_dog.navs_fetched_times, t
   except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
     cloudlog.warning(f"No orbit data found or parsing failure: {e}")
   return None, None, t
@@ -385,7 +390,15 @@ def process_msg(laikad, gnss_msg, mono_time, block=False):
   return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
 
 
+def clear_tmp_cache():
+  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()
+
   use_qcom = not Params().get_bool("UbloxAvailable", block=True)
   if use_qcom or (qc is not None and qc):
     raw_gnss_socket = "qcomGnss"
@@ -399,7 +412,7 @@ def main(sm=None, pm=None, qc=None):
 
   replay = "REPLAY" in os.environ
   use_internet = "LAIKAD_NO_INTERNET" not in os.environ
-  laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom)
+  laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
 
   while True:
     sm.update()
@@ -409,17 +422,15 @@ def main(sm=None, pm=None, qc=None):
 
       msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
       if msg is None:
+        # TODO: beautify this, locationd needs a valid message
         msg = messaging.new_message("gnssMeasurements")
-        msg.valid = False
-
       pm.send('gnssMeasurements', msg)
 
     if not laikad.got_first_gnss_msg and sm.updated['clocks']:
       clocks_msg = sm['clocks']
       t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
-      if laikad.auto_fetch_orbits:
-        laikad.fetch_orbits(t, block=replay)
-
+      if laikad.auto_fetch_navs:
+        laikad.fetch_navs(t, block=replay)
 
 if __name__ == "__main__":
   main()

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

@@ -30,10 +30,11 @@ const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
 const float  GPS_MUL_FACTOR = 10.0;
 const float  GPS_POS_STD_THRESHOLD = 50.0;
 const float  GPS_VEL_STD_THRESHOLD = 5.0;
-const float  GPS_POS_ERROR_RESET_THRESHOLD = 200.0;
-const float  GPS_POS_STD_RESET_THRESHOLD = 5.0;
+const float  GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
+const float  GPS_POS_STD_RESET_THRESHOLD = 2.0;
 const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
-const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 5.0;
+const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
+const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -84,6 +85,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();
@@ -227,7 +230,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");
@@ -308,7 +311,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   }
 
   if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) {
-    this->gps_valid = false;
+    //this->gps_valid = false;
     this->determine_gps_mode(current_time);
     return;
   }
@@ -316,7 +319,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   double sensor_time = current_time - sensor_time_offset;
 
   // Process message
-  this->gps_valid = true;
+  //this->gps_valid = true;
   this->gps_mode = true;
   Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
   this->converter = std::make_unique<LocalCoord>(geodetic);
@@ -357,12 +360,10 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
 void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
 
-  this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid();
-  if (!this->gps_valid) {
+  if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
     this->determine_gps_mode(current_time);
     return;
   }
-  this->gps_mode = true;
 
   double sensor_time = log.getMeasTime() * 1e-9;
   if (ublox_available)
@@ -406,29 +407,42 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
   VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
 
   if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
-    this->gps_valid = false;
     this->determine_gps_mode(current_time);
     return;
   }
 
+  // prevent jumping gnss measurements (covered lots, standstill...)
+  bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
+  orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
+  orientation_reset &= !this->standstill;
+  if (orientation_reset) {
+    this->orientation_reset_count++;
+  }
+  else {
+    this->orientation_reset_count = 0;
+  }
+
   if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
     // always reset on first gps message and if the location is off but the accuracy is high
     LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-  } else if (ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD && orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD) {
+  } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
     LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
     this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
     this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+    this->orientation_reset_count = 0;
   }
 
-  this->last_gps_msg = current_time;
+  this->gps_mode = true;
+  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 });
 }
 
 void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
   this->car_speed = std::abs(log.getVEgo());
-  if (log.getStandstill()) {
+  this->standstill = log.getStandstill();
+  if (this->standstill) {
     this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
     this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
   }
@@ -437,7 +451,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 +588,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()) {
@@ -604,7 +618,7 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
 }
 
 bool Localizer::is_gps_ok() {
-  return this->gps_valid;
+  return (this->kf->get_filter_time() - this->last_gps_msg) < 1.0;
 }
 
 bool Localizer::critical_services_valid(std::map<std::string, double> critical_services) {
@@ -616,7 +630,7 @@ bool Localizer::critical_services_valid(std::map<std::string, double> 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 +657,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, {"gnssMeasurements", "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();
 
@@ -77,9 +78,10 @@ private:
   double reset_tracker = 0.0;
   bool device_fell = false;
   bool gps_mode = false;
-  bool gps_valid = false;
   double last_gps_msg = 0;
   bool ublox_available = true;
   bool observation_timings_invalid = false;
-  std::map<std::string, double> observation_values_invalid;  
+  std::map<std::string, double> observation_values_invalid;
+  bool standstill = true;
+  int32_t orientation_reset_count = 0;
 };

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_laikad.py

@@ -4,7 +4,7 @@ import unittest
 from collections import defaultdict
 from datetime import datetime
 from unittest import mock
-from unittest.mock import Mock, patch
+from unittest.mock import patch
 
 from common.params import Params
 from laika.constants import SECS_IN_DAY
@@ -67,47 +67,47 @@ class TestLaikad(unittest.TestCase):
   def setUp(self):
     Params().remove(EPHEMERIS_CACHE)
 
-  def test_fetch_orbits_non_blocking(self):
+  def test_fetch_navs_non_blocking(self):
     gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
     laikad = Laikad()
-    laikad.fetch_orbits(gpstime, block=False)
+    laikad.fetch_navs(gpstime, block=False)
     laikad.orbit_fetch_future.result(30)
     # Get results and save orbits to laikad:
-    laikad.fetch_orbits(gpstime, block=False)
+    laikad.fetch_navs(gpstime, block=False)
 
-    ephem = laikad.astro_dog.orbits['G01'][0]
+    ephem = laikad.astro_dog.navs['G01'][0]
     self.assertIsNotNone(ephem)
 
-    laikad.fetch_orbits(gpstime+2*SECS_IN_DAY, block=False)
+    laikad.fetch_navs(gpstime+2*SECS_IN_DAY, block=False)
     laikad.orbit_fetch_future.result(30)
     # Get results and save orbits to laikad:
-    laikad.fetch_orbits(gpstime + 2 * SECS_IN_DAY, block=False)
+    laikad.fetch_navs(gpstime + 2 * SECS_IN_DAY, block=False)
 
-    ephem2 = laikad.astro_dog.orbits['G01'][0]
+    ephem2 = laikad.astro_dog.navs['G01'][0]
     self.assertIsNotNone(ephem)
     self.assertNotEqual(ephem, ephem2)
 
-  def test_fetch_orbits_with_wrong_clocks(self):
+  def test_fetch_navs_with_wrong_clocks(self):
     laikad = Laikad()
 
-    def check_has_orbits():
-      self.assertGreater(len(laikad.astro_dog.orbits), 0)
-      ephem = laikad.astro_dog.orbits['G01'][0]
+    def check_has_navs():
+      self.assertGreater(len(laikad.astro_dog.navs), 0)
+      ephem = laikad.astro_dog.navs['G01'][0]
       self.assertIsNotNone(ephem)
     real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1))
     wrong_future_clock_time = real_current_time + SECS_IN_DAY
 
-    laikad.fetch_orbits(wrong_future_clock_time, block=True)
-    check_has_orbits()
-    self.assertEqual(laikad.last_fetch_orbits_t, wrong_future_clock_time)
+    laikad.fetch_navs(wrong_future_clock_time, block=True)
+    check_has_navs()
+    self.assertEqual(laikad.last_fetch_navs_t, wrong_future_clock_time)
 
     # Test fetching orbits with earlier time
-    assert real_current_time < laikad.last_fetch_orbits_t
+    assert real_current_time < laikad.last_fetch_navs_t
 
     laikad.astro_dog.orbits = {}
-    laikad.fetch_orbits(real_current_time, block=True)
-    check_has_orbits()
-    self.assertEqual(laikad.last_fetch_orbits_t, real_current_time)
+    laikad.fetch_navs(real_current_time, block=True)
+    check_has_navs()
+    self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
 
   def test_ephemeris_source_in_msg(self):
     data_mock = defaultdict(str)
@@ -115,22 +115,22 @@ class TestLaikad(unittest.TestCase):
 
     gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC
     laikad = Laikad()
-    laikad.fetch_orbits(gpstime, block=True)
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
+    laikad.fetch_navs(gpstime, block=True)
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
     msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
     # Verify gps satellite returns same source
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
     msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
 
     # Test nasa source by using older date
     gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
     laikad = Laikad()
-    laikad.fetch_orbits(gpstime, block=True)
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0])
+    laikad.fetch_navs(gpstime, block=True)
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0])
     msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
 
     # Test nav source type
     ephem = GPSEphemeris(data_mock, gpstime)
@@ -142,7 +142,7 @@ class TestLaikad(unittest.TestCase):
     laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
     correct_msgs = verify_messages(self.logs, laikad)
 
-    correct_msgs_expected = 555
+    correct_msgs_expected = 554
     self.assertEqual(correct_msgs_expected, len(correct_msgs))
     self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
@@ -161,9 +161,8 @@ class TestLaikad(unittest.TestCase):
   def test_laika_online_nav_only(self):
     laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV)
     # Disable fetch_orbits to test NAV only
-    laikad.fetch_orbits = Mock()
     correct_msgs = verify_messages(self.logs, laikad)
-    correct_msgs_expected = 559
+    correct_msgs_expected = 554
     self.assertEqual(correct_msgs_expected, len(correct_msgs))
     self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
@@ -171,45 +170,45 @@ class TestLaikad(unittest.TestCase):
   def test_laika_offline(self, downloader_mock):
     downloader_mock.side_effect = DownloadFailed("Mock download failed")
     laikad = Laikad(auto_update=False)
-    laikad.fetch_orbits(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True)
+    laikad.fetch_navs(GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC, block=True)
 
   @mock.patch('laika.downloader.download_and_cache_file')
   def test_download_failed_russian_source(self, downloader_mock):
     downloader_mock.side_effect = DownloadFailed
     laikad = Laikad(auto_update=False)
     correct_msgs = verify_messages(self.logs, laikad)
-    self.assertEqual(16, len(correct_msgs))
-    self.assertEqual(16, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
+    self.assertEqual(0, len(correct_msgs))
+    self.assertEqual(0, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
   def test_laika_get_orbits(self):
     laikad = Laikad(auto_update=False)
     # Pretend process has loaded the orbits on startup by using the time of the first gps message.
-    laikad.fetch_orbits(self.first_gps_time, block=True)
-    self.dict_has_values(laikad.astro_dog.orbits)
+    laikad.fetch_navs(self.first_gps_time, block=True)
+    self.dict_has_values(laikad.astro_dog.navs)
 
   @unittest.skip("Use to debug live data")
-  def test_laika_get_orbits_now(self):
+  def test_laika_get_navs_now(self):
     laikad = Laikad(auto_update=False)
-    laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True)
+    laikad.fetch_navs(GPSTime.from_datetime(datetime.utcnow()), block=True)
     prn = "G01"
-    self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0)
+    self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
     prn = "R01"
-    self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0)
-    print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime())
+    self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
+    print(min(laikad.astro_dog.navs[prn], key=lambda e: e.epoch).epoch.as_datetime())
 
-  def test_get_orbits_in_process(self):
+  def test_get_navs_in_process(self):
     laikad = Laikad(auto_update=False)
-    has_orbits = False
+    has_navs = False
     for m in self.logs:
       laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False)
       if laikad.orbit_fetch_future is not None:
         laikad.orbit_fetch_future.result()
-      vals = laikad.astro_dog.orbits.values()
-      has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0
-      if has_orbits:
+      vals = laikad.astro_dog.navs.values()
+      has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0
+      if has_navs:
         break
-    self.assertTrue(has_orbits)
-    self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0)
+    self.assertTrue(has_navs)
+    self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0)
     self.assertEqual(None, laikad.orbit_fetch_future)
 
   def test_cache(self):
@@ -228,17 +227,16 @@ class TestLaikad(unittest.TestCase):
     wait_for_cache()
     Params().remove(EPHEMERIS_CACHE)
 
-    laikad.astro_dog.get_navs(self.first_gps_time)
-    laikad.fetch_orbits(self.first_gps_time, block=True)
+    #laikad.astro_dog.get_navs(self.first_gps_time)
+    laikad.fetch_navs(self.first_gps_time, block=True)
 
     # Wait for cache to save
     wait_for_cache()
 
     # Check both nav and orbits separate
     laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True)
-    # Verify orbits and nav are loaded from cache
-    self.dict_has_values(laikad.astro_dog.orbits)
-    self.dict_has_values(laikad.astro_dog.nav)
+    # Verify navs are loaded from cache
+    self.dict_has_values(laikad.astro_dog.navs)
     # Verify cache is working for only nav by running a segment
     msg = verify_messages(self.logs, laikad, return_one_success=True)
     self.assertIsNotNone(msg)
@@ -246,7 +244,7 @@ class TestLaikad(unittest.TestCase):
     with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method:
       # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently
       laikad.astro_dog.orbit_fetched_times = TimeRangeHolder()
-      laikad.fetch_orbits(self.first_gps_time, block=False)
+      laikad.fetch_navs(self.first_gps_time, block=False)
       mock_method.assert_not_called()
 
       # Verify cache is working for only orbits by running a segment

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

selfdrive/locationd/ublox_msg.cc

@@ -192,6 +192,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
         eph.setAf2(subframe_1->af_2() * pow(2, -55));
         eph.setAf1(subframe_1->af_1() * pow(2, -43));
         eph.setAf0(subframe_1->af_0() * pow(2, -31));
+        eph.setSvHealth(subframe_1->sv_health());
       }
 
       // Subframe 2

selfdrive/test/process_replay/process_replay.py

@@ -331,7 +331,7 @@ CONFIGS = [
     pub_sub={
       "cameraOdometry": ["liveLocationKalman"],
       "accelerometer": [], "gyroscope": [],
-      "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
+      "gnssMeasurements": [], "liveCalibration": [], "carState": [],
     },
     ignore=["logMonoTime", "valid"],
     init_callback=get_car_params,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-ff0348b1957c8f992125df77a36b3c0553a26e6a
+e41c3dd90cf61d0a630ba53c7e866cb2be3fc1a8