mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Pink Lady
This commit is contained in:
@@ -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():
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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",
|
||||
|
||||
Reference in New Issue
Block a user