Merge branch 'devel-en' into devel-zhs

# Conflicts:
#	apk/ai.comma.plus.offroad.apk

selfdrive/controls/controlsd.py

@@ -22,7 +22,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.controls.lib.driver_monitor import DriverStatus
+from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
 from selfdrive.controls.lib.planner import LON_MPC_STEP
 from selfdrive.locationd.calibration_helpers import Calibration, Filter
 
@@ -49,16 +49,22 @@ def events_to_bytes(events):
   return ret
 
 
-def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                 driver_status, state, mismatch_counter, params):
   """Receive data from sockets and create events for battery, temperature and disk space"""
 
   # Update carstate from CAN and create events
-  CS = CI.update(CC)
+  can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+  CS = CI.update(CC, can_strs)
+
+  sm.update(0)
+
   events = list(CS.events)
   enabled = isEnabled(state)
 
-  sm.update(0)
+  # Check for CAN timeout
+  if not can_strs:
+    events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
   if sm.updated['thermal']:
     overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
@@ -73,6 +79,7 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
   if free_space:
     events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
 
+
   # Handle calibration
   if sm.updated['liveCalibration']:
     cal_status = sm['liveCalibration'].calStatus
@@ -102,6 +109,9 @@ def data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_batt
   if sm.updated['driverMonitoring']:
     driver_status.get_pose(sm['driverMonitoring'], params)
 
+  if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
+    events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
+
   return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
 
 
@@ -402,6 +412,7 @@ def controlsd_thread(gctx=None):
 
   params = Params()
 
+
   # Pub Sockets
   sendcan = messaging.pub_sock(service_list['sendcan'].port)
   controlsstate = messaging.pub_sock(service_list['controlsState'].port)
@@ -414,10 +425,15 @@ def controlsd_thread(gctx=None):
   passive = params.get("Passive") != "0"
 
   sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
+
   logcan = messaging.sub_sock(service_list['can'].port)
+  CI, CP = get_car(logcan, sendcan)
+  logcan.close()
+
+  # TODO: Use the logcan socket from above, but that will currenly break the tests
+  can_sock = messaging.sub_sock(service_list['can'].port, timeout=100)
 
   CC = car.CarControl.new_message()
-  CI, CP = get_car(logcan, sendcan)
   AM = AlertManager()
 
   car_recognized = CP.carName != 'mock'
@@ -469,7 +485,7 @@ def controlsd_thread(gctx=None):
 
     # Sample data and compute car events
     CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
-      data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
+      data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
                   driver_status, state, mismatch_counter, params)
     prof.checkpoint("Sample")
 

selfdrive/controls/lib/alerts.py

@@ -300,6 +300,13 @@ ALERTS = [
       AlertStatus.normal, AlertSize.mid,
       Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
 
+  Alert(
+      "tooDistractedNoEntry",
+      "openpilot Unavailable",
+      "Distraction Level Too High",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
   # Cancellation alerts causing soft disabling
   Alert(
       "overheat",

selfdrive/controls/lib/driver_monitor.py

@@ -16,11 +16,12 @@ _PITCH_WEIGHT = 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
 _PITCH_POS_ALLOWANCE = 0.08   # rad, to not be too sensitive on positive pitch
 _PITCH_NATURAL_OFFSET = 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
-_YAW_NATURAL_OFFSET = 0.08   # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
+_YAW_NATURAL_OFFSET = 0.08    # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
 _STD_THRESHOLD = 0.1          # above this standard deviation consider the measurement invalid
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 _VARIANCE_FILTER_TS = 20.     # 0.008Hz
 
+MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426
 
@@ -70,6 +71,7 @@ class DriverStatus():
     self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
     self.ts_last_check = 0.
     self.face_detected = False
+    self.terminal_alert_cnt = 0
     self._set_timers()
 
   def _reset_filters(self):
@@ -135,6 +137,7 @@ class DriverStatus():
   def update(self, events, driver_engaged, ctrl_active, standstill):
 
     driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
+    awareness_prev = self.awareness
 
     if (driver_engaged and self.awareness > 0.) or not ctrl_active:
       # always reset if driver is in control (unless we are in red alert state) or op isn't active
@@ -145,19 +148,20 @@ class DriverStatus():
        not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
       self.awareness = max(self.awareness - self.step_change, -0.1)
 
-    if params.get("DragonDisableDriverSafetyCheck") == "0":
-      alert = None
-      if self.awareness <= 0.:
-        # terminal red alert: disengagement required
-        alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
-      elif self.awareness <= self.threshold_prompt:
-        # prompt orange alert
-        alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
-      elif self.awareness <= self.threshold_pre:
-        # pre green alert
-        alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
-      if alert is not None:
-        events.append(create_event(alert, [ET.WARNING]))
+    alert = None
+    if self.awareness < 0.:
+      # terminal red alert: disengagement required
+      alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
+      if awareness_prev >= 0.:
+        self.terminal_alert_cnt += 1
+    elif self.awareness <= self.threshold_prompt:
+      # prompt orange alert
+      alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
+    elif self.awareness <= self.threshold_pre:
+      # pre green alert
+      alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
+    if params.get("DragonDisableDriverSafetyCheck") == "0" and alert is not None:
+      events.append(create_event(alert, [ET.WARNING]))
 
     return events
 

selfdrive/controls/lib/fcw.py

@@ -43,8 +43,10 @@ class FCWChecker(object):
       ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
     return ttc
 
-  def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+  def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
     mpc_solution_a = list(mpc_solution[0].a_ego)
+    a_target = mpc_solution_a[1]
+
     self.last_min_a = min(mpc_solution_a)
     self.v_lead_max = max(self.v_lead_max, v_lead)
 
@@ -62,8 +64,11 @@ class FCWChecker(object):
       a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
       a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
 
-      fcw_allowed = all(c >= 10 for c in self.counters.values())
-      if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
+      future_fcw_allowed = all(c >= 10 for c in self.counters.values())
+      future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
+      current_fcw = a_target < -3.0 and active
+
+      if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
         self.last_fcw_time = cur_time
         self.last_fcw_a = self.last_min_a
         return True

selfdrive/controls/lib/planner.py

@@ -201,7 +201,9 @@ class Planner(object):
       self.fcw_checker.reset_lead(cur_time)
 
     blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo,
+    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+                                  sm['controlsState'].active,
+                                  v_ego, sm['carState'].aEgo,
                                   lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
                                   lead_1.yRel, lead_1.vLat,
                                   lead_1.fcw, blinkers) and not sm['carState'].brakePressed

selfdrive/controls/radard.py

@@ -26,6 +26,11 @@ DIMSV = 2
 XV, SPEEDV = 0, 1
 VISION_POINT = -1
 
+path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
+
+# Time-alignment
+rate = 1. / DT_MDL  # model and radar are both at 20Hz
+v_len = 20   # how many speed data points to remember for t alignment with rdr data
 
 class EKFV1D(EKF):
   def __init__(self):
@@ -43,6 +48,185 @@ class EKFV1D(EKF):
     tfj = tf
     return tf, tfj
 
+class RadarD(object):
+  def __init__(self, VM, mocked):
+    self.VM = VM
+    self.mocked = mocked
+
+    self.MP = ModelParser()
+    self.tracks = defaultdict(dict)
+
+    self.last_md_ts = 0
+    self.last_controls_state_ts = 0
+
+    self.active = 0
+    self.steer_angle = 0.
+    self.steer_override = False
+
+    # Kalman filter stuff:
+    self.ekfv = EKFV1D()
+    self.speedSensorV = SimpleSensor(XV, 1, 2)
+
+    # v_ego
+    self.v_ego = 0.
+    self.v_ego_hist_t = deque([0], maxlen=v_len)
+    self.v_ego_hist_v = deque([0], maxlen=v_len)
+    self.v_ego_t_aligned = 0.
+
+  def update(self, frame, delay, sm, rr):
+    ar_pts = {}
+    for pt in rr.points:
+      ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
+
+    if sm.updated['liveParameters']:
+      self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
+
+    if sm.updated['controlsState']:
+      self.active = sm['controlsState'].active
+      self.v_ego = sm['controlsState'].vEgo
+      self.steer_angle = sm['controlsState'].angleSteers
+      self.steer_override = sm['controlsState'].steerOverride
+
+      self.v_ego_hist_v.append(self.v_ego)
+      self.v_ego_hist_t.append(float(frame)/rate)
+
+      self.last_controls_state_ts = sm.logMonoTime['controlsState']
+
+    if sm.updated['model']:
+      self.last_md_ts = sm.logMonoTime['model']
+      self.MP.update(self.v_ego, sm['model'])
+
+    # run kalman filter only if prob is high enough
+    if self.MP.lead_prob > 0.7:
+      reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var))
+      self.ekfv.update_scalar(reading)
+      self.ekfv.predict(DT_MDL)
+
+      # When changing lanes the distance to the lead car can suddenly change,
+      # which makes the Kalman filter output large relative acceleration
+      if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0:
+        self.ekfv.state[XV] = self.MP.lead_dist
+        self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
+        self.ekfv.state[SPEEDV] = 0.
+
+      ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])),
+                              float(self.ekfv.state[SPEEDV]), False)
+    else:
+      self.ekfv.state[XV] = self.MP.lead_dist
+      self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init]))
+      self.ekfv.state[SPEEDV] = 0.
+
+      if VISION_POINT in ar_pts:
+        del ar_pts[VISION_POINT]
+
+    # *** compute the likely path_y ***
+    if (self.active and not self.steer_override) or self.mocked:
+      # use path from model (always when mocking as steering is too noisy)
+      path_y = np.polyval(self.MP.d_poly, path_x)
+    else:
+      # use path from steer, set angle_offset to 0 it does not only report the physical offset
+      path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0]
+
+    # *** remove missing points from meta data ***
+    for ids in self.tracks.keys():
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
+
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      # ignore standalone vision point, unless we are mocking the radar
+      if ids == VISION_POINT and not self.mocked:
+        continue
+      rpt = ar_pts[ids]
+
+      # align v_ego by a fixed time to align it with the radar measurement
+      cur_time = float(frame)/rate
+      self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v)
+
+      d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
+      # add sign
+      d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
+
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track()
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override)
+
+    # allow the vision model to remove the stationary flag if distance and rel speed roughly match
+    if VISION_POINT in ar_pts:
+      fused_id = None
+      best_score = NO_FUSION_SCORE
+      for ids in self.tracks:
+        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2)
+        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel)
+        self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
+        if best_score > self.tracks[ids].vision_score:
+          fused_id = ids
+          best_score = self.tracks[ids].vision_score
+
+      if fused_id is not None:
+        self.tracks[fused_id].vision_cnt += 1
+        self.tracks[fused_id].update_vision_fusion()
+
+    if DEBUG:
+      print("NEW CYCLE")
+      if VISION_POINT in ar_pts:
+        print("vision", ar_pts[VISION_POINT])
+
+    idens = list(self.tracks.keys())
+    track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens])
+
+    # If we have multiple points, cluster them
+    if len(track_pts) > 1:
+      cluster_idxs = cluster_points_centroid(track_pts, 2.5)
+      clusters = [None] * (max(cluster_idxs) + 1)
+
+      for idx in xrange(len(track_pts)):
+        cluster_i = cluster_idxs[idx]
+        if clusters[cluster_i] is None:
+          clusters[cluster_i] = Cluster()
+        clusters[cluster_i].add(self.tracks[idens[idx]])
+
+    elif len(track_pts) == 1:
+      # TODO: why do we need this?
+      clusters = [Cluster()]
+      clusters[0].add(self.tracks[idens[0]])
+    else:
+      clusters = []
+
+    if DEBUG:
+      for i in clusters:
+        print(i)
+    # *** extract the lead car ***
+    lead_clusters = [c for c in clusters
+                     if c.is_potential_lead(self.v_ego)]
+    lead_clusters.sort(key=lambda x: x.dRel)
+    lead_len = len(lead_clusters)
+
+    # *** extract the second lead from the whole set of leads ***
+    lead2_clusters = [c for c in lead_clusters
+                      if c.is_potential_lead2(lead_clusters)]
+    lead2_clusters.sort(key=lambda x: x.dRel)
+    lead2_len = len(lead2_clusters)
+
+    # *** publish radarState ***
+    dat = messaging.new_message()
+    dat.init('radarState')
+    dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
+    dat.radarState.mdMonoTime = self.last_md_ts
+    dat.radarState.canMonoTimes = list(rr.canMonoTimes)
+    dat.radarState.radarErrors = list(rr.errors)
+    dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
+    if lead_len > 0:
+      dat.radarState.leadOne = lead_clusters[0].toRadarState()
+      if lead2_len > 0:
+        dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
+      else:
+        dat.radarState.leadTwo.status = False
+    else:
+      dat.radarState.leadOne.status = False
+
+    return dat
 
 ## fuses camera and radar data for best lead detection
 def radard_thread(gctx=None):
@@ -59,210 +243,34 @@ def radard_thread(gctx=None):
   cloudlog.info("radard is importing %s", CP.carName)
   RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
 
+  can_sock = messaging.sub_sock(service_list['can'].port)
   sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
 
-  # Default parameters
-  live_parameters = messaging.new_message()
-  live_parameters.init('liveParameters')
-  live_parameters.liveParameters.valid = True
-  live_parameters.liveParameters.steerRatio = CP.steerRatio
-  live_parameters.liveParameters.stiffnessFactor = 1.0
-
-  MP = ModelParser()
   RI = RadarInterface(CP)
 
-  last_md_ts = 0
-  last_controls_state_ts = 0
-
   # *** publish radarState and liveTracks
   radarState = messaging.pub_sock(service_list['radarState'].port)
   liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
 
-  path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
-
-  # Time-alignment
-  rate = 1. / DT_MDL  # model and radar are both at 20Hz
-  v_len = 20   # how many speed data points to remember for t alignment with rdr data
-
-  active = 0
-  steer_angle = 0.
-  steer_override = False
-
-  tracks = defaultdict(dict)
-
-  # Kalman filter stuff:
-  ekfv = EKFV1D()
-  speedSensorV = SimpleSensor(XV, 1, 2)
-
-  # v_ego
-  v_ego = 0.
-  v_ego_hist_t = deque([0], maxlen=v_len)
-  v_ego_hist_v = deque([0], maxlen=v_len)
-  v_ego_t_aligned = 0.
-
   rk = Ratekeeper(rate, print_delay_threshold=None)
-  while 1:
-    rr = RI.update()
+  RD = RadarD(VM, mocked)
 
-    ar_pts = {}
-    for pt in rr.points:
-      ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
+  while 1:
+    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+    rr = RI.update(can_strings)
+
+    if rr is None:
+      continue
 
     sm.update(0)
 
-    if sm.updated['liveParameters']:
-        VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
-
-    if sm.updated['controlsState']:
-      active = sm['controlsState'].active
-      v_ego = sm['controlsState'].vEgo
-      steer_angle = sm['controlsState'].angleSteers
-      steer_override = sm['controlsState'].steerOverride
-
-      v_ego_hist_v.append(v_ego)
-      v_ego_hist_t.append(float(rk.frame)/rate)
-
-      last_controls_state_ts = sm.logMonoTime['controlsState']
-
-    if sm.updated['model']:
-      last_md_ts = sm.logMonoTime['model']
-      MP.update(v_ego, sm['model'])
-
-
-    # run kalman filter only if prob is high enough
-    if MP.lead_prob > 0.7:
-      reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
-      ekfv.update_scalar(reading)
-      ekfv.predict(DT_MDL)
-
-      # When changing lanes the distance to the lead car can suddenly change,
-      # which makes the Kalman filter output large relative acceleration
-      if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
-        ekfv.state[XV] = MP.lead_dist
-        ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
-        ekfv.state[SPEEDV] = 0.
-
-      ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
-                              float(ekfv.state[SPEEDV]), False)
-    else:
-      ekfv.state[XV] = MP.lead_dist
-      ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
-      ekfv.state[SPEEDV] = 0.
-
-      if VISION_POINT in ar_pts:
-        del ar_pts[VISION_POINT]
-
-    # *** compute the likely path_y ***
-    if (active and not steer_override) or mocked:
-      # use path from model (always when mocking as steering is too noisy)
-      path_y = np.polyval(MP.d_poly, path_x)
-    else:
-      # use path from steer, set angle_offset to 0 it does not only report the physical offset
-      path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0]
-
-    # *** remove missing points from meta data ***
-    for ids in tracks.keys():
-      if ids not in ar_pts:
-        tracks.pop(ids, None)
-
-    # *** compute the tracks ***
-    for ids in ar_pts:
-      # ignore standalone vision point, unless we are mocking the radar
-      if ids == VISION_POINT and not mocked:
-        continue
-      rpt = ar_pts[ids]
-
-      # align v_ego by a fixed time to align it with the radar measurement
-      cur_time = float(rk.frame)/rate
-      v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v)
-
-      d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
-      # add sign
-      d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
-
-      # create the track if it doesn't exist or it's a new track
-      if ids not in tracks:
-        tracks[ids] = Track()
-      tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
-
-    # allow the vision model to remove the stationary flag if distance and rel speed roughly match
-    if VISION_POINT in ar_pts:
-      fused_id = None
-      best_score = NO_FUSION_SCORE
-      for ids in tracks:
-        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
-        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
-        tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
-        if best_score > tracks[ids].vision_score:
-          fused_id = ids
-          best_score = tracks[ids].vision_score
-
-      if fused_id is not None:
-        tracks[fused_id].vision_cnt += 1
-        tracks[fused_id].update_vision_fusion()
-
-    if DEBUG:
-      print("NEW CYCLE")
-      if VISION_POINT in ar_pts:
-        print("vision", ar_pts[VISION_POINT])
-
-    idens = list(tracks.keys())
-    track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
-
-    # If we have multiple points, cluster them
-    if len(track_pts) > 1:
-      cluster_idxs = cluster_points_centroid(track_pts, 2.5)
-      clusters = [None] * (max(cluster_idxs) + 1)
-
-      for idx in xrange(len(track_pts)):
-        cluster_i = cluster_idxs[idx]
-        if clusters[cluster_i] is None:
-          clusters[cluster_i] = Cluster()
-        clusters[cluster_i].add(tracks[idens[idx]])
-
-    elif len(track_pts) == 1:
-      # TODO: why do we need this?
-      clusters = [Cluster()]
-      clusters[0].add(tracks[idens[0]])
-    else:
-      clusters = []
-
-    if DEBUG:
-      for i in clusters:
-        print(i)
-    # *** extract the lead car ***
-    lead_clusters = [c for c in clusters
-                     if c.is_potential_lead(v_ego)]
-    lead_clusters.sort(key=lambda x: x.dRel)
-    lead_len = len(lead_clusters)
-
-    # *** extract the second lead from the whole set of leads ***
-    lead2_clusters = [c for c in lead_clusters
-                      if c.is_potential_lead2(lead_clusters)]
-    lead2_clusters.sort(key=lambda x: x.dRel)
-    lead2_len = len(lead2_clusters)
-
-    # *** publish radarState ***
-    dat = messaging.new_message()
-    dat.init('radarState')
-    dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
-    dat.radarState.mdMonoTime = last_md_ts
-    dat.radarState.canMonoTimes = list(rr.canMonoTimes)
-    dat.radarState.radarErrors = list(rr.errors)
-    dat.radarState.controlsStateMonoTime = last_controls_state_ts
-    if lead_len > 0:
-      dat.radarState.leadOne = lead_clusters[0].toRadarState()
-      if lead2_len > 0:
-        dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
-      else:
-        dat.radarState.leadTwo.status = False
-    else:
-      dat.radarState.leadOne.status = False
-
+    dat = RD.update(rk.frame, RI.delay, sm, rr)
     dat.radarState.cumLagMs = -rk.remaining*1000.
+
     radarState.send(dat.to_bytes())
 
     # *** publish tracks for UI debugging (keep last) ***
+    tracks = RD.tracks
     dat = messaging.new_message()
     dat.init('liveTracks', len(tracks))