mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-19 03:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Better localizer unstable alert (#21660)

Browse Source 
* 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>
This commit is contained in:
HaraldSchafer
2021-07-20 19:20:00 -07:00
committed by GitHub
parent ced26b806f
commit 049a1bca34
3 changed files with 13 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/events.py
+1 -1
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 **********
selfdrive/locationd/locationd.cc
+11 -2
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);
selfdrive/locationd/locationd.h
+1
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);