sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Laikad: set active (#26850)

Browse Source 
* 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 a76124da50.

* 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: 88423e25df
beeps
Kurt Nistelberger 2 years ago committed by GitHub
parent f6f6feb922
commit 9570336e6c
12 changed files with 169 additions and 150 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .github/workflows/tools_tests.yaml
  2. 2
      laika_repo
  3. 85
      selfdrive/locationd/laikad.py
  4. 4
      selfdrive/locationd/liblocationd.cc
  5. 71
      selfdrive/locationd/locationd.cc
  6. 6
      selfdrive/locationd/locationd.h
  7. 4
      selfdrive/locationd/test/_test_locationd_lib.py
  8. 104
      selfdrive/locationd/test/test_laikad.py
  9. 36
      selfdrive/locationd/test/test_locationd.py
  10. 1
      selfdrive/locationd/ublox_msg.cc
  11. 2
      selfdrive/test/process_replay/process_replay.py
  12. 2
      selfdrive/test/process_replay/ref_commit

2
.github/workflows/tools_tests.yaml
Unescape View File

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

2
laika_repo

@ -1 +1 @@
Subproject commit 5eb0c3c2596dd12a232b83bdb057a716810e89cf Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d

85
selfdrive/locationd/laikad.py
Unescape View File

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

4
selfdrive/locationd/liblocationd.cc
Unescape View File

@ -3,8 +3,8 @@
extern "C" { extern "C" {
typedef Localizer* Localizer_t; typedef Localizer* Localizer_t;
Localizer *localizer_init() { Localizer *localizer_init(bool has_ublox) {
return new Localizer(); return new Localizer(has_ublox);
} }
void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,

71
selfdrive/locationd/locationd.cc
Unescape View File

@ -30,10 +30,11 @@ const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_MUL_FACTOR = 10.0; const float GPS_MUL_FACTOR = 10.0;
const float GPS_POS_STD_THRESHOLD = 50.0; const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0; const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 200.0; const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 5.0; const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; 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) { static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size()); 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] }); 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) { void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x(); VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P(); 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(); double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close // sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) { if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); 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) { 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); this->determine_gps_mode(current_time);
return; return;
} }
@ -316,7 +319,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
double sensor_time = current_time - sensor_time_offset; double sensor_time = current_time - sensor_time_offset;
// Process message // Process message
this->gps_valid = true; //this->gps_valid = true;
this->gps_mode = true; this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic); 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) { void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid(); if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
if (!this->gps_valid) {
this->determine_gps_mode(current_time); this->determine_gps_mode(current_time);
return; return;
} }
this->gps_mode = true;
double sensor_time = log.getMeasTime() * 1e-9; double sensor_time = log.getMeasTime() * 1e-9;
if (ublox_available) 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))); 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) { 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); this->determine_gps_mode(current_time);
return; 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) { 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 // 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"); 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); 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"); 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->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->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_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_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) { void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo()); 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_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) }); 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) { void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) { if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true; this->observation_timings_invalid = true;
return; return;
@ -574,12 +588,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
this->handle_sensor(t, log.getAccelerometer()); this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) { } else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope()); this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) { //} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); // this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) { //} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); // this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) { } else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements()); this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) { } else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState()); this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) { } else if (log.isCameraOdometry()) {
@ -604,7 +618,7 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
} }
bool Localizer::is_gps_ok() { 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) { 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; 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(); double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_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"); 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() { int Localizer::locationd_thread() {
ublox_available = Params().getBool("UbloxAvailable", true); 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"}; "carState", "carParams", "accelerometer", "gyroscope"};
PubMaster pm({"liveLocationKalman"});
// TODO: remove carParams once we're always sending at 100Hz // 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; uint64_t cnt = 0;
bool filterInitialized = false; bool filterInitialized = false;

6
selfdrive/locationd/locationd.h
Unescape View File

@ -24,6 +24,7 @@
class Localizer { class Localizer {
public: public:
Localizer(); Localizer();
Localizer(bool has_ublox);
int locationd_thread(); int locationd_thread();
@ -77,9 +78,10 @@ private:
double reset_tracker = 0.0; double reset_tracker = 0.0;
bool device_fell = false; bool device_fell = false;
bool gps_mode = false; bool gps_mode = false;
bool gps_valid = false;
double last_gps_msg = 0; double last_gps_msg = 0;
bool ublox_available = true; bool ublox_available = true;
bool observation_timings_invalid = false; 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;
}; };

4
selfdrive/locationd/test/_test_locationd_lib.py
Unescape View File

@ -19,7 +19,7 @@ LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../
class TestLocationdLib(unittest.TestCase): class TestLocationdLib(unittest.TestCase):
def setUp(self): def setUp(self):
header = '''typedef ...* Localizer_t; 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_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);''' 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.ffi.cdef(header)
self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) 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.buff_size = 2048
self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') self.msg_buff = self.ffi.new(f'char[{self.buff_size}]')

104
selfdrive/locationd/test/test_laikad.py
Unescape View File

@ -4,7 +4,7 @@ import unittest
from collections import defaultdict from collections import defaultdict
from datetime import datetime from datetime import datetime
from unittest import mock from unittest import mock
from unittest.mock import Mock, patch from unittest.mock import patch
from common.params import Params from common.params import Params
from laika.constants import SECS_IN_DAY from laika.constants import SECS_IN_DAY
@ -67,47 +67,47 @@ class TestLaikad(unittest.TestCase):
def setUp(self): def setUp(self):
Params().remove(EPHEMERIS_CACHE) 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)) gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad() laikad = Laikad()
laikad.fetch_orbits(gpstime, block=False) laikad.fetch_navs(gpstime, block=False)
laikad.orbit_fetch_future.result(30) laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad: # 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) 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) laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad: # 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.assertIsNotNone(ephem)
self.assertNotEqual(ephem, ephem2) self.assertNotEqual(ephem, ephem2)
def test_fetch_orbits_with_wrong_clocks(self): def test_fetch_navs_with_wrong_clocks(self):
laikad = Laikad() laikad = Laikad()
def check_has_orbits(): def check_has_navs():
self.assertGreater(len(laikad.astro_dog.orbits), 0) self.assertGreater(len(laikad.astro_dog.navs), 0)
ephem = laikad.astro_dog.orbits['G01'][0] ephem = laikad.astro_dog.navs['G01'][0]
self.assertIsNotNone(ephem) self.assertIsNotNone(ephem)
real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1)) real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1))
wrong_future_clock_time = real_current_time + SECS_IN_DAY wrong_future_clock_time = real_current_time + SECS_IN_DAY
laikad.fetch_orbits(wrong_future_clock_time, block=True) laikad.fetch_navs(wrong_future_clock_time, block=True)
check_has_orbits() check_has_navs()
self.assertEqual(laikad.last_fetch_orbits_t, wrong_future_clock_time) self.assertEqual(laikad.last_fetch_navs_t, wrong_future_clock_time)
# Test fetching orbits with earlier 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.astro_dog.orbits = {}
laikad.fetch_orbits(real_current_time, block=True) laikad.fetch_navs(real_current_time, block=True)
check_has_orbits() check_has_navs()
self.assertEqual(laikad.last_fetch_orbits_t, real_current_time) self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
def test_ephemeris_source_in_msg(self): def test_ephemeris_source_in_msg(self):
data_mock = defaultdict(str) data_mock = defaultdict(str)
@ -115,22 +115,22 @@ class TestLaikad(unittest.TestCase):
gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC
laikad = Laikad() laikad = Laikad()
laikad.fetch_orbits(gpstime, block=True) laikad.fetch_navs(gpstime, block=True)
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) 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 # 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) 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 # Test nasa source by using older date
gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad() laikad = Laikad()
laikad.fetch_orbits(gpstime, block=True) laikad.fetch_navs(gpstime, block=True)
meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0]) meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0])
msg = create_measurement_msg(meas) 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 # Test nav source type
ephem = GPSEphemeris(data_mock, gpstime) 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) laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
correct_msgs = verify_messages(self.logs, laikad) 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(correct_msgs))
self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) 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): def test_laika_online_nav_only(self):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV)
# Disable fetch_orbits to test NAV only # Disable fetch_orbits to test NAV only
laikad.fetch_orbits = Mock()
correct_msgs = verify_messages(self.logs, laikad) 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(correct_msgs))
self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) 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): def test_laika_offline(self, downloader_mock):
downloader_mock.side_effect = DownloadFailed("Mock download failed") downloader_mock.side_effect = DownloadFailed("Mock download failed")
laikad = Laikad(auto_update=False) 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') @mock.patch('laika.downloader.download_and_cache_file')
def test_download_failed_russian_source(self, downloader_mock): def test_download_failed_russian_source(self, downloader_mock):
downloader_mock.side_effect = DownloadFailed downloader_mock.side_effect = DownloadFailed
laikad = Laikad(auto_update=False) laikad = Laikad(auto_update=False)
correct_msgs = verify_messages(self.logs, laikad) correct_msgs = verify_messages(self.logs, laikad)
self.assertEqual(16, len(correct_msgs)) self.assertEqual(0, len(correct_msgs))
self.assertEqual(16, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) self.assertEqual(0, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_laika_get_orbits(self): def test_laika_get_orbits(self):
laikad = Laikad(auto_update=False) laikad = Laikad(auto_update=False)
# Pretend process has loaded the orbits on startup by using the time of the first gps message. # 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) laikad.fetch_navs(self.first_gps_time, block=True)
self.dict_has_values(laikad.astro_dog.orbits) self.dict_has_values(laikad.astro_dog.navs)
@unittest.skip("Use to debug live data") @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 = 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" prn = "G01"
self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
prn = "R01" prn = "R01"
self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) 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) laikad = Laikad(auto_update=False)
has_orbits = False has_navs = False
for m in self.logs: for m in self.logs:
laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False) laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False)
if laikad.orbit_fetch_future is not None: if laikad.orbit_fetch_future is not None:
laikad.orbit_fetch_future.result() laikad.orbit_fetch_future.result()
vals = laikad.astro_dog.orbits.values() vals = laikad.astro_dog.navs.values()
has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0 has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0
if has_orbits: if has_navs:
break break
self.assertTrue(has_orbits) self.assertTrue(has_navs)
self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0)
self.assertEqual(None, laikad.orbit_fetch_future) self.assertEqual(None, laikad.orbit_fetch_future)
def test_cache(self): def test_cache(self):
@ -228,17 +227,16 @@ class TestLaikad(unittest.TestCase):
wait_for_cache() wait_for_cache()
Params().remove(EPHEMERIS_CACHE) Params().remove(EPHEMERIS_CACHE)
laikad.astro_dog.get_navs(self.first_gps_time) #laikad.astro_dog.get_navs(self.first_gps_time)
laikad.fetch_orbits(self.first_gps_time, block=True) laikad.fetch_navs(self.first_gps_time, block=True)
# Wait for cache to save # Wait for cache to save
wait_for_cache() wait_for_cache()
# Check both nav and orbits separate # Check both nav and orbits separate
laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True) laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True)
# Verify orbits and nav are loaded from cache # Verify navs are loaded from cache
self.dict_has_values(laikad.astro_dog.orbits) self.dict_has_values(laikad.astro_dog.navs)
self.dict_has_values(laikad.astro_dog.nav)
# Verify cache is working for only nav by running a segment # Verify cache is working for only nav by running a segment
msg = verify_messages(self.logs, laikad, return_one_success=True) msg = verify_messages(self.logs, laikad, return_one_success=True)
self.assertIsNotNone(msg) 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: 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 # 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.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() mock_method.assert_not_called()
# Verify cache is working for only orbits by running a segment # Verify cache is working for only orbits by running a segment

36
selfdrive/locationd/test/test_locationd.py
Unescape View File

@ -8,13 +8,14 @@ import capnp
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal.services import service_list from cereal.services import service_list
from common.params import Params from common.params import Params
from common.transformations.coordinates import ecef2geodetic
from selfdrive.manager.process_config import managed_processes from selfdrive.manager.process_config import managed_processes
class TestLocationdProc(unittest.TestCase): class TestLocationdProc(unittest.TestCase):
MAX_WAITS = 1000 MAX_WAITS = 1000
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
'accelerometer', 'gyroscope', 'magnetometer'] 'accelerometer', 'gyroscope', 'magnetometer']
def setUp(self): def setUp(self):
@ -45,16 +46,14 @@ class TestLocationdProc(unittest.TestCase):
except capnp.lib.capnp.KjException: except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0) msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal": if name == "gnssMeasurements":
msg.gpsLocationExternal.flags = 1 msg.gnssMeasurements.measTime = t
msg.gpsLocationExternal.verticalAccuracy = 1.0 msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
msg.gpsLocationExternal.speedAccuracy = 1.0 msg.gnssMeasurements.positionECEF.std = [0,0,0]
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 msg.gnssMeasurements.positionECEF.valid = True
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] msg.gnssMeasurements.velocityECEF.value = []
msg.gpsLocationExternal.latitude = self.lat msg.gnssMeasurements.velocityECEF.std = [0,0,0]
msg.gpsLocationExternal.longitude = self.lon msg.gnssMeasurements.velocityECEF.valid = True
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = self.alt
elif name == 'cameraOdometry': elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [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 # first reset params
Params().remove('LastGPSPosition') Params().remove('LastGPSPosition')
self.lat = 30 + (random.random() * 10.0) self.x = -2710700 + (random.random() * 1e5)
self.lon = -70 + (random.random() * 10.0) self.y = -4280600 + (random.random() * 1e5)
self.alt = 5 + (random.random() * 10.0) self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
self.fake_duration = 90 # secs self.fake_duration = 90 # secs
# get fake messages at the correct frequency, listed in services.py # get fake messages at the correct frequency, listed in services.py
fake_msgs = [] fake_msgs = []
@ -83,10 +84,9 @@ class TestLocationdProc(unittest.TestCase):
time.sleep(1) # wait for async params write time.sleep(1) # wait for async params write
lastGPS = json.loads(Params().get('LastGPSPosition')) lastGPS = json.loads(Params().get('LastGPSPosition'))
self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4)
self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4)
self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4)
self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)
if __name__ == "__main__": if __name__ == "__main__":

1
selfdrive/locationd/ublox_msg.cc
Unescape View File

@ -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.setAf2(subframe_1->af_2() * pow(2, -55));
eph.setAf1(subframe_1->af_1() * pow(2, -43)); eph.setAf1(subframe_1->af_1() * pow(2, -43));
eph.setAf0(subframe_1->af_0() * pow(2, -31)); eph.setAf0(subframe_1->af_0() * pow(2, -31));
eph.setSvHealth(subframe_1->sv_health());
} }
// Subframe 2 // Subframe 2

2
selfdrive/test/process_replay/process_replay.py
Unescape View File

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

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
ff0348b1957c8f992125df77a36b3c0553a26e6a e41c3dd90cf61d0a630ba53c7e866cb2be3fc1a8
Write Preview
Loading…
Cancel
Save