spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Better localizer unstable alert (#21660) 

* use canonical language

* filter out when gps signal is flaky

* Update selfdrive/locationd/locationd.cc

Co-authored-by: Willem Melching <willem.melching@gmail.com>

Co-authored-by: Willem Melching <willem.melching@gmail.com>
pull/21669/head
HaraldSchafer 2021-07-20 19:20:00 -07:00 committed by GitHub
parent ced26b806f
commit 049a1bca34
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/lib/events.py
View File

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

13
selfdrive/locationd/locationd.cc
View File

@ -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<capnp::byte> 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<const char *> 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<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);

1
selfdrive/locationd/locationd.h
View File

@ -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<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK);