From 1c926d23da0c73ceb246ca6d61db6ed1c48a1ab9 Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Thu, 3 Jun 2021 09:26:53 -0700
Subject: [PATCH] Many localizer resets alert (#21116)

* add excessive reset

* add alert

* add event
---
 cereal                           | 2 +-
 selfdrive/controls/controlsd.py  | 2 ++
 selfdrive/controls/lib/events.py | 4 ++++
 selfdrive/locationd/locationd.cc | 9 +++++++++
 selfdrive/locationd/locationd.h  | 2 ++
 5 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/cereal b/cereal
index 1681cdbaf0..27823b1c73 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 1681cdbaf0e3f78f0e2643187809cefb693483ac
+Subproject commit 27823b1c73bb8c7dc6cd6ce75d59d6618df6c04c
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 56b74cfc3b..21ecc47a72 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -288,6 +288,8 @@ class Controls:
         self.events.add(EventName.cameraMalfunction)
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
+      if self.sm['liveLocationKalman'].excessiveResets:
+        self.events.add(EventName.localizerMalfunction)
 
       # Check if all manager processes are running
       not_running = set(p.name for p in self.sm['managerState'].processes if not p.running)
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index f9f91522cd..7e083d6ad8 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -491,6 +491,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
     ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Contact Support"),
   },
 
+  EventName.localizerMalfunction: {
+    ET.PERMANENT: NormalPermanentAlert("Localizer unstable", "Contact Support"),
+  },
+
   # ********** events that affect controls state transitions **********
 
   EventName.pcmEnable: {
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 176aac2380..45d036d70b 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -15,6 +15,7 @@ const double ALTITUDE_SANITY_CHECK = 10000; // m
 const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
 const double VALID_TIME_SINCE_RESET = 1.0; // s
 const double VALID_POS_STD = 50.0; // m
+const double MAX_RESET_TRACKER = 5.0;
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -156,6 +157,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
 
   fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
   fix.setDeviceStable(!this->device_fell);
+  fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
   this->device_fell = false;
 
   //fix.setGpsWeek(this->time.week);
@@ -366,6 +368,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;
+}
+
 void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
   // too nonlinear to init on completely wrong
   VectorXd init_x = this->kf->get_initial_x();
@@ -375,6 +382,7 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
 
   this->kf->init_state(init_x, init_P, current_time);
   this->last_reset_time = current_time;
+  this->reset_tracker += 1.0;
 }
 
 void Localizer::handle_msg_bytes(const char *data, const size_t size) {
@@ -401,6 +409,7 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
     this->handle_live_calib(t, log.getLiveCalibration());
   }
   this->finite_check();
+  this->update_reset_tracker();
 }
 
 kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index f9e3645fb0..b5b5a9bded 100755
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -31,6 +31,7 @@ public:
   void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
   void finite_check(double current_time = NAN);
   void time_check(double current_time = NAN);
+  void update_reset_tracker();
 
   kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
     bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -62,5 +63,6 @@ private:
 
   int64_t unix_timestamp_millis = 0;
   double last_gps_fix = 0;
+  double reset_tracker = 0.0;
   bool device_fell = false;
 };