mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-22 11:13:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Hyundai CAN Longitudinal: Parse lead info for camera-based SCC platforms (#299)

Browse Source 
* Hyundai CAN Longitudinal: Parse lead info for camera-based SCC platforms

* typo

* another typo

* camera SCC has them flipped

* disable `radarUnavailble`

* Missing on these platforms

* parse lateral position of lead car from vision model output

* fix CP init
This commit is contained in:
Jason Wen
2023-09-30 01:20:13 -04:00
committed by GitHub
parent b982079429
commit d6a768c96f
4 changed files with 26 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/hyundai/interface.py
+3
View File
@@ -121,6 +121,9 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6)
ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec
ret.tireStiffnessFactor = 0.385
if candidate == CAR.KONA_EV_2022:
ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value
ret.radarUnavailable = False
elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV):
ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
selfdrive/car/hyundai/radar_interface.py
+10 -4
View File
@@ -13,6 +13,8 @@ def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC:
lead_src, bus = "ESCC", 0
elif CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD:
lead_src, bus = "SCC11", 2
else:
return None
messages = [(lead_src, 50)]
@@ -26,8 +28,11 @@ class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.enhanced_scc = (CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None
self.camera_scc = CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD
self.updated_messages = set()
self.trigger_msg = 0x2AB if self.enhanced_scc else RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.trigger_msg = 0x2AB if self.enhanced_scc else \
0x420 if self.camera_scc else \
(RADAR_START_ADDR + RADAR_MSG_COUNT - 1)
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
@@ -59,8 +64,9 @@ class RadarInterface(RadarInterfaceBase):
errors.append("canError")
ret.errors = errors
if self.enhanced_scc:
msg = self.rcp.vl["ESCC"]
if self.enhanced_scc or self.camera_scc:
msg_src = "ESCC" if self.enhanced_scc else "SCC11"
msg = self.rcp.vl[msg_src]
valid = msg['ACC_ObjStatus']
for ii in range(1):
if valid:
@@ -70,7 +76,7 @@ class RadarInterface(RadarInterfaceBase):
self.track_id += 1
self.pts[ii].measured = True
self.pts[ii].dRel = msg['ACC_ObjDist']
self.pts[ii].yRel = -msg['ACC_ObjLatPos']
self.pts[ii].yRel = -msg['ACC_ObjLatPos'] if self.enhanced_scc else float('nan')
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
selfdrive/car/hyundai/values.py
+1
View File
@@ -71,6 +71,7 @@ class HyundaiFlagsSP(IntFlag):
SP_ENHANCED_SCC = 1
SP_CAN_LFA_BTN = 2
SP_NAV_MSG = 4
SP_CAMERA_SCC_LEAD = 8
class CAR:
selfdrive/controls/radard.py
+12 -8
View File
@@ -9,6 +9,7 @@ from cereal import messaging, log, car
from openpilot.common.numpy_fast import interp
from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process
from openpilot.selfdrive.car.hyundai.values import HyundaiFlagsSP
from openpilot.system.swaglog import cloudlog
from openpilot.common.kalman.simple_kalman import KF1D
@@ -91,10 +92,11 @@ class Track:
self.aLeadK = aLeadK
self.aLeadTau = aLeadTau
def get_RadarState(self, model_prob: float = 0.0):
def get_RadarState(self, CP: car.CarParams = None, lead_msg_y: float = 0.0, model_prob: float = 0.0):
y_rel_vision = False if CP is None else CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD
return {
"dRel": float(self.dRel),
"yRel": float(self.yRel),
"yRel": float(-lead_msg_y) if y_rel_vision else float(self.yRel),
"vRel": float(self.vRel),
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
@@ -167,7 +169,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: car.CarParams, 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)
@@ -176,7 +178,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 = track.get_RadarState(CP, lead_msg.y[0], lead_msg.prob)
elif (track is None) and ready and (lead_msg.prob > .5):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
@@ -193,7 +195,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
class RadarD:
def __init__(self, radar_ts: float, delay: int = 0):
def __init__(self, radar_ts: float, CP: car.CarParams, delay: int = 0):
self.current_time = 0.0
self.tracks: Dict[int, Track] = {}
@@ -207,6 +209,8 @@ class RadarD:
self.ready = False
self.CP = CP
def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
self.current_time = 1e-9*max(sm.logMonoTime.values())
@@ -256,8 +260,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, 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, low_speed_override=False)
def publish(self, pm: messaging.PubMaster, lag_ms: float):
assert self.radar_state is not None
@@ -305,7 +309,7 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
RD = RadarD(CP.radarTimeStep, CP, RI.delay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)