Hyundai-Kia-Genesis (HKG) (#1362)
* hkg * Cleanup * Update readme * more fixes and cleanup * Old genesis * Typoe * Test car models * Update comment * Fix brake pressed * Update release notes * Fix vEgo * Add sonata * Add sonata values * Temporarily remove doors check. It doesn't work * Sonata uses crc8 * Fix tests * Changes for LFA * Add comment * Does this improve the hud? * Proper signal name * Force the right value * some ui stuff * more comments * Show lane lines on sonata * cleanup dash * fix last ui issues * Fix doors * update CI Co-authored-by: Comma Device <device@comma.ai>pull/1372/head
Willem Melching
5 years ago
committed by
GitHub
parent
6e388948e3
commit
7d78cef34b
13 changed files with 515 additions and 179 deletions
@ -1 +1 @@ |
|||||||
Subproject commit a57e7ddbd72c92241d5d6442da9d47c55e95a8cf |
Subproject commit b69398525a4f3d590305df171572415568e365aa |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit bc90b60f973ddf422ea78ce8fb83fbf88448694f |
Subproject commit 435cabe7f7a02a0c732725b97cbb7192a68a406d |
||||||
@ -1,47 +1,103 @@ |
|||||||
|
from cereal import car |
||||||
from selfdrive.car import apply_std_steer_torque_limits |
from selfdrive.car import apply_std_steer_torque_limits |
||||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11 |
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa |
||||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams |
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR |
||||||
from opendbc.can.packer import CANPacker |
from opendbc.can.packer import CANPacker |
||||||
|
|
||||||
|
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||||
|
|
||||||
|
|
||||||
|
def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, |
||||||
|
right_lane, left_lane_depart, right_lane_depart): |
||||||
|
sys_warning = (visual_alert == VisualAlert.steerRequired) |
||||||
|
|
||||||
|
# initialize to no line visible |
||||||
|
sys_state = 1 |
||||||
|
if left_lane and right_lane or sys_warning: #HUD alert only display when LKAS status is active |
||||||
|
if enabled or sys_warning: |
||||||
|
sys_state = 3 |
||||||
|
else: |
||||||
|
sys_state = 4 |
||||||
|
elif left_lane: |
||||||
|
sys_state = 5 |
||||||
|
elif right_lane: |
||||||
|
sys_state = 6 |
||||||
|
|
||||||
|
# initialize to no warnings |
||||||
|
left_lane_warning = 0 |
||||||
|
right_lane_warning = 0 |
||||||
|
if left_lane_depart: |
||||||
|
left_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 |
||||||
|
if right_lane_depart: |
||||||
|
right_lane_warning = 1 if fingerprint in [CAR.GENESIS_G90, CAR.GENESIS_G80] else 2 |
||||||
|
|
||||||
|
return sys_warning, sys_state, left_lane_warning, right_lane_warning |
||||||
|
|
||||||
|
|
||||||
class CarController(): |
class CarController(): |
||||||
def __init__(self, dbc_name, CP, VM): |
def __init__(self, dbc_name, CP, VM): |
||||||
self.apply_steer_last = 0 |
self.apply_steer_last = 0 |
||||||
self.car_fingerprint = CP.carFingerprint |
self.car_fingerprint = CP.carFingerprint |
||||||
self.lkas11_cnt = 0 |
|
||||||
self.cnt = 0 |
|
||||||
self.last_resume_cnt = 0 |
|
||||||
self.packer = CANPacker(dbc_name) |
self.packer = CANPacker(dbc_name) |
||||||
self.steer_rate_limited = False |
self.steer_rate_limited = False |
||||||
|
self.resume_cnt = 0 |
||||||
|
self.last_resume_frame = 0 |
||||||
|
self.last_lead_distance = 0 |
||||||
|
|
||||||
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): |
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, |
||||||
|
left_lane, right_lane, left_lane_depart, right_lane_depart): |
||||||
### Steering Torque |
# Steering Torque |
||||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX |
new_steer = actuators.steer * SteerLimitParams.STEER_MAX |
||||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) |
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) |
||||||
self.steer_rate_limited = new_steer != apply_steer |
self.steer_rate_limited = new_steer != apply_steer |
||||||
|
|
||||||
if not enabled: |
# disable if steer angle reach 90 deg, otherwise mdps fault in some models |
||||||
apply_steer = 0 |
lkas_active = enabled and abs(CS.out.steeringAngle) < 90. |
||||||
|
|
||||||
|
# fix for Genesis hard fault at low speed |
||||||
|
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: |
||||||
|
lkas_active = 0 |
||||||
|
|
||||||
steer_req = 1 if enabled else 0 |
if not lkas_active: |
||||||
|
apply_steer = 0 |
||||||
|
|
||||||
self.apply_steer_last = apply_steer |
self.apply_steer_last = apply_steer |
||||||
|
|
||||||
|
sys_warning, sys_state, left_lane_warning, right_lane_warning =\ |
||||||
|
process_hud_alert(enabled, self.car_fingerprint, visual_alert, |
||||||
|
left_lane, right_lane, left_lane_depart, right_lane_depart) |
||||||
|
|
||||||
can_sends = [] |
can_sends = [] |
||||||
|
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, |
||||||
|
CS.lkas11, sys_warning, sys_state, enabled, |
||||||
|
left_lane, right_lane, |
||||||
|
left_lane_warning, right_lane_warning)) |
||||||
|
|
||||||
self.lkas11_cnt = self.cnt % 0x10 |
if pcm_cancel_cmd: |
||||||
self.clu11_cnt = self.cnt % 0x10 |
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) |
||||||
|
|
||||||
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, |
elif CS.out.cruiseState.standstill: |
||||||
enabled, CS.lkas11, hud_alert, keep_stock=True)) |
# run only first time when the car stopped |
||||||
|
if self.last_lead_distance == 0: |
||||||
|
# get the lead distance from the Radar |
||||||
|
self.last_lead_distance = CS.lead_distance |
||||||
|
self.resume_cnt = 0 |
||||||
|
# when lead car starts moving, create 6 RES msgs |
||||||
|
elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5: |
||||||
|
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) |
||||||
|
self.resume_cnt += 1 |
||||||
|
# interval after 6 msgs |
||||||
|
if self.resume_cnt > 5: |
||||||
|
self.last_resume_frame = frame |
||||||
|
self.clu11_cnt = 0 |
||||||
|
# reset lead distnce after the car starts moving |
||||||
|
elif self.last_lead_distance != 0: |
||||||
|
self.last_lead_distance = 0 |
||||||
|
|
||||||
if pcm_cancel_cmd: |
|
||||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) |
|
||||||
elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5: |
|
||||||
self.last_resume_cnt = self.cnt |
|
||||||
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) |
|
||||||
|
|
||||||
self.cnt += 1 |
# 20 Hz LFA MFA message |
||||||
|
if frame % 5 == 0 and self.car_fingerprint == CAR.SONATA: |
||||||
|
can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) |
||||||
|
|
||||||
|
|
||||||
return can_sends |
return can_sends |
||||||
|
|||||||
@ -1,5 +1,77 @@ |
|||||||
#!/usr/bin/env python3 |
#!/usr/bin/env python3 |
||||||
|
import os |
||||||
|
import time |
||||||
|
from cereal import car |
||||||
|
from opendbc.can.parser import CANParser |
||||||
from selfdrive.car.interfaces import RadarInterfaceBase |
from selfdrive.car.interfaces import RadarInterfaceBase |
||||||
|
from selfdrive.car.hyundai.values import DBC |
||||||
|
|
||||||
|
|
||||||
|
def get_radar_can_parser(CP): |
||||||
|
signals = [ |
||||||
|
# sig_name, sig_address, default |
||||||
|
("ACC_ObjStatus", "SCC11", 0), |
||||||
|
("ACC_ObjLatPos", "SCC11", 0), |
||||||
|
("ACC_ObjDist", "SCC11", 0), |
||||||
|
("ACC_ObjRelSpd", "SCC11", 0), |
||||||
|
] |
||||||
|
checks = [ |
||||||
|
# address, frequency |
||||||
|
("SCC11", 50), |
||||||
|
] |
||||||
|
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) |
||||||
|
|
||||||
|
|
||||||
class RadarInterface(RadarInterfaceBase): |
class RadarInterface(RadarInterfaceBase): |
||||||
pass |
def __init__(self, CP): |
||||||
|
# radar |
||||||
|
self.pts = {} |
||||||
|
self.delay = 0 # Delay of radar |
||||||
|
self.rcp = get_radar_can_parser(CP) |
||||||
|
self.updated_messages = set() |
||||||
|
self.trigger_msg = 0x420 |
||||||
|
self.track_id = 0 |
||||||
|
self.radar_off_can = CP.radarOffCan |
||||||
|
|
||||||
|
def update(self, can_strings): |
||||||
|
if self.radar_off_can: |
||||||
|
if 'NO_RADAR_SLEEP' not in os.environ: |
||||||
|
time.sleep(0.05) # radard runs on RI updates |
||||||
|
|
||||||
|
return car.RadarData.new_message() |
||||||
|
|
||||||
|
vls = self.rcp.update_strings(can_strings) |
||||||
|
self.updated_messages.update(vls) |
||||||
|
|
||||||
|
if self.trigger_msg not in self.updated_messages: |
||||||
|
return None |
||||||
|
|
||||||
|
rr = self._update(self.updated_messages) |
||||||
|
self.updated_messages.clear() |
||||||
|
|
||||||
|
return rr |
||||||
|
|
||||||
|
def _update(self, updated_messages): |
||||||
|
ret = car.RadarData.new_message() |
||||||
|
cpt = self.rcp.vl |
||||||
|
errors = [] |
||||||
|
if not self.rcp.can_valid: |
||||||
|
errors.append("canError") |
||||||
|
ret.errors = errors |
||||||
|
|
||||||
|
valid = cpt["SCC11"]['ACC_ObjStatus'] |
||||||
|
if valid: |
||||||
|
for ii in range(2): |
||||||
|
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].dRel = cpt["SCC11"]['ACC_ObjDist'] # from front of car |
||||||
|
self.pts[ii].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative |
||||||
|
self.pts[ii].vRel = cpt["SCC11"]['ACC_ObjRelSpd'] |
||||||
|
self.pts[ii].aRel = float('nan') |
||||||
|
self.pts[ii].yvRel = float('nan') |
||||||
|
self.pts[ii].measured = True |
||||||
|
|
||||||
|
ret.points = list(self.pts.values()) |
||||||
|
return ret |
||||||
|
|||||||
@ -1 +1 @@ |
|||||||
3523742130b9e0554bab4ac5bc5ab535f1342e90 |
9638f1d5495e92b2d633a193f00a65f8268b170c |
||||||
Loading…
Reference in new issue