Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

hkg can-fd hda1 CameraSCC radar parsing #209

Open
wants to merge 1 commit into
base: MAKE-PRS-HERE
Choose a base branch
from
Open

hkg can-fd hda1 CameraSCC radar parsing #209

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions selfdrive/car/hyundai/interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
if candidate not in CANFD_RADAR_SCC_CAR:
ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
ret.radarUnavailable = False
else:
# TODO: detect EV and hybrid
if candidate in HYBRID_CAR:
Expand Down
82 changes: 59 additions & 23 deletions selfdrive/car/hyundai/radar_interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.hyundai.values import DBC
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlags, CANFD_CAR

RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
Expand All @@ -12,7 +13,15 @@

def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
return None
if CP.carFingerprint in CANFD_CAR:
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
lead_src, bus = "SCC_CONTROL", CanBus(CP).CAM
else:
return None
else:
return None
messages = [(lead_src, 50)]
return CANParser(DBC[CP.carFingerprint]['pt'], messages, bus)

messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1)
Expand All @@ -21,8 +30,10 @@ def get_radar_can_parser(CP):
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.CP = CP
self.camera_scc = CP.flags & HyundaiFlags.CANFD_CAMERA_SCC
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.trigger_msg = 0x1A0 if self.camera_scc else (RADAR_START_ADDR + RADAR_MSG_COUNT - 1)
self.track_id = 0

self.radar_off_can = CP.radarUnavailable
Expand All @@ -41,6 +52,10 @@ def update(self, can_strings):
rr = self._update(self.updated_messages)
self.updated_messages.clear()

radar_error = []
if rr is not None:
radar_error = rr.errors

return rr

def _update(self, updated_messages):
Expand All @@ -54,26 +69,47 @@ def _update(self, updated_messages):
errors.append("canError")
ret.errors = errors

for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]

if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1

valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')

else:
del self.pts[addr]
if self.camera_scc:
msg_src = "SCC_CONTROL"
msg = self.rcp.vl[msg_src]
valid = msg['ACC_ObjDist'] < 204.6
for ii in range(1):
if valid:
if ii not in self.pts:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].measured = True
self.pts[ii].dRel = msg['ACC_ObjDist']
self.pts[ii].yRel = float('nan')
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')

else:
if ii in self.pts:
del self.pts[ii]
else:
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]

if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1

valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')

else:
del self.pts[addr]

ret.points = list(self.pts.values())
return ret
20 changes: 12 additions & 8 deletions selfdrive/controls/radard.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
from openpilot.common.swaglog import cloudlog

from openpilot.common.simple_kalman import KF1D
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags

from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables

Expand Down Expand Up @@ -92,10 +93,11 @@ def reset_a_lead(self, aLeadK: float, aLeadTau: float):
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 or CP.carName != "hyundai" else CP.flags & HyundaiFlags.CANFD_CAMERA_SCC
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),
Expand Down Expand Up @@ -181,7 +183,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, lead_detection_threshold: float, low_speed_override: bool = True) -> dict[str, Any]:
model_v_ego: float, CP: car.CarParams, lead_detection_threshold: float, 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 > lead_detection_threshold:
track = match_vision_to_track(v_ego, lead_msg, tracks)
Expand All @@ -190,7 +192,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 > lead_detection_threshold):
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)

Expand Down Expand Up @@ -218,7 +220,7 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct


class RadarD:
def __init__(self, radar_ts: float, delay: int = 0):
def __init__(self, radar_ts: float, CP: car.CarParams, delay: int = 0):
self.points: dict[int, tuple[float, float, float]] = {}

self.current_time = 0.0
Expand All @@ -236,6 +238,8 @@ def __init__(self, radar_ts: float, delay: int = 0):

self.ready = False

self.CP = CP

# FrogPilot variables
self.frogpilot_toggles = FrogPilotVariables.toggles
FrogPilotVariables.update_frogpilot_params()
Expand Down Expand Up @@ -295,8 +299,8 @@ def update(self, sm: messaging.SubMaster, rr):
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, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=True)
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, 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.frogpilot_toggles.lead_detection_threshold, 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.frogpilot_toggles.lead_detection_threshold, low_speed_override=False)

if self.frogpilot_toggles.adjacent_lead_tracking_ui:
self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True)
Expand Down Expand Up @@ -378,7 +382,7 @@ def main():
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)

# FrogPilot variables
frogpilot_toggles = FrogPilotVariables.toggles
Expand Down
Loading