time checks in locationd (#20880)

* time checks

* no debug print

* correct timestamps in test

* non nan

selfdrive/locationd/locationd.cc

@@ -181,7 +181,14 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   for (int i = 0; i < log.size(); i++) {
     const cereal::SensorEventData::Reader& sensor_reading = log[i];
     double sensor_time = 1e-9 * sensor_reading.getTimestamp();
-    // TODO: handle messages from two IMUs at the same time
+
+    // sensor time and log time should be close
+    if (abs(current_time - sensor_time) > 0.1) {
+      LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
+      return;
+    }
+
+      // TODO: handle messages from two IMUs at the same time
     if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) {
       continue;
     }
@@ -337,6 +344,15 @@ void Localizer::finite_check(double current_time) {
   }
 }
 
+void Localizer::time_check(double current_time) {
+  double filter_time = this->kf->get_filter_time();
+  bool big_time_gap = !isnan(filter_time) && (current_time - filter_time > 10);
+  if (big_time_gap){
+    LOGE("Time gap of over 10s detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
 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();
@@ -358,6 +374,7 @@ void Localizer::handle_msg_bytes(const char *data, const size_t size) {
 
 void Localizer::handle_msg(const cereal::Event::Reader& log) {
   double t = log.getLogMonoTime() * 1e-9;
+  this->time_check(t);
   if (log.isSensorEvents()) {
     this->handle_sensors(t, log.getSensorEvents());
   } else if (log.isGpsLocationExternal()) {

selfdrive/locationd/locationd.h

@@ -12,6 +12,7 @@
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/timing.h"
 #include "selfdrive/common/util.h"
+#include <math.h>
 
 #include "selfdrive/sensord/sensors/constants.h"
 #define VISION_DECIMATION 2
@@ -29,6 +30,7 @@ public:
   void reset_kalman(double current_time = NAN);
   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);
 
   kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
     bool inputsOK, bool sensorsOK, bool gpsOK);

selfdrive/locationd/models/live_kf.cc

@@ -66,6 +66,10 @@ MatrixXdr LiveKalman::get_P() {
   return this->filter->covs();
 }
 
+double LiveKalman::get_filter_time() {
+  return this->filter->get_filter_time();
+}
+
 std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
   std::vector<MatrixXdr> R;
   for (int i = 0; i < n; i++) {

selfdrive/locationd/models/live_kf.h

@@ -29,6 +29,7 @@ public:
 
   Eigen::VectorXd get_x();
   MatrixXdr get_P();
+  double get_filter_time();
   std::vector<MatrixXdr> get_R(int kind, int n);
 
   std::optional<Estimate> predict_and_observe(double t, int kind, std::vector<Eigen::VectorXd> meas, std::vector<MatrixXdr> R = {});

selfdrive/locationd/test/test_locationd.py

@@ -54,6 +54,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
   def test_device_fell(self):
     msg = messaging.new_message('sensorEvents', 1)
     msg.sensorEvents[0].sensor = 1
+    msg.sensorEvents[0].timestamp = msg.logMonoTime
     msg.sensorEvents[0].type = 1
     msg.sensorEvents[0].init('acceleration')
     msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
@@ -64,6 +65,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
 
     msg = messaging.new_message('sensorEvents', 1)
     msg.sensorEvents[0].sensor = 1
+    msg.sensorEvents[0].timestamp = msg.logMonoTime
     msg.sensorEvents[0].type = 1
     msg.sensorEvents[0].init('acceleration')
     msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2