diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index ef150709a5..c7af803332 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -545,7 +545,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo # current GPS position. This alert is thrown when the localizer is reset # more often than expected. EventName.localizerMalfunction: { - ET.PERMANENT: NormalPermanentAlert("Localizer unstable", "Contact Support"), + ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Contact Support"), }, # ********** events that affect controls state transitions ********** diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index d96246f94a..3f86ecf333 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -372,7 +372,11 @@ 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 - this->reset_tracker *= .99995; + if (this->isGpsOK()) { + this->reset_tracker *= .99995; + } else { + this->reset_tracker = 0.0; + } } void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) { @@ -427,6 +431,11 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build return msg_builder.toBytes(); } + +bool Localizer::isGpsOK() { + return this->kf->get_filter_time() - this->last_gps_fix < 1.0; +} + int Localizer::locationd_thread() { const std::initializer_list service_list = { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; @@ -448,7 +457,7 @@ int Localizer::locationd_thread() { uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); bool inputsOK = sm.allAliveAndValid(); bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); - bool gpsOK = (logMonoTime / 1e9) - this->last_gps_fix < 1.0; + bool gpsOK = this->isGpsOK(); MessageBuilder msg_builder; kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 82b7eb6b32..04284ba3b3 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -31,6 +31,7 @@ public: void finite_check(double current_time = NAN); void time_check(double current_time = NAN); void update_reset_tracker(); + bool isGpsOK(); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK);