Pink Lady

This commit is contained in:
firestar5683
2026-08-11 15:10:52 -05:00
parent f64d6e6a73
commit f1f065a77a
9 changed files with 245 additions and 28 deletions
@@ -930,16 +930,19 @@ class CarController(CarControllerBase):
CC.leftBlinker,
CC.rightBlinker))
if self.frame % 2 == 0:
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
acc_kwargs = {
"main_mode_acc": int(CS.out.cruiseState.available),
"direct_accel": True,
"jerk_lower": 5.0,
"jerk_upper": 3.0 if CC.actuators.longControlState == LongCtrlState.pid else 1.0,
"lead_distance": lead_distance,
"lead_rel_speed": lead_rel_speed,
"lead_visible": lead_visible,
}
if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN:
acc_kwargs = {}
else:
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
acc_kwargs = {
"main_mode_acc": int(CS.out.cruiseState.available),
"direct_accel": True,
"jerk_lower": 5.0,
"jerk_upper": 3.0 if CC.actuators.longControlState == LongCtrlState.pid else 1.0,
"lead_distance": lead_distance,
"lead_rel_speed": lead_rel_speed,
"lead_visible": lead_visible,
}
if use_egmp_dynamic_long_tuning:
if use_egmp_smoothed_accel:
acc_kwargs["jerk_lower"] = self._ioniq_6_long_tuning.jerk_lower
@@ -63,6 +63,61 @@ def _update_checksum(packer, address: int, dat: bytearray) -> None:
_set_value(dat, sig_checksum, checksum)
def _set_little_endian_bits(dat: bytearray, lsb: int, size: int, value: int) -> None:
"""Write the legacy HDA-II field layout without changing the generated DBC aliases."""
value &= (1 << size) - 1
bit = lsb
remaining = size
while remaining:
byte = bit // 8
shift = bit % 8
chunk_size = min(remaining, 8 - shift)
mask = ((1 << chunk_size) - 1) << shift
dat[byte] = (dat[byte] & ~mask) | ((value & ((1 << chunk_size) - 1)) << shift)
value >>= chunk_size
bit += chunk_size
remaining -= chunk_size
def _create_gv70_lka_status_msg(packer, CAN, message_name: str, bus: int, enabled: bool,
lat_active: bool, apply_torque: int):
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": apply_torque,
"STEER_REQ": 1 if lat_active else 0,
"LKA_ASSIST": 0,
"STEER_MODE": 0,
"DAMP_FACTOR": 100,
}
address, raw, _ = packer.make_can_msg(message_name, bus, values)
dat = bytearray(raw)
legacy_fields = (
(24, 3, 2),
(27, 3, 0),
(30, 2, 0),
(32, 2, 0),
(34, 2, 0),
(36, 2, 0),
(38, 3, 2 if enabled else 1),
(52, 2, 1 if lat_active else 0),
(54, 2, 0),
(56, 1, 0),
(60, 4, 0),
(80, 2, 0),
)
for lsb, size, value in legacy_fields:
_set_little_endian_bits(dat, lsb, size, value)
_set_little_endian_bits(dat, 64 if message_name == "LKAS" else 104, 8, 100)
if message_name == "LKAS":
_set_little_endian_bits(dat, 84, 3, 0)
_update_checksum(packer, address, dat)
return address, bytes(dat), bus
def _create_angle_lfa_msg(packer, CAN, values, apply_angle: float, lat_active: bool, torque_reduction_gain: float):
address = packer.dbc.name_to_msg["LFA"].address
dat = packer.pack(address, values)
@@ -101,6 +156,16 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
send_lfa_status=False, lfa_only=False):
if lka_icon is None:
lka_icon = 2 if enabled else 1
if CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN and CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
ret = []
if CP.openpilotLongitudinalControl or send_lfa_status:
ret.append(_create_gv70_lka_status_msg(packer, CAN, "LFA", CAN.ECAN, enabled, lat_active, apply_torque))
if lfa_only:
return ret
ret.append(_create_gv70_lka_status_msg(packer, CAN, "LKAS", CAN.ACAN, enabled, lat_active, apply_torque))
return ret
angle_lkas_alt = CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT
control_values = {
@@ -311,6 +311,19 @@ class TestHyundaiFingerprint:
HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.EV_GAS)
assert (CP.safetyConfigs[-1].safetyParam & expected_bits) == expected_bits
gv70_fingerprint = gen_empty_fingerprint()
gv70_fingerprint[CanBus(None, gv70_fingerprint).CAM][0x50] = 32
gv70_car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
CP = CarInterface.get_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, gv70_fingerprint, gv70_car_fw,
True, False, False, None)
assert CP.alphaLongitudinalAvailable
assert CP.openpilotLongitudinalControl
assert CP.radarUnavailable
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.EV_GAS
for candidate in HYUNDAI_NON_SCC_CARS:
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False, None)
assert bool(CP.flags & HyundaiFlags.NON_SCC)
@@ -1940,6 +1953,53 @@ class TestHyundaiFingerprint:
parser.update([(1, lkas_msgs)])
assert parser.can_valid
assert parser.vl["LKAS"]["HAS_LANE_SAFETY"] == 0
assert parser.vl["LKAS"]["DAMP_FACTOR"] == 100
assert parser.vl["LKAS"]["TORQUE_REQUEST"] == 0
assert parser.vl["LKAS"]["STEER_REQ"] == 1
CP.openpilotLongitudinalControl = True
lfa_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN)
lfa_msgs = hyundaicanfd.create_steering_messages(controller.packer, CP, can_bus, True, True, 0, 0.0)
assert [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in lfa_msgs] == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
lfa_parser.update([(1, [lfa_msgs[0]])])
assert lfa_parser.can_valid
assert lfa_parser.vl["LFA"]["DAMP_FACTOR"] == 100
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
CP.openpilotLongitudinalControl = True
controller = CarController(DBC[CP.carFingerprint], CP)
controller.frame = 2
controller.long_active_ecu = True
can_bus = CanBus(CP)
cc = SimpleNamespace(
enabled=True, latActive=True,
actuators=SimpleNamespace(longControlState=LongCtrlState.pid),
cruiseControl=SimpleNamespace(override=False, cancel=False, resume=False),
leftBlinker=False, rightBlinker=False,
hudControl=SimpleNamespace(leadDistanceBars=3),
)
cs = SimpleNamespace(
stock_lfa_msg=None, stock_lkas_msg=None,
out=SimpleNamespace(steeringAngleDeg=0.0, gearShifter=structs.CarState.GearShifter.drive),
)
msgs = controller.create_canfd_msgs(0, True, 0.0, 0.0, 42.0, -1.0, False,
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
scc_msgs = [msg for msg in msgs if msg[0] == 0x1A0]
assert len(scc_msgs) == 1
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC_CONTROL", 0)], can_bus.ECAN)
parser.update([(1, scc_msgs)])
assert parser.can_valid
assert parser.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(1.0)
assert parser.vl["SCC_CONTROL"]["ObjValid"] == 0
assert parser.vl["SCC_CONTROL"]["aReqValue"] == pytest.approx(-0.1)
assert parser.vl["SCC_CONTROL"]["aReqRaw"] == pytest.approx(-1.0)
def test_ev9_inactive_angle_steering_lets_safety_forward_stock_lkas(self):
CP = CarParams.new_message()
@@ -198,14 +198,16 @@ def test_ascent_2023_uses_d_platform_bus_layout():
assert CP.flags & SubaruFlags.D_PLATFORM
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
assert CP.flags & SubaruFlags.D_PLATFORM_CAMERA
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA
assert CanBus.main_for_cp(CP) == CanBus.alt
assert CanBus.angle_for_cp(CP) == CanBus.main
assert CanBus.angle_for_cp(CP) == CanBus.camera
assert parsers[Bus.pt].bus == CanBus.alt
assert parsers[Bus.cam].bus == CanBus.camera
assert parsers[Bus.alt].bus == CanBus.alt
assert parsers[Bus.main].bus == CanBus.main
assert controller.angle_bus == CanBus.main
assert controller.status_bus == CanBus.main
assert controller.angle_bus == CanBus.camera
assert controller.status_bus == CanBus.camera
def test_other_angle_platforms_keep_existing_bus_layout():
+1 -1
View File
@@ -247,7 +247,7 @@ class CAR(Platforms):
SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Ascent 2023-25", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
SUBARU_ASCENT.specs,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM | SubaruFlags.D_PLATFORM_CAMERA,
)
SUBARU_CROSSTREK_2025 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Crosstrek 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
@@ -89,13 +89,13 @@ def make_update_toggles():
)
def test_open_road_triggers_near_set_speed_without_a_lead():
def test_open_road_triggers_at_set_speed_without_a_lead():
cem = make_cem(model_length=80.0)
toggles = make_update_toggles()
toggles.conditional_open_road = True
triggered = cem.check_conditions(
55 * CV.MPH_TO_MS,
57 * CV.MPH_TO_MS,
make_sm(),
toggles,
v_cruise=57 * CV.MPH_TO_MS,
@@ -105,7 +105,7 @@ def test_open_road_triggers_near_set_speed_without_a_lead():
assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"]
def test_open_road_requires_at_or_below_set_speed_within_margin():
def test_open_road_does_not_trigger_below_or_far_above_set_speed():
toggles = make_update_toggles()
toggles.conditional_open_road = True
@@ -113,10 +113,10 @@ def test_open_road_requires_at_or_below_set_speed_within_margin():
above_set_speed = make_cem(model_length=80.0)
assert not too_far_below.check_conditions(
50 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
56 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
)
assert not above_set_speed.check_conditions(
58 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
59 * CV.MPH_TO_MS, make_sm(), toggles, v_cruise=57 * CV.MPH_TO_MS,
)
@@ -138,6 +138,54 @@ def test_open_road_requires_no_lead_vehicle():
)
def test_open_road_holds_exp_briefly_for_safe_lead_handoff(monkeypatch):
cem = make_cem(model_length=80.0)
toggles = make_update_toggles()
toggles.conditional_open_road = True
sm = make_update_sm(standstill=False)
monkeypatch.setattr(cem, "update_conditions", lambda *args: None)
now = [100.0]
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
cem.update(57 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
assert cem.experimental_mode
cem.starpilot_planner.lead_one.status = True
cem.starpilot_planner.lead_one.dRel = 60.0
cem.starpilot_planner.lead_one.vLead = 55 * CV.MPH_TO_MS
now[0] = 100.6
cem.update(55 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["SPEED"]
now[0] = 101.4
cem.update(55 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
assert not cem.experimental_mode
def test_open_road_does_not_delay_urgent_lead_handoff(monkeypatch):
cem = make_cem(model_length=80.0)
toggles = make_update_toggles()
toggles.conditional_open_road = True
sm = make_update_sm(standstill=False)
monkeypatch.setattr(cem, "update_conditions", lambda *args: None)
now = [100.0]
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
cem.update(57 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
cem.starpilot_planner.lead_one.status = True
cem.starpilot_planner.lead_one.dRel = 20.0
cem.starpilot_planner.lead_one.vLead = 0.0
now[0] = 101.6
cem.update(55 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
now[0] = 101.9
cem.update(55 * CV.MPH_TO_MS, sm, toggles, v_cruise=57 * CV.MPH_TO_MS)
assert not cem.experimental_mode
def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped():
v_ego = 10 * CV.MPH_TO_MS
model_length = v_ego * 10.0
@@ -64,9 +64,11 @@ class ConditionalExperimentalMode:
POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0
TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS
TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0
# Keep EXP available near the set speed on an empty road, where it can
# anticipate braking sooner. Stay at or below the set speed for safety.
OPEN_ROAD_SET_SPEED_MARGIN = 3.0 * CV.MPH_TO_MS
OPEN_ROAD_SET_SPEED_TOLERANCE = 1.0 * CV.MPH_TO_MS
OPEN_ROAD_LEAD_HANDOFF_DELAY = 0.75
OPEN_ROAD_LEAD_HANDOFF_MIN_DISTANCE = 35.0
OPEN_ROAD_LEAD_HANDOFF_MIN_TIME = 1.5
OPEN_ROAD_LEAD_HANDOFF_MAX_CLOSING_SPEED = 3.0
STOP_LIGHT_FILTER_TIME_OVERRIDES = {
"HYUNDAI_ELANTRA_2021": 0.25,
@@ -121,6 +123,9 @@ class ConditionalExperimentalMode:
self.prev_standstill_stop_hold = False
self.standstill_stop_release_pending = False
self.post_stop_launch_trigger_suppress_until = 0.0
self.open_road_triggered = False
self.prev_open_road_triggered = False
self.open_road_lead_hold_until = 0.0
def update(self, v_ego, sm, starpilot_toggles, v_cruise=None):
now = time.monotonic()
@@ -146,7 +151,9 @@ class ConditionalExperimentalMode:
self.update_conditions(v_ego, sm, starpilot_toggles)
triggered = self.check_conditions(v_ego, sm, starpilot_toggles, v_cruise)
open_road_enabled = bool(getattr(starpilot_toggles, "conditional_open_road", False))
if triggered:
self.open_road_lead_hold_until = 0.0
self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME
self.mode_false_since = 0.0
if self.status_value == CEStatus["LEAD"]:
@@ -158,8 +165,20 @@ class ConditionalExperimentalMode:
elif not self.prev_experimental_mode:
self.mode_false_since = 0.0
if not open_road_enabled:
self.open_road_lead_hold_until = 0.0
elif self.prev_open_road_triggered and self.open_road_lead_handoff_allowed(v_ego):
self.open_road_lead_hold_until = now + self.OPEN_ROAD_LEAD_HANDOFF_DELAY
hold_active = now < self.mode_hold_until
transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME
open_road_lead_hold_active = bool(
not triggered and
now < self.open_road_lead_hold_until and
self.open_road_lead_handoff_allowed(v_ego)
)
if open_road_lead_hold_active:
self.status_value = CEStatus["SPEED"]
slow_lead_hold_active = bool(
starpilot_toggles.conditional_lead and
now < self.slow_lead_mode_hold_until and
@@ -170,8 +189,9 @@ class ConditionalExperimentalMode:
elif not slow_lead_hold_active:
self.slow_lead_mode_hold_until = 0.0
self.experimental_mode = triggered or slow_lead_hold_active or hold_active or transition_buffer_active
self.experimental_mode = triggered or slow_lead_hold_active or open_road_lead_hold_active or hold_active or transition_buffer_active
self.prev_experimental_mode = self.experimental_mode
self.prev_open_road_triggered = self.open_road_triggered
ce_write_value = self.status_value if self.experimental_mode else CEStatus["OFF"]
if ce_write_value != self._prev_ce_status:
self.params_memory.put_int("CEStatus", ce_write_value)
@@ -180,6 +200,8 @@ class ConditionalExperimentalMode:
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
self.open_road_lead_hold_until = 0.0
self.prev_open_road_triggered = False
# Keep the stop-light path live at standstill so EXP stays pinned for a red
# light / stop sign. Stop signs latch until pedal, while stop lights can
@@ -207,6 +229,8 @@ class ConditionalExperimentalMode:
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
self.open_road_lead_hold_until = 0.0
self.prev_open_road_triggered = False
self._prev_ce_status = None
self.standstill_stop_release_pending = False
self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"]
@@ -257,6 +281,7 @@ class ConditionalExperimentalMode:
return bool(self.stop_light_detected or force_stop_active or model_stopped)
def check_conditions(self, v_ego, sm, starpilot_toggles, v_cruise=None):
self.open_road_triggered = False
launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until
below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead
below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead
@@ -272,18 +297,17 @@ class ConditionalExperimentalMode:
not getattr(self.starpilot_planner, "tracking_lead", False) and
not self.starpilot_planner.starpilot_following.following_lead
)
set_speed_error = float(v_cruise) - float(v_ego) if v_cruise is not None else -1.0
open_road = bool(
not launch_trigger_suppressed and
getattr(starpilot_toggles, "conditional_open_road", False) and
v_cruise is not None and
cruise_speed_available and
float(v_cruise) > 0.0 and
1.0 <= v_ego <= float(v_cruise) and
set_speed_error <= self.OPEN_ROAD_SET_SPEED_MARGIN and
float(v_cruise) <= v_ego <= float(v_cruise) + self.OPEN_ROAD_SET_SPEED_TOLERANCE and
no_lead
)
if open_road:
self.open_road_triggered = True
self.status_value = CEStatus["SPEED"]
return True
@@ -311,6 +335,21 @@ class ConditionalExperimentalMode:
return False
def open_road_lead_handoff_allowed(self, v_ego):
lead = self.starpilot_planner.lead_one
if not bool(getattr(lead, "status", False)):
return False
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", 0.0))
closing_speed = max(0.0, float(v_ego) - lead_speed)
minimum_distance = max(self.OPEN_ROAD_LEAD_HANDOFF_MIN_DISTANCE, float(v_ego) * self.OPEN_ROAD_LEAD_HANDOFF_MIN_TIME)
return bool(
lead_speed >= 1.0 and
lead_distance >= minimum_distance and
closing_speed <= self.OPEN_ROAD_LEAD_HANDOFF_MAX_CLOSING_SPEED
)
def update_conditions(self, v_ego, sm, starpilot_toggles):
self.curve_detection(v_ego, starpilot_toggles)
self.slow_lead(starpilot_toggles, v_ego)
@@ -691,7 +691,7 @@
{
"key": "CEOpenRoad",
"label": "Open Road",
"description": "Keep Experimental Mode active on an open road when driving within 3 mph below the set speed and no lead vehicle is detected. This can help the model anticipate braking sooner.",
"description": "Keep Experimental Mode active on an open road after reaching the set speed when no lead vehicle is detected. This can help the model anticipate braking sooner.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
+1 -1
View File
@@ -29,7 +29,7 @@ INJECTED_SECTION_PARAMS = {
{
"key": "CEOpenRoad",
"label": "Open Road",
"description": "Keep Experimental Mode active on an open road when driving within 3 mph below the set speed and no lead vehicle is detected. This can help the model anticipate braking sooner.",
"description": "Keep Experimental Mode active on an open road after reaching the set speed when no lead vehicle is detected. This can help the model anticipate braking sooner.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",