diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f9cb6d3f3..f32e2ea58 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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'])