Hyundai longitudinal prerequisites (#22121)
* panda * bring over changes * dont check car model * remove comment * fix typo * more stuff gated behind long * not * not used * gate that too * try honda tuning * clip accel values * fix up merge * fix stopping * add retry logic around knockout * increase timeout * keep flipping lead bit * true for now * less tuning * update comment * 0.1 s is fine now * merge honda and hyundai knockout * more lead fields * another obj bit * increase timeout * fix stopping flag * only lag compensate for braking * no lead * less tuning * only do knockout if not readonly * try controlling using jerk * tuning * try higher stopping rate * set stopping flag at higher speed * clip upper jerk when stopping * remove comments * tester present 1hz * use positive start accel * 1.0 to maybe improve low speed stuff * no point going over 0 * bump panda * bump panda * revert that change * update refpull/22010/head
Willem Melching
GitHub
parent
0dc45eaf1c
commit
c4bac6bd68
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit 1befaad8b013209aff58145d006d8d047359797e
|
||||
Subproject commit dd22fafc3c9f36c9d96dffee5437fb9f56d7ff6d
|
||||
|
|
@ -98,6 +98,7 @@ selfdrive/car/car_helpers.py
|
|||
selfdrive/car/fingerprints.py
|
||||
selfdrive/car/interfaces.py
|
||||
selfdrive/car/vin.py
|
||||
selfdrive/car/disable_ecu.py
|
||||
selfdrive/car/fw_versions.py
|
||||
selfdrive/car/isotp_parallel_query.py
|
||||
selfdrive/car/tests/__init__.py
|
||||
|
|
|
|||
|
|
@ -0,0 +1,34 @@
|
|||
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
EXT_DIAG_REQUEST = b'\x10\x03'
|
||||
EXT_DIAG_RESPONSE = b'\x50\x03'
|
||||
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
|
||||
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
|
||||
The ECU will stay silent as long as openpilot keeps sending Tester Present.
|
||||
|
||||
This is used to disable the radar in some cars. Openpilot will emulate the radar.
|
||||
WARNING: THIS DISABLES AEB!"""
|
||||
cloudlog.warning(f"ecu disable {hex(addr)} ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
cloudlog.warning("communication control disable tx/rx ...")
|
||||
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
|
||||
query.get_data(0)
|
||||
|
||||
cloudlog.warning("ecu disabled")
|
||||
return True
|
||||
except Exception:
|
||||
cloudlog.exception("ecu disable exception")
|
||||
|
||||
print(f"ecu disable retry ({i+1}) ...")
|
||||
cloudlog.warning("ecu disable failed")
|
||||
return False
|
||||
|
|
@ -1,7 +1,5 @@
|
|||
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from selfdrive.car.honda.values import HONDA_BOSCH, CAR
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
# CAN bus layout with relay
|
||||
# 0 = ACC-CAN - radar side
|
||||
|
|
@ -9,13 +7,6 @@ from selfdrive.swaglog import cloudlog
|
|||
# 2 = ACC-CAN - camera side
|
||||
# 3 = F-CAN A - OBDII port
|
||||
|
||||
RADAR_ADDR = 0x18DAB0F1
|
||||
EXT_DIAG_REQUEST = b'\x10\x03'
|
||||
EXT_DIAG_RESPONSE = b'\x50\x03'
|
||||
COM_CONT_REQUEST = b'\x28\x83\x03'
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
|
||||
def get_pt_bus(car_fingerprint):
|
||||
return 1 if car_fingerprint in HONDA_BOSCH else 0
|
||||
|
||||
|
|
@ -27,30 +18,6 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
|
|||
# normally steering commands are sent to radar, which forwards them to powertrain bus
|
||||
return 0
|
||||
|
||||
|
||||
def disable_radar(logcan, sendcan, bus=1, timeout=0.1, debug=False):
|
||||
"""Silence the radar by disabling sending and receiving messages using UDS 0x28.
|
||||
The radar will stay silent as long as openpilot keeps sending Tester Present.
|
||||
Openpilot will emulate the radar. WARNING: THIS DISABLES AEB!"""
|
||||
cloudlog.warning(f"radar disable {hex(RADAR_ADDR)} ...")
|
||||
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
cloudlog.warning("radar communication control disable tx/rx ...")
|
||||
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [COM_CONT_REQUEST], [COM_CONT_RESPONSE], debug=debug)
|
||||
query.get_data(0)
|
||||
|
||||
cloudlog.warning("radar disabled")
|
||||
return
|
||||
|
||||
except Exception:
|
||||
cloudlog.exception("radar disable exception")
|
||||
cloudlog.warning("radar disable failed")
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
|
|
|
|||
|
|
@ -4,9 +4,9 @@ from panda import Panda
|
|||
from common.numpy_fast import interp
|
||||
from common.params import Params
|
||||
from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
|
||||
from selfdrive.car.honda.hondacan import disable_radar
|
||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from selfdrive.car.disable_ecu import disable_ecu
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
|
||||
|
|
@ -300,7 +300,7 @@ class CarInterface(CarInterfaceBase):
|
|||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl:
|
||||
disable_radar(logcan, sendcan)
|
||||
disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
|
||||
|
||||
# returns a car.CarState
|
||||
def update(self, c, can_strings):
|
||||
|
|
|
|||
|
|
@ -1,11 +1,14 @@
|
|||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc
|
||||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
|
||||
from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
|
||||
|
|
@ -42,7 +45,7 @@ class CarController():
|
|||
self.steer_rate_limited = False
|
||||
self.last_resume_frame = 0
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart):
|
||||
# Steering Torque
|
||||
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
|
||||
|
|
@ -62,19 +65,41 @@ class CarController():
|
|||
left_lane, right_lane, left_lane_depart, right_lane_depart)
|
||||
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if CS.CP.openpilotLongitudinalControl:
|
||||
if (frame % 100) == 0:
|
||||
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
|
||||
|
||||
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))
|
||||
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
# send resume at a max freq of 10Hz
|
||||
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
|
||||
# send 25 messages at a time to increases the likelihood of resume being accepted
|
||||
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
|
||||
self.last_resume_frame = frame
|
||||
if not CS.CP.openpilotLongitudinalControl:
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
# send resume at a max freq of 10Hz
|
||||
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
|
||||
# send 25 messages at a time to increases the likelihood of resume being accepted
|
||||
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
|
||||
self.last_resume_frame = frame
|
||||
|
||||
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
lead_visible = False
|
||||
accel = actuators.accel if enabled else 0
|
||||
|
||||
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
|
||||
|
||||
if accel < 0:
|
||||
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
|
||||
|
||||
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
|
||||
|
||||
stopping = (actuators.longControlState == LongCtrlState.stopping)
|
||||
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
|
||||
can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
|
||||
|
|
@ -82,4 +107,12 @@ class CarController():
|
|||
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
|
||||
can_sends.append(create_lfahda_mfc(self.packer, enabled))
|
||||
|
||||
# 5 Hz ACC options
|
||||
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(create_acc_opt(self.packer))
|
||||
|
||||
# 2 Hz front radar options
|
||||
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_frt_radar_opt(self.packer))
|
||||
|
||||
return can_sends
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@ class CarState(CarStateBase):
|
|||
|
||||
# cruise state
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
|
||||
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
|
||||
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
|
||||
ret.cruiseState.standstill = False
|
||||
|
|
@ -57,11 +58,11 @@ class CarState(CarStateBase):
|
|||
ret.cruiseState.enabled = cp.vl["SCC12"]["ACCMode"] != 0
|
||||
ret.cruiseState.standstill = cp.vl["SCC11"]["SCCInfoDisplay"] == 4.
|
||||
|
||||
if ret.cruiseState.enabled:
|
||||
speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
|
||||
else:
|
||||
ret.cruiseState.speed = 0
|
||||
if ret.cruiseState.enabled:
|
||||
speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
|
||||
else:
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
# TODO: Find brake pressure
|
||||
ret.brake = 0
|
||||
|
|
@ -90,12 +91,13 @@ class CarState(CarStateBase):
|
|||
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
|
||||
|
||||
if self.CP.carFingerprint in FEATURES["use_fca"]:
|
||||
ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
|
||||
else:
|
||||
ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.CP.carFingerprint in FEATURES["use_fca"]:
|
||||
ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
|
||||
else:
|
||||
ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
|
||||
|
|
@ -106,7 +108,6 @@ class CarState(CarStateBase):
|
|||
self.clu11 = copy.copy(cp.vl["CLU11"])
|
||||
self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
|
||||
self.lead_distance = cp.vl["SCC11"]["ACC_ObjDist"]
|
||||
self.brake_hold = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
|
|
@ -168,12 +169,6 @@ class CarState(CarStateBase):
|
|||
|
||||
("SAS_Angle", "SAS11", 0),
|
||||
("SAS_Speed", "SAS11", 0),
|
||||
|
||||
("MainMode_ACC", "SCC11", 0),
|
||||
("VSetDis", "SCC11", 0),
|
||||
("SCCInfoDisplay", "SCC11", 0),
|
||||
("ACC_ObjDist", "SCC11", 0),
|
||||
("ACCMode", "SCC12", 1),
|
||||
]
|
||||
|
||||
checks = [
|
||||
|
|
@ -191,11 +186,31 @@ class CarState(CarStateBase):
|
|||
]
|
||||
|
||||
if not CP.openpilotLongitudinalControl:
|
||||
signals += [
|
||||
("MainMode_ACC", "SCC11", 0),
|
||||
("VSetDis", "SCC11", 0),
|
||||
("SCCInfoDisplay", "SCC11", 0),
|
||||
("ACC_ObjDist", "SCC11", 0),
|
||||
("ACCMode", "SCC12", 1),
|
||||
]
|
||||
|
||||
checks += [
|
||||
("SCC11", 50),
|
||||
("SCC12", 50),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in FEATURES["use_fca"]:
|
||||
signals += [
|
||||
("FCA_CmdAct", "FCA11", 0),
|
||||
("CF_VSM_Warn", "FCA11", 0),
|
||||
]
|
||||
checks += [("FCA11", 50)]
|
||||
else:
|
||||
signals += [
|
||||
("AEB_CmdAct", "SCC12", 0),
|
||||
("CF_VSM_Warn", "SCC12", 0),
|
||||
]
|
||||
|
||||
if CP.enableBsm:
|
||||
signals += [
|
||||
("CF_Lca_IndLeft", "LCA11", 0),
|
||||
|
|
@ -250,19 +265,6 @@ class CarState(CarStateBase):
|
|||
("LVR12", 100)
|
||||
]
|
||||
|
||||
if CP.carFingerprint in FEATURES["use_fca"]:
|
||||
signals += [
|
||||
("FCA_CmdAct", "FCA11", 0),
|
||||
("CF_VSM_Warn", "FCA11", 0),
|
||||
]
|
||||
if not CP.openpilotLongitudinalControl:
|
||||
checks += [("FCA11", 50)]
|
||||
else:
|
||||
signals += [
|
||||
("AEB_CmdAct", "SCC12", 0),
|
||||
("CF_VSM_Warn", "SCC12", 0),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
|
|
|
|||
|
|
@ -3,7 +3,6 @@ from selfdrive.car.hyundai.values import CAR, CHECKSUM
|
|||
|
||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
||||
|
||||
|
||||
def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
|
||||
lkas11, sys_warning, sys_state, enabled,
|
||||
left_lane, right_lane,
|
||||
|
|
@ -78,7 +77,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
|
|||
}
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, stopping):
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
|
|
@ -86,14 +85,19 @@ def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, st
|
|||
"TauGapSet": 4,
|
||||
"VSetDis": set_speed if enabled else 0,
|
||||
"AliveCounterACC": idx % 0x10,
|
||||
"ObjValid": 1 if lead_visible else 0,
|
||||
"ACC_ObjStatus": 1 if lead_visible else 0,
|
||||
"ACC_ObjLatPos": 0,
|
||||
"ACC_ObjRelSpd": 0,
|
||||
"ACC_ObjDist": 0,
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
||||
|
||||
scc12_values = {
|
||||
"ACCMode": 1 if enabled else 0,
|
||||
"StopReq": 1 if stopping else 0,
|
||||
"aReqRaw": accel,
|
||||
"aReqValue": accel, # stock ramps up at 1.0/s and down at 0.5/s until it reaches aReqRaw
|
||||
"StopReq": 1 if enabled and stopping else 0,
|
||||
"aReqRaw": accel if enabled else 0,
|
||||
"aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
|
||||
"CR_VSM_Alive": idx % 0xF,
|
||||
}
|
||||
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
|
||||
|
|
@ -104,10 +108,10 @@ def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, st
|
|||
scc14_values = {
|
||||
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"JerkUpperLimit": 1.0 if enabled else 0, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": 0.5 if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 3 if lead_visible else 0, # TODO: 1-5 based on distance to lead vehicle
|
||||
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
|
||||
|
||||
|
|
|
|||
|
|
@ -1,11 +1,21 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from common.params import Params
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR
|
||||
from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from selfdrive.car.disable_ecu import disable_ecu
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
|
|
@ -14,14 +24,28 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.safetyModel = car.CarParams.SafetyModel.hyundai
|
||||
ret.radarOffCan = True
|
||||
|
||||
ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and candidate in [CAR.SONATA, CAR.PALISADE]
|
||||
ret.safetyParam = 0
|
||||
|
||||
# Most Hyundai car ports are community features for now
|
||||
ret.communityFeature = candidate not in [CAR.SONATA, CAR.PALISADE]
|
||||
ret.communityFeature = candidate not in [CAR.SONATA, CAR.PALISADE] or ret.openpilotLongitudinalControl
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay
|
||||
ret.steerRateCost = 0.5
|
||||
ret.steerLimitTimer = 0.4
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
ret.stoppingControl = True
|
||||
ret.vEgoStopping = 1.0
|
||||
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.stoppingDecelRate = 2.0
|
||||
ret.startAccel = 0.0
|
||||
|
||||
ret.longitudinalActuatorDelay = 1.0 # s
|
||||
|
||||
if candidate == CAR.SANTA_FE:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
|
|
@ -239,8 +263,16 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
ret.enableBsm = 0x58b in fingerprint[0]
|
||||
|
||||
if ret.openpilotLongitudinalControl:
|
||||
ret.safetyParam |= Panda.FLAG_HYUNDAI_LONG
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.openpilotLongitudinalControl:
|
||||
disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01')
|
||||
|
||||
def update(self, c, can_strings):
|
||||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
|
@ -249,7 +281,46 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
events = self.create_common_events(ret)
|
||||
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise)
|
||||
|
||||
if self.CS.brake_error:
|
||||
events.add(EventName.brakeUnavailable)
|
||||
if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.brakeHold)
|
||||
if self.CS.park_brake:
|
||||
events.add(EventName.parkBrake)
|
||||
|
||||
if self.CS.CP.openpilotLongitudinalControl:
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
if self.CS.cruise_buttons != 0:
|
||||
be.pressed = True
|
||||
but = self.CS.cruise_buttons
|
||||
else:
|
||||
be.pressed = False
|
||||
but = self.CS.prev_cruise_buttons
|
||||
if but == Buttons.RES_ACCEL:
|
||||
be.type = ButtonType.accelCruise
|
||||
elif but == Buttons.SET_DECEL:
|
||||
be.type = ButtonType.decelCruise
|
||||
elif but == Buttons.GAP_DIST:
|
||||
be.type = ButtonType.gapAdjustCruise
|
||||
elif but == Buttons.CANCEL:
|
||||
be.type = ButtonType.cancel
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
|
||||
for b in ret.buttonEvents:
|
||||
# do enable on both accel and decel buttons
|
||||
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
|
||||
events.add(EventName.buttonEnable)
|
||||
# do disable on button down
|
||||
if b.type == ButtonType.cancel and b.pressed:
|
||||
events.add(EventName.buttonCancel)
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
|
|
@ -266,7 +337,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@ Ecu = car.CarParams.Ecu
|
|||
|
||||
# Steer torque limits
|
||||
class CarControllerParams:
|
||||
ACCEL_MIN = -3.5 # m/s
|
||||
ACCEL_MAX = 2.0 # m/s
|
||||
|
||||
def __init__(self, CP):
|
||||
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70,
|
||||
CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
|
||||
|
|
@ -19,7 +22,6 @@ class CarControllerParams:
|
|||
self.STEER_DRIVER_MULTIPLIER = 2
|
||||
self.STEER_DRIVER_FACTOR = 1
|
||||
|
||||
|
||||
class CAR:
|
||||
# Hyundai
|
||||
ELANTRA = "HYUNDAI ELANTRA 2017"
|
||||
|
|
|
|||
|
|
@ -333,7 +333,8 @@ class Controls:
|
|||
|
||||
all_valid = CS.canValid and self.sm.all_alive_and_valid()
|
||||
if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION):
|
||||
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
|
||||
if not self.read_only:
|
||||
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
|
||||
self.initialized = True
|
||||
Params().put_bool("ControlsReady", True)
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ EXT_DIAG_RESPONSE = b'\x50\x03'
|
|||
COM_CONT_REQUEST = b'\x28\x83\x03'
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
def disable_ecu(ecu_addr, logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
|
||||
def disable_ecu(ecu_addr, logcan, sendcan, bus, timeout=0.5, retry=5, debug=False):
|
||||
print(f"ecu disable {hex(ecu_addr)} ...")
|
||||
for i in range(retry):
|
||||
try:
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
d8ba159971afb46d9b936517d2911bb9d5cd3377
|
||||
68db6ee0d2afb80b0d271788b3028de6db2da45e
|
||||
Loading…
Reference in New Issue