diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 49404668a4..da57fb7ff4 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -26,4 +26,16 @@ extern "C" { memcpy(std_buff, stdev.data(), sizeof(double) * stdev.size()); } + bool is_gps_ok(Localizer *localizer){ + return localizer->is_gps_ok(); + } + + bool are_inputs_ok(Localizer *localizer){ + return localizer->are_inputs_ok(); + } + + void observation_timings_invalid_reset(Localizer *localizer){ + localizer->observation_timings_invalid_reset(); + } + } diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 8d9e247655..4325900c0e 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -19,6 +19,9 @@ const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m const double MAX_RESET_TRACKER = 5.0; const double SANE_GPS_UNCERTAINTY = 1500.0; // m +const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker +const double DECAY = 0.99995; // same as reset tracker +const double MAX_FILTER_REWIND_TIME = 0.8; // s // TODO: GPS sensor time offsets are empirically calculated // They should be replaced with synced time from a real clock @@ -200,6 +203,14 @@ VectorXd Localizer::get_stdev() { return this->kf->get_P().diagonal().array().sqrt(); } +bool Localizer::are_inputs_ok() { + return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; +} + +void Localizer::observation_timings_invalid_reset(){ + this->observation_timings_invalid = false; +} + void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { // TODO does not yet account for double sensor readings in the log @@ -209,10 +220,15 @@ 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"); + this->observation_timings_invalid = true; + return; + } + else if (!this->is_timestamp_valid(sensor_time)) { + this->observation_timings_invalid = true; return; } @@ -227,6 +243,10 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData auto meas = Vector3d(-v[2], -v[1], -v[0]); if (meas.norm() < ROTATION_SANITY_CHECK) { this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); + this->observation_values_invalid["gyroscope"] *= DECAY; + } + else{ + this->observation_values_invalid["gyroscope"] += 1.0; } } @@ -242,6 +262,10 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData auto meas = Vector3d(-v[2], -v[1], -v[0]); if (meas.norm() < ACCEL_SANITY_CHECK) { this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); + this->observation_values_invalid["accelerometer"] *= DECAY; + } + else{ + this->observation_values_invalid["accelerometer"] += 1.0; } } } @@ -335,8 +359,14 @@ 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; + } if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } @@ -344,10 +374,12 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; return; } @@ -363,12 +395,19 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry { rot_device }, { rot_device_cov }); this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, { trans_device }, { trans_device_cov }); + this->observation_values_invalid["cameraOdometry"] *= DECAY; } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + if (log.getRpyCalib().size() > 0) { auto live_calib = floatlist2vector(log.getRpyCalib()); if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { + this->observation_values_invalid["liveCalibration"] += 1.0; return; } @@ -376,6 +415,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); this->calibrated = log.getCalStatus() == 1; + this->observation_values_invalid["liveCalibration"] *= DECAY; } } @@ -407,8 +447,8 @@ void Localizer::time_check(double current_time) { void Localizer::update_reset_tracker() { // reset tracker is tuned to trigger when over 1reset/10s over 2min period - if (this->isGpsOK()) { - this->reset_tracker *= .99995; + if (this->is_gps_ok()) { + this->reset_tracker *= DECAY; } else { this->reset_tracker = 0.0; } @@ -483,10 +523,28 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build return msg_builder.toBytes(); } -bool Localizer::isGpsOK() { +bool Localizer::is_gps_ok() { return this->gps_valid; } +bool Localizer::critical_services_valid(std::map critical_services) { + for (auto &kv : critical_services){ + if (kv.second >= INPUT_INVALID_THRESHOLD){ + return false; + } + } + return true; +} + +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"); + return false; + } + return true; +} + void Localizer::determine_gps_mode(double current_time) { // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs @@ -521,10 +579,15 @@ int Localizer::locationd_thread() { uint64_t cnt = 0; bool filterInitialized = false; + const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; + for (std::string service : critical_input_services) { + this->observation_values_invalid.insert({service, 0.0}); + } while (!do_exit) { sm.update(); if (filterInitialized){ + this->observation_timings_invalid_reset(); for (const char* service : service_list) { if (sm.updated(service) && sm.valid(service)){ const cereal::Event::Reader log = sm[service]; @@ -538,8 +601,8 @@ int Localizer::locationd_thread() { // 100Hz publish for notcars, 20Hz for cars const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "accelerometer" : "cameraOdometry"; if (sm.updated(trigger_msg)) { - bool inputsOK = sm.allAliveAndValid(); - bool gpsOK = this->isGpsOK(); + bool inputsOK = sm.allAliveAndValid() && this->are_inputs_ok(); + bool gpsOK = this->is_gps_ok(); bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); MessageBuilder msg_builder; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index d6bb5347c5..f0872d9f56 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "cereal/messaging/messaging.h" @@ -32,8 +33,12 @@ public: void finite_check(double current_time = NAN); void time_check(double current_time = NAN); void update_reset_tracker(); - bool isGpsOK(); + bool is_gps_ok(); + bool critical_services_valid(std::map critical_services); + bool is_timestamp_valid(double current_time); void determine_gps_mode(double current_time); + bool are_inputs_ok(); + void observation_timings_invalid_reset(); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); @@ -73,4 +78,6 @@ private: bool gps_mode = false; bool gps_valid = false; bool ublox_available = true; + bool observation_timings_invalid = false; + std::map observation_values_invalid; };