From d5e75dd0afafac5e449f647e92320646831ff35d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kacper=20R=C4=85czy?= <gfw.kra@gmail.com>
Date: Fri, 20 Mar 2026 15:29:58 -0700
Subject: [PATCH] locationd: publish filter time (#37697)

* Include filter time in the message

* Move the line up

* Use nantonum
---
 cereal/log.capnp                           | 2 ++
 selfdrive/locationd/locationd.py           | 1 +
 selfdrive/test/process_replay/migration.py | 1 +
 3 files changed, 4 insertions(+)

diff --git a/cereal/log.capnp b/cereal/log.capnp
index d1f85d325..c26c1f9d3 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -1426,6 +1426,8 @@ struct LivePose {
   posenetOK @5 :Bool = false;
   sensorsOK @6 :Bool = false;
 
+  timestamp @8 :UInt64;
+
   debugFilterState @7 :FilterState;
 
   struct XYZMeasurement {
diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py
index a34a584ff..57aecb22e 100755
--- a/selfdrive/locationd/locationd.py
+++ b/selfdrive/locationd/locationd.py
@@ -239,6 +239,7 @@ class LocationEstimator:
     livePose.inputsOK = inputs_valid
     livePose.posenetOK = not std_spike or self.car_speed <= 5.0
     livePose.sensorsOK = sensors_valid
+    livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
 
     return msg
 
diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py
index 232722d1b..2a2633c34 100644
--- a/selfdrive/test/process_replay/migration.py
+++ b/selfdrive/test/process_replay/migration.py
@@ -175,6 +175,7 @@ def migrate_liveLocationKalman(msgs):
     m = messaging.new_message('livePose')
     m.valid = msg.valid
     m.logMonoTime = msg.logMonoTime
+    m.livePose.timestamp = msg.logMonoTime
     for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]:
       lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field)
       lp_field.x, lp_field.y, lp_field.z = llk_field.value or nans