From 10948af7ac262992ebe39b48b39b33e929c4e7d9 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 24 Mar 2026 17:55:06 -0500 Subject: [PATCH] longitudinal parity: restore StarPilot lead tracking and accel behaviors --- cereal/custom.capnp | 1 + frogpilot/controls/frogpilot_planner.py | 1 + .../controls/lib/longitudinal_planner.py | 10 +++++-- selfdrive/controls/radard.py | 28 ++++++++++++------- 4 files changed, 27 insertions(+), 13 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 498039cf..bb88c2c6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -185,6 +185,7 @@ struct FrogPilotPlan @0xf98d843bfd7004a3 { weatherDaytime @33 :Bool; weatherId @34 :Int16; disableThrottle @35 :Bool; + trackingLead @36 :Bool; } struct FrogPilotRadarState @0xb86e6369214c01c8 { diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 263a2d47..3fd5a5e4 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -151,6 +151,7 @@ class FrogPilotPlanner: frogpilotPlan.desiredFollowDistance = int(self.frogpilot_following.desired_follow_distance) frogpilotPlan.disableThrottle = self.frogpilot_following.disable_throttle + frogpilotPlan.trackingLead = self.tracking_lead frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5f2bba80..3c8efd50 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -19,8 +19,8 @@ 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_MAX_VALS = [1.6, 1.2, 0.8, 0.6] -A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] +A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] +A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 @@ -274,7 +274,11 @@ class LongitudinalPlanner: stop_distance=getattr(frogpilot_toggles, "stop_distance", 6.0)) self.mpc.set_accel_limits(accel_clip[0], accel_clip[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - tracking_lead = sm['frogpilotPlan'].desiredFollowDistance > 0 + try: + tracking_lead = bool(sm['frogpilotPlan'].trackingLead) + except AttributeError: + # Backward-compatible fallback for stale schema instances. + tracking_lead = sm['frogpilotPlan'].desiredFollowDistance > 0 self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['frogpilotPlan'].dangerFactor, sm['frogpilotPlan'].tFollow, personality=sm['selfdriveState'].personality, tracking_lead=tracking_lead) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index c29f2b82..366cd2aa 100644 --- a/selfdrive/controls/radard.py +++ b/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 @@ -68,7 +68,7 @@ class Track: 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 +87,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) @@ -139,11 +139,14 @@ class Track: else: return self.leadRight - def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader): + 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 and self.dRel < model_data.position.x[-1] and self.vLeadK > 1: + if left_lane < -self.yRel < right_lane: self.radarfulFilter.update(1) return True else: @@ -194,6 +197,9 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): + 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 lead_v_rel_pred = lead_msg.v[0] - model_v_ego return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), @@ -201,7 +207,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa "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]), + "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), @@ -212,7 +218,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_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]: # Determine leads, this is where the essential logic happens @@ -237,7 +243,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn 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] + 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() @@ -326,8 +332,10 @@ class RadarD: 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):