EV6 longitudinal (#26023)

* ev6 long

* update refs
old-commit-hash: 3c0904a18f
taco
Adeeb Shihadeh 3 years ago committed by GitHub
parent c4c087e145
commit 026668b5f8
  1. 2
      panda
  2. 94
      selfdrive/car/hyundai/carcontroller.py
  3. 40
      selfdrive/car/hyundai/carstate.py
  4. 109
      selfdrive/car/hyundai/hyundaicanfd.py
  5. 66
      selfdrive/car/hyundai/interface.py
  6. 2
      selfdrive/test/process_replay/ref_commit

@ -1 +1 @@
Subproject commit d68b1b0a98d5cefad438180e3c2ffcdcbcffdd76
Subproject commit ffb3109e28296cc86f1892c4e0690856e28fb35a

@ -52,17 +52,15 @@ class CarController:
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.accel = 0
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
# Steering Torque
# These cars have significantly more torque than most HKG. Limit to 70% of max.
# steering torque
steer = actuators.steer
if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
# these cars have significantly more torque than most HKG; limit to 70% of max
steer = clip(steer, -0.7, 0.7)
new_steer = int(round(steer * self.params.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
@ -72,48 +70,66 @@ class CarController:
self.apply_steer_last = apply_steer
# accel + longitudinal
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and self.CP.openpilotLongitudinalControl:
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, 5
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.append(hyundaicanfd.create_lkas(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer))
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, CC.latActive, apply_steer))
# block LFA on HDA2
if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.CANFD_HDA2):
# disable LFA on HDA2
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4))
# LFA and HDA icons
if self.frame % 2 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_HDA2):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CC.enabled))
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
# button presses
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_cruise_info(self.packer, CS.cruise_info_copy, True))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
else:
# tester present - w/ no response (keeps radar disabled)
if self.CP.openpilotLongitudinalControl:
if self.frame % 100 == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, accel, stopping, CC.cruiseControl.override,
set_speed_in_units))
else:
# button presses
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if not (self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
else:
# Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault
if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE:
self.angle_limit_counter += 1
@ -143,18 +159,10 @@ class CarController:
self.last_button_frame = self.frame
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
accel = actuators.accel
#TODO unclear if this is needed
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override))
self.accel = accel
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
@ -173,7 +181,7 @@ class CarController:
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.accel = self.accel
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends

@ -30,6 +30,7 @@ class CarState(CarStateBase):
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.is_metric = False
self.brake_error = False
self.buttons_counter = 0
@ -45,8 +46,8 @@ class CarState(CarStateBase):
ret = car.CarState.new_message()
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
@ -69,7 +70,7 @@ class CarState(CarStateBase):
self.cluster_speed_counter = 0
# mimic how dash converts to imperial
if not is_metric:
if not self.is_metric:
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
ret.vEgoCluster = self.cluster_speed * speed_conv
@ -187,16 +188,19 @@ class CarState(CarStateBase):
ret.cruiseState.available = True
ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1
cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS
ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1
if not self.CP.openpilotLongitudinalControl:
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"])
self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"]
self.cruise_info_copy = copy.copy(cp_cruise_info.vl["CRUISE_INFO"])
self.cruise_info_copy = {}
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"])
@ -448,13 +452,18 @@ class CarState(CarStateBase):
signals += [
("ACCELERATOR_PEDAL", "ACCELERATOR"),
("GEAR", "ACCELERATOR"),
("SET_SPEED", "CRUISE_INFO"),
("CRUISE_STANDSTILL", "CRUISE_INFO"),
]
checks += [
("CRUISE_INFO", 50),
("ACCELERATOR", 100),
]
if not CP.openpilotLongitudinalControl:
signals += [
("SET_SPEED", "CRUISE_INFO"),
("CRUISE_STANDSTILL", "CRUISE_INFO"),
]
checks += [
("CRUISE_INFO", 50),
]
else:
signals += [
("ACCELERATOR_PEDAL", "ACCELERATOR_ALT"),
@ -478,17 +487,14 @@ class CarState(CarStateBase):
("CRUISE_MAIN", "CRUISE_INFO"),
("CRUISE_STATUS", "CRUISE_INFO"),
("CRUISE_INACTIVE", "CRUISE_INFO"),
("NEW_SIGNAL_2", "CRUISE_INFO"),
("ZEROS_9", "CRUISE_INFO"),
("CRUISE_STANDSTILL", "CRUISE_INFO"),
("NEW_SIGNAL_3", "CRUISE_INFO"),
("BYTE11", "CRUISE_INFO"),
("ZEROS_5", "CRUISE_INFO"),
("DISTANCE_SETTING", "CRUISE_INFO"),
("SET_SPEED", "CRUISE_INFO"),
("NEW_SIGNAL_4", "CRUISE_INFO"),
]
signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(3, 7)]
signals += [(f"BYTE{i}", "CRUISE_INFO") for i in range(13, 31)]
checks = [
("CRUISE_INFO", 50),
]

@ -1,7 +1,17 @@
from selfdrive.car.hyundai.values import HyundaiFlags
def create_lkas(packer, CP, enabled, lat_active, apply_steer):
def get_e_can_bus(CP):
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a a different harness than the HDA1 and non-HDA variants in order to split
# a different bus, since the steering is done by different ECUs.
return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4
def create_steering_messages(packer, CP, enabled, lat_active, apply_steer):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
@ -14,8 +24,14 @@ def create_lkas(packer, CP, enabled, lat_active, apply_steer):
"NEW_SIGNAL_2": 0,
}
msg = "LKAS" if CP.flags & HyundaiFlags.CANFD_HDA2 else "LFA"
return packer.make_can_msg(msg, 4, values)
if CP.flags & HyundaiFlags.CANFD_HDA2:
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", 5, values))
ret.append(packer.make_can_msg("LKAS", 4, values))
else:
ret.append(packer.make_can_msg("LFA", 4, values))
return ret
def create_cam_0x2a4(packer, camera_values):
camera_values.update({
@ -36,11 +52,92 @@ def create_cruise_info(packer, cruise_info_copy, cancel):
if cancel:
values["CRUISE_STATUS"] = 0
values["CRUISE_INACTIVE"] = 1
return packer.make_can_msg("CRUISE_INFO", 4, values)
return packer.make_can_msg("CRUISE_INFO", 5, values)
def create_lfahda_cluster(packer, enabled):
def create_lfahda_cluster(packer, CP, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", 4, values)
return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values)
def create_acc_control(packer, CP, enabled, accel, stopping, gas_override, set_speed):
cruise_status = 0 if not enabled else (4 if gas_override else 2)
if not enabled or gas_override:
accel = 0
values = {
"CRUISE_STATUS": cruise_status,
"CRUISE_INACTIVE": 0 if enabled else 1,
"CRUISE_MAIN": 1,
"CRUISE_STANDSTILL": 0,
"STOP_REQ": 1 if stopping else 0,
"ACCEL_REQ": accel,
"ACCEL_REQ2": accel,
"SET_SPEED": set_speed,
"DISTANCE_SETTING": 4,
"ACC_ObjDist": 1,
"ObjValid": 1,
"OBJ_STATUS": 2,
"SET_ME_2": 0x2,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"NEW_SIGNAL_9": 2,
"NEW_SIGNAL_10": 4,
}
return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values)
def create_adrv_messages(packer, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", 4, values))
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
}
ret.append(packer.make_can_msg("ADRV_0x160", 5, values))
if frame % 5 == 0:
values = {
'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
'SET_ME_TMP_F': 0xf,
'SET_ME_TMP_F_2': 0xf,
}
ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values))
values = {
'SET_ME_E1': 0xe1,
'SET_ME_3A': 0x3a,
}
ret.append(packer.make_can_msg("ADRV_0x200", 5, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", 5, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", 5, values))
return ret

@ -27,31 +27,24 @@ class CarInterface(CarInterfaceBase):
ret.carName = "hyundai"
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
# WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR | CANFD_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
ret.pcmCruise = not ret.openpilotLongitudinalControl
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
if candidate in CANFD_CAR:
# detect HDA2 with LKAS message
if 0x50 in fingerprint[6]:
ret.flags |= HyundaiFlags.CANFD_HDA2.value
else:
# non-HDA2
if 0x1cf not in fingerprint[4]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
ret.stoppingControl = True
ret.startingState = True
ret.vEgoStarting = 0.1
ret.startAccel = 2.0
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.longitudinalActuatorDelayLowerBound = 0.5 # s
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
@ -291,20 +284,38 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
# panda safety config
# *** longitudinal control ***
if candidate in CANFD_CAR:
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.longitudinalActuatorDelayLowerBound = 0.15
ret.longitudinalActuatorDelayUpperBound = 0.5
ret.experimentalLongitudinalAvailable = bool(ret.flags & HyundaiFlags.CANFD_HDA2)
else:
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.longitudinalActuatorDelayLowerBound = 0.5
ret.longitudinalActuatorDelayUpperBound = 0.5
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
ret.startingState = True
ret.vEgoStarting = 0.1
ret.startAccel = 2.0
# *** panda safety config ***
if candidate in CANFD_CAR:
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)]
# detect HDA2 with LKAS message
if 0x50 in fingerprint[6]:
ret.flags |= HyundaiFlags.CANFD_HDA2.value
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_LONG
if ret.flags & HyundaiFlags.CANFD_HDA2:
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
else:
# non-HDA2
if 0x1cf not in fingerprint[4]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
else:
ret.enableBsm = 0x58b in fingerprint[0]
@ -342,7 +353,10 @@ class CarInterface(CarInterfaceBase):
@staticmethod
def init(CP, logcan, sendcan):
if CP.openpilotLongitudinalControl:
disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01')
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, 5
disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)

@ -1 +1 @@
5fa6a3743f2678eef13267fb946d7a03f2af5824
27022484e3f4c0265ee7243154659b2697de3af7
Loading…
Cancel
Save