Long?

frogpilot/controls/frogpilot_planner.py

@@ -60,33 +60,31 @@ class FrogPilotPlanner:
   def update(self, now, time_validated, sm, frogpilot_toggles):
     self.lead_one = sm["radarState"].leadOne
 
-    long_control_active = sm["carControl"].longActive
     controls_enabled = sm["selfdriveState"].enabled
-    planner_active = controls_enabled or long_control_active
 
-    v_cruise_kph = sm["carState"].vCruise
+    v_cruise_kph = min(sm["carState"].vCruise, V_CRUISE_MAX)
     if 0 < v_cruise_kph < V_CRUISE_UNSET and frogpilot_toggles.set_speed_offset > 0:
       v_cruise_kph += frogpilot_toggles.set_speed_offset
-    v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
     v_ego = max(sm["carState"].vEgo, 0)
 
-    if planner_active:
+    if controls_enabled:
       self.frogpilot_acceleration.update(v_ego, sm, frogpilot_toggles)
     else:
       self.frogpilot_acceleration.max_accel = 0
       self.frogpilot_acceleration.min_accel = 0
 
-    if planner_active and frogpilot_toggles.conditional_experimental_mode:
+    if controls_enabled and frogpilot_toggles.conditional_experimental_mode:
       self.frogpilot_cem.update(v_ego, sm, frogpilot_toggles)
     else:
-      self.frogpilot_cem.experimental_mode = False
+      self.frogpilot_cem.curve_detected = False
       self.frogpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
 
     self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION
 
-    self.frogpilot_events.update(planner_active, v_cruise, sm, frogpilot_toggles)
+    self.frogpilot_events.update(controls_enabled, v_cruise, sm, frogpilot_toggles)
 
-    self.frogpilot_following.update(planner_active, v_ego, sm, frogpilot_toggles)
+    self.frogpilot_following.update(controls_enabled, v_ego, sm, frogpilot_toggles)
 
     gps_location = sm[self.gps_location_service]
     self.gps_position = {
@@ -97,7 +95,8 @@ class FrogPilotPlanner:
     self.gps_valid = self.gps_position["latitude"] != 0 or self.gps_position["longitude"] != 0
     self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position))
 
-    if v_ego >= frogpilot_toggles.minimum_lane_change_speed:
+    check_lane_width = frogpilot_toggles.adjacent_paths or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection
+    if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed:
       self.lane_width_left = calculate_lane_width(sm["modelV2"].laneLines[0], sm["modelV2"].laneLines[1], sm["modelV2"].roadEdges[0])
       self.lane_width_right = calculate_lane_width(sm["modelV2"].laneLines[3], sm["modelV2"].laneLines[2], sm["modelV2"].roadEdges[1])
     else:
@@ -123,7 +122,7 @@ class FrogPilotPlanner:
     if not sm["carState"].standstill:
       self.tracking_lead = self.update_lead_status(frogpilot_toggles.stop_distance)
 
-    self.v_cruise = self.frogpilot_vcruise.update(planner_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles)
+    self.v_cruise = self.frogpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles)
 
     if self.gps_valid and time_validated and frogpilot_toggles.weather_presets:
       self.frogpilot_weather.update_weather(now, frogpilot_toggles)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -540,7 +540,7 @@ class LongitudinalMpc:
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':
-      self.params[:,5] = danger_factor
+      self.params[:,5] = LEAD_DANGER_FACTOR
 
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.

selfdrive/controls/lib/longitudinal_planner.py

@@ -16,8 +16,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
 
-from openpilot.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION
-
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.0
 A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.]
@@ -51,11 +49,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   # The lookup table for turns should also be updated if we do this
   a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
   a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
-
-  if abs(a_y) > MINIMUM_LATERAL_ACCELERATION:
-    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
-  else:
-    a_x_allowed = a_target[1]
+  a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
 
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
@@ -146,8 +140,11 @@ class LongitudinalPlanner:
 
   @staticmethod
   def get_model_speed_error(model_msg, v_ego):
-    if len(model_msg.temporalPose.trans):
-      return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0))
+    try:
+      if len(model_msg.temporalPose.trans):
+        return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0))
+    except AttributeError:
+      pass
     if len(model_msg.velocity.x) == ModelConstants.IDX_N:
       return float(np.clip(model_msg.velocity.x[0] - v_ego, -5.0, 5.0))
     return 0.0

selfdrive/controls/radard.py

@@ -18,7 +18,7 @@ from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpi
 
 
 # Default lead acceleration decay set to 50% at 1s
-_LEAD_ACCEL_TAU = 1.5
+_LEAD_ACCEL_TAU = 0.6
 
 # radar tracks
 SPEED, ACCEL = 0, 1     # Kalman filter states enum
@@ -37,9 +37,6 @@ class KalmanParams:
     assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
     self.A = [[1.0, dt], [0.0, 1.0]]
     self.C = [1.0, 0.0]
-    #Q = np.matrix([[10., 0.0], [0.0, 100.]])
-    #R = 1e3
-    #K = np.matrix([[ 0.05705578], [ 0.03073241]])
     dts = [i * 0.01 for i in range(1, 21)]
     K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689,  0.21372394,
           0.22761098, 0.24069424, 0.253096,   0.26491023, 0.27621103, 0.28705801,
@@ -63,12 +60,8 @@ class Track:
     self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
 
     # FrogPilot variables
-    self.leadLeft = False
-    self.leadRight = False
-
     self.leadTrackID = 0
-
-    self.radarfulFilter = FirstOrderFilter(0, 1, self.K_A[0][1])
+    self.radarfulFilter = FirstOrderFilter(0, 0.5, self.K_A[0][1])
 
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
@@ -87,7 +80,7 @@ class Track:
 
     # Learn if constant acceleration
     if abs(self.aLeadK) < 0.5:
-      self.aLeadTau.x = _LEAD_ACCEL_TAU
+      self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
     else:
       self.aLeadTau.update(0.0)
 
@@ -109,6 +102,29 @@ class Track:
       "radarTrackId": self.identifier,
     }
 
+  def potential_adjacent_lead(self, left: bool, standstill: bool, model_data: capnp._DynamicStructReader):
+    if standstill or self.vLead < 1 or self.leadTrackID == self.identifier:
+      return False
+
+    if left:
+      left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+      return -self.yRel < left_lane
+    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+    return -self.yRel > right_lane
+
+  def potential_far_lead(self, standstill: bool, model_data: capnp._DynamicStructReader):
+    if standstill or self.vLead < 1 or abs(self.yRel) > 1:
+      return False
+
+    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+
+    if left_lane < -self.yRel < right_lane:
+      self.radarfulFilter.update(1)
+      return True
+    self.radarfulFilter.update(0)
+    return False
+
   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
@@ -121,35 +137,6 @@ class Track:
     ret = f"x: {self.dRel:4.1f}  y: {self.yRel:4.1f}  v: {self.vRel:4.1f}  a: {self.aLeadK:4.1f}"
     return ret
 
-  # FrogPilot variables
-  def potential_adjacent_lead(self, left: bool, model_data: capnp._DynamicStructReader):
-    if self.vLeadK < 1 or self.leadTrackID == self.identifier:
-      return False
-
-    far_left_lane = np.interp(self.dRel, model_data.laneLines[0].x, model_data.laneLines[0].y)
-    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
-    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
-    far_right_lane = np.interp(self.dRel, model_data.laneLines[3].x, model_data.laneLines[3].y)
-
-    self.leadLeft = far_left_lane < -self.yRel < left_lane and self.dRel < model_data.position.x[-1]
-    self.leadRight = right_lane < -self.yRel < far_right_lane and self.dRel < model_data.position.x[-1]
-
-    if left:
-      return self.leadLeft
-    else:
-      return self.leadRight
-
-  def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader):
-    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
-    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
-
-    if left_lane < -self.yRel < right_lane and self.dRel < model_data.position.x[-1] and self.vLeadK > 1:
-      self.radarfulFilter.update(1)
-      return True
-    else:
-      self.radarfulFilter.update(0)
-      return False
-
 
 def laplacian_pdf(x: float, mu: float, b: float):
   b = max(b, 1e-4)
@@ -157,19 +144,12 @@ def laplacian_pdf(x: float, mu: float, b: float):
 
 
 def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace):
-  # FrogPilot variables
-  if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and frogpilot_toggles.human_lane_changes:
+  if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(frogpilot_toggles, "human_lane_changes", False):
     direction = model_data.meta.laneChangeDirection
-
     if direction == LaneChangeDirection.left:
-      left_tracks = [track for track in tracks.values() if track.leadLeft]
-      if left_tracks:
-        return min(left_tracks, key=lambda c: c.dRel)
-
+      tracks = {k: v for k, v in tracks.items() if v.yRel > 0}
     elif direction == LaneChangeDirection.right:
-      right_tracks = [track for track in tracks.values() if track.leadRight]
-      if right_tracks:
-        return min(right_tracks, key=lambda c: c.dRel)
+      tracks = {k: v for k, v in tracks.items() if v.yRel < 0}
 
   offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
 
@@ -177,31 +157,30 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
     prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
     prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
     prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
-
-    # This isn't exactly right, but it's a good heuristic
     return prob_d * prob_y * prob_v
 
   track = max(tracks.values(), key=prob)
 
   # if no 'sane' match is found return -1
   # stationary radar points can be false positives
-  dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+  dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist) * .25, 5.0])
   vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
   if dist_sane and vel_sane:
     return track
-  else:
-    return None
+  return None
 
 
 def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
-  lead_v_rel_pred = lead_msg.v[0] - model_v_ego
+  prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0)
+  blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK
+  get_RadarState_from_vision.prev_aLeadK = blended_aLeadK
   return {
     "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
     "yRel": float(-lead_msg.y[0]),
-    "vRel": float(lead_v_rel_pred),
-    "vLead": float(v_ego + lead_v_rel_pred),
-    "vLeadK": float(v_ego + lead_v_rel_pred),
-    "aLeadK": float(lead_msg.a[0]),
+    "vRel": float(lead_msg.v[0] - model_v_ego),
+    "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)),
+    "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)),
+    "aLeadK": blended_aLeadK,
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),
@@ -212,13 +191,10 @@ 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_data: capnp._DynamicStructReader,
+             model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool,
              frogpilot_plan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace,
              low_speed_override: bool = True) -> dict[str, Any]:
-  lead_detection_probability = float(getattr(frogpilot_toggles, "lead_detection_probability", 0.35) or 0.35)
-  if lead_detection_probability > 1.0:
-    lead_detection_probability *= 0.01
-  lead_detection_probability = float(np.clip(lead_detection_probability, 0.25, 0.5))
+  lead_detection_probability = float(getattr(frogpilot_toggles, "lead_detection_probability", 0.35))
 
   # Determine leads, this is where the essential logic happens
   if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability:
@@ -241,11 +217,11 @@ 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 low_speed_override and not lead_dict['status'] and len(tracks) > 0:
-    far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(lead_msg, model_data) and c.radarfulFilter.x >= THRESHOLD]
-    if len(far_lead_tracks) > 0:
-      closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
-      lead_dict = closest_track.get_RadarState()
+    if not lead_dict['status'] and len(tracks) > 0:
+      far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD]
+      if len(far_lead_tracks) > 0:
+        closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
+        lead_dict = closest_track.get_RadarState()
 
   # FrogPilot variables
   for track in tracks.values():
@@ -257,11 +233,10 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
   return lead_dict
 
 
-# FrogPilot variables
-def get_adjacent_lead(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, left: bool = True) -> dict[str, Any]:
+def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: capnp._DynamicStructReader, left: bool = True) -> dict[str, Any]:
   lead_dict = {'status': False}
 
-  adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(left, model_data)]
+  adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(left, standstill, model_data)]
   if len(adjacent_tracks) > 0:
     closest_track = min(adjacent_tracks, key=lambda c: c.dRel)
     lead_dict = closest_track.get_RadarState()
@@ -277,7 +252,7 @@ class RadarD:
     self.kalman_params = KalmanParams(DT_MDL)
 
     self.v_ego = 0.0
-    self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
+    self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
     self.last_v_ego_frame = -1
 
     self.radar_state: capnp._DynamicStructBuilder | None = None
@@ -287,12 +262,11 @@ class RadarD:
 
     # FrogPilot variables
     self.frogpilot_radar_state = custom.FrogPilotRadarState.new_message()
-
     self.frogpilot_toggles = get_frogpilot_toggles()
 
   def update(self, sm: messaging.SubMaster, rr: car.RadarData):
     self.ready = sm.seen['modelV2']
-    self.current_time = 1e-9*max(sm.logMonoTime.values())
+    self.current_time = 1e-9 * max(sm.logMonoTime.values())
 
     if sm.recv_frame['carState'] != self.last_v_ego_frame:
       self.v_ego = sm['carState'].vEgo
@@ -307,9 +281,7 @@ class RadarD:
         self.tracks.pop(ids, None)
 
     # *** compute the tracks ***
-    for ids in ar_pts:
-      rpt = ar_pts[ids]
-
+    for ids, rpt in ar_pts.items():
       # align v_ego by a fixed time to align it with the radar measurement
       v_lead = rpt[2] + self.v_ego_hist[0]
 
@@ -325,19 +297,24 @@ class RadarD:
     self.radar_state.radarErrors = rr.errors
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
+    self.frogpilot_radar_state = custom.FrogPilotRadarState.new_message()
+
     if len(sm['modelV2'].velocity.x):
       model_v_ego = sm['modelV2'].velocity.x[0]
     else:
       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, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True)
-      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False)
+      self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'],
+                                          sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True)
+      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'],
+                                          sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False)
 
     # FrogPilot variables
     if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking or self.frogpilot_toggles.human_lane_changes):
-      self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['modelV2'], left=True)
-      self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['modelV2'], left=False)
+      self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
+      self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=False)
 
     self.frogpilot_toggles = get_frogpilot_toggles(sm)