From ab052d6034d4f03e9684df3500f4596f07de67e8 Mon Sep 17 00:00:00 2001 From: JamesL787 Date: Tue, 25 Aug 2026 19:19:07 -0400 Subject: [PATCH] honda: port Bosch-A-aware radard lead tracking from tested branch is_bosch_a_radar_car() and its downstream handling in radard.py never landed alongside the Bosch-A radar decoder (from #16, pending merge into nrdr/opendbc) -- 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, renamed to this repo's HONDA_BOSCH_A 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 radarTracks 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 Purely additive relative to this file's current content -- all other car behavior is untouched. Tested on the branch this was developed on (109/109 relevant tests pass); this port is a mechanical, unmodified transplant -- worth a run through this repo's own test suite before merge. Co-Authored-By: Claude Sonnet 5 --- openpilot/selfdrive/controls/radard.py | 212 +++++++++++++++++++++++-- 1 file changed, 195 insertions(+), 17 deletions(-) diff --git a/openpilot/selfdrive/controls/radard.py b/openpilot/selfdrive/controls/radard.py index e6b5a0aea8b..50b625665e4 100755 --- a/openpilot/selfdrive/controls/radard.py +++ b/openpilot/selfdrive/controls/radard.py @@ -12,6 +12,8 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D +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 @@ -24,6 +26,15 @@ V_EGO_STATIONARY = 4. # no stationary object flag below this speed RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +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 class KalmanParams: @@ -58,27 +69,38 @@ 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) - def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float): + def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: bool = True, + measurement_update: bool | None = None): # relative values, copy self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED 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 = _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 = _LEAD_ACCEL_TAU + else: + self.aLeadTau.update(0.0) - self.cnt += 1 + self.cnt += 1 def get_RadarState(self, model_prob: float = 0.0): return { @@ -110,6 +132,29 @@ 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 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: + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA + dist_sane = abs(track.dRel - offset_vision_dist) < max(abs(offset_vision_dist) * dist_scale, dist_floor) + vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < vel_limit) or (v_ego + track.vRel > 3) + lat_sane = abs(track.yRel + lead.y[0]) < max(y_floor, y_std_scale * max(float(lead.yStd[0]), 0.2)) + return dist_sane and vel_sane and lat_sane + + def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA @@ -151,7 +196,8 @@ 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, lead_prob: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, lead_prob: float, low_speed_override: bool = True, + preferred_track_id: int = -1, honda_bosch_a_radar: bool = False) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -165,7 +211,48 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob) if low_speed_override: - low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] + if 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 lead_prob > .5 + + # 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('present', False) or + lead_dict.get('radarTrackId', -1) == preferred_track_id or + (lead_dict.get('present', False) and not lead_dict.get('radar', False))) + if preferred_is_current and preferred_matches_model: + lead_dict = preferred_track.get_RadarState(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('present', 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) @@ -177,20 +264,85 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn class RadarD: - def __init__(self, delay: float = 0.0): + def __init__(self, delay: float = 0.0, honda_bosch_a_radar: bool = False): + self.honda_bosch_a_radar = honda_bosch_a_radar self.tracks: dict[int, Track] = {} - self.kalman_params = KalmanParams(DT_MDL) + # 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 DT_MDL + self.kalman_params = KalmanParams(kf_dt) 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 self.ready = False + 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) + + preferred_track = self.tracks.get(preferred_id) + if preferred_id < 0 or preferred_track is None or not self.ready or lead_prob <= .5: + 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'] @@ -199,7 +351,12 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.v_ego_hist.append(self.v_ego) self.last_v_ego_frame = sm.recv_frame['carState'] - ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel] for pt in rr.points} + radar_fresh = True + if self.honda_bosch_a_radar: + radar_fresh = sm.recv_frame['radarTracks'] != self._last_tracks_frame + self._last_tracks_frame = sm.recv_frame['radarTracks'] + + ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points} # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): @@ -216,7 +373,11 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): # 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) + 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 Honda Bosch-A + # suppresses duplicate measurement updates when radarTracks 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() @@ -238,8 +399,24 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): else: self.lead_prob_filters[i].update(lead_prob) - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_prob_filters[1].x, low_speed_override=False) + 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, self.lead_prob_filters[0].x, + low_speed_override=True, 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, self.lead_prob_filters[1].x, + low_speed_override=False, 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.present and getattr(lead, "radar", False): + 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.present) or (self.prev_lead_track_ids[i] not in self.tracks): + self.prev_lead_track_ids[i] = -1 + self._reset_preferred_stale_evidence(i) def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None @@ -263,7 +440,8 @@ def main() -> None: sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2') pm = messaging.PubMaster(['radarState']) - RD = RadarD(CP.radarDelay) + honda_bosch_a_radar = is_bosch_a_radar_car(CP) + RD = RadarD(CP.radarDelay, honda_bosch_a_radar=honda_bosch_a_radar) while 1: sm.update()