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

updatz

Browse Source
This commit is contained in:
firestar5683
2026-04-29 22:51:37 -05:00
parent 86db8e39c6
commit b3a2f141d6
16 changed files with 114 additions and 217 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+1 -107
View File
@@ -23,12 +23,6 @@ MAX_ANGLE_CONSECUTIVE_FRAMES = 2
CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000
HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0
HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0
HYUNDAI_LONG_MIN_JERK = 0.5
HYUNDAI_LONG_JERK_LIMIT = 4.0
HYUNDAI_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
HYUNDAI_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6]
HYUNDAI_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
HYUNDAI_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
IONIQ_6_LONG_MIN_JERK = 0.5
IONIQ_6_LONG_JERK_LIMIT = 4.0
IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
@@ -49,32 +43,11 @@ class Ioniq6LongitudinalTuningState:
long_control_state_last: LongCtrlState = LongCtrlState.off
@dataclass
class HyundaiLongitudinalTuningState:
desired_accel: float = 0.0
actual_accel: float = 0.0
accel_last: float = 0.0
jerk_upper: float = 0.0
jerk_lower: float = 0.0
comfort_band_upper: float = 0.0
comfort_band_lower: float = 0.0
stopping: bool = False
stopping_count: int = 0
long_control_state_last: LongCtrlState = LongCtrlState.off
def _jerk_limited_integrator(desired_accel: float, last_accel: float, jerk_upper: float, jerk_lower: float) -> float:
step = (jerk_upper if desired_accel >= last_accel else jerk_lower) * DT_CTRL * 5.0
return float(np.clip(desired_accel, last_accel - step, last_accel + step))
def _calculate_hyundai_dynamic_lower_jerk(accel_error: float) -> float:
if accel_error < 0.0:
scaled_values = np.array(HYUNDAI_DYNAMIC_LOWER_JERK_V) * (HYUNDAI_LONG_JERK_LIMIT / HYUNDAI_DYNAMIC_LOWER_JERK_V[0])
return float(np.interp(accel_error, HYUNDAI_DYNAMIC_LOWER_JERK_BP, scaled_values))
return HYUNDAI_LONG_MIN_JERK
def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float:
if accel_error < 0.0:
scaled_values = np.array(IONIQ_6_DYNAMIC_LOWER_JERK_V) * (IONIQ_6_LONG_JERK_LIMIT / IONIQ_6_DYNAMIC_LOWER_JERK_V[0])
@@ -132,65 +105,6 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
return state
def update_hyundai_longitudinal_tuning(state: HyundaiLongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float,
long_control_state: LongCtrlState, long_active: bool, radar_unavailable: bool) -> HyundaiLongitudinalTuningState:
stopping = long_control_state == LongCtrlState.stopping
if not long_active:
state.desired_accel = 0.0
state.actual_accel = 0.0
state.accel_last = 0.0
state.jerk_upper = 0.0
state.jerk_lower = 0.0
state.comfort_band_upper = 0.0
state.comfort_band_lower = 0.0
state.stopping = False
state.stopping_count = 0
state.long_control_state_last = long_control_state
return state
if 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
upper_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0])) if long_control_state == LongCtrlState.pid else HYUNDAI_LONG_MIN_JERK
lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
future_t_upper = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V))
future_t_lower = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V))
accel_error = accel_cmd - state.accel_last
j_ego_upper = float(np.clip(accel_error / future_t_upper, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT))
j_ego_lower = float(np.clip(accel_error / future_t_lower, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT))
desired_jerk_upper = min(max(j_ego_upper, HYUNDAI_LONG_MIN_JERK), upper_speed_limit)
dynamic_lower_jerk = _calculate_hyundai_dynamic_lower_jerk(a_ego - state.accel_last)
state.jerk_upper = desired_jerk_upper
state.jerk_lower = 5.0 if radar_unavailable else min(dynamic_lower_jerk, lower_speed_limit)
if state.stopping:
state.desired_accel = 0.0
else:
state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower)
state.accel_last = state.actual_accel
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
state.comfort_band_upper = float(np.interp(a_ego, accel_vals, comfort_band_vals))
state.comfort_band_lower = float(np.interp(a_ego, decel_vals, comfort_band_vals[::-1]))
state.long_control_state_last = long_control_state
return state
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -235,7 +149,6 @@ class CarController(CarControllerBase):
self.long_active_ecu = self.CP.openpilotLongitudinalControl
self._ioniq_6_lane_change_ui_side = None
self._ioniq_6_lane_change_ui_trigger_frames = 0
self._hyundai_long_tuning = HyundaiLongitudinalTuningState()
self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState()
def update(self, CC, CS, now_nanos, starpilot_toggles):
@@ -298,18 +211,6 @@ class CarController(CarControllerBase):
# longitudinal messages - stock ECU is still active and these would conflict
self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
use_classic_hyundai_long_tuning = not (self.CP.flags & HyundaiFlags.CANFD) and self.long_active_ecu
if use_classic_hyundai_long_tuning and self.frame % 5 == 0:
self._hyundai_long_tuning = update_hyundai_longitudinal_tuning(self._hyundai_long_tuning, accel_cmd,
CS.out.vEgo, CS.out.aEgo,
actuators.longControlState, CC.longActive,
self.CP.radarUnavailable)
accel = self._hyundai_long_tuning.actual_accel
stopping = self._hyundai_long_tuning.stopping
elif use_classic_hyundai_long_tuning:
accel = self._hyundai_long_tuning.actual_accel
stopping = self._hyundai_long_tuning.stopping
use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \
actuators.longControlState == LongCtrlState.pid
if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0:
@@ -398,14 +299,7 @@ class CarController(CarControllerBase):
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, self.CP,
main_mode_acc=int(CS.out.cruiseState.available),
desired_accel=self._hyundai_long_tuning.desired_accel,
actual_accel=self._hyundai_long_tuning.actual_accel,
jerk_lower=self._hyundai_long_tuning.jerk_lower,
comfort_band_upper=self._hyundai_long_tuning.comfort_band_upper,
comfort_band_lower=self._hyundai_long_tuning.comfort_band_lower,
stop_req=self._hyundai_long_tuning.stopping))
CC.cruiseControl.override, use_fca, self.CP))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
opendbc_repo/opendbc/car/hyundai/hyundaican.py
+11 -13
View File
@@ -125,29 +125,27 @@ def create_lfahda_mfc(packer, enabled):
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP,
main_mode_acc=1, desired_accel=None, actual_accel=None, jerk_lower=5.0,
comfort_band_upper=0.0, comfort_band_lower=0.0, stop_req=None):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP):
commands = []
scc11_values = {
"MainMode_ACC": main_mode_acc,
"MainMode_ACC": 1,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
"ObjValid": int(hud_control.leadVisible), # close lead makes controls tighter
"ACC_ObjStatus": int(hud_control.leadVisible), # close lead makes controls tighter
"ObjValid": 1, # close lead makes controls tighter
"ACC_ObjStatus": 1, # close lead makes controls tighter
"ACC_ObjLatPos": 0,
"ACC_ObjRelSpd": 0,
"ACC_ObjDist": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
"ACC_ObjDist": 1, # close lead makes controls tighter
}
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
scc12_values = {
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
"StopReq": 1 if (stopping if stop_req is None else stop_req) else 0,
"aReqRaw": accel if desired_accel is None else desired_accel,
"aReqValue": accel if actual_accel is None else actual_accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"StopReq": 1 if stopping else 0,
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
}
@@ -163,10 +161,10 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": comfort_band_upper, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": comfort_band_lower, # stock usually is 0 but sometimes uses higher 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": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": jerk_lower, # stock usually is 0.5 but sometimes uses higher values
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
}
opendbc_repo/opendbc/car/hyundai/interface.py
+6 -17
View File
@@ -23,26 +23,15 @@ ECU_DISABLE_TIMESTAMP = 0.0
def apply_platform_longitudinal_params(ret: structs.CarParams) -> None:
if not (ret.flags & HyundaiFlags.CANFD):
return
ret.startingState = True
ret.startAccel = 1.0
ret.longitudinalActuatorDelay = 0.5
if ret.flags & HyundaiFlags.CANFD:
ret.vEgoStopping = 0.3
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 0.4
elif ret.flags & HyundaiFlags.EV:
ret.vEgoStopping = 0.35
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 0.45
elif ret.flags & HyundaiFlags.HYBRID:
ret.vEgoStopping = 0.4
ret.vEgoStarting = 0.15
ret.stoppingDecelRate = 0.45
else:
ret.vEgoStopping = 0.3
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 0.4
ret.vEgoStopping = 0.3
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 0.4
class CarInterface(CarInterfaceBase):
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+15 -43
View File
@@ -7,8 +7,7 @@ from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
from opendbc.car.hyundai.carcontroller import HyundaiLongitudinalTuningState, Ioniq6LongitudinalTuningState, \
update_hyundai_longitudinal_tuning, update_ioniq_6_longitudinal_tuning
from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, update_ioniq_6_longitudinal_tuning
from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai import hyundaican, hyundaicanfd
@@ -97,19 +96,13 @@ class TestHyundaiFingerprint:
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
assert CP.flags & HyundaiFlags.SEND_LFA
@pytest.mark.parametrize(("candidate", "expected"), [
(CAR.KIA_EV6, (0.3, 0.1, 0.4)),
(CAR.HYUNDAI_KONA_EV, (0.35, 0.1, 0.45)),
(CAR.HYUNDAI_SONATA_HYBRID, (0.4, 0.15, 0.45)),
(CAR.GENESIS_G70, (0.3, 0.1, 0.4)),
])
def test_platform_longitudinal_params_match_family_tune(self, candidate, expected):
def test_canfd_longitudinal_params_match_family_tune(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, toggles)
CP = CarInterface.get_params(CAR.KIA_EV6, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.vEgoStopping == pytest.approx(expected[0])
assert CP.vEgoStarting == pytest.approx(expected[1])
assert CP.stoppingDecelRate == pytest.approx(expected[2])
assert CP.vEgoStopping == pytest.approx(0.3)
assert CP.vEgoStarting == pytest.approx(0.1)
assert CP.stoppingDecelRate == pytest.approx(0.4)
assert CP.longitudinalActuatorDelay == pytest.approx(0.5)
assert CP.startingState
@@ -282,24 +275,6 @@ class TestHyundaiFingerprint:
assert state.jerk_upper == pytest.approx(0.0)
assert state.jerk_lower == pytest.approx(0.0)
def test_hyundai_longitudinal_tuning_helper_softens_stopping(self):
state = HyundaiLongitudinalTuningState(accel_last=-3.5)
state = update_hyundai_longitudinal_tuning(state, accel_cmd=-3.5, v_ego=0.9, a_ego=-1.0,
long_control_state=LongCtrlState.stopping, long_active=True,
radar_unavailable=True)
assert state.stopping
assert state.desired_accel == pytest.approx(0.0)
assert state.actual_accel > -3.5
assert state.jerk_lower == pytest.approx(5.0)
state = update_hyundai_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.8, a_ego=0.1,
long_control_state=LongCtrlState.stopping, long_active=True,
radar_unavailable=True)
assert state.stopping
assert state.desired_accel == pytest.approx(0.0)
assert state.actual_accel <= 0.0
def test_canfd_acc_control_uses_direct_accel(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
@@ -321,7 +296,7 @@ class TestHyundaiFingerprint:
assert parser.vl["SCC_CONTROL"]["JerkLowerLimit"] == pytest.approx(5.0)
assert parser.vl["SCC_CONTROL"]["JerkUpperLimit"] == pytest.approx(1.0)
def test_can_acc_commands_use_tuned_values(self):
def test_can_acc_commands_use_default_values(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.GENESIS_G90
@@ -330,20 +305,17 @@ class TestHyundaiFingerprint:
msgs = hyundaican.create_acc_commands(packer, enabled=True, accel=-1.0, upper_jerk=2.5, idx=3,
hud_control=SimpleNamespace(leadDistanceBars=3, leadVisible=False), set_speed=42,
stopping=False, long_override=False, use_fca=False, CP=CP,
main_mode_acc=0, desired_accel=0.0, actual_accel=-0.35,
jerk_lower=4.0, comfort_band_upper=0.08, comfort_band_lower=0.02, stop_req=True)
stopping=False, long_override=False, use_fca=False, CP=CP)
parser.update([(1, msgs)])
assert parser.can_valid
assert parser.vl["SCC11"]["MainMode_ACC"] == 0
assert parser.vl["SCC11"]["ObjValid"] == 0
assert parser.vl["SCC12"]["StopReq"] == 1
assert parser.vl["SCC12"]["aReqRaw"] == pytest.approx(0.0)
assert parser.vl["SCC12"]["aReqValue"] == pytest.approx(-0.35)
assert parser.vl["SCC14"]["ComfortBandUpper"] == pytest.approx(0.08)
assert parser.vl["SCC14"]["ComfortBandLower"] == pytest.approx(0.02)
assert parser.vl["SCC14"]["JerkLowerLimit"] == pytest.approx(4.0)
assert parser.vl["SCC11"]["MainMode_ACC"] == 1
assert parser.vl["SCC12"]["StopReq"] == 0
assert parser.vl["SCC12"]["aReqRaw"] == pytest.approx(-1.0)
assert parser.vl["SCC12"]["aReqValue"] == pytest.approx(-1.0)
assert parser.vl["SCC14"]["ComfortBandUpper"] == pytest.approx(0.0)
assert parser.vl["SCC14"]["ComfortBandLower"] == pytest.approx(0.0)
assert parser.vl["SCC14"]["JerkLowerLimit"] == pytest.approx(5.0)
def test_sportage_angle_steering_uses_adas_cmd_with_send_lfa(self):
fingerprint = gen_empty_fingerprint()
selfdrive/controls/lib/latcontrol_torque.py
+27 -27
View File
@@ -211,43 +211,43 @@ IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 0.72
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.58
IONIQ_6_UNWIND_TAPER_LEFT = 1.22
IONIQ_6_UNWIND_TAPER_RIGHT = 2.18
IONIQ_6_TURN_IN_BOOST_LEFT = 0.76
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.76
IONIQ_6_UNWIND_TAPER_LEFT = 1.36
IONIQ_6_UNWIND_TAPER_RIGHT = 2.40
IONIQ_6_FRICTION_MULT = 0.995
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.18
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.02
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.10
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.08
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.08
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 0.88
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.72
IONIQ_6_CENTER_TAPER_MAX = 0.038
IONIQ_6_CENTER_TAPER_LAT = 0.16
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.20
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.26
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.20
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.45
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.09
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.14
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.02
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.96
IONIQ_6_CENTER_TAPER_MAX = 0.042
IONIQ_6_CENTER_TAPER_LAT = 0.18
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.050
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.22
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.035
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 8.0
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 15.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.2
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.080
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 7.0
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 16.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.15
IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.07
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.40
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.46
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.18
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.44
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.60
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.30
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.82
IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.95
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97
KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6
KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C"
@@ -780,7 +780,7 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT)
unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT)
reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight)
return max(1.0 - reduction, 0.62)
return max(1.0 - reduction, 0.56)
def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
selfdrive/controls/lib/longitudinal_planner.py
+5 -2
View File
@@ -30,6 +30,8 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5
RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
RAW_LEAD_SAFETY_TTC = 7.0
RAW_LEAD_SAFETY_DISTANCE = 40.0
STANDSTILL_LEAD_NUDGE_ACCEL = 0.05
STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0
CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
@@ -767,10 +769,11 @@ class LongitudinalPlanner:
output_a_target = min(output_a_target, close_lead_brake_cap)
if lead_control_active and sm['carState'].standstill:
standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5
moving_leads = [lead for lead in (self.lead_one, self.lead_two)
if lead.status and lead.vLead > 0.0 and lead.dRel >= STOP_DISTANCE - 0.5]
if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
if moving_leads:
output_a_target = max(output_a_target, 0.3)
output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)
if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)):
# Keep follow/catchup behavior from pulling past the cruise target. Using the
selfdrive/controls/tests/test_latcontrol.py
+2 -2
View File
@@ -243,14 +243,14 @@ class TestLatControl:
right_turn_in = get_ioniq_6_friction_scale(6.0, -0.5, -0.8)
left_unwind = get_ioniq_6_friction_scale(6.0, 0.5, -0.8)
right_unwind = get_ioniq_6_friction_scale(6.0, -0.5, 0.8)
assert left_turn_in >= right_turn_in > base
assert right_turn_in >= left_turn_in > base
assert base > left_unwind >= right_unwind
def test_ioniq_6_center_taper_curve(self):
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) < 5e-3
assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 1.2e-2
def test_kia_ev6_ff_scale_curve(self):
assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0
selfdrive/controls/tests/test_longitudinal_planner.py
+24
View File
@@ -393,6 +393,30 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(
assert planner.output_a_target < -1.0
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version):
v_ego = 0.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
v_ego,
desired_accel=0.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=7.1, v_lead=2.3, a_lead=1.8, radar=True, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["starpilotPlan"].vCruise = 10.0
planner.update(sm, make_toggles(model_version))
assert planner.output_should_stop
assert planner.output_a_target < 0.1
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
far_v_ego = 29.26
selfdrive/controls/tests/test_starpilot_vcruise.py
+3 -1
View File
@@ -4,9 +4,10 @@ from openpilot.common.constants import CV
from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target
def test_active_slc_control_target_ignores_set_speed_limit_toggle():
def test_active_slc_control_target_does_not_require_set_speed_limit():
target = get_active_slc_control_target(
speed_limit_controller=True,
set_speed_limit=False,
slc_target=45.0 * CV.MPH_TO_MS,
slc_offset=3.0 * CV.MPH_TO_MS,
overridden_speed=0.0,
@@ -19,6 +20,7 @@ def test_active_slc_control_target_ignores_set_speed_limit_toggle():
def test_active_slc_control_target_applies_offset_and_cluster_diff():
target = get_active_slc_control_target(
speed_limit_controller=True,
set_speed_limit=True,
slc_target=45.0 * CV.MPH_TO_MS,
slc_offset=3.0 * CV.MPH_TO_MS,
overridden_speed=0.0,
selfdrive/ui/layouts/settings/starpilot/longitudinal.py
+1 -1
View File
@@ -945,7 +945,7 @@ class StarPilotSLCQOLLayout(StarPilotPanel):
super().__init__()
self.CATEGORIES = [
{
"title": tr_noop("Match Speed Limit on Engage"),
"title": tr_noop("Auto Match Speed Limits"),
"type": "toggle",
"get_state": lambda: self._params.get_bool("SetSpeedLimit"),
"set_state": lambda s: self._params.put_bool("SetSpeedLimit", s),
selfdrive/ui/qt/onroad/annotated_camera.cc
+11 -2
View File
@@ -36,7 +36,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState
const cereal::CarState::Reader &carState = sm["carState"].getCarState();
starpilot_nvg->experimentalButtonPosition = QPoint(experimental_btn->x(), experimental_btn->y());
const bool hide_steering_wheel = starpilot_toggles.value("hide_steering_wheel").toBool();
experimental_btn->setVisible(!hide_steering_wheel);
const QPoint experimental_button_position = hide_steering_wheel
? QPoint(width() - UI_BORDER_SIZE - btn_size, UI_BORDER_SIZE)
: QPoint(experimental_btn->x(), experimental_btn->y());
starpilot_nvg->experimentalButtonPosition = experimental_button_position;
bool onroad_distance_btn_enabled = starpilot_nvg->dmIconPosition != QPoint(0, 0) && !starpilot_nvg->hideBottomIcons && starpilot_toggles.value("onroad_distance_button").toBool();
personality_btn->setVisible(onroad_distance_btn_enabled);
@@ -47,7 +53,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState
dmon.onroad_distance_btn_enabled = onroad_distance_btn_enabled;
screen_recorder->move(experimental_btn->x() - UI_BORDER_SIZE - btn_size, experimental_btn->y());
const QPoint screen_recorder_position = hide_steering_wheel
? experimental_button_position
: QPoint(experimental_button_position.x() - UI_BORDER_SIZE - btn_size, experimental_button_position.y());
screen_recorder->move(screen_recorder_position);
screen_recorder->setVisible(starpilot_nvg->standstillDuration == 0 && !(starpilot_nvg->signalStyle == "static" && carState.getRightBlinker()) && starpilot_toggles.value("screen_recorder").toBool());
}
starpilot/common/starpilot_variables.py
+1
View File
@@ -555,6 +555,7 @@ class StarPilotVariables:
toggle.hide_max_speed = self.get_value("HideMaxSpeed", condition=advanced_custom_ui and not toggle.debug_mode)
toggle.hide_speed = self.get_value("HideSpeed", condition=advanced_custom_ui and not toggle.debug_mode)
toggle.hide_speed_limit = self.get_value("HideSpeedLimit", condition=advanced_custom_ui and not toggle.debug_mode)
toggle.hide_steering_wheel = self.get_value("HideSteeringWheel", condition=advanced_custom_ui and not toggle.debug_mode)
toggle.use_wheel_speed = self.get_value("WheelSpeed", condition=advanced_custom_ui)
advanced_lateral_tuning = self.get_value("AdvancedLateralTune")
starpilot/controls/lib/starpilot_acceleration.py
+1
View File
@@ -204,6 +204,7 @@ class StarPilotAcceleration:
v_ego_diff = v_ego_cluster - v_ego
effective_slc_target = get_active_slc_control_target(
getattr(starpilot_toggles, "speed_limit_controller", False),
getattr(starpilot_toggles, "set_speed_limit", False),
getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0),
getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
starpilot/controls/lib/starpilot_vcruise.py
+4 -1
View File
@@ -27,7 +27,9 @@ OFFSET_FT_MIN = -20
OFFSET_FT_MAX = 20
def get_active_slc_control_target(speed_limit_controller, slc_target, slc_offset, overridden_speed, v_ego_diff):
def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, v_ego_diff):
# `SetSpeedLimit` only controls engage-time set-speed initialization. Ongoing
# SLC speed matching must remain active whenever Speed Limit Controller is on.
if not speed_limit_controller:
return 0.0
@@ -197,6 +199,7 @@ class StarPilotVCruise:
targets = [self.csc_target, v_cruise]
slc_control_target = get_active_slc_control_target(
starpilot_toggles.speed_limit_controller,
getattr(starpilot_toggles, "set_speed_limit", False),
self.slc_target,
self.slc_offset,
self.slc.overridden_speed,
starpilot/ui/qt/offroad/visual_settings.cc
+1
View File
@@ -37,6 +37,7 @@ StarPilotVisualsPanel::StarPilotVisualsPanel(StarPilotSettingsWindow *parent, bo
{"HideMaxSpeed", tr("Hide Max Speed"), tr("<b>Hide the max speed</b> from the driving screen."), ""},
{"HideAlerts", tr("Hide Non-Critical Alerts"), tr("<b>Hide non-critical alerts</b> from the driving screen."), ""},
{"HideSpeedLimit", tr("Hide Speed Limits"), tr("<b>Hide posted speed limits</b> from the driving screen."), ""},
{"HideSteeringWheel", tr("Hide Steering Wheel"), tr("<b>Hide the steering-wheel button</b> from the top-right of the driving screen."), ""},
{"WheelSpeed", tr("Use Wheel Speed"), tr("<b>Use the vehicle's wheel speed</b> instead of the cluster speed. This is purely a visual change and doesn't impact how openpilot drives!"), ""},
{"CustomUI", tr("Driving Screen Widgets"), tr("<b>Custom StarPilot widgets</b> for the driving screen."), "../assets/icons/calibration.png"},
starpilot/ui/qt/offroad/visual_settings.h
+1 -1
View File
@@ -23,7 +23,7 @@ private:
std::map<QString, AbstractControl*> toggles;
QSet<QString> advancedCustomOnroadUIKeys = {"HideAlerts", "HideLeadMarker", "HideMaxSpeed", "HideSpeed", "HideSpeedLimit", "WheelSpeed"};
QSet<QString> advancedCustomOnroadUIKeys = {"HideAlerts", "HideLeadMarker", "HideMaxSpeed", "HideSpeed", "HideSpeedLimit", "HideSteeringWheel", "WheelSpeed"};
QSet<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "Compass", "OnroadDistanceButton", "PedalsOnUI", "RotatingWheel"};
QSet<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth"};
QSet<QString> navigationUIKeys = {"RoadNameUI", "ShowSpeedLimits", "SLCMapboxFiller", "UseVienna"};