diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 98fce1cb26..a5dd3400e3 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -5,13 +5,16 @@ from collections import deque from typing import Any import capnp -from cereal import messaging, log, car +from cereal import messaging, log, car, custom from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params 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 import structs +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP + # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 @@ -157,7 +160,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -167,6 +170,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg) elif (track is None) and ready and (lead_msg.prob > .5): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) @@ -182,8 +186,19 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn return lead_dict +def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict: dict[str, Any], + lead_msg: capnp._DynamicStructReader) -> dict[str, Any]: + if CP.brand == "hyundai" and CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC: + lead_dict['yRel'] = float(-lead_msg.y[0]) + + return lead_dict + + class RadarD: - def __init__(self, delay: float = 0.0): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0): + self.CP = CP + self.CP_SP = CP_SP + self.current_time = 0.0 self.tracks: dict[int, Track] = {} @@ -239,8 +254,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, 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.CP, self.CP_SP, low_speed_override=False) def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None @@ -260,11 +275,15 @@ def main() -> None: CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams) cloudlog.info("radard got CarParams") + cloudlog.info("radard is waiting for CarParamsSP") + CP_SP = messaging.log_from_bytes(Params().get("CarParamsSP", block=True), custom.CarParamsSP) + cloudlog.info("radard got CarParamsSP") + # *** setup messaging sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2') pm = messaging.PubMaster(['radarState']) - RD = RadarD(CP.radarDelay) + RD = RadarD(CP, CP_SP, CP.radarDelay) while 1: sm.update()