This commit is contained in:
firestar5683
2026-09-10 20:49:06 -05:00
parent 52c61da75d
commit 334f32f5d8
18 changed files with 418 additions and 61 deletions
@@ -860,13 +860,13 @@ class CarController(CarControllerBase):
can_sends = []
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
lfa_status_cars = (
persistent_lfa_status_cars = (
CAR.HYUNDAI_IONIQ_6,
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
CAR.KIA_EV6,
)
lfa_longitudinal_active = self.CP.openpilotLongitudinalControl \
if self.CP.carFingerprint in lfa_status_cars else longitudinal_active
if self.CP.carFingerprint in persistent_lfa_status_cars else self.long_active_ecu
lka_steering_long = lka_steering and lfa_longitudinal_active
ccnc_non_hda2 = self.CP.flags & HyundaiFlags.CCNC and not lka_steering
use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \
@@ -29,7 +29,7 @@ Ecu = structs.CarParams.Ecu
def get_communication_control_request(car_fingerprint):
if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR and car_fingerprint != CAR.HYUNDAI_IONIQ_5:
if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR:
return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
uds.MESSAGE_TYPE.NORMAL])
@@ -139,9 +139,13 @@ class TestHyundaiFingerprint:
assert get_communication_control_request(CAR.KIA_EV6) == stock_request
assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_ECU_KEEPALIVE_CAR
assert CAR.HYUNDAI_IONIQ_5 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_5) == stock_request
assert CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
assert CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN not in CANFD_RADAR_ECU_KEEPALIVE_CAR
assert get_communication_control_request(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) == stock_request
assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
def test_carnival_hev_low_speed_torque_rate_limits(self):
@@ -2594,7 +2598,7 @@ class TestHyundaiFingerprint:
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in inactive_msgs
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
assert steering_names == [("LKAS", can_bus.ACAN)]
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
controller.frame = 1
cc.longActive = True
@@ -2604,11 +2608,18 @@ class TestHyundaiFingerprint:
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
@pytest.mark.parametrize("car", [CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6])
def test_egmp_keeps_lfa_status_when_longitudinal_is_inactive(self, car):
@pytest.mark.parametrize(("car", "powertrain_flag"), [
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.EV),
(CAR.HYUNDAI_IONIQ_6, HyundaiFlags.EV),
(CAR.KIA_EV6, HyundaiFlags.EV),
(CAR.KIA_CARNIVAL_2025, 0),
(CAR.KIA_CARNIVAL_HEV_4TH_GEN, HyundaiFlags.HYBRID),
(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, HyundaiFlags.EV),
])
def test_hda2_keeps_lfa_status_when_longitudinal_is_inactive(self, car, powertrain_flag):
CP = CarParams.new_message()
CP.carFingerprint = car
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | powertrain_flag)
CP.openpilotLongitudinalControl = True
controller = CarController(DBC[CP.carFingerprint], CP)
@@ -2624,10 +2635,39 @@ class TestHyundaiFingerprint:
)
controller.frame = 1
for controller.long_active_ecu in (False, True):
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
assert any(addr == 0x12A for addr, _, _ in msgs)
controller.long_active_ecu = True
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
assert any(addr == 0x12A for addr, _, _ in msgs)
@pytest.mark.parametrize("car", [
CAR.HYUNDAI_IONIQ_6,
CAR.KIA_EV6,
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
])
def test_egmp_persistent_lfa_status_survives_ecu_fallback_state(self, car):
CP = CarParams.new_message()
CP.carFingerprint = car
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
CP.openpilotLongitudinalControl = True
controller = CarController(DBC[CP.carFingerprint], CP)
controller.frame = 1
controller.long_active_ecu = False
cc = SimpleNamespace(
enabled=False, latActive=False, longActive=False,
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace(),
)
cs = SimpleNamespace(
stock_lfa_msg=None, stock_lkas_msg=None,
left_blindspot_from_radar=False, right_blindspot_from_radar=False,
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
)
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
assert any(addr == 0x12A for addr, _, _ in msgs)
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
CP = CarParams.new_message()
+6 -1
View File
@@ -1220,7 +1220,12 @@ CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {
CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_5_PE, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV6, CAR.KIA_EV9, CAR.GENESIS_GV60_EV_1ST_GEN,
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
}
CANFD_RADAR_ECU_KEEPALIVE_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR - {CAR.KIA_EV6}
CANFD_RADAR_ECU_KEEPALIVE_CAR = {
CAR.HYUNDAI_IONIQ_5_PE,
CAR.HYUNDAI_IONIQ_6,
CAR.KIA_EV9,
CAR.GENESIS_GV60_EV_1ST_GEN,
}
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
CAR.HYUNDAI_IONIQ,
CAR.HYUNDAI_KONA_EV_2022,
@@ -20,6 +20,7 @@ _ANGLE_OVERRIDE_CONFIRM_FRAMES = 2
_ANGLE_REENGAGE_MAX_STEER_RATE = 2.0
_ANGLE_MADS_MIN_SPEED = 0.44704
_ANGLE_MADS_MAX_STEER_ANGLE = 120.0
_ASCENT_AOL_ARM_FRAMES = 30
_STOP_START_STARTUP_DELAY_FRAMES = 100
# StarPilot's first populated toggle message can arrive several seconds after
# the car controller starts while fingerprinting and settings settle.
@@ -44,6 +45,7 @@ class CarController(CarControllerBase):
self.angle_override_confirm_frames = 0
self.angle_lkas_active = False
self.angle_handoff_active = False
self.ascent_aol_arm_frames = 0
self.cruise_button_prev = 0
self.steer_rate_counter = 0
@@ -170,6 +172,14 @@ class CarController(CarControllerBase):
return self.driver_override
def _ascent_aol_ready(self, ready):
if not ready:
self.ascent_aol_arm_frames = 0
return False
self.ascent_aol_arm_frames = min(self.ascent_aol_arm_frames + 1, _ASCENT_AOL_ARM_FRAMES)
return self.ascent_aol_arm_frames >= _ASCENT_AOL_ARM_FRAMES
def lateral_angle(self, CC, CS):
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
mads_only = CC.latActive and not CC.enabled
@@ -199,6 +209,12 @@ class CarController(CarControllerBase):
abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
if mads_only:
cruise_available = getattr(getattr(CS.out, "cruiseState", None), "available", True)
lkas_available = self._ascent_aol_ready(lkas_available and cruise_available)
else:
self.ascent_aol_arm_frames = _ASCENT_AOL_ARM_FRAMES if lkas_available else 0
manual_handoff = self._angle_manual_handoff(
CS, lkas_available, use_steering_pressed=self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023,
@@ -284,7 +300,7 @@ class CarController(CarControllerBase):
return subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)
def _lkas_status_active(self, CC):
if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023:
if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023):
return self.angle_lkas_active
return CC.latActive
@@ -8,7 +8,7 @@ from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, fw_versions, gen_empty_fingerprint, structs
from opendbc.car.fw_query_definitions import StdQueries
from opendbc.car.subaru import subarucan
from opendbc.car.subaru.carcontroller import CarController
from opendbc.car.subaru.carcontroller import CarController, _ASCENT_AOL_ARM_FRAMES
from opendbc.car.subaru.carstate import CarState
from opendbc.car.subaru.fingerprints import FW_VERSIONS
from opendbc.car.fw_versions import match_fw_to_car
@@ -692,6 +692,7 @@ def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
steeringRateDeg=96.0,
steeringTorque=7.0,
steeringPressed=False,
cruiseState=SimpleNamespace(available=True),
gearShifter=structs.CarState.GearShifter.drive,
standstill=False,
))
@@ -704,18 +705,74 @@ def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope():
CS.out.steeringAngleDeg = -100.0
CS.out.steeringRateDeg = 0.0
for frame in range(2, _ASCENT_AOL_ARM_FRAMES + 1):
msg = controller.lateral_angle(CC, CS)
parser.update([(frame, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
msg = controller.lateral_angle(CC, CS)
parser.update([(2, [msg])])
parser.update([(_ASCENT_AOL_ARM_FRAMES + 1, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
CS.out.gearShifter = structs.CarState.GearShifter.reverse
msg = controller.lateral_angle(CC, CS)
parser.update([(12, [msg])])
parser.update([(_ASCENT_AOL_ARM_FRAMES + 2, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
def test_lkas_hud_state_uses_outback_angle_request_state():
def test_ascent_aol_does_not_arm_before_cruise_main_is_available():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=5.0))
CS = SimpleNamespace(out=SimpleNamespace(
vEgoRaw=10.0,
steeringAngleDeg=0.0,
steeringRateDeg=0.0,
steeringTorque=0.0,
steeringPressed=False,
cruiseState=SimpleNamespace(available=False),
gearShifter=structs.CarState.GearShifter.drive,
standstill=False,
))
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
for frame in range(_ASCENT_AOL_ARM_FRAMES):
msg = controller.lateral_angle(CC, CS)
parser.update([(frame + 1, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert controller.ascent_aol_arm_frames == 0
CS.out.cruiseState.available = True
msg = controller.lateral_angle(CC, CS)
parser.update([(_ASCENT_AOL_ARM_FRAMES + 1, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
assert controller.ascent_aol_arm_frames == 1
def test_ascent_angle_controller_does_not_delay_normal_engagement():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=5.0))
CS = SimpleNamespace(out=SimpleNamespace(
vEgoRaw=10.0,
steeringAngleDeg=0.0,
steeringRateDeg=0.0,
steeringTorque=0.0,
steeringPressed=False,
gearShifter=structs.CarState.GearShifter.drive,
standstill=False,
))
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
msg = controller.lateral_angle(CC, CS)
parser.update([(1, [msg])])
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
def test_lkas_hud_state_uses_angle_request_state():
update_source = inspect.getsource(CarController.update)
assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC)" in update_source
@@ -765,6 +822,16 @@ def test_outback_manual_steering_releases_angle_request_before_lkas_fault():
assert not controller._lkas_status_active(CC)
def test_ascent_hud_waits_for_angle_request():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
controller = CarController({}, CP)
CC = SimpleNamespace(latActive=True)
assert not controller._lkas_status_active(CC)
controller.angle_lkas_active = True
assert controller._lkas_status_active(CC)
def test_other_angle_cars_keep_lateral_status_behavior():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
controller = CarController({}, CP)
-7
View File
@@ -404,7 +404,6 @@ class Controls:
self.turn_blinker_swept = 0.0
self.twitch_guard_remaining = 0.0
self.kona_non_scc_lateral_active = False
self.kona_non_scc_lateral_faulted = False
self.elantra_hev_2024_lateral_faulted = False
self.elantra_hev_2024_previous_cruise_enabled = False
@@ -504,11 +503,6 @@ class Controls:
standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
if self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC:
always_on_lateral_enabled = self.sm['starpilotCarState'].alwaysOnLateralEnabled
lateral_requested = (CC.enabled and self.sm['selfdriveState'].active) or always_on_lateral_enabled
if not lateral_requested:
self.kona_non_scc_lateral_faulted = False
elif CS.steerFaultTemporary:
self.kona_non_scc_lateral_faulted = True
CC.latActive = get_kona_non_scc_lateral_active(
CC.enabled, self.sm['selfdriveState'].active,
always_on_lateral_enabled,
@@ -516,7 +510,6 @@ class Controls:
standstill, self.CP.steerAtStandstill,
self.sm['starpilotPlan'].lateralCheck,
CS.steeringPressed, self.kona_non_scc_lateral_active,
self.kona_non_scc_lateral_faulted,
)
self.kona_non_scc_lateral_active = CC.latActive
elif self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024:
+2 -3
View File
@@ -89,13 +89,12 @@ def update_lateral_fault_latch(previous_latched: bool, lateral_requested: bool,
def get_kona_non_scc_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool,
steer_fault_temporary: bool, steer_fault_permanent: bool,
standstill: bool, steer_at_standstill: bool, lateral_check: bool,
steering_pressed: bool, previous_lateral_active: bool,
steer_fault_latched: bool = False) -> bool:
steering_pressed: bool, previous_lateral_active: bool) -> bool:
"""Avoid the Kona EPS torque fault when AOL is enabled over driver steering input."""
lateral_active = get_lateral_active(enabled, active, always_on_lateral_enabled,
steer_fault_temporary, steer_fault_permanent,
standstill, steer_at_standstill, lateral_check)
if not lateral_active or steer_fault_latched:
if not lateral_active:
return False
aol_rising_edge = always_on_lateral_enabled and not enabled and not previous_lateral_active
+42 -4
View File
@@ -9,26 +9,54 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
_ASCENT_ANGLE_TRACKING_GAIN = 0.25
_ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0
_ASCENT_ANGLE_TRACKING_MIN_SPEED = 5.0
_ASCENT_ANGLE_TRACKING_MIN_SPEED = 9.0
_ASCENT_ANGLE_TRACKING_FULL_SPEED = 15.0
_ASCENT_ANGLE_TRACKING_TURN_START = 15.0
_ASCENT_ANGLE_TRACKING_TURN_FULL = 35.0
_ASCENT_LOW_SPEED_FILTER_MAX_SPEED = 10.0
_ASCENT_LOW_SPEED_FILTER_CENTER_ANGLE = 35.0
_ASCENT_LOW_SPEED_FILTER_TIME_CONSTANT = 0.18
def _clipped_weight(value: float, start: float, end: float) -> float:
return max(0.0, min(1.0, (value - start) / (end - start)))
def _ascent_angle_tracking_target(target_angle: float, steering_angle: float,
v_ego: float, steering_pressed: bool) -> float:
if steering_pressed or v_ego < _ASCENT_ANGLE_TRACKING_MIN_SPEED:
if steering_pressed:
return target_angle
correction = (target_angle - steering_angle) * _ASCENT_ANGLE_TRACKING_GAIN
speed_weight = _clipped_weight(v_ego, _ASCENT_ANGLE_TRACKING_MIN_SPEED, _ASCENT_ANGLE_TRACKING_FULL_SPEED)
turn_weight = _clipped_weight(abs(target_angle), _ASCENT_ANGLE_TRACKING_TURN_START, _ASCENT_ANGLE_TRACKING_TURN_FULL)
correction = (target_angle - steering_angle) * _ASCENT_ANGLE_TRACKING_GAIN * max(speed_weight, turn_weight)
correction = max(-_ASCENT_ANGLE_TRACKING_MAX_CORRECTION,
min(_ASCENT_ANGLE_TRACKING_MAX_CORRECTION, correction))
return target_angle + correction
def _ascent_low_speed_angle_target(target_angle: float, previous_target: float,
v_ego: float, steering_pressed: bool, dt: float) -> float:
if steering_pressed:
return target_angle
speed_weight = 1.0 - _clipped_weight(v_ego, 0.0, _ASCENT_LOW_SPEED_FILTER_MAX_SPEED)
center_weight = 1.0 - _clipped_weight(abs(target_angle), 0.0, _ASCENT_LOW_SPEED_FILTER_CENTER_ANGLE)
time_constant = _ASCENT_LOW_SPEED_FILTER_TIME_CONSTANT * speed_weight * center_weight
if time_constant <= 0.0:
return target_angle
alpha = dt / (time_constant + dt)
return previous_target + alpha * (target_angle - previous_target)
class LatControlAngle(LatControl):
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
self.sat_check_min_speed = 5.
self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023
self.ascent_angle_target = None
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles):
angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -36,14 +64,24 @@ class LatControlAngle(LatControl):
if not active:
angle_log.active = False
angle_steers_des = float(CS.steeringAngleDeg)
self.ascent_angle_target = angle_steers_des
else:
angle_log.active = True
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg
if self.is_ascent:
angle_steers_des = _ascent_angle_tracking_target(
if self.ascent_angle_target is None:
self.ascent_angle_target = float(CS.steeringAngleDeg)
self.ascent_angle_target = _ascent_low_speed_angle_target(
angle_steers_des,
self.ascent_angle_target,
CS.vEgo,
bool(getattr(CS, "steeringPressed", False)),
self.dt,
)
angle_steers_des = _ascent_angle_tracking_target(
self.ascent_angle_target,
CS.steeringAngleDeg,
CS.vEgo,
bool(getattr(CS, "steeringPressed", False)),
@@ -667,6 +667,10 @@ class LatControlTorque(LatControl):
output_torque *= get_genesis_gv70_reversal_output_scale(
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
)
if not CS.steeringPressed:
output_torque = get_genesis_gv70_stabilized_output(
output_torque, self.prev_output_torque, setpoint, desired_lateral_jerk, CS.vEgo, self.dt,
)
elif sonata_hybrid_active:
output_torque *= sonata_hybrid_center_taper
output_torque *= sonata_hybrid_center_output_taper
@@ -260,6 +260,17 @@ GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT_WIDTH = 0.08
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MIN = 0.06
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT = 0.12
GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH = 0.10
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED = 38.0 * CV.MPH_TO_MS
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.48
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42
GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
GENESIS_G70_FRICTION_SPEED_ONSET = 10.0
@@ -3230,6 +3241,32 @@ def get_genesis_gv70_low_speed_center_overshoot_scale(setpoint: float, measured_
speed_weight * speed_cutoff)
def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque: float,
desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float, dt: float) -> float:
speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_GV70_OUTPUT_SMOOTHING_SPEED) /
GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH)
curve_weight = 1.0 - center_weight
response_time = (GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight +
GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC * center_weight)
unwind_phase = -desired_lateral_accel * desired_lateral_jerk
unwind_weight = _sigmoid((unwind_phase - GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE) /
GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH)
response_time += GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight
changing_direction = (abs(desired_lateral_accel) >= GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and
prev_output_torque * desired_lateral_accel <= 0.0)
if changing_direction:
response_time = min(response_time, GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC)
output_alpha = dt / (max(response_time, 0.0) + dt)
smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque)
return float(output_torque + speed_weight * (smoothed_output - output_torque))
def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_standard_friction_threshold(v_ego)
@@ -35,9 +35,15 @@ def test_kona_non_scc_aol_gate_does_not_change_fault_or_normal_lateral_gates():
)
def test_kona_non_scc_does_not_retry_after_a_latched_temporary_fault():
def test_kona_non_scc_recovers_after_temporary_fault_clears():
assert not get_kona_non_scc_lateral_active(
False, False, True, False, False, False, False, True, False, False, True,
False, False, True, True, False, False, False, True, False, True,
)
assert not get_kona_non_scc_lateral_active(
False, False, True, False, False, False, False, True, True, False,
)
assert get_kona_non_scc_lateral_active(
False, False, True, False, False, False, False, True, False, False,
)
+52 -1
View File
@@ -20,6 +20,7 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_angle import (
LatControlAngle,
_ascent_angle_tracking_target,
_ascent_low_speed_angle_target,
)
from openpilot.selfdrive.controls.lib.latcontrol_pid import (
LatControlPID,
@@ -54,6 +55,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
get_rav4_tss2_pid_output,
get_subaru_impreza_pid_output_scale,
get_genesis_gv70_low_speed_center_overshoot_scale,
get_genesis_gv70_stabilized_output,
get_genesis_g70_high_speed_transition_scale,
get_genesis_g70_stabilized_output,
normalize_flm_overrides,
@@ -203,9 +205,19 @@ class TestLatControl:
def test_ascent_angle_tracking_correction_is_bounded_and_handoff_safe(self):
assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, False) == pytest.approx(12.5)
assert _ascent_angle_tracking_target(40.0, 0.0, 20.0, False) == pytest.approx(48.0)
assert _ascent_angle_tracking_target(10.0, 0.0, 4.0, False) == pytest.approx(10.0)
assert _ascent_angle_tracking_target(10.0, 0.0, 9.0, False) == pytest.approx(10.0)
assert 10.0 < _ascent_angle_tracking_target(10.0, 0.0, 12.0, False) < 12.5
assert _ascent_angle_tracking_target(40.0, 0.0, 4.0, False) == pytest.approx(48.0)
assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, True) == pytest.approx(10.0)
def test_ascent_low_speed_filter_is_center_gated_and_handoff_safe(self):
filtered = _ascent_low_speed_angle_target(10.0, 0.0, 4.0, False, DT_CTRL)
assert 0.0 < filtered < 10.0
assert _ascent_low_speed_angle_target(10.0, 0.0, 10.0, False, DT_CTRL) == pytest.approx(10.0)
assert _ascent_low_speed_angle_target(40.0, 0.0, 4.0, False, DT_CTRL) == pytest.approx(40.0)
assert _ascent_low_speed_angle_target(10.0, 0.0, 4.0, True, DT_CTRL) == pytest.approx(10.0)
def test_torque_log_exposes_friction_controller_state(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023)
@@ -1724,6 +1736,45 @@ class TestLatControl:
assert base_output != 0.0
assert tapered_output == pytest.approx(base_output * 0.5)
def test_genesis_gv70_output_stabilizer_is_speed_and_phase_aware(self):
low_speed = get_genesis_gv70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 5.0, DT_CTRL)
high_speed_center = get_genesis_gv70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 30.0, DT_CTRL)
high_speed_wind = get_genesis_gv70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL)
high_speed_unwind = get_genesis_gv70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL)
high_speed_direction_change = get_genesis_gv70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL)
assert low_speed == pytest.approx(-0.2, abs=0.005)
assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2)
assert high_speed_unwind > high_speed_wind > 0.1
assert abs(high_speed_direction_change - 0.3) > abs(high_speed_center - 0.2)
def test_genesis_gv70_output_stabilizer_update_path(self, monkeypatch):
calls = []
def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel,
desired_lateral_jerk, v_ego, dt):
calls.append((output_torque, prev_output_torque, desired_lateral_accel,
desired_lateral_jerk, v_ego, dt))
return 0.123
monkeypatch.setattr(latcontrol_torque, "get_genesis_gv70_stabilized_output", stabilized_output)
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_GV70_ELECTRIFIED_1ST_GEN)
CS.vEgo = 25.0
output, _, lac_log = controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert calls
assert lac_log.active
assert output == pytest.approx(-0.123)
call_count = len(calls)
CS.steeringPressed = True
controller.update(
True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
)
assert len(calls) == call_count
def test_genesis_g70_low_speed_output_guard_update_path(self):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020)
CS.vEgo = 2.0
+18 -14
View File
@@ -34,9 +34,9 @@ MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06
STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL
CURVATURE_LOOKAHEAD_MIN = 0.20
CURVATURE_LOOKAHEAD_MAX = 0.40
MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.40
MACH_E_TURN_IN_MIN_CURVATURE = 0.006
MACH_E_TURN_IN_FULL_CURVATURE = 0.009
MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.80
MACH_E_TURN_IN_MIN_CURVATURE = 0.002
MACH_E_TURN_IN_FULL_CURVATURE = 0.008
MACH_E_TURN_IN_LAG_CURVATURE = 0.006
FORD_CURVATURE_LOOKAHEAD = {
CAR.FORD_EXPLORER_MK6: 0.20,
@@ -184,21 +184,23 @@ class FordLateralController:
precision = 0
return requested, precision
def _turn_in_preview_weight(self, desired: float, predicted: float, current: float) -> float:
def _turn_in_preview_weight(self, desired: float, preview: float, current: float) -> float:
if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS:
return 0.0
if desired * predicted <= 0.0 or desired * self.desired_curvature_last < 0.0:
if desired * preview <= 0.0 or desired * self.desired_curvature_last < 0.0:
return 0.0
if abs(desired) <= abs(self.desired_curvature_last) or abs(current) >= abs(desired):
if abs(desired) <= abs(self.desired_curvature_last):
return 0.0
target = max(abs(desired), abs(preview))
curvature_weight = float(np.interp(
abs(desired),
target,
[MACH_E_TURN_IN_MIN_CURVATURE, MACH_E_TURN_IN_FULL_CURVATURE],
[0.0, 1.0],
))
direction = float(np.sign(desired))
lag_weight = float(np.clip(
(abs(desired) - abs(current)) / MACH_E_TURN_IN_LAG_CURVATURE,
(target - direction * current) / MACH_E_TURN_IN_LAG_CURVATURE,
0.0, 1.0,
))
return curvature_weight * lag_weight
@@ -263,13 +265,15 @@ class FordLateralController:
manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS))
v_ego = float(CS.out.vEgoRaw)
predicted = self._predicted_curvature(v_ego, self._curvature_lookahead())
lookahead = self._curvature_lookahead()
predicted = self._predicted_curvature(v_ego, lookahead)
desired = float(actuators.curvature)
turn_in_weight = self._turn_in_preview_weight(desired, predicted, current)
if turn_in_weight > 0.0:
turn_in_predicted = self._predicted_curvature(
v_ego, self._curvature_lookahead() + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_predicted]))
if self.CP.carFingerprint in FORD_CONSERVATIVE_PREVIEW_CARS:
turn_in_predicted = self._predicted_curvature(v_ego, lookahead + MACH_E_TURN_IN_LOOKAHEAD_EXTRA)
turn_in_weight = self._turn_in_preview_weight(desired, turn_in_predicted, current)
if turn_in_weight > 0.0:
turn_in_target = float(np.copysign(max(abs(desired), abs(turn_in_predicted)), desired))
predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_target]))
requested, precision = self._blend_and_scale(desired, predicted, v_ego, current)
self.desired_curvature_last = desired
+30 -6
View File
@@ -144,12 +144,22 @@ def test_mach_e_preview_remains_available_on_curve_entry(controller):
def test_mach_e_turn_in_preview_leads_when_path_lags(controller):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.desired_curvature_last = 0.007
controller.desired_curvature_last = 0.004
weight = controller._turn_in_preview_weight(
desired=0.009, predicted=0.007, current=0.006)
desired=0.005, preview=0.005, current=0.002)
assert weight == pytest.approx(0.5)
assert weight == pytest.approx(0.25)
def test_mach_e_turn_in_preview_leads_opposite_measured_curvature(controller):
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
controller.desired_curvature_last = 0.001
weight = controller._turn_in_preview_weight(
desired=0.003, preview=0.009, current=-0.003)
assert weight == pytest.approx(1.0)
def test_mach_e_turn_in_preview_is_not_carried_into_unwind(controller):
@@ -157,7 +167,7 @@ def test_mach_e_turn_in_preview_is_not_carried_into_unwind(controller):
controller.desired_curvature_last = 0.010
assert controller._turn_in_preview_weight(
desired=0.008, predicted=0.009, current=0.004) == 0.0
desired=0.008, preview=0.009, current=0.004) == 0.0
def test_mach_e_turn_in_preview_uses_extra_model_horizon(controller, monkeypatch):
@@ -165,6 +175,20 @@ def test_mach_e_turn_in_preview_uses_extra_model_horizon(controller, monkeypatch
controller.sm["liveDelay"].lateralDelay = 0.4
controller.desired_curvature_last = 0.007
lookaheads = []
monkeypatch.setattr(controller, "_predicted_curvature",
lambda _v_ego, lookahead: lookaheads.append(lookahead) or 0.012)
controller.update(
SimpleNamespace(latActive=True), car_state(speed=8.0, curvature=0.002),
SimpleNamespace(curvature=0.010),
)
assert lookaheads == [pytest.approx(0.4), pytest.approx(1.2)]
def test_non_mach_e_does_not_request_extra_model_horizon(controller, monkeypatch):
controller.sm["liveDelay"].lateralDelay = 0.4
lookaheads = []
monkeypatch.setattr(controller, "_predicted_curvature",
lambda _v_ego, lookahead: lookaheads.append(lookahead) or 0.007)
@@ -173,14 +197,14 @@ def test_mach_e_turn_in_preview_uses_extra_model_horizon(controller, monkeypatch
SimpleNamespace(curvature=0.010),
)
assert lookaheads == [pytest.approx(0.4), pytest.approx(0.8)]
assert lookaheads == [pytest.approx(0.4)]
def test_non_mach_e_turn_in_preview_is_unchanged(controller):
controller.desired_curvature_last = 0.007
assert controller._turn_in_preview_weight(
desired=0.010, predicted=0.007, current=0.004) == 0.0
desired=0.010, preview=0.007, current=0.004) == 0.0
def test_non_mach_e_preview_blend_is_unchanged(controller):
@@ -8,12 +8,25 @@ export const GalaxyModal = {
cancelLabel: { type: String, default: "Cancel" },
danger: { type: Boolean, default: false },
sheet: { type: Boolean, default: true },
input: { type: Boolean, default: false },
inputValue: { type: String, default: "" },
inputPlaceholder: { type: String, default: "" },
inputRequired: { type: Boolean, default: false },
},
emits: ["update:modelValue", "confirm", "cancel"],
data() { return { value: this.inputValue } },
mounted() { this.focusInput() },
methods: {
focusInput() {
if (this.input) this.$nextTick(() => this.$refs.input?.focus())
},
close() { this.$emit("update:modelValue", false) },
cancel() { this.close(); this.$emit("cancel") },
confirm() { this.$emit("confirm"); this.close() },
confirm() {
if (this.inputRequired && !String(this.value || "").trim()) return
this.$emit("confirm", this.input ? this.value : undefined)
this.close()
},
},
template: `
<transition name="gx-fade">
@@ -22,9 +35,11 @@ export const GalaxyModal = {
<div class="gx-sheet" role="dialog" :aria-label="title">
<h3 class="gx-sheet__title">{{ title }}</h3>
<p v-if="message" style="color: var(--text-muted); line-height: 1.5;">{{ message }}</p>
<input v-if="input" ref="input" v-model="value" class="gx-field gx-field--full" type="text"
:placeholder="inputPlaceholder" @keyup.enter="confirm" />
<div class="gx-dialog__actions">
<button type="button" class="gx-btn gx-btn--text" @click="cancel">{{ cancelLabel }}</button>
<button type="button" class="gx-btn" :style="danger ? 'background: var(--error); color: var(--on-error);' : ''" @click="confirm">{{ confirmLabel }}</button>
<button type="button" class="gx-btn" :disabled="input && inputRequired && !String(value || '').trim()" :style="danger ? 'background: var(--error); color: var(--on-error);' : ''" @click="confirm">{{ confirmLabel }}</button>
</div>
</div>
</transition>
@@ -64,3 +79,38 @@ export function GalaxyConfirm({ title, message, confirmLabel = "Confirm", danger
app.mount(host)
})
}
export function GalaxyPrompt({ title, message, initialValue = "", placeholder = "", confirmLabel = "Confirm" } = {}) {
return new Promise((resolve) => {
const host = document.createElement("div")
document.body.appendChild(host)
const { createApp, h } = window.__galaxyVue
let app
let settled = false
const finish = (value) => {
if (settled) return
settled = true
resolve(value)
try { app?.unmount?.() } catch (e) {}
host.remove()
}
app = createApp({
render() {
return h(GalaxyModal, {
modelValue: true,
title,
message,
input: true,
inputValue: String(initialValue || ""),
inputPlaceholder: placeholder,
inputRequired: true,
confirmLabel,
"onUpdate:modelValue": (v) => { if (!v) finish(null) },
onConfirm: (value) => finish(String(value || "").trim()),
onCancel: () => finish(null),
})
},
})
app.mount(host)
})
}
@@ -1,6 +1,6 @@
import { api, showSnackbar } from "../api.js"
import { usePolling } from "../composables.js"
import { GalaxyConfirm } from "./GalaxyModal.js"
import { GalaxyConfirm, GalaxyPrompt } from "./GalaxyModal.js"
import { GxNotice } from "./GxNotice.js"
const MAX_ROUTES = 250
@@ -479,6 +479,11 @@ export const LateralTuningPanel = {
startPending(kind, opts = {}) {
this.pending = { kind, tuneId: opts.tuneId || "" }
this.pendingName = opts.name || ""
this.$nextTick(() => {
const editor = this.$refs.pendingEditor
editor?.scrollIntoView({ behavior: "smooth", block: "center" })
editor?.querySelector("input")?.focus()
})
},
cancelPending() { this.pending = null; this.pendingName = "" },
async confirmPending() {
@@ -520,6 +525,18 @@ export const LateralTuningPanel = {
async applySavedTune(tune) {
await this.runWith(() => api.flmApplySavedTune(tune.tuneId), "Saved tune applied.")
},
async renameSavedTune(tune) {
if (!tune?.tuneId || this.busy) return
const name = await GalaxyPrompt({
title: "Rename Saved Tune",
message: `Choose a new name for “${tune.name || "Saved Tune"}”.`,
initialValue: tune.name || "",
placeholder: "Name this tune...",
confirmLabel: "Rename",
})
if (name === null) return
await this.runWith(() => api.flmRenameSavedTune(tune.tuneId, name), "Tune renamed.")
},
async submitTune(tune) {
const ok = await GalaxyConfirm({
title: "Send to Firestar",
@@ -582,7 +599,7 @@ export const LateralTuningPanel = {
</div>
</section>
<div v-if="pending" class="gx-card" style="margin-top: var(--sp-3);">
<div v-if="pending" ref="pendingEditor" class="gx-card" style="margin-top: var(--sp-3);">
<div style="padding: var(--sp-4); display:grid; gap:10px;">
<div class="gx-section__header" style="padding:0 0 6px;">
<i class="bi bi-pencil-square"></i>
@@ -786,7 +803,7 @@ export const LateralTuningPanel = {
</div>
<div style="display:flex; gap:6px; flex-wrap:wrap; justify-content:flex-end;">
<button type="button" class="gx-btn gx-btn--tonal" :disabled="busy || tune.active" @click="applySavedTune(tune)">{{ tune.active ? 'Active' : 'Apply' }}</button>
<button type="button" class="gx-btn gx-btn--text" :disabled="busy" @click="startPending('rename', { tuneId: tune.tuneId, name: tune.name })">Rename</button>
<button type="button" class="gx-btn gx-btn--text" :disabled="busy" @click="renameSavedTune(tune)">Rename</button>
<button type="button" class="gx-btn gx-btn--text" :disabled="busy || tune.active" style="color:var(--error);" @click="deleteSavedTune(tune)">Delete</button>
<button type="button" class="gx-btn gx-btn--text" :disabled="busy" @click="submitTune(tune)">Firestar</button>
</div>
@@ -495,10 +495,16 @@ def test_ui_all_remaining_classic_tools_native_no_embed():
assert "fetch(" not in _read(rel), f"{rel} should not use raw fetch()"
lateral = _read("js/components/LateralTuningPanel.js")
modal = _read("js/components/GalaxyModal.js")
assert "MAX_SEGMENTS = 5" in lateral
assert "segmentRanges" in lateral and "selectedSegmentRanges" in lateral
assert "flmAnalyze(this.selectedRoutes, this.selectedSegmentRanges())" in lateral
assert "routeSelectedSegmentCount" in lateral
assert "GalaxyPrompt" in lateral
assert "renameSavedTune(tune)" in lateral
assert "initialValue: tune.name" in lateral
assert "export function GalaxyPrompt" in modal
assert "inputRequired" in modal
def test_ui_cameras_hub_vasm_and_pip_native_no_embed():