From 1cc2ac6e69e3c830cc42c49d04d4ba64bd1b5684 Mon Sep 17 00:00:00 2001
From: roenthomas <43324106+roenthomas@users.noreply.github.com>
Date: Tue, 7 Oct 2025 14:26:51 -0400
Subject: [PATCH] Restore changes to get_RadarState_from_vision

Minimal deviations from upstream FP
---
 selfdrive/controls/radard.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 30b398e7a..74cab654b 100644
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -173,15 +173,16 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
 
 
 def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
+  lead_v_rel_pred = lead_msg.v[0] - model_v_ego
   prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0)
   blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK
   get_RadarState_from_vision.prev_aLeadK = blended_aLeadK
   return {
     "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
     "yRel": float(-lead_msg.y[0]),
-    "vRel": float(lead_msg.v[0] - model_v_ego),
-    "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)),
-    "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)),
+    "vRel": float(lead_v_rel_pred),
+    "vLead": float(v_ego + lead_v_rel_pred),
+    "vLeadK": float(v_ego + lead_v_rel_pred),
     "aLeadK": blended_aLeadK,
     "aLeadTau": 0.3,
     "fcw": False,