diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml index 94cc3c2580..7ad0ce35b1 100644 --- a/.github/workflows/tools_tests.yaml +++ b/.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 && \ diff --git a/laika_repo b/laika_repo index 5eb0c3c259..73bf110ae0 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 5eb0c3c2596dd12a232b83bdb057a716810e89cf +Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 1f13c2b69a..b8d86c63fd 100755 --- a/selfdrive/locationd/laikad.py +++ b/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() 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..928e19081e 100755 --- a/selfdrive/locationd/locationd.cc +++ b/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::Reader& floatlist) { VectorXd res(floatlist.size()); @@ -84,6 +85,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 +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(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 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 critical_services) { @@ -616,7 +630,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 +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 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, {"gnssMeasurements", "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..a292a3c936 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(); @@ -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 observation_values_invalid; + std::map observation_values_invalid; + bool standstill = true; + int32_t orientation_reset_count = 0; }; 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_laikad.py b/selfdrive/locationd/test/test_laikad.py index 198ecfe935..6059eab68f 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/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 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__": diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index b45dffae33..6431e9d48b 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -192,6 +192,7 @@ kj::Array 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 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 22ec099285..24ee87e471 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": [], - "gpsLocationExternal": [], "liveCalibration": [], "carState": [], + "gnssMeasurements": [], "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 157d146ade..58b6139d3e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ff0348b1957c8f992125df77a36b3c0553a26e6a +e41c3dd90cf61d0a630ba53c7e866cb2be3fc1a8 \ No newline at end of file