mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-31 13:13:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

honda: port Bosch-A-aware radard lead tracking from tested branch

Browse Source 
is_bosch_a_radar_car() and its downstream handling in radard.py never
landed alongside the radar decoder (interface.py/values.py/radar_interface.py)
-- without it, the newly-decoded Bosch-A radar points feed into radard's
generic path, which assumes the wrong Kalman filter timestep and is missing
a real staleness/freshness gate this radar's actual cadence needs.

Ports (unmodified in behavior, only renamed to match this repo's
HONDA_BOSCH_A / HONDA_BOSCH_A_RADAR_VERIFIED naming instead of "civic_bosch"):
- Kalman filter dt selected from the radar's real ~15Hz cadence
  (HONDA_BOSCH_A_RADAR_TS), not the ~20Hz model-cycle default
- measurement_update gating so a duplicate liveTracks payload (radar runs
  slower than the model loop) isn't absorbed twice by the KF
- a "preferred track" continuity path: a previously-selected lead stays
  selected through ordinary model-probability fluctuations as long as it's
  still coherent, instead of re-arbitrating from scratch every cycle
- staleness detection with two independent triggers (a better-scoring
  challenger track sustained over several cycles, or gross absolute-range
  disagreement with the model sustained over several cycles) that drops a
  stale preferred track rather than holding it indefinitely
- honda_bosch_a_low_speed_radar_lead_sane(), requiring a few real sweeps
  before a radar-only low-speed takeover

Tested on the branch this was developed on (109/109 relevant tests pass);
this port is a mechanical, unmodified transplant onto this repo's current
radard.py -- worth a run through this repo's own test suite before merge.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
This commit is contained in:
JamesL787
2026-08-25 19:13:42 -04:00
parent 903631ee53
commit 8d23733946
1 changed files with 192 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+192 -39
View File
@@ -14,6 +14,8 @@ from openpilot.common.swaglog import cloudlog
from openpilot.common.simple_kalman import KF1D
from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection, LaneChangeState
from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
from opendbc.car.honda.radar_interface import BOSCH_A_FREQ_HZ
from opendbc.car.honda.values import HONDA_BOSCH_A
# Default lead acceleration decay set to 50% at 1s
@@ -29,6 +31,16 @@ RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
G90_RADAR_LOW_SPEED_MAX_DIST = 12.0
G90_RADAR_LOW_SPEED_MAX_Y = 0.6
HONDA_BOSCH_A_RADAR_TS = 1.0 / BOSCH_A_FREQ_HZ
HONDA_BOSCH_A_LOW_SPEED_MIN_COUNT = 3
HONDA_BOSCH_A_CHALLENGER_STALE_CYCLES = 2
HONDA_BOSCH_A_GROSS_DISTANCE_STALE_CYCLES = 3
HONDA_BOSCH_A_GROSS_DISTANCE_M = 25.0
def is_bosch_a_radar_car(CP) -> bool:
return CP.brand == "honda" and CP.carFingerprint in HONDA_BOSCH_A and not CP.radarUnavailable
# Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light
# approaches. The qualifier is the DECELERATION HISTORY, not the current speed: roadside
@@ -79,7 +91,8 @@ class Track:
self.rest_frames = 0
self.seen_moving = False
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: bool,
measurement_update: bool | None = None):
# relative values, copy
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
@@ -87,35 +100,44 @@ class Track:
self.vLead = v_lead
self.measured = measured # measured or estimate
# `measurement_update` is separate from the published measured bit so legacy radar sources keep
# their existing behaviour. Civic Bosch emits real measurements at ~15 Hz while radard is driven
# at the ~20 Hz model rate; duplicate liveTracks payloads must not be absorbed twice.
if measurement_update is None:
# Preserve the historical Track.update behaviour for direct/legacy callers. The radar source
# adapter supplies an explicit False only for a duplicate Civic Bosch payload.
measurement_update = True
# computed velocity and accelerations
if self.cnt > 0:
if measurement_update and self.cnt > 0:
self.kf.update(self.vLead)
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
else:
self.aLeadTau.update(0.0)
if measurement_update:
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU)
else:
self.aLeadTau.update(0.0)
# Track the moving -> stopped transition. Only sustained runs count, so one noisy
# speed sample can neither arm nor trip the detector.
if self.vLead > ADJACENT_STOP_MOVING_V:
self.moving_frames += 1
self.rest_frames = 0
if self.moving_frames >= ADJACENT_STOP_MOVING_FRAMES:
self.seen_moving = True
elif abs(self.vLead) < ADJACENT_STOP_REST_V:
self.moving_frames = 0
self.rest_frames += 1
else:
# coasting between the two bands: hold state, restart both runs
self.moving_frames = 0
self.rest_frames = 0
# Track the moving -> stopped transition. Only sustained runs count, so one noisy
# speed sample can neither arm nor trip the detector.
if self.vLead > ADJACENT_STOP_MOVING_V:
self.moving_frames += 1
self.rest_frames = 0
if self.moving_frames >= ADJACENT_STOP_MOVING_FRAMES:
self.seen_moving = True
elif abs(self.vLead) < ADJACENT_STOP_REST_V:
self.moving_frames = 0
self.rest_frames += 1
else:
# coasting between the two bands: hold state, restart both runs
self.moving_frames = 0
self.rest_frames = 0
self.cnt += 1
self.cnt += 1
def get_RadarState(self, model_prob: float = 0.0):
return {
@@ -186,6 +208,14 @@ def laplacian_pdf(x: float, mu: float, b: float):
return math.exp(-abs(x-mu)/b)
def vision_track_probability(track: Track, lead: capnp._DynamicStructReader, v_ego: float) -> float:
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
prob_d = laplacian_pdf(track.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(track.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_pdf(track.vRel + v_ego, lead.v[0], lead.vStd[0])
return prob_d * prob_y * prob_v
def g90_radar_lead_lateral_sane(track: Track) -> bool:
# The G90 extended radar channels can report close side ghosts in tight turns.
# Keep the gate tight at close range, then widen gradually with distance.
@@ -199,6 +229,11 @@ def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
abs(track.yRel) < G90_RADAR_LOW_SPEED_MAX_Y)
def honda_bosch_a_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
"""Require a few real Bosch sweeps before a radar-only low-speed takeover."""
return track.cnt >= HONDA_BOSCH_A_LOW_SPEED_MIN_COUNT and track.potential_low_speed_lead(v_ego)
def track_matches_vision(track: Track, lead: capnp._DynamicStructReader, v_ego: float, *,
dist_scale: float, dist_floor: float, vel_limit: float,
y_std_scale: float, y_floor: float) -> bool:
@@ -225,14 +260,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
if not tracks:
return None
def prob(c):
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
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])
return prob_d * prob_y * prob_v
track = max(tracks.values(), key=prob)
track = max(tracks.values(), key=lambda candidate: vision_track_probability(candidate, lead, v_ego))
# if no 'sane' match is found return -1
# stationary radar points can be false positives
@@ -278,7 +306,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool,
starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace,
low_speed_override: bool = True, g90_radar_filter: bool = False, lead_prob: float | None = None,
preferred_track_id: int = -1) -> dict[str, Any]:
preferred_track_id: int = -1, honda_bosch_a_radar: bool = False) -> dict[str, Any]:
lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35))
filtered_lead_prob = float(lead_msg.prob if lead_prob is None else lead_prob)
@@ -298,8 +326,48 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
if low_speed_override:
if g90_radar_filter:
low_speed_tracks = [c for c in tracks.values() if g90_low_speed_radar_lead_sane(c, v_ego)]
elif honda_bosch_a_radar:
low_speed_tracks = [c for c in tracks.values() if honda_bosch_a_low_speed_radar_lead_sane(c, v_ego)]
else:
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
model_lead_available = ready and filtered_lead_prob > lead_detection_probability
# Keep a previously selected Bosch radar track through ordinary model-probability fluctuations
# when it is still coherent. If the model has a valid lead, the old track must still agree with
# that lead; no model lead leaves the mature radar track eligible for continuity.
if honda_bosch_a_radar:
preferred_track = tracks.get(preferred_track_id)
if (preferred_track is not None and honda_bosch_a_low_speed_radar_lead_sane(preferred_track, v_ego)):
preferred_matches_model = (not model_lead_available or
track_matches_vision(preferred_track, lead_msg, v_ego,
dist_scale=0.25, dist_floor=5.0,
vel_limit=10.0, y_std_scale=1.0, y_floor=1.0))
preferred_is_current = (not lead_dict.get('status', False) or
lead_dict.get('radarTrackId', -1) == preferred_track_id or
(lead_dict.get('status', False) and not lead_dict.get('radar', False)))
if preferred_is_current and preferred_matches_model:
lead_dict = preferred_track.get_RadarState(filtered_lead_prob)
def candidate_is_established(candidate: Track) -> bool:
if not honda_bosch_a_radar:
return True
if candidate.cnt < HONDA_BOSCH_A_LOW_SPEED_MIN_COUNT:
return False
if not lead_dict.get('status', False):
# A mature centered Bosch point may provide the radar-only low-speed lead.
return True
if lead_dict.get('radarTrackId', -1) == candidate.identifier:
return True
# Do not replace an established lead with an unrelated closer point when there is no model
# evidence to arbitrate them. A different candidate may take over only after it agrees with
# the available model lead; radar-only takeover remains possible when lead_dict is invalid.
return (model_lead_available and
track_matches_vision(candidate, lead_msg, v_ego,
dist_scale=0.25, dist_floor=5.0,
vel_limit=10.0, y_std_scale=1.0, y_floor=1.0))
low_speed_tracks = [c for c in low_speed_tracks if candidate_is_established(c)]
if len(low_speed_tracks) > 0:
closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
@@ -351,18 +419,27 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr
class RadarD:
def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False):
def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False,
honda_bosch_a_radar: bool = False):
self.current_time = 0.0
self.tracks: dict[int, Track] = {}
self.kalman_params = KalmanParams(radar_ts)
self.honda_bosch_a_radar = honda_bosch_a_radar
# The lead KF consumes Bosch measurements at the physical radar cadence. Lead probability
# filters, however, consume modelV2 leads every model cycle and must retain model-loop timing.
kf_dt = HONDA_BOSCH_A_RADAR_TS if self.honda_bosch_a_radar else radar_ts
self.kalman_params = KalmanParams(kf_dt)
self.g90_radar_filter = g90_radar_filter
self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, radar_ts) for _ in range(2)]
self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)]
self.prev_lead_track_ids = [-1, -1]
self.preferred_stale_track_ids = [-1, -1]
self.preferred_challenger_stale_counts = [0, 0]
self.preferred_gross_distance_stale_counts = [0, 0]
self.v_ego = 0.0
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
self.last_v_ego_frame = -1
self._last_tracks_frame = -1
self.radar_state: capnp._DynamicStructBuilder | None = None
self.radar_state_valid = False
@@ -372,6 +449,63 @@ class RadarD:
self.starpilot_radar_state = custom.StarPilotRadarState.new_message()
self.starpilot_toggles = get_starpilot_toggles()
def _reset_preferred_stale_evidence(self, lead_index: int, track_id: int = -1) -> None:
self.preferred_stale_track_ids[lead_index] = track_id
self.preferred_challenger_stale_counts[lead_index] = 0
self.preferred_gross_distance_stale_counts[lead_index] = 0
def _update_honda_bosch_a_preferred_staleness(self, lead_index: int, lead: capnp._DynamicStructReader,
lead_prob: float) -> None:
if not self.honda_bosch_a_radar:
return
preferred_id = self.prev_lead_track_ids[lead_index]
if self.preferred_stale_track_ids[lead_index] != preferred_id:
self._reset_preferred_stale_evidence(lead_index, preferred_id)
lead_detection_probability = float(getattr(self.starpilot_toggles, "lead_detection_probability", 0.35))
preferred_track = self.tracks.get(preferred_id)
if preferred_id < 0 or preferred_track is None or not self.ready or lead_prob <= lead_detection_probability:
self._reset_preferred_stale_evidence(lead_index, preferred_id)
return
strict_match = track_matches_vision(preferred_track, lead, self.v_ego,
dist_scale=0.25, dist_floor=5.0,
vel_limit=10.0, y_std_scale=1.0, y_floor=1.0)
relaxed_match = track_matches_vision(preferred_track, lead, self.v_ego,
dist_scale=0.40, dist_floor=8.0,
vel_limit=13.0, y_std_scale=2.0, y_floor=1.5)
# Arm A: a preferred track that no longer passes continuity may be stale when another live
# track has a better association score. Clearing preference never selects that challenger;
# the unchanged strict match path below remains the only way it can become a radar lead.
if relaxed_match:
self.preferred_challenger_stale_counts[lead_index] = 0
else:
best_track = max(self.tracks.values(), key=lambda candidate: vision_track_probability(candidate, lead, self.v_ego))
preferred_score = vision_track_probability(preferred_track, lead, self.v_ego)
best_score = vision_track_probability(best_track, lead, self.v_ego)
if best_track.identifier != preferred_id and best_score > preferred_score:
self.preferred_challenger_stale_counts[lead_index] += 1
else:
self.preferred_challenger_stale_counts[lead_index] = 0
# Arm B: gross absolute range disagreement is independent evidence of staleness, but a strict
# match is authoritative and resets the streak even when model uncertainty permits >25 m error.
distance_mismatch = abs(preferred_track.dRel - (lead.x[0] - RADAR_TO_CAMERA))
if strict_match:
self.preferred_gross_distance_stale_counts[lead_index] = 0
elif distance_mismatch > HONDA_BOSCH_A_GROSS_DISTANCE_M:
self.preferred_gross_distance_stale_counts[lead_index] += 1
else:
self.preferred_gross_distance_stale_counts[lead_index] = 0
challenger_stale = self.preferred_challenger_stale_counts[lead_index] >= HONDA_BOSCH_A_CHALLENGER_STALE_CYCLES
distance_stale = self.preferred_gross_distance_stale_counts[lead_index] >= HONDA_BOSCH_A_GROSS_DISTANCE_STALE_CYCLES
if challenger_stale or distance_stale:
self.prev_lead_track_ids[lead_index] = -1
self._reset_preferred_stale_evidence(lead_index)
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
self.ready = sm.seen['modelV2']
self.current_time = 1e-9 * max(sm.logMonoTime.values())
@@ -381,6 +515,11 @@ class RadarD:
self.v_ego_hist.append(self.v_ego)
self.last_v_ego_frame = sm.recv_frame['carState']
radar_fresh = True
if self.honda_bosch_a_radar:
radar_fresh = sm.recv_frame['liveTracks'] != self._last_tracks_frame
self._last_tracks_frame = sm.recv_frame['liveTracks']
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points}
# *** remove missing points from meta data ***
@@ -396,7 +535,11 @@ class RadarD:
# 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].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
measured = rpt[3] if not self.honda_bosch_a_radar else bool(rpt[3] and radar_fresh)
# Non-Bosch sources retain the historical per-model-cycle update semantics. Only Civic Bosch
# suppresses duplicate measurement updates when liveTracks has not advanced.
measurement_update = True if not self.honda_bosch_a_radar else measured
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, measured, measurement_update)
# *** publish radarState ***
self.radar_state_valid = sm.all_checks()
@@ -421,20 +564,28 @@ class RadarD:
else:
self.lead_prob_filters[i].update(lead_prob)
self._update_honda_bosch_a_preferred_staleness(i, leads_v3[i], self.lead_prob_filters[i].x)
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['starpilotPlan'], self.starpilot_toggles, low_speed_override=True,
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[0].x,
preferred_track_id=self.prev_lead_track_ids[0])
preferred_track_id=self.prev_lead_track_ids[0],
honda_bosch_a_radar=self.honda_bosch_a_radar)
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['starpilotPlan'], self.starpilot_toggles, low_speed_override=False,
g90_radar_filter=self.g90_radar_filter, lead_prob=self.lead_prob_filters[1].x,
preferred_track_id=self.prev_lead_track_ids[1])
preferred_track_id=self.prev_lead_track_ids[1],
honda_bosch_a_radar=self.honda_bosch_a_radar)
for i, lead in enumerate((self.radar_state.leadOne, self.radar_state.leadTwo)):
if lead.status and getattr(lead, "radar", False):
self.prev_lead_track_ids[i] = int(getattr(lead, "radarTrackId", -1))
track_id = int(getattr(lead, "radarTrackId", -1))
if track_id != self.prev_lead_track_ids[i]:
self._reset_preferred_stale_evidence(i, track_id)
self.prev_lead_track_ids[i] = track_id
elif (not lead.status) or (self.prev_lead_track_ids[i] not in self.tracks):
self.prev_lead_track_ids[i] = -1
self._reset_preferred_stale_evidence(i)
if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes):
self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
@@ -481,7 +632,9 @@ def main() -> None:
radar_ts = DT_MDL
g90_radar_filter = CP.brand == "hyundai" and CP.carFingerprint == "GENESIS_G90"
RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay, g90_radar_filter=g90_radar_filter)
honda_bosch_a_radar = is_bosch_a_radar_car(CP)
RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay, g90_radar_filter=g90_radar_filter,
honda_bosch_a_radar=honda_bosch_a_radar)
sm = sm.extend(['starpilotPlan'])
pm = pm.extend(['starpilotRadarState'])