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

Tone

Browse Source
This commit is contained in:
firestar5683
2026-07-21 15:20:28 -05:00
parent 4e70755895
commit 80f465616a
6 changed files with 82 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+39 -12
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
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm, get_max_angle_vm
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.hyundai import hyundaicanfd, hyundaican
from opendbc.car.hyundai.hyundaicanfd import CanBus
@@ -343,6 +343,12 @@ 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 direct_angle_request_allowed(v_ego_raw, measured_angle, last_angle, drive_gear, VM, params):
safety_v_ego = max(v_ego_raw - 1.0, 1.0)
max_safety_angle = get_max_angle_vm(safety_v_ego, VM, params)
return drive_gear and abs(measured_angle) <= max_safety_angle and abs(last_angle) <= max_safety_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])
@@ -394,6 +400,7 @@ class CarController(CarControllerBase):
self.VM = VehicleModel(CP)
self.BASELINE_VM = VehicleModel(get_baseline_safety_cp()) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else self.VM
self.angle_filter = FirstOrderFilter(0.0, 0.2, DT_CTRL)
self.direct_angle_request_allowed = True
self.accel_last = 0
self.apply_torque_last = 0
@@ -501,7 +508,15 @@ class CarController(CarControllerBase):
if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
apply_angle = CS.out.steeringAngleDeg
direct_angle_control = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and self.long_active_ecu
measured_steering_angle = CS.angle_steering_angle if direct_angle_control else CS.out.steeringAngleDeg
angle_lat_active = CC.latActive
if direct_angle_control and CC.latActive:
drive_gear = CS.out.gearShifter == structs.CarState.GearShifter.drive
angle_lat_active = direct_angle_request_allowed(CS.out.vEgoRaw, measured_steering_angle, self.apply_angle_last,
drive_gear, self.BASELINE_VM, self.params) and not CS.angle_steering_fault
self.direct_angle_request_allowed = angle_lat_active
apply_angle = measured_steering_angle
if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
v_ego_raw = CS.out.vEgoRaw
@@ -512,24 +527,33 @@ class CarController(CarControllerBase):
desired_angle = self.angle_filter.update(desired_angle)
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)
measured_steering_angle, angle_lat_active, self.params, self.VM)
if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL:
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)
measured_steering_angle, angle_lat_active, 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
if direct_angle_control and angle_lat_active:
# Match Panda's 1 m/s speed tolerance so a shrinking absolute limit stays inside its jerk envelope.
safety_v_ego = max(v_ego_raw - 1.0, 1.0)
max_angle_delta = min(get_max_angle_delta_vm(safety_v_ego, self.BASELINE_VM, self.params),
self.params.ANGLE_LIMITS.MAX_ANGLE_RATE)
apply_angle = float(np.clip(apply_angle,
self.apply_angle_last - max_angle_delta,
self.apply_angle_last + max_angle_delta))
apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, angle_lat_active, self.apply_torque_last)
apply_steer_req = angle_lat_active and apply_torque != 0.0
torque_fault = False
if apply_angle is None:
apply_torque = 0
apply_angle = CS.out.steeringAngleDeg
apply_angle = measured_steering_angle
apply_steer_req = False
self.apply_angle_last = apply_angle
if not CC.latActive:
self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg,
if not angle_lat_active:
self.apply_angle_last = float(np.clip(measured_steering_angle,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
self.angle_filter.x = self.apply_angle_last
@@ -756,16 +780,19 @@ class CarController(CarControllerBase):
CS.stock_lfa_msg,
CS.stock_lkas_msg if preserve_stock_lkas else None,
lka_icon=lka_icon))
direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and not CS.angle_steering_fault
direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault
inactive_steering_angle = float(np.clip(CS.angle_steering_angle,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) if ccnc_angle_long else 0.0
if ccnc_angle_long and drive_gear:
can_sends.append(hyundaicanfd.create_angle_adas_cmd(
self.packer, self.CAN,
apply_angle if direct_steering_active else CS.angle_steering_angle,
apply_angle if direct_steering_active else inactive_steering_angle,
direct_steering_active, apply_torque if direct_steering_active else 0.0,
))
if ccnc_angle_long and not drive_gear:
can_sends.extend(hyundaicanfd.create_inactive_angle_steering_messages(self.packer, self.CAN,
CS.angle_steering_angle))
inactive_steering_angle))
# prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU
suppress_lfa = bool(lka_steering)
opendbc_repo/opendbc/car/hyundai/carstate.py
+1 -1
View File
@@ -461,7 +461,7 @@ class CarState(CarStateBase):
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
if self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR:
self.angle_steering_angle = cp.vl["MDPS"]["STEERING_ANGLE"]
self.angle_steering_angle = cp.vl["MDPS"]["STEERING_ANGLE_2"]
self.angle_steering_fault = cp.vl["MDPS"]["LKA_ANGLE_FAULT"] != 0
ret.steerFaultTemporary = ret.steerFaultTemporary or self.angle_steering_fault
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+10 -1
View File
@@ -14,7 +14,8 @@ 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, should_use_ev6_gt_line_stop_direct_tracking
direct_angle_request_allowed, get_angle_smoothing_alpha, \
should_use_ev6_gt_line_stop_direct_tracking
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
@@ -299,6 +300,14 @@ 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_ev9_direct_angle_waits_for_safety_envelope(self):
CP = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], True, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
assert not direct_angle_request_allowed(8.47, 155.5, 155.6, True, controller.BASELINE_VM, controller.params)
assert direct_angle_request_allowed(8.47, 140.0, 140.0, True, controller.BASELINE_VM, controller.params)
assert not direct_angle_request_allowed(8.47, 140.0, 140.0, False, controller.BASELINE_VM, controller.params)
def test_ev9_matches_other_angle_platform_standstill_behavior(self):
ev9_cp = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], False, False, False, None)
sportage_cp = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None)
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+3 -1
View File
@@ -125,7 +125,9 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
torque_driver_new -= 4095;
update_sample(&torque_driver, torque_driver_new);
int angle_meas_new = (msg->data[13] << 8U) | msg->data[12];
// CCNC angle-long platforms publish the usable angle in STEERING_ANGLE_2.
const unsigned int angle_offset = hyundai_canfd_ccnc_angle_long ? 16U : 12U;
int angle_meas_new = (msg->data[angle_offset + 1U] << 8U) | msg->data[angle_offset];
angle_meas_new = to_signed(angle_meas_new, 16);
update_sample(&angle_meas, angle_meas_new);
}
opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py
+14 -4
View File
@@ -23,9 +23,12 @@ def is_steering_msg(mode, param, addr):
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
ret = addr == 832
elif mode == CarParams.SafetyModel.hyundaiCanfd:
ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else
0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING else
0x12A)
if param & HyundaiSafetyFlags.CCNC and param & HyundaiSafetyFlags.LONG and param & HyundaiSafetyFlags.CANFD_ANGLE_STEERING:
ret = addr == 0xCB
else:
ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else
0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING else
0x12A)
elif mode == CarParams.SafetyModel.chrysler:
ret = addr == 0x292
elif mode == CarParams.SafetyModel.subaru:
@@ -62,7 +65,14 @@ def get_steer_value(mode, param, msg):
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
torque = (((msg.data[3] & 0x7) << 8) | msg.data[2]) - 1024
elif mode == CarParams.SafetyModel.hyundaiCanfd:
torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024
if param & HyundaiSafetyFlags.CANFD_ANGLE_STEERING:
if param & HyundaiSafetyFlags.CCNC and param & HyundaiSafetyFlags.LONG:
angle = ((msg.data[5] & 0x3F) << 8) | msg.data[4]
else:
angle = (msg.data[11] << 6) | (msg.data[10] >> 2)
angle = to_signed(angle, 14)
else:
torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024
elif mode == CarParams.SafetyModel.chrysler:
torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024
elif mode == CarParams.SafetyModel.subaru:
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+15
View File
@@ -752,6 +752,21 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu
with self.subTest(address=address):
self.assertFalse(self._tx(common.make_msg(1 if address != 0x51 else 0, address, length)))
def test_ccnc_angle_long_uses_second_mdps_angle(self):
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM | HyundaiSafetyFlags.CCNC)
self.safety.init_tests()
angle = -38.5
for _ in range(common.MAX_SAMPLE_VALS):
self._rx(self.packer.make_can_msg_safety("MDPS", self.PT_BUS, {
"STEERING_ANGLE": 0.0,
"STEERING_ANGLE_2": angle,
}))
expected = round(angle * self.DEG_TO_CAN)
self.assertEqual(self.safety.get_angle_meas_min(), expected)
self.assertEqual(self.safety.get_angle_meas_max(), expected)
def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
values = {
"aReqRaw": accel,