Update

common/clutil.cc

@@ -79,6 +79,10 @@ cl_context cl_create_context(cl_device_id device_id) {
   return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
 }
 
+void cl_release_context(cl_context context) {
+  clReleaseContext(context);
+}
+
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
   return cl_program_from_source(ctx, device_id, util::read_file(path), args);
 }

common/clutil.h

@@ -23,6 +23,7 @@
 
 cl_device_id cl_get_device_id(cl_device_type device_type);
 cl_context cl_create_context(cl_device_id device_id);
+void cl_release_context(cl_context context);
 cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
 cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);

common/params.cc

@@ -313,7 +313,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ExperimentalModeViaLKAS", PERSISTENT | FROGPILOT_CONTROLS},
     {"ExperimentalModeViaTap", PERSISTENT | FROGPILOT_CONTROLS},
     {"Fahrenheit", PERSISTENT | FROGPILOT_VISUALS},
-    {"FingerprintLogged", CLEAR_ON_MANAGER_START},
     {"FlashPanda", CLEAR_ON_MANAGER_START},
     {"ForceAutoTune", PERSISTENT | FROGPILOT_CONTROLS},
     {"ForceAutoTuneOff", PERSISTENT | FROGPILOT_CONTROLS},
@@ -539,6 +538,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Updated", PERSISTENT},
     {"UpdateWheelImage", CLEAR_ON_MANAGER_START},
     {"UserCurvature", PERSISTENT},
+    {"UserLogged", CLEAR_ON_MANAGER_START},
     {"UseSI", PERSISTENT | FROGPILOT_VISUALS},
     {"UseStockColors", CLEAR_ON_MANAGER_START},
     {"UseVienna", PERSISTENT | FROGPILOT_CONTROLS},

selfdrive/SConscript

@@ -3,5 +3,5 @@ SConscript(['controls/lib/lateral_mpc_lib/SConscript'])
 SConscript(['controls/lib/longitudinal_mpc_lib/SConscript'])
 SConscript(['locationd/SConscript'])
 SConscript(['navd/SConscript'])
-SConscript(['modeld/SConscript'])
-SConscript(['ui/SConscript'])
+SConscript(['tinygrad_modeld/SConscript'])
+SConscript(['ui/SConscript'])

selfdrive/classic_modeld/fill_model_msg.py

@@ -93,34 +93,34 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
 
   # lane lines
   modelV2.init('laneLines', 6)
-  lane_probs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+  road_edge_stds = net_output_data['road_edges_stds'][0,:,0,0]
   for i in range(6):
     lane_line = modelV2.laneLines[i]
     if i < 4:
       fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
     elif i == 4:
-      if lane_probs[0] > 0:
+      if road_edge_stds[0] >= 0.5:
         leftLane_x = 0.5 * (net_output_data['lane_lines'][0,0,:,0] + net_output_data['lane_lines'][0,1,:,0])
         leftLane_y = 0.5 * (net_output_data['lane_lines'][0,0,:,1] + net_output_data['lane_lines'][0,1,:,1])
         fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y)
       else:
         fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
     elif i == 5:
-      if lane_probs[3] > 0:
+      if road_edge_stds[1] >= 0.5:
         rightLane_x = 0.5 * (net_output_data['lane_lines'][0,2,:,0] + net_output_data['lane_lines'][0,3,:,0])
         rightLane_y = 0.5 * (net_output_data['lane_lines'][0,2,:,1] + net_output_data['lane_lines'][0,3,:,1])
         fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y)
       else:
         fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
   modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
-  modelV2.laneLineProbs = lane_probs
+  modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
 
   # road edges
   modelV2.init('roadEdges', 2)
   for i in range(2):
     road_edge = modelV2.roadEdges[i]
     fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
-  modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
+  modelV2.roadEdgeStds = road_edge_stds.tolist()
 
   # leads
   modelV2.init('leadsV3', 3)

selfdrive/controls/controlsd.py

@@ -728,7 +728,6 @@ def update_frogpilot_variables(self, CS):
     self.always_on_lateral_active &= self.sm['liveCalibration'].calPerc >= 1
     self.always_on_lateral_active &= not (self.frogpilot_toggles.always_on_lateral_lkas and self.sm['frogpilotCarState'].alwaysOnLateralDisabled)
     self.always_on_lateral_active &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill
-    self.always_on_lateral_active = bool(self.always_on_lateral_active)
 
     if self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental_mode:
       self.experimental_mode = self.sm['frogpilotPlan'].experimentalMode

selfdrive/controls/radard.py

@@ -7,14 +7,15 @@
 
 import capnp
 from cereal import messaging, log, car
+from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL, Ratekeeper, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 
 from openpilot.common.simple_kalman import KF1D
 
-from openpilot.selfdrive.frogpilot.frogpilot_variables import LANE_WIDTH, get_frogpilot_toggles
+from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles
 
 # Default lead acceleration decay set to 50% at 1s
 _LEAD_ACCEL_TAU = 1.5
@@ -52,7 +53,7 @@ def __init__(self, dt: float):
 
 
 class Track:
-  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams, update_rate):
     self.identifier = identifier
     self.cnt = 0
     self.aLeadTau = _LEAD_ACCEL_TAU
@@ -61,6 +62,9 @@ def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
     self.K_K = kalman_params.K
     self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
 
+    # FrogPilot variables
+    self.far_lead_filter = FirstOrderFilter(0, 1, update_rate)
+
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
     self.dRel = d_rel   # LONG_DIST
@@ -109,19 +113,40 @@ def get_RadarState(self, model_prob: float = 0.0):
       "radarTrackId": self.identifier,
     }
 
-  def potential_adjacent_lead(self, far: bool, lane_width: float, left: bool, model_data: capnp._DynamicStructReader):
-    adjacent_lane_max = float('inf') if far else lane_width * 1.5
-    adjacent_lane_min = max(lane_width * 1.5, LANE_WIDTH * 1.5) if far else max(lane_width * 0.5, LANE_WIDTH / 2)
+  def potential_adjacent_lead(self, far: bool, left: bool, model_data: capnp._DynamicStructReader, standstill: bool):
+    if standstill or self.vLead < 1:
+      return False
 
-    y_delta = self.yRel + interp(self.dRel, model_data.position.x, model_data.position.y)
+    if far:
+      near_lane_index = 0 if left else 3
 
-    if left and adjacent_lane_min < y_delta < adjacent_lane_max:
-      return True
-    elif not left and adjacent_lane_min < -y_delta < adjacent_lane_max:
-      return True
+      lane_position = interp(self.dRel, model_data.laneLines[near_lane_index].x, model_data.laneLines[near_lane_index].y)
     else:
+      near_lane_index = 1 if left else 2
+      far_lane_index = 0 if left else 3
+
+      near_lane = interp(self.dRel, model_data.laneLines[near_lane_index].x, model_data.laneLines[near_lane_index].y)
+      far_lane = interp(self.dRel, model_data.laneLines[far_lane_index].x, model_data.laneLines[far_lane_index].y)
+
+    lead_position = self.yRel + interp(self.dRel, model_data.position.x, model_data.position.y)
+
+    if far:
+      return lead_position < lane_position if left else lead_position > lane_position
+    else:
+      return min(near_lane, far_lane) < lead_position < max(near_lane, far_lane)
+
+  def potential_far_lead(self, model_data: capnp._DynamicStructReader, standstill: bool):
+    if standstill or self.vLead < 1:
       return False
 
+    left_lane = interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+    right_lane = interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+
+    lead_position = self.yRel + interp(self.dRel, model_data.position.x, model_data.position.y)
+
+    self.far_lead_filter.update(min(left_lane, right_lane) < lead_position < max(left_lane, right_lane))
+    return self.far_lead_filter.x >= 0.99
+
   def potential_low_speed_lead(self, v_ego: float):
     # stop for stuff in front of you and low speed, even without model confirmation
     # Radar points closer than 0.75, are almost always glitches on toyota radars
@@ -182,7 +207,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
 
 
 def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
-             model_v_ego: float,
+             model_v_ego: float, standstill: bool,
              frogpilot_toggles: SimpleNamespace, frogpilotCarState: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader,
              low_speed_override: bool = True) -> dict[str, Any]:
   # Determine leads, this is where the essential logic happens
@@ -206,8 +231,8 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
       if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
         lead_dict = closest_track.get_RadarState()
 
-    if not lead_dict['status'] and v_ego > 1 and frogpilot_toggles.allow_far_lead_tracking:
-      far_lead_tracks = [c for c in tracks.values() if abs(c.yRel + interp(c.dRel, model_data.position.x, model_data.position.y)) < LANE_WIDTH / 2 and c.dRel > 50 and c.vLead > 1]
+    if not lead_dict['status'] and frogpilot_toggles.allow_far_lead_tracking:
+      far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(model_data, standstill)]
       if len(far_lead_tracks) > 0:
         closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
         lead_dict = closest_track.get_RadarState()
@@ -218,10 +243,10 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
   return lead_dict
 
 
-def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, lane_width: float, left: bool = True, far: bool = False) -> dict[str, Any]:
+def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, standstill: bool, left: bool = True, far: bool = False) -> dict[str, Any]:
   lead_dict = {'status': False}
 
-  adjacent_tracks = [c for c in tracks.values() if c.vLead > 1 and c.potential_adjacent_lead(far, lane_width, left, model_data)]
+  adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(far, left, model_data, standstill)]
   if len(adjacent_tracks) > 0:
     closest_track = min(adjacent_tracks, key=lambda c: c.dRel)
     lead_dict = closest_track.get_RadarState()
@@ -253,7 +278,7 @@ def __init__(self, frogpilot_toggles, radar_ts: float, delay: int = 0):
 
     self.classic_model = self.frogpilot_toggles.classic_model
 
-  def update(self, sm: messaging.SubMaster, rr):
+  def update(self, sm: messaging.SubMaster, rr, update_rate):
     self.ready = sm.seen['modelV2']
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
@@ -286,7 +311,7 @@ def update(self, sm: messaging.SubMaster, rr):
 
       # create the track if it doesn't exist or it's a new track
       if ids not in self.tracks:
-        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params, update_rate)
       self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
 
     # *** publish radarState ***
@@ -304,14 +329,14 @@ def update(self, sm: messaging.SubMaster, rr):
       model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3
     if len(leads_v3) > 1:
-      self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], sm['modelV2'], low_speed_override=True)
-      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], sm['modelV2'], low_speed_override=False)
+      self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotCarState'], sm['modelV2'], low_speed_override=True)
+      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotCarState'], sm['modelV2'], low_speed_override=False)
 
     if self.frogpilot_toggles.adjacent_lead_tracking and self.ready:
-      self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True)
-      self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True, far=True)
-      self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False)
-      self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False, far=True)
+      self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['carState'].standstill, left=True)
+      self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['carState'].standstill, left=True, far=True)
+      self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['carState'].standstill, left=False)
+      self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['carState'].standstill, left=False, far=True)
 
     # Update FrogPilot parameters
     if sm['frogpilotPlan'].togglesUpdated:
@@ -400,7 +425,7 @@ def main():
       if rr is None:
         continue
 
-      RD.update(sm, rr)
+      RD.update(sm, rr, 1.0 / CP.radarTimeStep)
       RD.publish(pm, -rk.remaining*1000.0)
       rk.monitor_time()
   else: