mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-19 16:52:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master-new' into HKG-long-tune

Browse Source
This commit is contained in:
Discountchubbs
2025-04-11 12:35:05 -07:00
committed by GitHub
parent 7cb530e70c 6a00ac9cd0
commit 0fb04ab520
1 changed files with 25 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+25 -6
View File
@@ -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()