mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

longitudinal parity: restore StarPilot lead tracking and accel behaviors

Browse Source
This commit is contained in:
firestar5683
2026-03-24 17:55:06 -05:00
parent 38a9eef286
commit 10948af7ac
4 changed files with 27 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+1
View File
@@ -185,6 +185,7 @@ struct FrogPilotPlan @0xf98d843bfd7004a3 {
weatherDaytime @33 :Bool;
weatherId @34 :Int16;
disableThrottle @35 :Bool;
trackingLead @36 :Bool;
}
struct FrogPilotRadarState @0xb86e6369214c01c8 {
frogpilot/controls/frogpilot_planner.py
+1
View File
@@ -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
selfdrive/controls/lib/longitudinal_planner.py
+7 -3
View File
@@ -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)
selfdrive/controls/radard.py
+18 -10
View File
@@ -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):