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.hyundai.hyundaican import create_lkas11, create_clu11 |
||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams |
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa |
||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR |
||||
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(): |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.apply_steer_last = 0 |
||||
self.car_fingerprint = CP.carFingerprint |
||||
self.lkas11_cnt = 0 |
||||
self.cnt = 0 |
||||
self.last_resume_cnt = 0 |
||||
self.packer = CANPacker(dbc_name) |
||||
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): |
||||
|
||||
### Steering Torque |
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, |
||||
left_lane, right_lane, left_lane_depart, right_lane_depart): |
||||
# Steering Torque |
||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX |
||||
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 |
||||
|
||||
if not enabled: |
||||
apply_steer = 0 |
||||
# disable if steer angle reach 90 deg, otherwise mdps fault in some models |
||||
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 |
||||
|
||||
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.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 |
||||
self.clu11_cnt = self.cnt % 0x10 |
||||
if pcm_cancel_cmd: |
||||
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, |
||||
enabled, CS.lkas11, hud_alert, keep_stock=True)) |
||||
elif CS.out.cruiseState.standstill: |
||||
# 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 |
||||
|
||||
@ -1,5 +1,77 @@ |
||||
#!/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.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): |
||||
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