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

Restore changes to get_RadarState_from_vision

Browse Source
This commit is contained in:
firestar5683
2025-10-07 21:17:04 -05:00
committed by GitHub
parent 413fadbe36 1cc2ac6e69
commit 8352e71960
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,