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

The Well-Tempered Clavier

Browse Source
This commit is contained in:
firestar5683
2026-05-07 13:49:19 -05:00
parent c1804b1fd8
commit 5f3d71798d
7 changed files with 79 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+8 -14
View File
@@ -45,10 +45,10 @@ IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -
IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5]
IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0]
IONIQ_6_STOP_HOLD_SPEED_BP = [0.0, 0.08, 0.25, 0.6, 1.2]
IONIQ_6_STOP_HOLD_SPEED_V = [-0.09, -0.095, -0.10, -0.05, 0.0]
IONIQ_6_STOP_HOLD_JERK_BP = [0.0, 0.15, 0.6, 1.2]
IONIQ_6_STOP_HOLD_JERK_V = [0.35, 0.40, 0.52, 0.65]
IONIQ_6_STOP_BRAKE_CAP_SPEED_BP = [0.0, 0.08, 0.25, 0.6, 1.2, 2.0, 3.0]
IONIQ_6_STOP_BRAKE_CAP_ACCEL_V = [-0.09, -0.095, -0.10, -0.18, -0.40, -0.80, -1.30]
IONIQ_6_STOP_HOLD_JERK_BP = [0.0, 0.15, 0.6, 1.2, 2.0, 3.0]
IONIQ_6_STOP_HOLD_JERK_V = [0.35, 0.40, 0.48, 0.65, 0.85, 1.10]
IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5]
IONIQ_6_STOP_RELEASE_JERK_V = [3.6 * IONIQ_6_RESPONSE_MULTIPLIER,
4.2 * IONIQ_6_RESPONSE_MULTIPLIER,
@@ -104,15 +104,8 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
restart_from_stop = state.long_control_state_last in (LongCtrlState.stopping, LongCtrlState.starting) and \
long_control_state in (LongCtrlState.starting, LongCtrlState.pid) and accel_cmd > 0.0 and v_ego < 0.5
if not long_active or not stopping:
state.stopping = False
state.stopping_count = 0
elif state.long_control_state_last == LongCtrlState.off:
state.stopping = True
else:
if state.stopping_count > 1 / (DT_CTRL * 5):
state.stopping = True
state.stopping_count += 1
state.stopping = long_active and stopping
state.stopping_count = state.stopping_count + 1 if state.stopping else 0
if not long_active:
state.desired_accel = 0.0
@@ -147,7 +140,8 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit)
if state.stopping:
state.desired_accel = float(np.interp(v_ego, IONIQ_6_STOP_HOLD_SPEED_BP, IONIQ_6_STOP_HOLD_SPEED_V))
stop_brake_cap = float(np.interp(v_ego, IONIQ_6_STOP_BRAKE_CAP_SPEED_BP, IONIQ_6_STOP_BRAKE_CAP_ACCEL_V))
state.desired_accel = min(0.0, max(accel_cmd, stop_brake_cap))
state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_HOLD_JERK_BP, IONIQ_6_STOP_HOLD_JERK_V)) * IONIQ_6_RESPONSE_MULTIPLIER)
else:
state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+10 -5
View File
@@ -2,7 +2,7 @@ import copy
import numpy as np
from opendbc.car import CanBusBase, CanData
from opendbc.car.crc import CRC16_XMODEM
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.car.hyundai.values import CAR, HyundaiFlags
def _set_value(msg: bytearray, sig, ival: int) -> None:
@@ -134,10 +134,15 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
else:
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
if CP.flags & HyundaiFlags.SEND_LFA:
# Some CAN-FD angle-steering trims still expect the stock-style LFA status/UI
# message to remain present even though angle actuation comes through ADAS_CMD.
ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
ret.append(_create_angle_adas_cmd_msg(packer, CAN, apply_angle, lat_active, apply_torque))
# Sportage HEV 2026 tracks more reliably and stays fault-free on the stock-style
# LFA angle request path. Other SEND_LFA angle cars still use ADAS_CMD.
if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
ret.append(_create_angle_lfa_msg(packer, CAN, lfa_values, apply_angle, lat_active, apply_torque))
else:
# Some CAN-FD angle-steering trims still expect the stock-style LFA status/UI
# message to remain present even though angle actuation comes through ADAS_CMD.
ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
ret.append(_create_angle_adas_cmd_msg(packer, CAN, apply_angle, lat_active, apply_torque))
else:
ret.append(_create_angle_lfa_msg(packer, CAN, lfa_values, apply_angle, lat_active, apply_torque))
else:
opendbc_repo/opendbc/car/hyundai/interface.py
+10
View File
@@ -200,6 +200,16 @@ class CarInterface(CarInterfaceBase):
ret.stoppingDecelRate = 0.55
ret.vEgoStopping = 0.8
if candidate == CAR.HYUNDAI_PALISADE_2023:
ret.stopAccel = -1.5
ret.stoppingDecelRate = 0.55
ret.vEgoStopping = 0.7
if candidate == CAR.KIA_NIRO_PHEV_2022:
ret.stopAccel = -1.5
ret.stoppingDecelRate = 0.55
ret.vEgoStopping = 0.7
if candidate == CAR.KIA_OPTIMA_G4_FL:
ret.steerActuatorDelay = 0.2
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+31 -8
View File
@@ -171,6 +171,22 @@ class TestHyundaiFingerprint:
assert CP.vEgoStopping == pytest.approx(0.8)
assert CP.stoppingDecelRate == pytest.approx(0.55)
def test_palisade_2023_longitudinal_params_soften_final_stop_hold(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.stopAccel == pytest.approx(-1.5)
assert CP.vEgoStopping == pytest.approx(0.7)
assert CP.stoppingDecelRate == pytest.approx(0.55)
def test_kia_niro_phev_2022_longitudinal_params_soften_final_stop_hold(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(CAR.KIA_NIRO_PHEV_2022, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.stopAccel == pytest.approx(-1.5)
assert CP.vEgoStopping == pytest.approx(0.7)
assert CP.stoppingDecelRate == pytest.approx(0.55)
def test_kia_forte_no_scc_fw_match(self):
car_fw = [
CarParams.CarFw(
@@ -573,12 +589,6 @@ class TestHyundaiFingerprint:
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
long_control_state=LongCtrlState.stopping, long_active=True)
assert not state.stopping
assert state.desired_accel == pytest.approx(1.0)
for _ in range(25):
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
long_control_state=LongCtrlState.stopping, long_active=True)
assert state.stopping
assert state.desired_accel == pytest.approx(0.0)
actual_accel_after_stop = state.actual_accel
@@ -634,6 +644,20 @@ class TestHyundaiFingerprint:
assert state.jerk_upper == pytest.approx(0.42)
assert state.actual_accel == pytest.approx(-0.099)
def test_ioniq_6_longitudinal_tuning_helper_caps_late_low_speed_stop_brake(self):
state = Ioniq6LongitudinalTuningState(actual_accel=-2.82, accel_last=-2.82,
long_control_state_last=LongCtrlState.pid)
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4,
long_control_state=LongCtrlState.stopping, long_active=True)
assert state.stopping
assert state.desired_accel == pytest.approx(-1.05)
for _ in range(10):
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=-2.82, v_ego=2.5, a_ego=-2.4,
long_control_state=LongCtrlState.stopping, long_active=True)
assert state.actual_accel == pytest.approx(-2.49)
def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self):
state = GenesisG90LongitudinalTuningState()
@@ -787,7 +811,7 @@ class TestHyundaiFingerprint:
assert parser.vl["FCA12"]["FCA_DrvSetState"] == 2
assert parser.vl["FCA12"]["FCA_USM"] == 2
def test_sportage_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
def test_sportage_angle_steering_uses_lfa_only_with_send_lfa(self):
fingerprint = gen_empty_fingerprint()
cam_can = CanBus(None, fingerprint).CAM
fingerprint[cam_can][0xCB] = 24
@@ -801,7 +825,6 @@ class TestHyundaiFingerprint:
msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, True, True, 1.0, 12.3)
assert [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [
("LFA", can_bus.ECAN),
("ADAS_CMD_35_10ms", can_bus.ECAN),
]
def test_ioniq_6_lfa_helper_preserves_stock_ui_fields(self):
opendbc_repo/opendbc/car/hyundai/values.py
+5 -5
View File
@@ -11,12 +11,12 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = CarParams.Ecu
AVERAGE_ROAD_ROLL = 0.06 # conservative roll margin used by Hyundai CAN-FD angle steering safety
SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.25
SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.6
SPORTAGE_HEV_2026_BASE_LATERAL_JERK = 3.25
SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.75
SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 12.0
SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 6.0
SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.0
SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.55
SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 11.0
SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 5.0
SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.5
SPORTAGE_HEV_2026_STEER_ANGLE_MAX = 220.0
selfdrive/controls/lib/latcontrol_torque.py
+13 -13
View File
@@ -76,16 +76,16 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.17
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.25
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.10
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.11
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.82
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 1.08
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.05
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.055
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.60
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.94
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.19
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.28
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.12
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.12
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.92
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 1.18
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.06
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.72
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 1.04
BOLT_2022_2023_CARS = (
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
@@ -244,7 +244,7 @@ GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22
IONIQ_6_FF_GAIN_LEFT = 0.045
IONIQ_6_FF_GAIN_RIGHT = 0.015
IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.23
IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
IONIQ_6_BASE_FRICTION_THRESHOLD = 0.36
IONIQ_6_FF_ONSET = 0.10
IONIQ_6_FF_ONSET_WIDTH = 0.04
@@ -291,8 +291,8 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.13
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.22
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.56
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.94
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
selfdrive/controls/tests/test_latcontrol.py
+2 -2
View File
@@ -286,7 +286,7 @@ class TestLatControl:
assert get_ioniq_6_center_taper_scale(0.0, 10.0) < get_ioniq_6_center_taper_scale(0.0, 30.0)
assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.2, 30.0)
assert get_ioniq_6_center_taper_scale(0.0, 12.0) < get_ioniq_6_center_taper_scale(0.25, 12.0)
assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 4.2e-2
assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 4.5e-2
def test_kia_ev6_ff_scale_curve(self):
assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0
@@ -372,7 +372,7 @@ class TestLatControl:
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
assert lac_log.active
assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.23)
assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.22)
def test_ioniq_6_update_path_does_not_post_taper_output(self, monkeypatch):
base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6)