From 65ab98a6c83cc81fe5916bac964ffafe34bfc47c Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Wed, 12 May 2021 16:47:52 +0200
Subject: [PATCH] locationd: ignore empty sensorEvents (#20887)

---
 selfdrive/locationd/locationd.cc | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 44812c778..b9749bb70 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -180,6 +180,12 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
   // TODO does not yet account for double sensor readings in the log
   for (int i = 0; i < log.size(); i++) {
     const cereal::SensorEventData::Reader& sensor_reading = log[i];
+
+    // Ignore empty readings (e.g. in case the magnetometer had no data ready)
+    if (sensor_reading.getTimestamp() == 0){
+      continue;
+    }
+
     double sensor_time = 1e-9 * sensor_reading.getTimestamp();
 
     // sensor time and log time should be close