mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-07 01:05:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

ev9

Browse Source
This commit is contained in:
firestar5683
2026-06-28 00:26:27 -05:00
parent 460b61ebe8
commit be811cbe61
3 changed files with 55 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+6 -27
View File
@@ -4,7 +4,7 @@ import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.hyundai import hyundaicanfd, hyundaican
from opendbc.car.hyundai.hyundaicanfd import CanBus
@@ -66,7 +66,6 @@ REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40]
ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026)
DEFAULT_ANGLE_SMOOTHING_VEGO_BP = [5.0, 10.0, 20.0]
DEFAULT_ANGLE_SMOOTHING_ALPHA_V = [0.2, 0.1, 0.0]
EV9_HIGH_ANGLE_CONTROL_LIMIT = MAX_ANGLE
def egmp_dynamic_longitudinal_tuning(CP) -> bool:
@@ -227,16 +226,6 @@ def get_angle_smoothing_alpha(CP, v_ego: float) -> float:
return float(np.interp(v_ego, DEFAULT_ANGLE_SMOOTHING_VEGO_BP, DEFAULT_ANGLE_SMOOTHING_ALPHA_V))
def apply_steer_angle_limits_vm_checked(apply_angle: float, apply_angle_last: float, v_ego_raw: float,
steering_angle: float, lat_active: bool, limits, VM: VehicleModel) -> float | None:
new_apply_angle = apply_steer_angle_limits_vm(apply_angle, apply_angle_last, v_ego_raw, steering_angle, lat_active, limits, VM)
v_ego_raw = max(v_ego_raw, 1.0)
max_angle_delta = min(get_max_angle_delta_vm(v_ego_raw, VM, limits), limits.ANGLE_LIMITS.MAX_ANGLE_RATE)
safety_violation = lat_active and not np.isclose(new_apply_angle,
rate_limit(new_apply_angle, apply_angle_last, -max_angle_delta, max_angle_delta))
return None if safety_violation else new_apply_angle
def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain):
if lat_active:
ceiling = np.interp(v_ego, [0.5, 1.5], [1.0, 0.85])
@@ -395,7 +384,6 @@ class CarController(CarControllerBase):
if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
v_ego_raw = CS.out.vEgoRaw
ev9_high_angle_inhibit = self.CP.carFingerprint == CAR.KIA_EV9 and abs(CS.out.steeringAngleDeg) >= EV9_HIGH_ANGLE_CONTROL_LIMIT
desired_angle = float(np.clip(actuators.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
@@ -403,13 +391,12 @@ class CarController(CarControllerBase):
self.angle_filter.update_alpha(get_angle_smoothing_alpha(self.CP, CS.out.vEgo))
desired_angle = self.angle_filter.update(desired_angle)
angle_limit_fn = apply_steer_angle_limits_vm_checked if self.CP.carFingerprint == CAR.KIA_EV9 else apply_steer_angle_limits_vm
apply_angle = angle_limit_fn(desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL:
apply_angle = angle_limit_fn(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, CC.latActive, self.apply_torque_last)
apply_steer_req = CC.latActive and apply_torque != 0.0
@@ -420,14 +407,6 @@ class CarController(CarControllerBase):
apply_angle = CS.out.steeringAngleDeg
apply_steer_req = False
if ev9_high_angle_inhibit:
apply_torque = 0
apply_angle = float(np.clip(CS.out.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
apply_steer_req = False
self.angle_filter.x = apply_angle
self.apply_angle_last = apply_angle
if not CC.latActive:
self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg,
@@ -627,7 +606,7 @@ class CarController(CarControllerBase):
steering_msg_active = apply_steer_req
if self.CP.carFingerprint == CAR.KIA_EV9 and self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
# EV9 faults if the angle-steering status drops inactive during torque limiting.
# Hold the angle status active while lateral is active; high-angle inhibit only zeros gain.
# Hold the angle status active while lateral is active; VM/safety limits handle actuation.
steering_msg_active = CC.latActive
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled,
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+14
View File
@@ -154,6 +154,20 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
"TORQUE_REQUEST": 0,
"STEER_REQ": 0,
})
else:
lkas_values.update({
"LKA_MODE": 0,
"LKA_AVAILABLE": 0,
"LKA_WARNING": 0,
"LKA_ICON": lka_icon,
"FCA_SYSWARN": 0,
"TORQUE_REQUEST": 0,
"STEER_REQ": 0,
"LFA_BUTTON": 0,
"LKA_ASSIST": 0,
"DAMP_FACTOR": 100,
"HAS_LANE_SAFETY": 0,
})
# These signals overlap DAMP_FACTOR in the local DBC naming; omitting them
# preserves the stock angle-steering damping byte expected by the ADAS ECU.
lkas_values.pop("STEER_MODE", None)
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+35 -13
View File
@@ -11,7 +11,7 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT
update_ioniq_6_longitudinal_tuning, \
update_genesis_g90_longitudinal_tuning, egmp_dynamic_longitudinal_tuning, \
should_reset_ev6_gt_line_longitudinal_tuning, reset_ev6_gt_line_longitudinal_tuning, \
get_angle_smoothing_alpha, apply_steer_angle_limits_vm_checked
get_angle_smoothing_alpha
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai import hyundaican, hyundaicanfd
@@ -21,9 +21,8 @@ from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR3
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
LEGACY_LONGITUDINAL_CAR, CarControllerParams, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
HyundaiStarPilotSafetyFlags, Buttons, kia_ev6_gt_line_longitudinal_tuning
from opendbc.car.vehicle_model import VehicleModel
LongCtrlState = CarControl.Actuators.LongControlState
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
@@ -288,13 +287,6 @@ class TestHyundaiFingerprint:
assert get_angle_smoothing_alpha(ev9_cp, 20.0) == pytest.approx(get_angle_smoothing_alpha(other_cp, 20.0))
assert get_angle_smoothing_alpha(other_cp, 20.0) == pytest.approx(0.0)
def test_checked_angle_limiter_blocks_rate_accel_conflict(self):
CP = CarInterface.get_non_essential_params(CAR.KIA_EV9)
params = CarControllerParams(CP)
assert apply_steer_angle_limits_vm_checked(0.0, 100.0, 20.0, 100.0, True,
params, VehicleModel(CP)) is None
def test_ccnc_hda2_lka_layout_does_not_set_ccnc_safety_param(self):
fingerprint = gen_empty_fingerprint()
cam_can = CanBus(None, fingerprint).CAM
@@ -1405,7 +1397,37 @@ class TestHyundaiFingerprint:
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(8.5)
def test_ev9_high_steering_angle_keeps_active_status_with_zero_gain(self):
def test_ev9_inactive_angle_status_keeps_standby_damping_without_stock_lkas(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV9
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING |
HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
CP.openpilotLongitudinalControl = False
controller = CarController(DBC[CP.carFingerprint], CP)
can_bus = CanBus(CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
cc = SimpleNamespace(enabled=False, latActive=False, actuators=SimpleNamespace(longControlState=LongCtrlState.off),
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg={},
out=SimpleNamespace(steeringAngleDeg=-201.0))
msgs = controller.create_canfd_msgs(0, False, 0.0, -201.0, 0.0, 0.0, False, cc.hudControl, cs, cc,
get_test_toggles(), lka_icon=1, lfa_icon=1)
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
assert len(lkas_msgs) == 1
parser.update([(1, lkas_msgs)])
assert parser.can_valid
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(-201.0)
assert parser.vl["LKAS_ALT"]["DAMP_FACTOR"] == pytest.approx(100.0)
assert parser.vl["LKAS_ALT"]["STEER_MODE"] == 2
assert parser.vl["LKAS_ALT"]["NEW_SIGNAL_2"] == 3
def test_ev9_high_steering_angle_keeps_active_status_and_gain(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV9
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING |
@@ -1443,7 +1465,7 @@ class TestHyundaiFingerprint:
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas, lfa_block_msg=lfa_block_msg,
out=SimpleNamespace(steeringAngleDeg=120.0))
msgs = controller.create_canfd_msgs(0, False, 0.0, 120.0, 0.0, 0.0, False, cc.hudControl, cs, cc,
msgs = controller.create_canfd_msgs(0, True, 0.44, 120.0, 0.0, 0.0, False, cc.hudControl, cs, cc,
get_test_toggles(), lka_icon=2, lfa_icon=2)
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
suppress_msgs = [msg for msg in msgs if msg[0] == 0x362]
@@ -1454,7 +1476,7 @@ class TestHyundaiFingerprint:
assert parser.can_valid
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 2
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0)
assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.44)
assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(120.0)
def test_can_acc_commands_use_default_values(self):