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