rely on carstate to be sure (#1363)

* rely on carstate to be sure

* relies on carstate now too

* don't know how else to make this work

* update ref

* clean up hacks

* prev ref was weirdly bad?

* lets try that from my machine

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/locationd/calibrationd.py

@@ -59,6 +59,7 @@ class Calibrator():
     self.valid_blocks = 0
     self.cal_status = Calibration.UNCALIBRATED
     self.just_calibrated = False
+    self.v_ego = 0
 
     # Read calibration
     calibration_params = Params().get("CalibrationParams")
@@ -88,8 +89,11 @@ class Calibrator():
     if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
       self.just_calibrated = True
 
+  def handle_v_ego(self, v_ego):
+    self.v_ego = v_ego
+
   def handle_cam_odom(self, trans, rot, trans_std, rot_std):
-    straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
+    straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
     certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
                         (self.valid_blocks < INPUTS_NEEDED))
     if straight_and_fast and certain_if_calib:
@@ -132,7 +136,7 @@ class Calibrator():
 
 def calibrationd_thread(sm=None, pm=None):
   if sm is None:
-    sm = messaging.SubMaster(['cameraOdometry'])
+    sm = messaging.SubMaster(['cameraOdometry', 'carState'])
 
   if pm is None:
     pm = messaging.PubMaster(['liveCalibration'])
@@ -143,18 +147,22 @@ def calibrationd_thread(sm=None, pm=None):
   while 1:
     sm.update()
 
+    if sm.updated['carState']:
+      calibrator.handle_v_ego(sm['carState'].vEgo)
+      if send_counter % 25 == 0:
+        calibrator.send_data(pm)
+      send_counter += 1
+
     if sm.updated['cameraOdometry']:
       new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
                                           sm['cameraOdometry'].rot,
                                           sm['cameraOdometry'].transStd,
                                           sm['cameraOdometry'].rotStd)
+
       if DEBUG and new_vp is not None:
         print('got new vp', new_vp)
 
       # decimate outputs for efficiency
-    if (send_counter % 5) == 0:
-      calibrator.send_data(pm)
-    send_counter += 1
 
 
 def main(sm=None, pm=None):

selfdrive/test/process_replay/process_replay.py

@@ -92,6 +92,7 @@ class FakeSubMaster(messaging.SubMaster):
     wait_for_event(self.update_ready)
     self.update_ready.clear()
 
+
   def update_msgs(self, cur_time, msgs):
     wait_for_event(self.update_called)
     self.update_called.clear()
@@ -186,8 +187,14 @@ def radar_rcv_callback(msg, CP, cfg, fsm):
 def calibration_rcv_callback(msg, CP, cfg, fsm):
   # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
   # should_recv always true to increment frame
-  recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else []
+  if msg.which() == 'carState':
+    if ((fsm.frame +1)% 25) == 0:
+      recv_socks = ["liveCalibration"]
+    else:
+      recv_socks = []
     return recv_socks, True
+  else:
+    return [], False
 
 
 CONFIGS = [
@@ -225,7 +232,8 @@ CONFIGS = [
   ProcessConfig(
     proc_name="calibrationd",
     pub_sub={
-      "cameraOdometry": ["liveCalibration"]
+      "carState": ["liveCalibration"],
+      "cameraOdometry": []
     },
     ignore=["logMonoTime", "valid"],
     init_callback=get_car_params,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-9b4b08487380a6741646e22e191fa5bd1615f2d3
+ea2820c0708a95e8f392d503c7b76090ade380b0