diff --git a/cereal/custom.capnp b/cereal/custom.capnp index d649d3c7e1..b510805fe3 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -478,6 +478,14 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } +# sunnypilot radarState annotations — mirrors the per-lead coast state from radard's Tier-3 +# vision-coast (see selfdrive/controls/radard.py). Kept here (not in log.capnp) for fork +# merge-hygiene; the UI subscribes to this alongside the stock radarState. +struct RadarStateSP @0x81ed2f1cb07f25b1 { + leadOneCoasting @0 :Bool; # leadOne is currently held by vision-coast (a phantom was rejected) + leadTwoCoasting @1 :Bool; # leadTwo is currently held by vision-coast +} + struct CustomReserved10 @0xcb9fd56c7057593a { } diff --git a/cereal/log.capnp b/cereal/log.capnp index d2c7e2c896..eee4cfd00b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2558,7 +2558,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - customReserved10 @136 :Custom.CustomReserved10; + radarStateSP @136 :Custom.RadarStateSP; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/cereal/services.py b/cereal/services.py index f933fc0125..18d2a5ece7 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -86,6 +86,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "modelManagerSP": (False, 1., 1, QueueSize.BIG), "backupManagerSP": (False, 1., 1, QueueSize.BIG), "selfdriveStateSP": (True, 100., 10), + "radarStateSP": (True, 20., 5), "longitudinalPlanSP": (True, 20., 10), "onroadEventsSP": (True, 1., 1), "carParamsSP": (True, 0.02, 1), diff --git a/common/params_keys.h b/common/params_keys.h index b04b2e1620..b4a24ad026 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -167,6 +167,7 @@ inline static std::unordered_map keys = { {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}}, {"LastGPSPositionLLK", {PERSISTENT, STRING}}, {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"LeadTrackingMode", {PERSISTENT | BACKUP, INT, "1"}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}}, {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}}, @@ -182,6 +183,7 @@ inline static std::unordered_map keys = { {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"ShowVisionCoastOnChevron", {PERSISTENT | BACKUP, BOOL, "0"}}, {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}}, {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d51eb6edb0..e19e4969ed 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -2,7 +2,7 @@ import math import numpy as np from collections import deque -from typing import Any +from dataclasses import dataclass, asdict, replace import capnp from cereal import messaging, log, car, custom @@ -29,6 +29,73 @@ 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 +# Rivian Mando radar lateral-scale correction. +# opendbc's rivian/radar_interface.py computes yRel = 0.5 * -sin(azimuth) * LONG_DIST — an +# undocumented 0.5 factor (original to opendbc #1806, never examined) that over-compresses the +# lateral offset. With everything pulled toward the centerline, prob_y can't separate an +# adjacent-lane track from the in-path lead, which feeds the lead-bounce. Measurement +# (scratch_yrel_transform.py, 269k clean corpus samples) shows the radar lateral that best AGREES +# with the camera — the exact quantity match_vision_to_track's prob_y compares — uses a scale of +# ~0.70, not 0.5; and that scale also minimizes lead-bounce brake-jumps through the full pipeline +# (scratch_yrel_validate.py: tier-2 -4.7%, 0 gross-streak regressions corpus-wide). We correct it +# here, brand-gated, rather than in the shared opendbc interface, so it stays Rivian-only and +# adjacent to the lead-tracking tier system. Correction = target / source = 0.70 / 0.5 = 1.4. +RIVIAN_YREL_TARGET_SCALE = 0.70 +_RADAR_INTERFACE_YREL_SCALE = 0.5 # the factor baked into opendbc rivian/radar_interface.py +RIVIAN_YREL_CORRECTION = RIVIAN_YREL_TARGET_SCALE / _RADAR_INTERFACE_YREL_SCALE # 1.4 + +# Tier 3 "vision-coast". +# +# A phantom is a radar lead whose speed disagrees with the (confident) +# vision lead AND that arrived via an abrupt radar discontinuity the camera does NOT corroborate, either: +# - the chosen track-id just changed, or +# - the radar lead jumped laterally while the vision lead held still +# +# On such a frame, hold the last-good lead for up to COAST_MAX_S instead of +# emitting the phantom; then hand back to the radar until a real lead returns. +# +# The corroborating discontinuity is what lets the speed gate stay loose without coasting on clean frames. +# Holding the last-good (Kalman-smoothed) radar lead beats switching to the noisier vision lead. +COAST_V_GROSS = 5.0 # m/s — speed disagreement vs vision + # a corroborating discontinuity (another sus event) + # is also required, so this stays strict +COAST_LAT_RADAR_JUMP = 1.0 # m — chosen-lead yRel jump (Rivian yRel is halved → ~2 m real) +COAST_LAT_VISION_STILL = 0.5 # m — vision lead barely moved laterally (so the jump is radar-only) +COAST_MAX_S = 0.3 # s — hard cap on continuous coasting, then hand back to the radar. + # bounds the worst-case coast distance to v·0.3 + # assuming freeway speeds: + # 90 mph (~41 m/s) (faster than most cruise) + # that comes out to: 12.3 m coasting + # this does not include the COAST_NEAR_DREL lockout + # so any close range radar lead would immediately cancel the coast +COAST_NEAR_DREL = 40.0 # m — close-cut-in failsafe: never coast past a radar lead nearer than + # this. A close return is a hazard — respond to it, don't hold a + # stale far lead. The inverse of the gross-distance trigger: here + # closeness SUPPRESSES coast. Set from the corpus coast-distance + # distribution: coasts under ~40 m are ~100% vision-confirmed real + # objects (often stopped, stop-and-go), where coasting past = a + # rear-end risk; the phantom benefit lives >60 m. 0 disables. +COAST_VIS_PROB = 0.5 # — only trust the phantom judgment when the vision lead is confident + + +@dataclass +class RadarLead: + """A radarState leadOne/leadTwo estimate. Field names mirror cereal LeadData, so an instance + maps onto the cereal struct via asdict(); the defaults match cereal's defaults, so a bare + RadarLead() is the 'no lead' value (status=False) — it replaces the old {'status': False} dict.""" + status: bool = False + dRel: float = 0.0 + yRel: float = 0.0 + vRel: float = 0.0 + vLead: float = 0.0 + vLeadK: float = 0.0 + aLeadK: float = 0.0 + aLeadTau: float = 0.0 + fcw: bool = False + modelProb: float = 0.0 + radar: bool = False + radarTrackId: int = -1 + class KalmanParams: def __init__(self, dt: float): @@ -85,21 +152,21 @@ def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measur self.cnt += 1 - def get_RadarState(self, model_prob: float = 0.0): - return { - "dRel": float(self.dRel), - "yRel": float(self.yRel), - "vRel": float(self.vRel), - "vLead": float(self.vLead), - "vLeadK": float(self.vLeadK), - "aLeadK": float(self.aLeadK), - "aLeadTau": float(self.aLeadTau.x), - "status": True, - "fcw": self.is_potential_fcw(model_prob), - "modelProb": model_prob, - "radar": True, - "radarTrackId": self.identifier, - } + def get_RadarState(self, model_prob: float = 0.0) -> RadarLead: + return RadarLead( + dRel=float(self.dRel), + yRel=float(self.yRel), + vRel=float(self.vRel), + vLead=float(self.vLead), + vLeadK=float(self.vLeadK), + aLeadK=float(self.aLeadK), + aLeadTau=float(self.aLeadTau.x), + status=True, + fcw=self.is_potential_fcw(model_prob), + modelProb=model_prob, + radar=True, + radarTrackId=self.identifier, + ) def potential_low_speed_lead(self, v_ego: float): # stop for stuff in front of you and low speed, even without model confirmation @@ -119,7 +186,8 @@ def laplacian_pdf(x: float, mu: float, b: float): return math.exp(-abs(x-mu)/b) -def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): +def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track], + prev_track_id: int | None = None, stickiness: float = 5.0) -> Track | None: offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): @@ -128,7 +196,16 @@ def prob(c): prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) # This isn't exactly right, but it's a good heuristic - return prob_d * prob_y * prob_v + base = prob_d * prob_y * prob_v + # Hysteresis: bias toward the previously-chosen track so a challenger must beat it + # by a margin before we switch — prevents frame-to-frame lead flipping when two + # radar tracks have similar probability. The bonus is scaled by the lateral-agreement + # probability so the held track releases its bonus when it drifts away from the + # camera's predicted lateral position (i.e. we only stay sticky while the camera + # still supports the held track laterally). + if prev_track_id is not None and c.identifier == prev_track_id: + base *= 1.0 + (stickiness - 1.0) * prob_y + return base track = max(tracks.values(), key=prob) @@ -142,39 +219,39 @@ def prob(c): return None -def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float): +def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float) -> RadarLead: lead_v_rel_pred = lead_msg.v[0] - model_v_ego - return { - "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), - "yRel": float(-lead_msg.y[0]), - "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]), - "aLeadTau": 0.3, - "fcw": False, - "modelProb": float(lead_prob), - "status": True, - "radar": False, - "radarTrackId": -1, - } + return RadarLead( + dRel=float(lead_msg.x[0] - RADAR_TO_CAMERA), + yRel=float(-lead_msg.y[0]), + 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]), + aLeadTau=0.3, + fcw=False, + modelProb=float(lead_prob), + status=True, + radar=False, + radarTrackId=-1, + ) def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, lead_prob: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP, - low_speed_override: bool = True) -> dict[str, Any]: + low_speed_override: bool = True, prev_track_id: int | None = None) -> RadarLead: # 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) + track = match_vision_to_track(v_ego, lead_msg, tracks, prev_track_id=prev_track_id) else: track = None - lead_dict = {'status': False} + lead = RadarLead() if track is not None: - lead_dict = track.get_RadarState(lead_prob) - lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg) + lead = track.get_RadarState(lead_prob) + lead = get_custom_yrel(CP, CP_SP, lead, lead_msg) elif (track is None) and ready and (lead_prob > .5): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob) + lead = 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)] @@ -182,19 +259,19 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn closest_track = min(low_speed_tracks, key=lambda c: c.dRel) # Only choose new track if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): - lead_dict = closest_track.get_RadarState() + if (not lead.status) or (closest_track.dRel < lead.dRel): + lead = closest_track.get_RadarState() - return lead_dict + return lead -def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict: dict[str, Any], - lead_msg: capnp._DynamicStructReader) -> dict[str, Any]: +def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead: RadarLead, + lead_msg: capnp._DynamicStructReader) -> RadarLead: if CP.brand == "hyundai" and (CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC or CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)): - lead_dict['yRel'] = float(-lead_msg.y[0]) + lead.yRel = float(-lead_msg.y[0]) - return lead_dict + return lead class RadarD: @@ -202,6 +279,9 @@ def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float self.CP = CP self.CP_SP = CP_SP + # brand-gated radar lateral correction (see RIVIAN_YREL_CORRECTION); 1.0 = no-op for other brands + self.yrel_correction = RIVIAN_YREL_CORRECTION if CP.brand == "rivian" else 1.0 + self.current_time = 0.0 self.tracks: dict[int, Track] = {} @@ -217,7 +297,40 @@ def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float self.ready = False + # remember the last selected radar trackId per lead, for the hysteresis bonus in + # match_vision_to_track (the id is also reused by the Tier-3 id-changed corroborator) + self.prev_lead_track_id: dict[int, int | None] = {0: None, 1: None} + + # behavior tier: 1=no hysteresis, 2=id-hysteresis (committed Fix A), 3=id-hysteresis + + # vision-coast. Chosen by the LeadTrackingMode UI selector, which stores a button index + # (0/1/2 -> tier 1/2/3); main() refreshes it ~1 Hz via the DEC throttled-read pattern (never + # per-frame disk I/O). update() never reads Params, so offline eval harnesses just set + # self.lead_tracking_mode directly and it sticks. Defaults to Tier 2 (the validated behavior). + self.params = Params() + self.frame = 0 + self.lead_tracking_mode = 2 + # Tier 3 vision-coast state (per lead): last lead we trusted, how long we've coasted, and + # the previous-frame lateral positions powering the radar-lateral-jump corroborator. + self.last_good_lead: dict[int, RadarLead | None] = {0: None, 1: None} + self.coast_frames: dict[int, int] = {0: 0, 1: 0} + # per-lead coast flag, published as radarStateSP for the onroad UI indicator (chevron tint + # + developer-UI element). Reset each update(); set by _vision_coast when a lead is held. + self.lead_coasting: dict[int, bool] = {0: False, 1: False} + self.prev_lead_yRel: dict[int, float | None] = {0: None, 1: None} + self.prev_vision_y: dict[int, float | None] = {0: None, 1: None} + # Tier 3 thresholds — instance attrs so they can be swept at runtime during tuning + self.coast_v_gross = COAST_V_GROSS + self.coast_lat_radar_jump = COAST_LAT_RADAR_JUMP + self.coast_lat_vision_still = COAST_LAT_VISION_STILL + self.coast_max_s = COAST_MAX_S + self.coast_vis_prob = COAST_VIS_PROB + self.coast_near_dRel = COAST_NEAR_DREL + def update(self, sm: messaging.SubMaster, rr: car.RadarData): + # clear the per-lead coast flags for this frame (set again below only if Tier 3 holds a lead) + self.frame += 1 + self.lead_coasting[0] = self.lead_coasting[1] = False + self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -226,7 +339,9 @@ 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, pt.measured] for pt in rr.points} + # correct the radar lateral at ingestion (brand-gated) so the matcher, the stored Track, and the + # emitted lead all see the same corrected yRel. dRel/vRel are unaffected — only the lateral scale. + ar_pts = {pt.trackId: [pt.dRel, pt.yRel * self.yrel_correction, pt.vRel, pt.measured] for pt in rr.points} # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): @@ -258,18 +373,144 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: + # Asymmetric filter on lead prob to keep lead when uncertain (from dev base) for i in range(2): - # Asymmetric filter on lead prob to keep lead when uncertain lead_prob = leads_v3[i].prob if lead_prob > self.lead_prob_filters[i].x: self.lead_prob_filters[i].x = lead_prob 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, - self.CP, self.CP_SP, 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, - self.CP, self.CP_SP, low_speed_override=False) + tier = self.lead_tracking_mode + # Tier 1: no hysteresis (stock greedy match). Tier 2/3: id-keyed hysteresis bonus on + # the previously-held trackId (when it's still present this frame). + if tier >= 2: + sticky0 = self.prev_lead_track_id[0] + sticky1 = self.prev_lead_track_id[1] + else: + sticky0 = sticky1 = None + one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x, + self.CP, self.CP_SP, low_speed_override=True, prev_track_id=sticky0) + two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_prob_filters[1].x, + self.CP, self.CP_SP, low_speed_override=False, prev_track_id=sticky1) + # Tier 3: vision-coast — hold the last-good lead through a gross phantom (e.g. a + # single-frame radar dropout) instead of emitting it. + if tier >= 3: + one, c0 = self._vision_coast(0, one, leads_v3[0]) + two, c1 = self._vision_coast(1, two, leads_v3[1]) + self.lead_coasting[0] = c0 + self.lead_coasting[1] = c1 + else: + self.coast_frames[0] = self.coast_frames[1] = 0 + self.radar_state.leadOne = asdict(one) + self.radar_state.leadTwo = asdict(two) + # remember the chosen radar track id (hysteresis bonus + id-changed corroborator) and the + # lateral positions (radar-lateral-jump corroborator) for the next frame + for i, ld in ((0, one), (1, two)): + if ld.status and ld.radar and ld.radarTrackId >= 0: + self.prev_lead_track_id[i] = ld.radarTrackId + self.prev_lead_yRel[i] = ld.yRel + else: + self.prev_lead_track_id[i] = None + self.prev_lead_yRel[i] = None + self.prev_vision_y[i] = float(leads_v3[i].y[0]) if len(leads_v3[i].y) else None + + def _vision_coast(self, i: int, lead: RadarLead, + vis: capnp._DynamicStructReader) -> tuple[RadarLead, bool]: + """Tier 3. When this frame looks like a phantom (see is_phantom), hold the last-good lead + instead of emitting it, bounded by COAST_MAX_S, after which we hand back to the radar until a + real lead returns. Returns (possibly-held lead, coasting?). Holding the last-good (Kalman- + smoothed) radar lead beats substituting the raw vision lead, which is noisier.""" + phantom = self.is_phantom(i, lead, vis) + + # When it's not a phantom, reset the coast timer and refresh last-good to this frame's lead — or + # clear it to None when there's no radar lead. Never keep a stale last-good around: a non-phantom + # lead is safe to hold (it's never a phantom by definition), and if there's nothing to hold we'd + # rather not coast at all than coast onto a stale value. + if not phantom: + self.coast_frames[i] = 0 + self.last_good_lead[i] = replace(lead) if (lead.status and lead.radar) else None + return lead, False + + # --- BELOW HERE WE DO NOT TRUST THE CURRENT RADAR LEAD (phantom) --- + + # We only want to coast for a max time, so that becomes a max number of frames; if we hit that + # limit we drop back to returning the potential phantom lead, which is safer than coasting on. + cap = int(round(self.coast_max_s / DT_MDL)) + + # if we have a last-good lead to coast with and are within the coast budget, coast + if self.last_good_lead[i] is not None and self.coast_frames[i] < cap: + self.coast_frames[i] += 1 + return replace(self.last_good_lead[i]), True + + # no last-good to coast with, or we hit the coast limit → drop back to the (suspected) phantom + return lead, False + + def is_phantom(self, i: int, lead: RadarLead, vis: capnp._DynamicStructReader) -> bool: + """Tier 3 per-frame verdict: do we think this radar lead is a phantom we should NOT trust? It + is, when its speed grossly disagrees with the confident vision lead AND it arrived via an abrupt + radar discontinuity the camera does not corroborate (the chosen track-id changed, or the radar + lead jumped laterally while the vision lead held still). Pure predicate — no coast cap, no state + mutation — so eval harnesses can label frames with it directly. Cheap checks short-circuit first.""" + if not (lead.status and lead.radar): + return False + # Close-cut-in failsafe: a very close radar return is a real hazard (e.g. a side cut-in), never + # a phantom to coast past — respond to it instead of holding a stale far lead. The inverse of a + # distance trigger: closeness vetoes the phantom verdict outright. + if 0.0 < lead.dRel < self.coast_near_dRel: + return False + # A phantom's speed grossly disagrees with the vision lead; if it agrees (or we can't tell), + # it isn't a phantom. + if self._speed_agrees_with_vision(lead, vis): + return False + + # --- BELOW HERE WE SUSPECT SOMETHING FISHY --- + # but we need proof to back that up. So far we know: + # - this is a medium-distance object (the close-cut-in failsafe above already let it through) + # - it's moving at a dramatically different speed from the vision lead + # That alone is suspicious but not damning, so we look for a radar discontinuity to corroborate: + # - did the radar swap to a new track id? + # - did the radar lead hop dramatically to the side (while vision didn't)? + # Either can mean the radar locked onto something like a stationary object in an adjacent lane — + # e.g. driving past a row of stopped cars, or reflectors between lanes (toll plaza). There ARE + # real situations like this where you'd want to brake, which is why we keep the distance gate + # (nothing fires too close — 40 m default, giving vision time to pick it up) and the vision check. + + # Corroborator 1: the chosen radar track-id just changed (a dropout grabbed a different object). + if lead.radarTrackId != -1 and lead.radarTrackId != self.prev_lead_track_id[i]: + return True + + # Corroborator 2: the radar lead jumped laterally while the vision lead held still. + prev_lead_yRel, prev_vision_y = self.prev_lead_yRel[i], self.prev_vision_y[i] + if prev_lead_yRel is None or prev_vision_y is None: + return False + + has_dramatic_lateral_radar_jump = abs(lead.yRel - prev_lead_yRel) > self.coast_lat_radar_jump + vision_lead_laterally_stable = abs(vis.y[0] - prev_vision_y) < self.coast_lat_vision_still + return has_dramatic_lateral_radar_jump and vision_lead_laterally_stable + + def _speed_agrees_with_vision(self, lead: RadarLead, vis: capnp._DynamicStructReader) -> bool: + """True when the radar lead's speed is consistent with the vision lead — i.e. NOT a gross + disagreement. Also True when there's no basis to call a disagreement (ego ~stopped, or no + confident vision lead), so those frames default to 'not a phantom'. The negation is the phantom + speed symptom: a dropout that re-points the radar slot at a slower/closer object craters the + speed relative to the lead the camera still tracks.""" + if self.v_ego <= V_EGO_STATIONARY: + return True + if not len(vis.x) or not len(vis.y) or vis.prob <= self.coast_vis_prob: + return True + return abs(lead.vLead - vis.v[0]) <= self.coast_v_gross + + def _read_lead_tracking_mode(self) -> None: + # The UI selector stores a button index (0/1/2); map it to the tier (1/2/3). Called ~1 Hz from + # main() only (NOT update()), so a harness that sets lead_tracking_mode directly isn't clobbered. + # Tolerate the key being absent (e.g. before a params_pyx rebuild) by keeping the current tier. + try: + idx = self.params.get("LeadTrackingMode", return_default=True) + if idx is not None: + self.lead_tracking_mode = int(idx) + 1 + except Exception: + pass def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None @@ -279,6 +520,13 @@ def publish(self, pm: messaging.PubMaster): radar_msg.radarState = self.radar_state pm.send("radarState", radar_msg) + # sunnypilot: per-lead Tier-3 vision-coast state for the onroad UI indicator + radar_sp = messaging.new_message("radarStateSP") + radar_sp.valid = self.radar_state_valid + radar_sp.radarStateSP.leadOneCoasting = self.lead_coasting[0] + radar_sp.radarStateSP.leadTwoCoasting = self.lead_coasting[1] + pm.send("radarStateSP", radar_sp) + # fuses camera and radar data for best lead detection def main() -> None: @@ -295,13 +543,19 @@ def main() -> None: # *** setup messaging sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') - pm = messaging.PubMaster(['radarState']) + pm = messaging.PubMaster(['radarState', 'radarStateSP']) RD = RadarD(CP, CP_SP, CP.radarDelay) while 1: sm.update() + # refresh the selected tier ~1 Hz from the LeadTrackingMode UI param — the same sm.frame-gated + # main-loop param-read idiom as selfdrived/card/paramsd (cheap; never per-frame disk I/O). sm.frame + # is 0 on the first pass so the tier is live from boot. Kept out of update() so offline harnesses + # pin the tier by setting rd.lead_tracking_mode directly. + if sm.frame % int(1. / DT_MDL) == 0: + RD._read_lead_tracking_mode() RD.update(sm, sm['liveTracks']) RD.publish(pm) diff --git a/selfdrive/controls/tests/test_radard_tiers.py b/selfdrive/controls/tests/test_radard_tiers.py new file mode 100644 index 0000000000..e2f19ecbb7 --- /dev/null +++ b/selfdrive/controls/tests/test_radard_tiers.py @@ -0,0 +1,333 @@ +"""Unit tests for radard lead selection — the RadarLead type, vision↔radar matching, the +tier-2 hysteresis, and the tier-3 vision-coast. These drive the pure-Python functions/objects +directly (no process_replay / msgq), so they run on macOS as well as in CI.""" +from dataclasses import asdict + +import cereal.messaging as messaging +from cereal import car, log, custom +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.radard import ( + RadarLead, Track, KalmanParams, RadarD, get_lead, match_vision_to_track, RADAR_TO_CAMERA, + RIVIAN_YREL_CORRECTION, +) + +V_EGO = 25.0 + + +def vision_msg(x, y, v, prob=1.0, x_std=2.0, y_std=1.0, v_std=1.0, a=0.0): + """Build a modelV2 message with one lead. Return the message (keep it in scope) — index + `.modelV2.leadsV3[0]` to get the reader the radard functions consume.""" + msg = messaging.new_message('modelV2') + msg.modelV2.leadsV3 = [{'prob': prob, 'x': [x], 'xStd': [x_std], 'y': [y], 'yStd': [y_std], + 'v': [v], 'vStd': [v_std], 'a': [a]}] + return msg + + +def make_track(identifier, d_rel, y_rel, v_rel, v_ego=V_EGO): + t = Track(identifier, v_rel + v_ego, KalmanParams(DT_MDL)) + t.update(d_rel, y_rel, v_rel, v_rel + v_ego, 1.0) + return t + + +def make_radard(): + return RadarD(car.CarParams.new_message(), custom.CarParamsSP.new_message(), delay=0.0) + + +class TestRadarLead: + def test_default_is_no_lead(self): + lead = RadarLead() + assert lead.status is False and lead.radar is False and lead.radarTrackId == -1 + + def test_cereal_roundtrip(self): + # a full radar lead maps onto the cereal LeadData via asdict() with no loss + lead = RadarLead(status=True, dRel=52.3, yRel=-1.2, vRel=-3.0, vLead=22.0, vLeadK=22.1, + aLeadK=0.4, aLeadTau=1.5, fcw=False, modelProb=0.9, radar=True, radarTrackId=7) + rs = log.RadarState.new_message() + rs.leadOne = asdict(lead) + # cereal LeadData floats are Float32, so compare floats with tolerance and the rest exactly + for f in ("status", "fcw", "radar", "radarTrackId"): + assert getattr(rs.leadOne, f) == getattr(lead, f), f + for f in ("dRel", "yRel", "vRel", "vLead", "vLeadK", "aLeadK", "aLeadTau", "modelProb"): + assert abs(getattr(rs.leadOne, f) - getattr(lead, f)) < 1e-4, f + + def test_no_lead_cereal_matches_defaults(self): + # the bare RadarLead() must produce the same struct the old {'status': False} dict did + rs = log.RadarState.new_message() + rs.leadTwo = asdict(RadarLead()) + assert rs.leadTwo.status is False + assert rs.leadTwo.radarTrackId == -1 # cereal schema default + assert rs.leadTwo.dRel == 0.0 + + +class TestGetLead: + def test_returns_matched_radar_lead(self): + tracks = {5: make_track(5, d_rel=50.0, y_rel=0.0, v_rel=0.0)} + vm = vision_msg(x=50.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + lead = get_lead(V_EGO, True, tracks, vm.modelV2.leadsV3[0], V_EGO, 1.0, + car.CarParams.new_message(), custom.CarParamsSP.new_message()) + assert isinstance(lead, RadarLead) + assert lead.status and lead.radar and lead.radarTrackId == 5 + assert abs(lead.dRel - 50.0) < 1e-6 + + def test_falls_back_to_vision_when_no_track(self): + vm = vision_msg(x=60.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + lead = get_lead(V_EGO, True, {}, vm.modelV2.leadsV3[0], V_EGO, 1.0, + car.CarParams.new_message(), custom.CarParamsSP.new_message()) + assert isinstance(lead, RadarLead) + assert lead.status and not lead.radar and lead.radarTrackId == -1 + assert abs(lead.dRel - 60.0) < 1e-6 + + +class TestHysteresis: + def test_prev_track_gets_sticky_bonus(self): + # two near-equal tracks; the held one should win once it gets the hysteresis bonus + tracks = {1: make_track(1, d_rel=50.0, y_rel=0.3, v_rel=0.0), + 2: make_track(2, d_rel=50.0, y_rel=-0.3, v_rel=0.0)} + vm = vision_msg(x=50.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + lead = vm.modelV2.leadsV3[0] + no_hyst = match_vision_to_track(V_EGO, lead, tracks, prev_track_id=None) + held = match_vision_to_track(V_EGO, lead, tracks, prev_track_id=2) + assert no_hyst is not None and held is not None + assert held.identifier == 2 # the held track sticks + assert match_vision_to_track(V_EGO, lead, tracks, prev_track_id=1).identifier == 1 + + +class TestVisionCoast: + def _setup_held(self, rd): + rd.v_ego = V_EGO + rd.prev_lead_track_id[0] = 100 + rd.prev_lead_yRel[0] = 0.0 + rd.prev_vision_y[0] = 0.0 + rd.last_good_lead[0] = RadarLead(status=True, radar=True, dRel=95.0, yRel=0.0, + vLead=V_EGO, radarTrackId=100) + + def test_holds_through_phantom(self): + rd = make_radard() + self._setup_held(rd) + # phantom: new id, much closer & slower, while vision still sees the far/fast lead + phantom = RadarLead(status=True, radar=True, dRel=77.0, yRel=-3.0, vLead=8.0, radarTrackId=200) + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + out, coasting = rd._vision_coast(0, phantom, vm.modelV2.leadsV3[0]) + assert coasting is True + assert out.radarTrackId == 100 and abs(out.dRel - 95.0) < 1e-6 # held the last-good lead + + def test_passes_real_lead_and_stores_it(self): + rd = make_radard() + self._setup_held(rd) + real = RadarLead(status=True, radar=True, dRel=96.0, yRel=0.0, vLead=V_EGO, radarTrackId=100) + vm = vision_msg(x=96.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + out, coasting = rd._vision_coast(0, real, vm.modelV2.leadsV3[0]) + assert coasting is False + assert out is real # emitted unchanged + assert rd.last_good_lead[0].dRel == 96.0 # and remembered as last-good + + def test_no_coast_when_vision_agrees_with_close_slow(self): + # a genuinely close/slow lead the camera ALSO sees must not be coasted through + rd = make_radard() + self._setup_held(rd) + real_slow = RadarLead(status=True, radar=True, dRel=77.0, yRel=0.0, vLead=8.0, radarTrackId=200) + vm = vision_msg(x=77.0 + RADAR_TO_CAMERA, y=0.0, v=8.0) # vision agrees: close & slow + out, coasting = rd._vision_coast(0, real_slow, vm.modelV2.leadsV3[0]) + assert coasting is False and out is real_slow + + def test_close_cut_in_failsafe_suppresses_coast(self): + # a very close radar return (< coast_near_dRel) that LOOKS like a phantom (new id, slow, vision + # still sees the far/fast lead) must NEVER be coasted — it's a hazard (e.g. a side cut-in) + rd = make_radard() + self._setup_held(rd) + assert rd.coast_near_dRel == 40.0 + cut_in = RadarLead(status=True, radar=True, dRel=18.0, yRel=-3.0, vLead=8.0, radarTrackId=200) + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) # vision still sees the far/fast lead + out, coasting = rd._vision_coast(0, cut_in, vm.modelV2.leadsV3[0]) + assert coasting is False # failsafe overrode the phantom verdict + assert out is cut_in and out.dRel == 18.0 # emitted the close lead, did not hold last-good + + def test_phantom_just_beyond_near_threshold_still_coasts(self): + # the failsafe must not kill the benefit: a phantom just past coast_near_dRel still coasts + rd = make_radard() + self._setup_held(rd) + far_phantom = RadarLead(status=True, radar=True, dRel=55.0, yRel=-3.0, vLead=8.0, radarTrackId=200) + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + out, coasting = rd._vision_coast(0, far_phantom, vm.modelV2.leadsV3[0]) + assert coasting is True and out.radarTrackId == 100 # held last-good, as before (beyond the floor) + + def test_last_good_refreshes_then_clears(self): + # last-good must never go stale: refresh it to the current non-phantom lead every frame, and + # clear it to None when there's no radar lead (so we never coast onto a stale value) + rd = make_radard() + rd.v_ego = V_EGO + rd.prev_lead_track_id[0] = 100 # same id below → no corroborator → not a phantom + # a non-phantom lead that speed-disagrees with vision is now stored fresh (was skipped before) + lead = RadarLead(status=True, radar=True, dRel=80.0, yRel=0.0, vLead=8.0, radarTrackId=100) + vm = vision_msg(x=80.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) # vision 25 vs lead 8 → speed disagrees + rd._vision_coast(0, lead, vm.modelV2.leadsV3[0]) + assert rd.last_good_lead[0] is not None and rd.last_good_lead[0].dRel == 80.0 + # next frame has no radar lead → last-good cleared, not left stale + rd._vision_coast(0, RadarLead(), vm.modelV2.leadsV3[0]) + assert rd.last_good_lead[0] is None + + +class TestIsPhantom: + """The pure per-frame phantom predicate (no cap, no state mutation) — what the harnesses label + frames with. Each case isolates one gate.""" + def _rd(self): + rd = make_radard() + rd.v_ego = V_EGO + rd.prev_lead_track_id[0] = 100 + rd.prev_lead_yRel[0] = 0.0 + rd.prev_vision_y[0] = 0.0 + return rd + + def _lead(self, dRel=80.0, vLead=8.0, radarTrackId=200, yRel=0.0): + # a far (>coast_near_dRel), speed-disagreeing radar lead with a changed id, by default a phantom + return RadarLead(status=True, radar=True, dRel=dRel, yRel=yRel, vLead=vLead, radarTrackId=radarTrackId) + + def test_phantom_true_speed_disagree_plus_id_change(self): + rd = self._rd() + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) # vision: far, fast + assert rd.is_phantom(0, self._lead(), vm.modelV2.leadsV3[0]) is True + + def test_not_phantom_when_speed_agrees(self): + rd = self._rd() + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + assert rd.is_phantom(0, self._lead(vLead=V_EGO), vm.modelV2.leadsV3[0]) is False + + def test_not_phantom_without_a_corroborator(self): + # speed disagrees but the id is unchanged (100) and no lateral jump → not a phantom + rd = self._rd() + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + assert rd.is_phantom(0, self._lead(radarTrackId=100), vm.modelV2.leadsV3[0]) is False + + def test_not_phantom_when_close_failsafe(self): + rd = self._rd() + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO) + assert rd.is_phantom(0, self._lead(dRel=18.0), vm.modelV2.leadsV3[0]) is False + + def test_not_phantom_when_vision_unconfident(self): + rd = self._rd() + vm = vision_msg(x=95.0 + RADAR_TO_CAMERA, y=0.0, v=V_EGO, prob=0.3) + assert rd.is_phantom(0, self._lead(), vm.modelV2.leadsV3[0]) is False + + +# --- update() integration: drive the whole RadarD.update path via a minimal fake SubMaster, so +# the per-lead lead_coasting flags (published as radarStateSP for the UI) are exercised. msgq / +# process_replay aborts on macOS, but update() only touches a handful of SubMaster attributes. --- + +def model_msg(leads_v3, v_ego=V_EGO): + msg = messaging.new_message('modelV2') + msg.modelV2.velocity.x = [v_ego] + msg.modelV2.leadsV3 = leads_v3 + return msg.modelV2 + + +def lead3(x, y, v, prob=1.0): + return {'prob': prob, 'x': [x], 'xStd': [2.0], 'y': [y], 'yStd': [1.0], 'v': [v], 'vStd': [1.0], 'a': [0.0]} + + +def carstate_msg(v_ego=V_EGO): + msg = messaging.new_message('carState') + msg.carState.vEgo = v_ego + return msg.carState + + +def live_tracks(points): + """points: list of (trackId, dRel, yRel, vRel) → a liveTracks reader.""" + msg = messaging.new_message('liveTracks') + msg.liveTracks.points = [{'trackId': t, 'dRel': d, 'yRel': y, 'vRel': v, 'measured': True} + for (t, d, y, v) in points] + return msg.liveTracks + + +class FakeSM: + def __init__(self, model, carstate): + self._data = {'modelV2': model, 'carState': carstate} + self.seen = {'modelV2': True} + self.logMonoTime = {'modelV2': 1000, 'carState': 1000} + self.recv_frame = {'carState': 1} + + def __getitem__(self, key): + return self._data[key] + + def all_checks(self): + return True + + +def make_radard_tier(tier): + rd = make_radard() + rd.lead_tracking_mode = tier # update() never reads Params, so a directly-set tier sticks + return rd + + +# vision sees two steady, far, same-speed leads the entire time +VIS_TWO_FAR = [lead3(95.0 + RADAR_TO_CAMERA, 0.0, V_EGO), lead3(120.0 + RADAR_TO_CAMERA, 0.0, V_EGO)] + + +class TestUpdateCoastFlags: + def _prime(self, rd): + # several frames of two real radar leads matching vision (id 100 near, 101 far) so leadOne + # locks onto id 100 and it is remembered as last-good + sm = FakeSM(model_msg(VIS_TWO_FAR), carstate_msg()) + for _ in range(8): + rd.update(sm, live_tracks([(100, 95.0, 0.0, 0.0), (101, 120.0, 0.0, 0.0)])) + + def test_tier3_update_sets_coast_flag_and_holds_lead(self): + rd = make_radard_tier(3) + self._prime(rd) + assert rd.radar_state.leadOne.radarTrackId == 100 + assert rd.lead_coasting == {0: False, 1: False} + assert rd.last_good_lead[0] is not None + + # phantom frame: the held track (100) drops out and only a close/slow clutter track (200) + # remains, while vision still sees the far/fast lead → coast must hold the last-good lead + sm = FakeSM(model_msg(VIS_TWO_FAR), carstate_msg()) + rd.update(sm, live_tracks([(200, 77.0, -3.0, -17.0)])) + assert rd.lead_coasting[0] is True + assert rd.lead_coasting[1] is False + assert rd.radar_state.leadOne.radarTrackId == 100 # held last-good, not the phantom + assert abs(rd.radar_state.leadOne.dRel - 95.0) < 1e-6 + + def test_tier2_update_never_coasts(self): + # the same phantom under Tier 2 (no vision-coast) must NOT set the coast flags + rd = make_radard_tier(2) + self._prime(rd) + sm = FakeSM(model_msg(VIS_TWO_FAR), carstate_msg()) + rd.update(sm, live_tracks([(200, 77.0, -3.0, -17.0)])) + assert rd.lead_coasting == {0: False, 1: False} + assert rd.radar_state.leadOne.radarTrackId == 200 # the phantom is emitted, not held + + def test_radarstatesp_carries_coast_flags(self): + # contract: the flags RadarD.publish() copies into radarStateSP survive the cereal roundtrip + sp = messaging.new_message('radarStateSP') + sp.radarStateSP.leadOneCoasting = True + sp.radarStateSP.leadTwoCoasting = False + out = messaging.log_from_bytes(sp.to_bytes()).radarStateSP + assert out.leadOneCoasting is True and out.leadTwoCoasting is False + + +class TestYrelCorrection: + """The Rivian-gated radar lateral-scale correction (the 0.5 in opendbc's interface over-compresses; + ~0.70 best agrees with the camera). Brand-gated, applied at ingestion so matcher + track + emitted + lead all see the same corrected lateral.""" + def _rivian_radard(self, tier=2): + cp = car.CarParams.new_message() + cp.brand = "rivian" + rd = RadarD(cp, custom.CarParamsSP.new_message(), delay=0.0) + rd.lead_tracking_mode = tier + return rd + + def test_non_rivian_is_noop(self): + assert make_radard().yrel_correction == 1.0 # bare CarParams → other brand → no-op + + def test_rivian_correction_factor(self): + assert abs(self._rivian_radard().yrel_correction - RIVIAN_YREL_CORRECTION) < 1e-9 + assert abs(RIVIAN_YREL_CORRECTION - 1.4) < 1e-9 # 0.70 / 0.5 + + def test_correction_applied_to_ingested_lateral(self): + # a raw radar yRel of 1.0 becomes 1.4 in the stored track (what the matcher sees) for Rivian + rd = self._rivian_radard() + rd.update(FakeSM(model_msg(VIS_TWO_FAR), carstate_msg()), live_tracks([(100, 50.0, 1.0, 0.0)])) + assert abs(rd.tracks[100].yRel - 1.4) < 1e-6 + # but an unchanged lateral for a non-Rivian radard (the no-op gate) + rd2 = make_radard_tier(2) + rd2.update(FakeSM(model_msg(VIS_TWO_FAR), carstate_msg()), live_tracks([(100, 50.0, 1.0, 0.0)])) + assert abs(rd2.tracks[100].yRel - 1.0) < 1e-6 diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 353cc5aa40..b69aca6515 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -314,12 +314,14 @@ def _draw_path(self, sm): draw_polygon(self._rect, self._path.projected_points, gradient=gradient) def _draw_lead_indicator(self): - # Draw lead vehicles if available - for lead in self._lead_vehicles: + # Draw lead vehicles if available. The glow color comes from a per-lead hook + # (ModelRendererSP._lead_glow_color) so sunnypilot can tint a chevron while Tier-3 + # vision-coast is holding that lead; default is the stock gold. + for i, lead in enumerate(self._lead_vehicles): if not lead.glow or not lead.chevron: continue - rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) + rl.draw_triangle_fan(lead.glow, len(lead.glow), self._lead_glow_color(i)) rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha)) @staticmethod diff --git a/selfdrive/ui/sunnypilot/layouts/settings/cruise.py b/selfdrive/ui/sunnypilot/layouts/settings/cruise.py index 9aac7dad6f..c4060df9ed 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/cruise.py +++ b/selfdrive/ui/sunnypilot/layouts/settings/cruise.py @@ -9,7 +9,7 @@ from openpilot.selfdrive.ui.sunnypilot.layouts.settings.cruise_sub_layouts.speed_limit_settings import SpeedLimitSettingsLayout from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.multilang import tr, tr_noop -from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp, option_item_sp, simple_button_item_sp +from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp, option_item_sp, simple_button_item_sp, multiple_button_item_sp from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.scroller_tici import Scroller @@ -93,6 +93,17 @@ def _initialize_items(self): description=tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."), param="DynamicExperimentalControl") + self.lead_tracking_mode = multiple_button_item_sp( + title=tr("Radar Lead Tracking"), + description=tr("How sunnypilot picks and holds the radar lead. Stock: per-frame match. " + + "Stable Lead: sticks to the held track to stop lead flicker. " + + "Stable + Vision Coast: also briefly holds the lead through a sudden radar phantom the camera doesn't confirm."), + buttons=[lambda: tr("Stock"), lambda: tr("Stable Lead"), lambda: tr("Stable + Vision Coast")], + param="LeadTrackingMode", + button_width=400, + inline=False, + ) + self.rivian_resume_toggle = toggle_item_sp( title=tr("Rivian: Enable Resume"), description=tr('When enabled a full stalk down action held for at least 0.5s, provided activation of ACC is available on stock Rivian, will set the cruise speed to be equal to that from the last time cruise was deactivated. If cruise has never been activated it will set the cruise speed to the current vehicle speed. It is recommended to disable the stock Rivian feature: "Set to speed limit on divided highways", which uses the same activation mechanism.'), @@ -101,6 +112,7 @@ def _initialize_items(self): items = [ self.icbm_toggle, self.dec_toggle, + self.lead_tracking_mode, self.scc_v_toggle, self.scc_m_toggle, self.curve_speed_toggle, @@ -132,6 +144,8 @@ def _set_current_panel(self, panel: PanelType): def _update_state(self): super()._update_state() + self.lead_tracking_mode.action_item.set_selected_button(ui_state.params.get("LeadTrackingMode", return_default=True)) + if ui_state.CP is not None and ui_state.CP_SP is not None: has_icbm = ui_state.has_icbm has_long = ui_state.has_longitudinal_control diff --git a/selfdrive/ui/sunnypilot/layouts/settings/visuals.py b/selfdrive/ui/sunnypilot/layouts/settings/visuals.py index 84be5a26ab..35bc7e66ca 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/visuals.py +++ b/selfdrive/ui/sunnypilot/layouts/settings/visuals.py @@ -93,6 +93,12 @@ def _initialize_items(self): "This displays what the car is currently doing, not what the planner is requesting."), None, ), + "ShowVisionCoastOnChevron": ( + lambda: tr("Show Vision-Coast on Chevron"), + tr("Tint the lead chevron while sunnypilot is holding a lead through a sudden radar phantom (Tier-3 vision-coast). " + + "Debug aid; has no effect unless Radar Lead Tracking is set to Stable + Vision Coast."), + None, + ), } self._toggles = {} for param, (title, desc, callback) in self._toggle_defs.items(): diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py index 889e1737ee..499f372ebb 100644 --- a/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py +++ b/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py @@ -12,7 +12,8 @@ UiElement, RelDistElement, RelSpeedElement, SteeringAngleElement, DesiredLateralAccelElement, ActualLateralAccelElement, DesiredSteeringAngleElement, AEgoElement, LeadSpeedElement, FrictionCoefficientElement, LatAccelFactorElement, - SteeringTorqueEpsElement, BearingDegElement, AltitudeElement, DesiredSteeringPIDElement + SteeringTorqueEpsElement, BearingDegElement, AltitudeElement, DesiredSteeringPIDElement, + VisionCoastElement ) from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.text_measure import measure_text_cached @@ -53,6 +54,7 @@ def __init__(self): self.steering_torque_elem = SteeringTorqueEpsElement() self.bearing_elem = BearingDegElement() self.altitude_elem = AltitudeElement() + self.vision_coast_elem = VisionCoastElement() def _update_state(self) -> None: self.dev_ui_mode = ui_state.developer_ui @@ -95,6 +97,7 @@ def _draw_right_dev_ui(self, rect: rl.Rectangle) -> None: elements.append(self.desired_pid_steer_elem.update(sm, ui_state.is_metric)) elements.append(self.actual_lat_accel_elem.update(sm, ui_state.is_metric)) + elements.append(self.vision_coast_elem.update(sm, ui_state.is_metric)) current_y = y for element in elements: diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py index 389692d30b..cd8c18ee69 100644 --- a/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py +++ b/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py @@ -346,3 +346,25 @@ def update(self, sm, is_metric: bool) -> UiElement: value = f"{altitude:.1f}" if gps_accuracy != 0.0 else "-" return UiElement(value, "ALT.", self.unit, rl.WHITE) + + +class VisionCoastElement: + def __init__(self): + self.unit = "" + + def update(self, sm, is_metric: bool) -> UiElement: + if not sm.valid['radarStateSP']: + return UiElement("-", "COAST", self.unit, rl.WHITE) + rs = sm['radarStateSP'] + one, two = rs.leadOneCoasting, rs.leadTwoCoasting + if one and two: + value = "L1+L2" + elif one: + value = "L1" + elif two: + value = "L2" + else: + value = "-" + active = one or two + color = rl.Color(86, 199, 214, 255) if active else rl.WHITE # muted cyan when coasting + return UiElement(value, "COAST", self.unit, color) diff --git a/selfdrive/ui/sunnypilot/onroad/model_renderer.py b/selfdrive/ui/sunnypilot/onroad/model_renderer.py index 5d78997662..8b203d20a5 100644 --- a/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -4,11 +4,38 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +import pyray as rl + +from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath +LEAD_GLOW_GOLD = rl.Color(218, 202, 37, 255) # stock lead chevron glow +COAST_CYAN = rl.Color(86, 199, 214, 255) # vision-coast tint (matches the dev-UI COAST element) + + +def _lerp_color(a: rl.Color, b: rl.Color, t: float) -> rl.Color: + inv = 1.0 - t + return rl.Color(int(inv * a.r + t * b.r), int(inv * a.g + t * b.g), + int(inv * a.b + t * b.b), int(inv * a.a + t * b.a)) + class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() self.chevron_metrics = ChevronMetrics() + + def _lead_glow_color(self, i: int) -> rl.Color: + """Per-lead chevron glow color, called by the base _draw_lead_indicator. Stock gold unless the + ShowVisionCoastOnChevron debug toggle is on AND Tier-3 vision-coast is currently holding lead i + (from radarStateSP) — then lerp toward cyan so it reads as 'held by vision', per-lead.""" + if not getattr(ui_state, "show_vision_coast", False): + return LEAD_GLOW_GOLD + + sm = ui_state.sm + if not sm.valid["radarStateSP"]: + return LEAD_GLOW_GOLD + + rs = sm["radarStateSP"] + coasting = rs.leadOneCoasting if i == 0 else rs.leadTwoCoasting + return _lerp_color(LEAD_GLOW_GOLD, COAST_CYAN, 0.6) if coasting else LEAD_GLOW_GOLD diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index 1c21c35c6f..85383cfd81 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -32,7 +32,8 @@ def __init__(self): self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch") self.sm_services_ext = [ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", - "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay" + "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay", + "radarStateSP" ] self.sunnylink_state = SunnylinkState() @@ -156,6 +157,7 @@ def update_params(self) -> None: self.rainbow_path = self.params.get_bool("RainbowMode") self.road_name_toggle = self.params.get_bool("RoadNameToggle") self.rocket_fuel = self.params.get_bool("RocketFuel") + self.show_vision_coast = self.params.get_bool("ShowVisionCoastOnChevron") self.speed_limit_mode = self.params.get("SpeedLimitMode", return_default=True) self.standstill_timer = self.params.get_bool("StandstillTimer") self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")