From 47c5f5206764353d9035c468c175034b2aaf33d6 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Wed, 20 May 2020 14:18:28 -0700
Subject: [PATCH] Locationd fixes

old-commit-hash: 8f3e932f96dbd6c6c27a74644bc5d1abbb166898
---
 selfdrive/locationd/locationd.py | 24 +++++++++++++++---------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py
index e6a3e38c13..6b4928872a 100755
--- a/selfdrive/locationd/locationd.py
+++ b/selfdrive/locationd/locationd.py
@@ -21,6 +21,7 @@ from sympy.utilities.lambdify import lambdify
 from rednose.helpers.sympy_helpers import euler_rotate
 
 
+OUTPUT_DECIMATION = 2
 VISION_DECIMATION = 2
 SENSOR_DECIMATION = 10
 
@@ -61,6 +62,9 @@ class Localizer():
     self.posenet_speed = 0
     self.car_speed = 0
 
+    self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
+    self.unix_timestamp_millis = 0
+
   @staticmethod
   def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
     predicted_std = np.sqrt(np.diagonal(predicted_cov))
@@ -271,15 +275,15 @@ class Localizer():
     self.speed_counter = 0
     self.cam_counter = 0
 
-
 def locationd_thread(sm, pm, disabled_logs=[]):
   if sm is None:
     socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
-    sm = messaging.SubMaster(socks)
+    sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'])
   if pm is None:
     pm = messaging.PubMaster(['liveLocationKalman'])
 
   localizer = Localizer(disabled_logs=disabled_logs)
+  camera_odometry_cnt = 0
 
   while True:
     sm.update()
@@ -298,15 +302,17 @@ def locationd_thread(sm, pm, disabled_logs=[]):
         elif sock == "liveCalibration":
           localizer.handle_live_calib(t, sm[sock])
 
-    if sm.updated['gpsLocationExternal']:
-      t = sm.logMonoTime['gpsLocationExternal']
-      msg = messaging.new_message('liveLocationKalman')
-      msg.logMonoTime = t
+    if sm.updated['cameraOdometry']:
+      camera_odometry_cnt += 1
 
-      msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
-      msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
+      if camera_odometry_cnt % OUTPUT_DECIMATION == 0:
+        t = sm.logMonoTime['cameraOdometry']
+        msg = messaging.new_message('liveLocationKalman')
+        msg.logMonoTime = t
 
-      pm.send('liveLocationKalman', msg)
+        msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
+        msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
+        pm.send('liveLocationKalman', msg)
 
 
 def main(sm=None, pm=None):