mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-05 13:32:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Restore changes to get_RadarState_from_vision

Browse Source 
Minimal deviations from upstream FP
This commit is contained in:
roenthomas
2025-10-07 14:26:51 -04:00
parent 08460b76f1
commit 1cc2ac6e69
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+4 -3
View File
@@ -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,