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
Split 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
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 && \

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

4
selfdrive/locationd/liblocationd.cc
Unescape View File

@ -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,

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_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;

6
selfdrive/locationd/locationd.h
Unescape View File

@ -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;
};

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

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

@ -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

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

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

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

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

@ -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,

2
selfdrive/test/process_replay/ref_commit
Unescape View File

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