diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 8f96168712846d..b0ee67be9dfc6a 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -131,22 +131,20 @@ def get_RadarState(self, model_prob=0.0): } def get_RadarState_from_vision(self, lead_msg, v_ego, vision_v_ego): - # Learn vision model velocity error to correct vlead and alead + # Learn vision model velocity error to correct vlead if v_ego > 0 and vision_v_ego > 0: vision_velocity_factor = v_ego / vision_v_ego corrected_v_lead = lead_msg.v[0] * vision_velocity_factor - corrected_a_lead = lead_msg.a[0] * vision_velocity_factor #if no v_ego or vision_v_ego set, fallback to uncalibrated values else: corrected_v_lead = lead_msg.v[0] - corrected_a_lead = lead_msg.a[0] return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), "vRel": float(corrected_v_lead - v_ego), "vLead": float(corrected_v_lead), "vLeadK": float(corrected_v_lead), - "aLeadK": float(corrected_a_lead), + "aLeadK": float(lead_msg.a[0]), "aLeadTau": _LEAD_ACCEL_TAU, "fcw": False, "modelProb": float(lead_msg.prob),